Add support for Apple's OPACK encoding and TLV format

1 files changed, 196 insertions, 0 deletions

diff --git a/src/tlv.c b/src/tlv.c
new file mode 100644
index 0000000..d08c8f3
--- /dev/null
+++ b/src/tlv.c
@@ -0,0 +1,196 @@
+/*
+ * tlv.c
+ * Simple TLV implementation.
+ *
+ * Copyright (c) 2021 Nikias Bassen, All Rights Reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "common.h"
+#include "libimobiledevice-glue/tlv.h"
+#include "endianness.h"
+
+LIBIMOBILEDEVICE_GLUE_API tlv_buf_t tlv_buf_new()
+{
+	tlv_buf_t tlv = malloc(sizeof(struct tlv_buf));
+	tlv->capacity = 1024;
+	tlv->length = 0;
+	tlv->data = malloc(tlv->capacity);
+	return tlv;	
+}
+
+LIBIMOBILEDEVICE_GLUE_API void tlv_buf_free(tlv_buf_t tlv)
+{
+	if (tlv) {
+		free(tlv->data);
+		free(tlv);
+	}
+}
+
+LIBIMOBILEDEVICE_GLUE_API void tlv_buf_append(tlv_buf_t tlv, uint8_t tag, unsigned int length, void* data)
+{
+	if (!tlv || !tlv->data) {
+		return;
+	}
+	unsigned int req_len = (length > 255) ? (length / 255) * 257 + (2 + (length % 255)) : length;
+	if (tlv->length + req_len > tlv->capacity) {
+		unsigned int newcapacity = tlv->capacity + ((req_len / 1024) + 1) * 1024;
+		unsigned char* newdata = realloc(tlv->data, newcapacity);
+		if (!newdata) {
+			fprintf(stderr, "%s: ERROR: Failed to realloc\n", __func__);
+			return;
+		}
+		tlv->data = newdata;
+		tlv->capacity = newcapacity;
+	}
+	unsigned char* p = tlv->data + tlv->length;
+	unsigned int cur = 0;
+	while (length - cur > 0) {
+		if (length - cur > 255) {
+			*(p++) = tag;
+			*(p++) = 0xFF;
+			memcpy(p, (unsigned char*)data + cur, 255);
+			p += 255;
+			cur += 255;
+		} else {
+			uint8_t rem = length - cur;
+			*(p++) = tag;
+			*(p++) = rem;
+			memcpy(p, (unsigned char*)data + cur, rem);
+			p += rem;
+			cur += rem;
+		}
+	}
+	tlv->length = p - tlv->data;
+}
+
+LIBIMOBILEDEVICE_GLUE_API unsigned char* tlv_get_data_ptr(const void* tlv_data, void* tlv_end, uint8_t tag, uint8_t* length)
+{
+	unsigned char* p = (unsigned char*)tlv_data;
+	unsigned char* end = (unsigned char*)tlv_end;
+	while (p < end) {
+		uint8_t cur_tag = *(p++);
+		uint8_t len = *(p++);
+		if (cur_tag == tag) {
+			*length = len;
+			return p;
+		}
+		p+=len;
+	}
+	return NULL;
+}
+
+LIBIMOBILEDEVICE_GLUE_API int tlv_data_get_uint(const void* tlv_data, unsigned int tlv_length, uint8_t tag, uint64_t* value)
+{
+	if (!tlv_data || tlv_length < 2 || !value) {
+		return 0;
+	}
+	uint8_t length = 0;
+	unsigned char* ptr = tlv_get_data_ptr(tlv_data, (unsigned char*)tlv_data+tlv_length, tag, &length);
+	if (!ptr) {
+		return 0;
+	}
+	if (ptr + length > (unsigned char*)tlv_data + tlv_length) {
+		return 0;
+	}
+	if (length == 1) {
+		uint8_t val = *ptr;
+		*value = val;
+	} else if (length == 2) {
+		uint16_t val = *(uint16_t*)ptr;
+		val = le16toh(val);
+		*value = val;
+	} else if (length == 4) {
+		uint32_t val = *(uint32_t*)ptr;
+		val = le32toh(val);
+		*value = val;
+	} else if (length == 8) {
+		uint64_t val = *(uint64_t*)ptr;
+		val = le64toh(val);
+		*value = val;
+	} else {
+		return 0;
+	}
+	return 1;
+}
+
+LIBIMOBILEDEVICE_GLUE_API int tlv_data_get_uint8(const void* tlv_data, unsigned int tlv_length, uint8_t tag, uint8_t* value)
+{
+	if (!tlv_data || tlv_length < 2 || !value) {
+		return 0;
+	}
+	uint8_t length = 0;
+	unsigned char* ptr = tlv_get_data_ptr(tlv_data, (unsigned char*)tlv_data+tlv_length, tag, &length);
+	if (!ptr) {
+		return 0;
+	}
+	if (length != 1) {
+		return 0;
+	}
+	if (ptr + length > (unsigned char*)tlv_data + tlv_length) {
+		return 0;
+	}
+	*value = *ptr;
+	return 1;
+}
+
+LIBIMOBILEDEVICE_GLUE_API int tlv_data_copy_data(const void* tlv_data, unsigned int tlv_length, uint8_t tag, void** out, unsigned int* out_len)
+{
+	if (!tlv_data || tlv_length < 2 || !out || !out_len) {
+		return 0;
+	}
+	*out = NULL;
+	*out_len = 0;
+
+	unsigned char* dest = NULL;
+	unsigned int dest_len = 0;
+
+	unsigned char* ptr = (unsigned char*)tlv_data;
+	unsigned char* end = (unsigned char*)tlv_data + tlv_length;
+	while (ptr < end) {
+		uint8_t length = 0;
+		ptr = tlv_get_data_ptr(ptr, end, tag, &length);
+		if (!ptr) {
+			break;
+		}
+		unsigned char* newdest = realloc(dest, dest_len + length);
+		if (!newdest) {
+			free(dest);
+			return 0;
+		}
+		dest = newdest;
+		memcpy(dest + dest_len, ptr, length);
+		dest_len += length;
+		ptr += length;
+	}
+	if (!dest) {
+		return 0;
+	}
+
+	*out = (void*)dest;
+	*out_len = dest_len;
+
+	return 1;
+}