tango-kni/common/src/kni_cmsg.cpp

#include "kni_utils.h"
#include "kni_cmsg.h"

struct kni_cmsg_tlv
{
	uint16_t type;
	uint16_t length;
	unsigned char value_as_string[0];
} __attribute__((packed));

struct kni_cmsg
{
	uint16_t nr_tlvs;
	struct kni_cmsg_tlv* tlvs[KNI_CMSG_TLV_NR_MAX];
	uint16_t size;
} __attribute__((packed));

struct kni_cmsg_serialize_header
{
	uint8_t __magic__[2]; /* Must be 0x4d, 0x5a */
	uint16_t nr_tlvs;
	struct kni_cmsg_tlv tlvs[0];
} __attribute__((packed));

struct kni_cmsg* kni_cmsg_init()
{
	struct kni_cmsg *cmsg = ALLOC(struct kni_cmsg, 1);
	cmsg->size = sizeof(struct kni_cmsg_serialize_header);
	return cmsg;
}

void kni_cmsg_destroy(struct kni_cmsg *cmsg)
{
	if(cmsg != NULL)
	{
		for(int i = 0; i < KNI_CMSG_TLV_NR_MAX; i++)
		{
			FREE(&(cmsg->tlvs[i]));
		}
	}
	FREE(&cmsg);
}

int kni_cmsg_set(struct kni_cmsg *cmsg, uint16_t type, const unsigned char *value, uint16_t size)
{
	if(type >= KNI_CMSG_TLV_NR_MAX)
	{
		return KNI_CMSG_INVALID_TYPE;
	}
	struct kni_cmsg_tlv *tlv = cmsg->tlvs[type];
	uint16_t length = sizeof(struct kni_cmsg_tlv) + size;
	if(tlv == NULL)
	{
		tlv = (struct kni_cmsg_tlv*)ALLOC(char, length);
		cmsg->nr_tlvs++;
		cmsg->size += length;
	}
	tlv->type = type;
	tlv->length = length;
	memcpy(tlv->value_as_string, value, size);
	cmsg->tlvs[type] = tlv;
	return 0;
}

int kni_cmsg_get(struct kni_cmsg *cmsg, uint16_t type, uint16_t *size, unsigned char **pvalue)
{
	struct kni_cmsg_tlv *tlv = NULL;
	if(type >= KNI_CMSG_TLV_NR_MAX || (tlv = cmsg->tlvs[type]) == NULL)
	{
		*size = 0;
		return KNI_CMSG_INVALID_TYPE;
	}
	*size = tlv->length - sizeof(struct kni_cmsg_tlv);
	*pvalue = tlv->value_as_string;
	return 0;
}

uint16_t kni_cmsg_serialize_size_get(struct kni_cmsg *cmsg)
{
	return cmsg->size;
}

int kni_cmsg_serialize(struct kni_cmsg *cmsg, unsigned char *buff, uint16_t bufflen, uint16_t *serialize_len)
{
	uint16_t size = cmsg->size;
	if(bufflen < size)
	{
		return KNI_CMSG_BUFF_NOT_ENOUGH;
	}
	if(size < sizeof(struct kni_cmsg_serialize_header))
	{
		return KNI_CMSG_INVALID_FORMAT;
	}
	struct kni_cmsg_serialize_header *header = (struct kni_cmsg_serialize_header*)buff;
	header->__magic__[0] = 0x4d;
	header->__magic__[1] = 0x5a;
	header->nr_tlvs = htons(cmsg->nr_tlvs);
	uint16_t offset = sizeof(struct kni_cmsg_serialize_header);
	int count = 0;
	for(int i = 0; i < KNI_CMSG_TLV_NR_MAX; i++){
		if(cmsg->tlvs[i] != NULL)
		{
			count++;
		}
	}
	if(count != cmsg->nr_tlvs)
	{
		return KNI_CMSG_INVALID_FORMAT;
	}
	for(int i = 0; i < KNI_CMSG_TLV_NR_MAX; i++)
	{
		struct kni_cmsg_tlv *tlv = cmsg->tlvs[i];
		if(tlv == NULL)
		{
			continue;
		}
		if(i != tlv->type)
		{
			return KNI_CMSG_INVALID_FORMAT;
		}
		uint16_t length = tlv->length;
		if(length < sizeof(struct kni_cmsg_tlv) || offset + length > size)
		{
			return KNI_CMSG_INVALID_FORMAT;
		}
		memcpy((char*)header + offset, (void*)tlv, length);
		struct kni_cmsg_tlv *tlv1 = (struct kni_cmsg_tlv*)((char*)header + offset);
		tlv1->type = htons(tlv->type);
		tlv1->length = htons(tlv->length);
		offset += length;
	}
	if(offset != size)
	{
		return KNI_CMSG_INVALID_FORMAT;
	}
	*serialize_len = size;
	return 0;
}

int kni_cmsg_deserialize(const unsigned char *data, uint16_t len, struct kni_cmsg** pcmsg)
{
	struct kni_cmsg *cmsg = NULL;
	struct kni_cmsg_serialize_header *header = (struct kni_cmsg_serialize_header*)data;
	int offset = 0, nr_tlvs = -1;
	if(len < sizeof(struct kni_cmsg_serialize_header))
	{
		goto error_out;
	}
	if(header->__magic__[0] != 0x4d || header->__magic__[1] != 0x5a)
	{
		goto error_out;
	}
	cmsg = ALLOC(struct kni_cmsg, 1);
	offset = sizeof(struct kni_cmsg_serialize_header);
	nr_tlvs = ntohs(header->nr_tlvs);
	for(int i = 0; i < nr_tlvs; i++)
	{
		struct kni_cmsg_tlv *tlv = (struct kni_cmsg_tlv*)(data + offset);
		if(offset + sizeof(struct kni_cmsg_tlv) > len)
		{
			goto error_out;
		}
		uint16_t type = ntohs(tlv->type);
		uint16_t length = ntohs(tlv->length);
		if(length < sizeof(struct kni_cmsg_tlv) || offset + length > len)
		{
			goto error_out;
		}
		int ret = kni_cmsg_set(cmsg, type, data + offset + sizeof(struct kni_cmsg_tlv), length - sizeof(struct kni_cmsg_tlv));
		if(ret < 0)
		{
			goto error_out;
		}
		offset += length;
	}
	cmsg->size = offset;
	*pcmsg = cmsg;
	return 0;

error_out:
	kni_cmsg_destroy(cmsg);
	return KNI_CMSG_INVALID_FORMAT;
}