diff options
Diffstat (limited to 'src/tlv.c')
| -rw-r--r-- | src/tlv.c | 196 |
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; +} |