#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "datatype.h"
#include "utilities.h"
static void parse_tlv_tag(pBytes tag, BYTE *src)
{
if (0x1f == (src[0]&0x1f))
tag->size = 2;
else
tag->size = 1;
tag->data = src;
return;
}
static int parse_tlv_length(pBytes tlv, BYTE *src)
{
BYTE *t;
if (0 == src[0])
{
tlv->size = 0;
tlv->data = NULL;
}
else if (0x80 > src[0])
{
tlv->size = (int)src[0];
tlv->data = &src[1];
}
else if (0x81 == src[0])
{
tlv->size = (int)src[1];
tlv->data = &src[2];
}
else if (0x82 == src[0])
{
t = (BYTE*)&(tlv->size);
t[0] = src[2];
t[1] = src[1];
t[2] = 0x00;
t[3] = 0x00;
tlv->data = &src[3];
}
else if (0x83 == src[0])
{
t = (BYTE*)&(tlv->size);
t[0] = src[3];
t[1] = src[2];
t[2] = src[1];
t[3] = 0x00;
tlv->data = &src[4];
}
else
{
return -1;
}
return 0;
}
//parse the first tlv data
static int tlv_parser(pTlvData tlv, BYTE *src, int size)
{
int rv;
int offset;
BYTE *ptr = src;
BYTE *head = src;
//Parse Tlv Data Tag
parse_tlv_tag(&(tlv->tag), ptr);
//ptr point to the head of the length data of tlv data
ptr = &(tlv->tag.data[tlv->tag.size]);
//Parse Tlv Data Length
rv = parse_tlv_length(&(tlv->val), ptr);
if (rv != 0)
{
return -1;
}
//ptr point to the head of the next tlv data
if (tlv->val.data==NULL && tlv->val.size==0)
ptr = &(tlv->tag.data[tlv->tag.size+1]);
else
ptr = &(tlv->val.data[tlv->val.size]);
//check format
offset = (int)(ptr - head);
return offset<=size ? 0 : -1;
}
// @brief parse the tlv data which has the same tag value in the data of src
// @Param[IN]
// tag the tag value of the tlv data to be returned
// src input tlv strings
// size the size of tlv strings
// @Param[OUT]
// buf two level pointer, point to the head of the V value of TLV data in the src data
// @Return
// -1 failed
// >0 success
static int parse_tlv_data_t(BYTE **buf, BYTE *tag, BYTE *src, int size)
{
int rv;
int len;
int offset = 0;
BYTE *last = src;
BYTE *next = src;
TlvData tlv;
len = strlen(tag);
while(size > 0)
{
rv = tlv_parser(&tlv, next, size);
if (rv != 0)
return -1;
if ((len==tlv.tag.size) && (!memcmp(tag, tlv.tag.data, len)))
{
//memcpy(buf, tlv.val.data, tlv.val.size);
*buf = tlv.val.data;
return tlv.val.size;
}
last = next;
//next point to the start of next tlv data
if (tlv.val.data==NULL && tlv.val.size==0)
next = &(tlv.tag.data[tlv.tag.size+1]);
else
next = &(tlv.val.data[tlv.val.size]);
//compute the size of the tlv data
offset = (int)(next - last);
size -= offset;
}
return -2;
}
// @Brief parse the tlv data which has the same tag value in the data of src
// @Param[IN]
// tag the tag value of the tlv data to be returned
// src input tlv strings
// size the size of tlv strings
// @Param[OUT]
// buf one level pointer
// @Return
// -1 failed
// >0 success
int parse_tlv_data(BYTE *buf, BYTE *tag, BYTE *src, int size)
{
int len;
BYTE *ptr = NULL;
len = parse_tlv_data_t(&ptr, tag, src, size);
if (len < 0 )
return -1;
memcpy(buf, ptr, len);
return len;
}
// @Brief 解析处于模块中的TLV数据
// @Param[IN]
// tag 返回的数据被Tag标识
// tmplt TLV模板，指定返回的Tag数据存在于其子域中
// src 源TLV串
// size 源TLV串长度
// @Param[OUT]
// buf 被Tag指定的返回值
// @Return
// -1 failed
// >0 success
int parse_tlv_data_tmplt(BYTE *buf, BYTE *tag, BYTE *tmplt, BYTE *src, int size)
{
int len;
BYTE buf_tag[2] = {0};
BYTE *last = src;
BYTE *next = NULL;
if (tmplt != NULL)
{
while(tmplt[0] != '\0' && size > 0)
{
buf_tag[0] = tmplt[0];
len = parse_tlv_data_t(&next, buf_tag, last, size);
if (len < 0)
{
printf("error code:%d\n", len);
return -1;
}
size = len;
last = next;
++tmplt;
}
}
if (size <= 0)
return -2;
len = parse_tlv_data_t(&next, tag, last, size);
if (len < 0)
return -3;
memcpy(buf, next, len);
return len;
}
//计算tlv对象的个数
int count_tlv_substring(BYTE *src, int size)
{
int rv;
int count = 0;
int offset = 0;
BYTE *last;
BYTE *next = src;
TlvData tlv;
while(size > 0)
{
rv = tlv_parser(&tlv, next, size);
if (rv != 0)
return -1;
last = next;
if (tlv.val.data==NULL && tlv.val.size==0)
next = &(tlv.tag.data[tlv.tag.size+1]); //skip Length
else
next = &(tlv.val.data[tlv.val.size]);
offset = (int)(next - last);
size -= offset;
++count;
}
return count;
}
//解析出所有的tlv对象
int unpack_tlv_data(pTlvData *dst, BYTE *src, int size)
{
int i;
int rv;
int offset;
int count;
BYTE *last;
BYTE *next = src;
pTlvData ptr = NULL;
count = count_tlv_substring(src, size);
if (count < 0)
return -1;
//Assign memory for dst
*dst = (pTlvData)malloc(sizeof(TlvData)*count);
if (NULL == *dst)
return -2;
i = 0;
ptr = *dst;
while(i < count)
{
//parse tlv data
rv = tlv_parser(&(ptr[i]), next, size);
if (rv != 0)
{
free(*dst);
*dst = NULL;
return -3;
}
last = next;
if (ptr[i].val.data==NULL && ptr[i].val.size==0)
next = &(ptr[i].tag.data[ptr[i].tag.size+1]);
else
next = &(ptr[i].val.data[ptr[i].val.size]);
offset = (int)(next - last);
size -= offset;
++i;
}
return count;
}
int pack_tlv_data(BYTE *buf, BYTE *tag, BYTE *src, int size)
{
int tag_len;
int length = 0;
BYTE *ptr = buf;
BYTE *tmp = NULL;
tag_len = strlen(tag);
//Check Tag Format
if ((tag_len<1 || tag_len > 2)
|| ((tag[0]&0x1f)==0x1f && tag_len!=2)
|| ((tag[0]&0x1f)!=0x1f && tag_len!=1))
return -1;
memcpy(ptr, tag, tag_len);
ptr += tag_len;
if (size < 0x80)
{
ptr[0] = size&0xff;
memcpy(&ptr[1], src, size);
length += tag_len + 1 + size;
}
else if (size > 0x80 && size <= 0xff)
{
ptr[0] = 0x81;
ptr[1] = size&0xff;
memcpy(&ptr[2], src, size);
length += tag_len + 2 + size;
}
else if (size > 0xff && size <= 0xffff)
{
tmp = (BYTE*)&size;
ptr[0] = 0x82;
ptr[1] = tmp[1];
ptr[2] = tmp[0];
memcpy(&ptr[3], src, size);
length += tag_len + 3 + size;
}
else if (size > 0xffff && size <= 0xffffff)
{
tmp = (BYTE*)&size;
ptr[0] = 0x83;
ptr[1] = tmp[2];
ptr[2] = tmp[1];
ptr[3] = tmp[0];
memcpy(&ptr[4], src, size);
length += tag_len + 4 + size;
}
else
{
return -2;
}
return length;
}
