cmdu.c

/*
 * cmdu.c - IEEE1905 CMDU and TLV handling
 *
 * Copyright (C) 2021 IOPSYS Software Solutions AB. All rights reserved.
 *
 * Author: anjan.chanda@iopsys.eu
 *
 * See LICENSE file for license related information.
 *
 */


#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>

#include "util.h"
#include "bufutil.h"
#include "1905_tlvs.h"
#include "cmdu.h"


const char *cmdu_type2str(uint16_t type)
{

#define T2STR(t)	case CMDU_TYPE_ ## t: return #t;

	switch (type) {
	T2STR(TOPOLOGY_DISCOVERY)
	T2STR(TOPOLOGY_NOTIFICATION)
	T2STR(TOPOLOGY_QUERY)
	T2STR(TOPOLOGY_RESPONSE)
	T2STR(VENDOR_SPECIFIC)
	T2STR(LINK_METRIC_QUERY)
	T2STR(LINK_METRIC_RESPONSE)
	T2STR(AP_AUTOCONFIGURATION_SEARCH)
	T2STR(AP_AUTOCONFIGURATION_RESPONSE)
	T2STR(AP_AUTOCONFIGURATION_WSC)
	T2STR(AP_AUTOCONFIGURATION_RENEW)
	T2STR(PUSH_BUTTON_EVENT_NOTIFICATION)
	T2STR(PUSH_BUTTON_JOIN_NOTIFICATION)
	T2STR(HIGHER_LAYER_QUERY)
	T2STR(HIGHER_LAYER_RESPONSE)
	T2STR(INTERFACE_POWER_CHANGE_REQUEST)
	T2STR(INTERFACE_POWER_CHANGE_RESPONSE)
	T2STR(GENERIC_PHY_QUERY)
	T2STR(GENERIC_PHY_RESPONSE)
	}

	return "UNKNOWN";

#undef T2STR
}

const char *tlv_type2str(uint8_t type)
{

#define T2STR(t)	case TLV_TYPE_ ## t: return #t;

	switch (type) {
	T2STR(END_OF_MESSAGE)
	T2STR(AL_MAC_ADDRESS_TYPE)
	T2STR(DEVICE_INFORMATION_TYPE)
	T2STR(DEVICE_BRIDGING_CAPABILITIES)
	T2STR(NON_1905_NEIGHBOR_DEVICE_LIST)
	T2STR(NEIGHBOR_DEVICE_LIST)
	T2STR(TRANSMITTER_LINK_METRIC)
	T2STR(RECEIVER_LINK_METRIC)
	T2STR(SEARCHED_ROLE)
	T2STR(AUTOCONFIG_FREQ_BAND)
	T2STR(SUPPORTED_ROLE)
	T2STR(SUPPORTED_FREQ_BAND)
	T2STR(WSC)
	}

	return "UNKNOWN";

#undef T2STR
}

struct tlv *tlv_alloc(uint16_t datalen)
{
	struct tlv *n = calloc(1, sizeof(*n) + datalen);

	return n;
}

void tlv_zero(struct tlv *t)
{
	if (t)
		memset(t, 0, t->len + sizeof(*t));
}

void tlv_free_linear(struct tlv *t)
{
	if (t)
		free(t);
}

int tlv_ok(struct tlv *t, int rem)
{
	uint16_t l = buf_get_be16(&((uint8_t *)t)[1]);
	if (rem >= sizeof(struct tlv) && l <= rem - 3)
		return 1;

	return 0;
}

struct tlv *tlv_next(struct tlv *t, int *rem)
{
	uint16_t l = buf_get_be16(&((uint8_t *)t)[1]);

	*rem -= (l + 3);
	return (struct tlv *)((uint8_t *)t + l + 3);
}

uint16_t tlv_length(struct tlv *t)
{
	return buf_get_be16(&((uint8_t *)t)[1]);
}

uint16_t tlv_total_length(struct tlv *t)
{
	return tlv_length(t) + 3;
}

int is_cmdu_type_valid(uint16_t type)
{
	return (type >= CMDU_TYPE_TOPOLOGY_DISCOVERY &&
		type <= CMDU_TYPE_MAX);
}

int is_cmdu_tlv_required(uint16_t type)
{
	return !(type == CMDU_TYPE_TOPOLOGY_QUERY ||
		type == CMDU_TYPE_HIGHER_LAYER_QUERY ||
		type == CMDU_TYPE_GENERIC_PHY_QUERY);
}

int cmdu_should_relay(uint16_t type)
{
	return (type == CMDU_TYPE_TOPOLOGY_NOTIFICATION ||
		type == CMDU_TYPE_AP_AUTOCONFIGURATION_SEARCH ||
		type == CMDU_TYPE_AP_AUTOCONFIGURATION_RENEW ||
		type == CMDU_TYPE_PUSH_BUTTON_EVENT_NOTIFICATION ||
		type == CMDU_TYPE_PUSH_BUTTON_JOIN_NOTIFICATION);
}

/* for unicast request types only */
int is_cmdu_type_response(uint16_t type)
{
	return (type == CMDU_TYPE_TOPOLOGY_RESPONSE ||
		type == CMDU_TYPE_LINK_METRIC_RESPONSE ||
		/* type == CMDU_TYPE_AP_AUTOCONFIGURATION_RESPONSE || */
		type == CMDU_TYPE_HIGHER_LAYER_RESPONSE ||
		type == CMDU_TYPE_INTERFACE_POWER_CHANGE_RESPONSE);
}

uint16_t cmdu_expect_response(uint16_t req_type)
{
	switch (req_type) {
		case CMDU_TYPE_TOPOLOGY_QUERY:
			return CMDU_TYPE_TOPOLOGY_RESPONSE;
		case CMDU_TYPE_LINK_METRIC_QUERY:
			return CMDU_TYPE_LINK_METRIC_RESPONSE;
		case CMDU_TYPE_AP_AUTOCONFIGURATION_SEARCH:
			return CMDU_TYPE_AP_AUTOCONFIGURATION_RESPONSE;
		case CMDU_TYPE_AP_AUTOCONFIGURATION_WSC:
			return CMDU_TYPE_AP_AUTOCONFIGURATION_WSC;
		case CMDU_TYPE_AP_AUTOCONFIGURATION_RENEW:
			return CMDU_TYPE_AP_AUTOCONFIGURATION_RESPONSE;
		case CMDU_TYPE_HIGHER_LAYER_QUERY:
			return CMDU_TYPE_HIGHER_LAYER_RESPONSE;
		case CMDU_TYPE_INTERFACE_POWER_CHANGE_REQUEST:
			return CMDU_TYPE_INTERFACE_POWER_CHANGE_RESPONSE;
		case CMDU_TYPE_GENERIC_PHY_QUERY:
			return CMDU_TYPE_GENERIC_PHY_RESPONSE;
		default:
			break;
	}

	return CMDU_TYPE_NONE;
}

struct cmdu_buff *cmdu_alloc(int size)
{
#define CMDU_RESERVE_HEADSPACE	32

	struct cmdu_buff *n;
	uint8_t *p;


	p = calloc(1, sizeof(*n) + size + CMDU_RESERVE_HEADSPACE);
	if (!p)
		return NULL;

	n = (struct cmdu_buff *)p;
	n->head = (uint8_t *)(n + 1) + CMDU_RESERVE_HEADSPACE;
	n->end = n->head + size;
	n->data = n->head;
	n->cdata = NULL;
	//n->cdata = (struct cmdu_linear *)(n->head + 18);
	//n->data = (uint8_t *)(n->cdata + 1);
	n->tail = n->data;
	n->num_frags = 0;
	n->datalen = 0;
	n->len = 0;
	n->head -= 18;
	INIT_LIST_HEAD(&n->fraglist);

	return n;
}

struct cmdu_buff *cmdu_alloc_frame(int size)
{
	struct cmdu_buff *f;

	f = cmdu_alloc(size + sizeof(struct cmdu_header));
	if (!f)
		return NULL;

	f->cdata = (struct cmdu_linear *)(f->head + 18);
	f->data = (uint8_t *)(f->cdata + 1);
	f->tail = f->data;

	return f;
}

struct cmdu_buff *cmdu_alloc_default(void)
{
#define ETH_FRAME_SZ	1500

	return cmdu_alloc(ETH_FRAME_SZ);
}

struct cmdu_buff *cmdu_alloc_nohdr(void)
{
	struct cmdu_buff *f;

	f = cmdu_alloc(ETH_FRAME_SZ);
	if (f) {
		f->cdata = NULL;
		f->data = (uint8_t *)(f->head + 18);
		f->tail = f->data;
	}

	return f;
}

struct cmdu_buff *cmdu_alloc_simple(uint16_t type, uint16_t *mid)
{
	struct cmdu_buff *f;


	f = cmdu_alloc_default();
	if (!f) {
		fprintf(stderr, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	f->cdata = (struct cmdu_linear *)(f->head + 18);
	f->data = (uint8_t *)(f->cdata + 1);
	f->tail = f->data;

	cmdu_set_type(f, type);

	if (*mid == 0)
		*mid = cmdu_get_next_mid();

	cmdu_set_mid(f, *mid);
	CMDU_SET_LAST_FRAGMENT(f->cdata);

	return f;
}

void cmdu_free(struct cmdu_buff *c)
{
	if (c) {
		list_flush(&c->fraglist, struct cmdu_frag, list);
		free(c);
	}
}

int cmdu_size(struct cmdu_buff *c)
{
	return c ? c->datalen + sizeof(struct cmdu_header) : 0;
}

#if 0
int cmdu_reserve(struct cmdu_buff *c, size_t s)
{
	if (!c)
		return -1;

	if (c->head - (uint8_t *)(c + 1) < s)
		return -1;

	c->head -= s;

	return 0;
}
#endif


int cmdu_copy_tlvs_linear(struct cmdu_buff *c, uint8_t *tlvs, uint32_t tlvslen)
{
	if (c->end - c->tail < tlvslen)
		return -1;

	memcpy(c->tail, tlvs, tlvslen);
	c->tail += tlvslen;
	c->datalen += tlvslen;

	return 0;
}

struct cmdu_buff *cmdu_alloc_custom(uint16_t type, uint16_t *mid, char *ifname,
				    uint8_t *origin, uint8_t *tlvs,
				    uint32_t tlvslen)
{
	struct cmdu_buff *f;
	int ret;


	f = cmdu_alloc_frame(tlvslen + 128);
	if (!f) {
		fprintf(stderr, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	cmdu_set_type(f, type);
	cmdu_set_mid(f, *mid);

	ret = cmdu_copy_tlvs_linear(f, tlvs, tlvslen);
	if (ret) {
		fprintf(stderr, "%s: tlv-length > max cmdu size!\n", __func__);
		cmdu_free(f);
		return NULL;
	}

	memcpy(f->dev_macaddr, origin, 6);
	if (ifname)
		strncpy(f->dev_ifname, ifname, 15);

	return f;
}

struct cmdu_buff *cmdu_clone(struct cmdu_buff *frm)
{
	struct cmdu_buff *f;
	int len;


	if (!frm || !frm->cdata) {
		fprintf(stderr, "%s: cmdu for cloning is invalid!\n", __func__);
		return NULL;
	}

	len = cmdu_size(frm);
	f = cmdu_alloc(len);
	if (!f) {
		fprintf(stderr, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	f->cdata = (struct cmdu_linear *)(f->head + 18);
	f->data = (uint8_t *)(f->cdata + 1);
	f->datalen = frm->datalen;
	f->tail = f->data + f->datalen;
	memcpy(f->cdata, frm->cdata, len);

	memcpy(f->dev_macaddr, frm->dev_macaddr, 6);
	strncpy(f->dev_ifname, frm->dev_ifname, 15);
	memcpy(f->origin, frm->origin, 6);

	return f;
}

struct cmdu_buff *cmdu_realloc(struct cmdu_buff *c, size_t size)
{
	ptrdiff_t head_off, data_off, cdata_off;
	struct cmdu_buff *f;
	ptrdiff_t origsize;
	uint8_t *n;


	if (!c)
		return NULL;

	origsize = (c->end - (uint8_t *)c);
	if (size < origsize)
		return c;

	head_off = c->head - (uint8_t *)c;
	data_off = c->data - (uint8_t *)c;
	cdata_off = c->cdata ? (uint8_t *)c->cdata - (uint8_t *)c : 0;

	n = realloc((uint8_t *)c, sizeof(*f) + size + CMDU_RESERVE_HEADSPACE);
	if (!n)
		return NULL;

	f = (struct cmdu_buff *)n;
	f->head = n + head_off;
	f->data = n + data_off;
	f->tail = f->data + f->datalen;
	f->cdata = cdata_off ? (struct cmdu_linear *)(n + cdata_off) : NULL;
	f->end = f->head + size;

	/* TODO: reconstruct fraglist */
	INIT_LIST_HEAD(&f->fraglist);

	return f;
}

int cmdu_copy_tlvs(struct cmdu_buff *c, struct tlv *tv[], int tv_arrsize)
{
	uint16_t tlvslen = 0;
	int i;


	for (i = 0; i < tv_arrsize; i++)
		tlvslen += tv[i]->len;


	if (c->end - c->tail < tlvslen)
		return -1;


	for (i = 0; i < tv_arrsize; i++) {
		uint16_t tlen = tv[i]->len;

		*c->tail = tv[i]->type;
		buf_put_be16(c->tail + 1, tlen);
		memcpy(c->tail + 3, tv[i]->data, tlen);
		c->tail += tlen + sizeof(struct tlv);
		c->datalen += tlen + sizeof(struct tlv);
	}

	return 0;
}

int cmdu_put_tlv(struct cmdu_buff *c, struct tlv *t)
{
	uint16_t tlen;


	if (!c || !t)
		return -1;


	tlen = t->len;

	if (c->end - c->tail < tlen) {
		fprintf(stderr, "%s: %d: c->end = %p c->tail = %p\n",
			__func__, __LINE__, c->end, c->tail);

		return -1;
	}

	if ((uint8_t *)t != c->tail) {
		fprintf(stderr, "%s: tlv outside cmdu buffer; use cmdu_copy_tlv() instead\n",
			__func__);
		return -1;
	}


	buf_put_be16(c->tail + 1, tlen);
	c->tail += tlen + sizeof(*t);
	c->datalen += tlen + sizeof(*t);

	return 0;
}

int cmdu_put(struct cmdu_buff *c, uint8_t *bytes, int len)
{
	if (!c || !bytes)
		return -1;

	if (c->end - c->tail < len) {
		fprintf(stderr, "%s: %d: c->end = %p c->tail = %p\n",
			__func__, __LINE__, c->end, c->tail);

		return -1;
	}

	memcpy(c->tail, bytes, len);
	c->tail += len;
	c->datalen += len;

	return 0;
}

int cmdu_put_eom(struct cmdu_buff *c)
{
	uint8_t eom[3] = {0};

	return cmdu_put(c, eom, sizeof(eom));
}

struct tlv *cmdu_reserve_tlv(struct cmdu_buff *c, uint16_t tlv_datalen)
{
	uint16_t len = tlv_datalen + TLV_HLEN;

	if (!c)
		return NULL;

	if (c->end - c->tail < len) {
		fprintf(stderr, "%s: Failed to reserve %hu! Allocate new cmdu fragment\n",
			__func__, tlv_datalen);

		return NULL;
	}

	return (struct tlv *)c->tail;
}

int cmdu_parse_tlv_single(struct cmdu_buff *c, struct tlv *tv[],
			  struct tlv_policy *policy, int *num)
{
	struct tlv *t;
	int i = 0;
	int len;


	if (!c)
		return -1;

	if (*num == 0)
		return 0;

	memset(tv, 0, *num * sizeof(struct tlv *));
	len = c->datalen;

	cmdu_for_each_tlv(t, c->data, len) {
		if (policy->type != t->type)
			continue;

		if (policy->minlen > 0 && tlv_length(t) < policy->minlen)
			continue;

		if (policy->maxlen > 0 && tlv_length(t) > policy->maxlen)
			continue;

		if (tv[0] && policy->present == TLV_PRESENT_ONE)
			break;

		if (i >= *num)
			break;

		tv[i++] = t;
	}

	*num = i;
	return 0;
}

int cmdu_parse_tlvs(struct cmdu_buff *c, struct tlv *tv[][16],
		    struct tlv_policy *policy, int policy_len)
{
	int idx[policy_len];
	struct tlv *t;
	int len;
	int i;


	if (!c)
		return -1;

	for (i = 0; i < policy_len; i++) {
		memset(tv[i], 0, 16 * sizeof(struct tlv *));
		idx[i] = 0;
	}
	len = c->datalen;

	cmdu_for_each_tlv(t, c->data, len) {
		for (i = 0; i < policy_len; i++) {
			if (policy[i].type != t->type)
				continue;

			if (policy[i].minlen > 0 &&
			    tlv_length(t) < policy[i].minlen)
				continue;

			if (policy[i].maxlen > 0 &&
			    tlv_length(t) > policy[i].maxlen)
				continue;

			if (tv[i][0]) {
				if (policy[i].present == TLV_PRESENT_ONE)
					continue;
			}

			tv[i][idx[i]++] = t;
		}
	}

	return 0;
}

/* Extracts the first matching tlv from tlv-stream.
 * This function is destructive, i.e. it modifies the passed cmdu buffer.
 * Use cmdu_peek_tlv() for the non-destructive version.
 */
struct tlv *cmdu_extract_tlv(struct cmdu_buff *c, uint8_t tlv_type)
{
	struct tlv *t, *tmp;
	int found = 0;
	int inlen;


	if (!c)
		return NULL;

	inlen = c->datalen;

	cmdu_for_each_tlv(t, c->data, inlen) {
		if (t->type == tlv_type) {
			found = 1;
			break;
		}
	}

	if (found) {
		uint16_t tlen = tlv_total_length(t);

		tmp = tlv_alloc(tlv_length(t));
		if (tmp)
			memcpy(tmp, t, tlen);

		inlen -= tlen;
		memmove(t, (uint8_t *)t + tlen, inlen);
		c->datalen -= tlen;
		return tmp;
	}

	return NULL;
}

struct tlv *cmdu_peek_tlv(struct cmdu_buff *c, uint8_t tlv_type)
{
	struct tlv *t;
	int len;

	if (!c)
		return NULL;

	len = c->datalen;

	cmdu_for_each_tlv(t, c->data, len) {
		if (t->type == tlv_type)
			return t;
	}

	return NULL;
}