95 lines
2.3 KiB
C
95 lines
2.3 KiB
C
#include <tlv.h>
|
|
|
|
int decode_ber_tlv_item(uint8_t* data, tlvtag* returnedtag)
|
|
{
|
|
uint8_t tag[TAG_LENGTH] = {0x00, 0x00, 0x00, 0x00};
|
|
uint16_t length = 0;
|
|
//uint8_t value[VALUE_LENGTH];
|
|
uint8_t lenlen = 0;
|
|
int i = 0;
|
|
int z = 0;
|
|
//decode tag
|
|
tag[0] = data[0];
|
|
if ((tag[0] & TLV_TAG_NUMBER_MASK) == TLV_TAG_NUMBER_MASK) { //see subsequent bytes
|
|
i++;
|
|
|
|
tag[i] = data[i];
|
|
|
|
while((data[i] & TLV_TAG_MASK) == TLV_TAG_MASK){
|
|
i++;
|
|
tag[i] = data[i];
|
|
}
|
|
|
|
}
|
|
i++;
|
|
//decode length
|
|
if ((data[i] & TLV_LENGTH_MASK) == TLV_LENGTH_MASK) {
|
|
lenlen = data[i] ^ TLV_LENGTH_MASK;
|
|
i++;
|
|
length = (uint16_t)data[i];
|
|
z = 1;
|
|
while (z < lenlen) {
|
|
i++;
|
|
z++;
|
|
length <<= 8;
|
|
length += (uint16_t)data[i];
|
|
}
|
|
i++;
|
|
} else {
|
|
length = (uint16_t)data[i];
|
|
i++;
|
|
}
|
|
// copy results into the structure and return
|
|
memcpy(returnedtag->tag, tag, TAG_LENGTH);
|
|
|
|
// return length of tag value
|
|
(*returnedtag).valuelength = length;
|
|
|
|
// return length of total field
|
|
(*returnedtag).fieldlength = length + i + 1;
|
|
|
|
memcpy(returnedtag->value, &(data[i]), length);
|
|
return 0;
|
|
}
|
|
|
|
//generate a TLV tag off input data
|
|
int encode_ber_tlv_item(uint8_t* tag, uint8_t taglen, uint8_t* data, uint32_t datalen, uint8_t* outputtag, uint32_t* outputtaglen)
|
|
{
|
|
if (!tag || !data || !outputtag || !outputtaglen)
|
|
return 0;
|
|
|
|
// field length of the tag
|
|
uint8_t datafieldlen = (datalen / 128) + 1;
|
|
|
|
// total length of the tag
|
|
uint8_t tlvtotallen = taglen + datafieldlen + datalen;
|
|
|
|
// buffer for the returned tag
|
|
uint8_t returnedtag[tlvtotallen];
|
|
|
|
uint8_t counter = 0;
|
|
|
|
// copy tag into buffer
|
|
memcpy(returnedtag, tag, taglen);
|
|
counter += taglen;
|
|
|
|
// 1 byte length value
|
|
if (datalen < 128){
|
|
returnedtag[counter++] = datalen;
|
|
} else {
|
|
|
|
// high bit set and number of length bytes
|
|
returnedtag[counter++] = datafieldlen | 0x80;
|
|
|
|
for (uint8_t i = datafieldlen; i !=0; i--) {
|
|
// get current byte
|
|
returnedtag[counter++] = (datalen >> (i * 8)) & 0xFF;
|
|
}
|
|
}
|
|
memcpy(&returnedtag[counter], data, datalen);
|
|
*outputtaglen = tlvtotallen;
|
|
memcpy(outputtag, returnedtag, tlvtotallen);
|
|
return 0;
|
|
}
|
|
|