pipewire/src/modules/module-raop/tlv.c

/* 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;
}