]> git.zerfleddert.de Git - proxmark3-svn/blame_incremental - armsrc/tlv.c
CHG: syntax suger
[proxmark3-svn] / armsrc / tlv.c
... / ...
CommitLineData
1#include <tlv.h>
2#include <stddef.h>
3
4int decode_ber_tlv_item(uint8_t* data, tlvtag* returnedtag)
5{
6 uint8_t tag[TAG_LENGTH] = {0x00,0x00};
7 uint16_t length = 0;
8 //uint8_t value[VALUE_LENGTH];
9 uint8_t lenlen = 0;
10 int i = 0;
11 int z = 0;
12 //decode tag
13 tag[0] = data[0];
14 if((tag[0] & TLV_TAG_NUMBER_MASK) == TLV_TAG_NUMBER_MASK) { //see subsequent bytes
15 i++;
16 tag[i] = data[i];
17 //assume tag is only two bytes long for now
18 /*
19 while((data[i] & TLV_TAG_MASK) == TLV_TAG_MASK){
20 i++;
21 tag[i] = data[i];
22 }
23 */
24 }
25 i++;
26 //decode length
27 if((data[i] & TLV_LENGTH_MASK) == TLV_LENGTH_MASK) {
28 lenlen = data[i] ^ TLV_LENGTH_MASK;
29 i++;
30 length = (uint16_t)data[i];
31 z = 1;
32 while(z < lenlen){
33 i++;
34 z++;
35 length <<= 8;
36 length += (uint16_t)data[i];
37 }
38 i++;
39 }
40 else {
41 length = (uint16_t)data[i];
42 i++;
43 }
44 //copy results into the structure and return
45 memcpy(returnedtag->tag, tag, TAG_LENGTH);
46 (*returnedtag).valuelength = length; //return length of tag value
47 (*returnedtag).fieldlength = length + i + 1; //return length of total field
48 memcpy(returnedtag->value, &(data[i]), length);
49 return 0;
50}
51
52//generate a TLV tag off input data
53int encode_ber_tlv_item(uint8_t* tag, uint8_t taglen, uint8_t* data, uint32_t datalen, uint8_t* outputtag, uint32_t* outputtaglen)
54{
55 if(!tag || !data || !outputtag || !outputtaglen) //null pointer check
56 return 0;
57
58 uint8_t datafieldlen = (datalen / 128) + 1; //field length of the tag
59 uint8_t tlvtotallen = taglen + datafieldlen + datalen; //total length of the tag
60 uint8_t returnedtag[tlvtotallen]; //buffer for the returned tag
61 uint8_t counter = 0;
62 memcpy(returnedtag, tag, taglen); //copy tag into buffer
63 counter += taglen;
64 if(datalen < 128){ // 1 byte length value
65 returnedtag[counter++] = datalen;
66 }
67 else{
68 returnedtag[counter++] = datafieldlen | 0x80; //high bit set and number of length bytes
69 for(uint8_t i=datafieldlen; i !=0; i--){
70 returnedtag[counter++] = (datalen >> (i * 8)) & 0xFF; //get current byte
71 }
72 }
73 memcpy(&returnedtag[counter], data, datalen);
74 *outputtaglen = tlvtotallen;
75 memcpy(outputtag, returnedtag,tlvtotallen);
76 return 0;
77}
78
Impressum, Datenschutz