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

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