git.zerfleddert.de Git - proxmark3-svn/blob - armsrc/tlv.c

   1 #include <tlv.h>
   2 #include <stddef.h>
   3
   4 int 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
  53 int 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