cmdu.c

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

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


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


uint16_t g_mid;


uint16_t cmdu_get_next_mid(void)
{
	return g_mid++;
}

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

static 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);
}

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;

	fprintf(stderr, "%s: %d\n", __func__, __LINE__);
	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;
	//n->cdata->hdr.version = 0;
	//n->data = (uint8_t *)(n->cdata + 1);
	n->tail = n->data;
	n->num_frags = 0;
	n->datalen = 0;
	n->len = 0;
	n->head -= 18;

	return n;
}

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

	return cmdu_alloc(ETH_FRAME_SZ);
}

void cmdu_free(struct cmdu_buff *c)
{
	if (c) {
		free(c);
		// TODO: fraglist
	}
}

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 0
	struct cmdu *n;


	if (!is_cmdu_type_valid(type))
		return NULL;

	//if (!is_tlv_stream_valid(type, tlvs, tlvslen))
	//	return NULL;


	n = calloc(1, sizeof(*n) + tlvslen * sizeof(uint8_t));
	if (!n)
		return NULL;

	n->hdr.version = 0;
	n->hdr.type = type;

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

	n->hdr.mid = *mid;

	if (cmdu_should_relay(type))
		CMDU_SET_RELAY_MCAST(n);

#endif

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

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

	return 0;
}

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

	*c->tail = t->type;
	buf_put_be16(c->tail + 1, t->len);
	memcpy(c->tail + 3, t->data, 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_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;
}

/* Find a tlv and pulls it from tlv stream.
 * This function is destructive.
 * Use cmdu_peek_tlv() for the non-destructive version.
 */
struct tlv *cmdu_get_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;
}