module-raop: Add TLV utils

Adds MIT-licensed TLV helper utilities,
taken from https://github.com/maximkulkin/esp-homekit
and adapted for usage in PipeWire.

src/modules/module-raop/tlv.c

@@ -0,0 +1,183 @@
+/* PipeWire */
+/* SPDX-FileCopyrightText: Copyright © 2018 Maxim Kulkin */
+/* SPDX-License-Identifier: MIT */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <inttypes.h>
+
+#include "tlv.h"
+
+tlv_values_t * tlv_new(void)
+{
+	tlv_values_t *values = malloc(sizeof(tlv_values_t));
+	if (!values)
+		return NULL;
+
+	values->head = NULL;
+	return values;
+}
+
+void tlv_free(tlv_values_t *values)
+{
+	if (!values)
+		return;
+
+	tlv_t *t = values->head;
+	while (t) {
+		tlv_t *t2 = t;
+		t = t->next;
+		if (t2->value)
+			free(t2->value);
+		free(t2);
+	}
+	free(values);
+}
+
+static int tlv_add_value_(tlv_values_t *values, uint8_t type, uint8_t *value, size_t size)
+{
+	tlv_t *tlv = malloc(sizeof(tlv_t));
+	if (!tlv) {
+		return TLV_ERROR_MEMORY;
+	}
+	tlv->type = type;
+	tlv->size = size;
+	tlv->value = value;
+	tlv->next = NULL;
+
+	if (!values->head) {
+		values->head = tlv;
+	} else {
+		tlv_t *t = values->head;
+		while (t->next) {
+			t = t->next;
+		}
+		t->next = tlv;
+	}
+
+	return 0;
+}
+
+int tlv_add_value(tlv_values_t *values, uint8_t type, const uint8_t *value, size_t size)
+{
+	uint8_t *data = NULL;
+	int ret;
+	if (size) {
+		data = malloc(size);
+		if (!data) {
+			return TLV_ERROR_MEMORY;
+		}
+		memcpy(data, value, size);
+	}
+	ret = tlv_add_value_(values, type, data, size);
+	if (ret < 0)
+		free(data);
+	return ret;
+}
+
+tlv_t * tlv_get_value(const tlv_values_t *values, uint8_t type)
+{
+	tlv_t *t = values->head;
+	while (t) {
+		if (t->type == type)
+			return t;
+		t = t->next;
+	}
+	return NULL;
+}
+
+int tlv_format(const tlv_values_t *values, uint8_t *buffer, size_t *size)
+{
+	size_t required_size = 0;
+	tlv_t *t = values->head;
+	while (t) {
+		required_size += t->size + 2 * ((t->size + 254) / 255);
+		t = t->next;
+	}
+
+	if (*size < required_size) {
+		*size = required_size;
+		return TLV_ERROR_INSUFFICIENT_SIZE;
+	}
+
+	*size = required_size;
+
+	t = values->head;
+	while (t) {
+		uint8_t *data = t->value;
+		if (!t->size) {
+			buffer[0] = t->type;
+			buffer[1] = 0;
+			buffer += 2;
+			t = t->next;
+			continue;
+		}
+
+		size_t remaining = t->size;
+
+		while (remaining) {
+			buffer[0] = t->type;
+			size_t chunk_size = (remaining > 255) ? 255 : remaining;
+			buffer[1] = chunk_size;
+			memcpy(&buffer[2], data, chunk_size);
+			remaining -= chunk_size;
+			buffer += chunk_size + 2;
+			data += chunk_size;
+		}
+
+		t = t->next;
+	}
+
+	return 0;
+}
+
+int tlv_parse(const uint8_t *buffer, size_t length, tlv_values_t *values)
+{
+	size_t i = 0;
+	int ret;
+	while (i < length) {
+		uint8_t type = buffer[i];
+		size_t size = 0;
+		uint8_t *data = NULL;
+
+		// scan TLVs to accumulate total size of subsequent TLVs with same type (chunked data)
+		size_t j = i;
+		while (j < length && buffer[j] == type && buffer[j+1] == 255) {
+			size_t chunk_size = buffer[j+1];
+			size += chunk_size;
+			j += chunk_size + 2;
+		}
+		if (j < length && buffer[j] == type) {
+			size_t chunk_size = buffer[j+1];
+			size += chunk_size;
+		}
+
+		// allocate memory to hold all pieces of chunked data and copy data there
+		if (size == 0)
+			return -2;
+
+		data = malloc(size);
+		if (!data)
+			return TLV_ERROR_MEMORY;
+
+		uint8_t *p = data;
+
+		size_t remaining = size;
+		while (remaining) {
+			size_t chunk_size = buffer[i+1];
+			memcpy(p, &buffer[i+2], chunk_size);
+			p += chunk_size;
+			i += chunk_size + 2;
+			remaining -= chunk_size;
+		}
+
+		ret = tlv_add_value_(values, type, data, size);
+		if (ret < 0) {
+			free(data);
+			return ret;
+		}
+	}
+
+	return 0;
+}