src/tlv_parser.c

   1 #include <stdio.h>
   2 #include <stdint.h>
   3 #include <osmocore/utils.h>
   4 #include <osmocore/tlv.h>
   5
   6 struct tlv_definition tvlv_att_def;
   7
   8 int tlv_dump(struct tlv_parsed *dec)
   9 {
  10         int i;
  11
  12         for (i = 0; i <= 0xff; i++) {
  13                 if (!dec->lv[i].val)
  14                         continue;
  15                 printf("T=%02x L=%d\n", i, dec->lv[i].len);
  16         }
  17         return 0;
  18 }
  19
  20 /* o_tag:  output: tag found
  21  * o_len:  output: length of the data
  22  * o_val:  output: pointer to the data
  23  * def:     input: a structure defining the valid TLV tags / configurations
  24  * buf:     input: the input data buffer to be parsed
  25  * buf_len: input: the length of the input data buffer
  26  *
  27  * Also, returns the number of bytes consumed by the TLV entry
  28  */
  29 int tlv_parse_one(uint8_t *o_tag, uint16_t *o_len, const uint8_t **o_val,
  30                   const struct tlv_definition *def,
  31                   const uint8_t *buf, int buf_len)
  32 {
  33         uint8_t tag;
  34         int len;
  35
  36         tag = *buf;
  37         *o_tag = tag;
  38
  39         /* FIXME: use tables for knwon IEI */
  40         switch (def->def[tag].type) {
  41         case TLV_TYPE_T:
  42                 /* GSM TS 04.07 11.2.4: Type 1 TV or Type 2 T */
  43                 *o_val = buf;
  44                 *o_len = 0;
  45                 len = 1;
  46                 break;
  47         case TLV_TYPE_TV:
  48                 *o_val = buf+1;
  49                 *o_len = 1;
  50                 len = 2;
  51                 break;
  52         case TLV_TYPE_FIXED:
  53                 *o_val = buf+1;
  54                 *o_len = def->def[tag].fixed_len;
  55                 len = def->def[tag].fixed_len + 1;
  56                 break;
  57         case TLV_TYPE_TLV:
  58                 /* GSM TS 04.07 11.2.4: Type 4 TLV */
  59                 if (buf + 1 > buf + buf_len)
  60                         return -1;
  61                 *o_val = buf+2;
  62                 *o_len = *(buf+1);
  63                 len = *o_len + 2;
  64                 if (len > buf_len)
  65                         return -2;
  66                 break;
  67         case TLV_TYPE_TvLV:
  68                 if (*(buf+1) & 0x80) {
  69                         /* like TLV, but without highest bit of len */
  70                         if (buf + 1 > buf + buf_len)
  71                                 return -1;
  72                         *o_val = buf+2;
  73                         *o_len = *(buf+1) & 0x7f;
  74                         len = *o_len + 2;
  75                         if (len > buf_len)
  76                                 return -2;
  77                         break;
  78                 }
  79                 /* like TL16V, fallthrough */
  80         case TLV_TYPE_TL16V:
  81                 if (2 > buf_len)
  82                         return -1;
  83                 *o_val = buf+3;
  84                 *o_len = *(buf+1) << 8 | *(buf+2);
  85                 len = *o_len + 3;
  86                 if (len > buf_len)
  87                         return -2;
  88                 break;
  89         default:
  90                 return -3;
  91         }
  92
  93         return len;
  94 }
  95
  96 /* dec:    output: a caller-allocated pointer to a struct tlv_parsed,
  97  * def:     input: a structure defining the valid TLV tags / configurations
  98  * buf:     input: the input data buffer to be parsed
  99  * buf_len: input: the length of the input data buffer
 100  * lv_tag:  input: an initial LV tag at the start of the buffer
 101  * lv_tag2: input: a second initial LV tag following lv_tag
 102  */
 103 int tlv_parse(struct tlv_parsed *dec, const struct tlv_definition *def,
 104               const uint8_t *buf, int buf_len, uint8_t lv_tag,
 105               uint8_t lv_tag2)
 106 {
 107         int ofs = 0, num_parsed = 0;
 108         uint16_t len;
 109
 110         memset(dec, 0, sizeof(*dec));
 111
 112         if (lv_tag) {
 113                 if (ofs > buf_len)
 114                         return -1;
 115                 dec->lv[lv_tag].val = &buf[ofs+1];
 116                 dec->lv[lv_tag].len = buf[ofs];
 117                 len = dec->lv[lv_tag].len + 1;
 118                 if (ofs + len > buf_len)
 119                         return -2;
 120                 num_parsed++;
 121                 ofs += len;
 122         }
 123         if (lv_tag2) {
 124                 if (ofs > buf_len)
 125                         return -1;
 126                 dec->lv[lv_tag2].val = &buf[ofs+1];
 127                 dec->lv[lv_tag2].len = buf[ofs];
 128                 len = dec->lv[lv_tag2].len + 1;
 129                 if (ofs + len > buf_len)
 130                         return -2;
 131                 num_parsed++;
 132                 ofs += len;
 133         }
 134
 135         while (ofs < buf_len) {
 136                 int rv;
 137                 uint8_t tag;
 138                 const uint8_t *val;
 139
 140                 rv = tlv_parse_one(&tag, &len, &val, def,
 141                                    &buf[ofs], buf_len-ofs);
 142                 if (rv < 0)
 143                         return rv;
 144                 dec->lv[tag].val = val;
 145                 dec->lv[tag].len = len;
 146                 ofs += rv;
 147                 num_parsed++;
 148         }
 149         //tlv_dump(dec);
 150         return num_parsed;
 151 }
 152
 153 /* take a master (src) tlvdev and fill up all empty slots in 'dst' */
 154 void tlv_def_patch(struct tlv_definition *dst, const struct tlv_definition *src)
 155 {
 156         int i;
 157
 158         for (i = 0; i < ARRAY_SIZE(dst->def); i++) {
 159                 if (src->def[i].type == TLV_TYPE_NONE)
 160                         continue;
 161                 if (dst->def[i].type == TLV_TYPE_NONE)
 162                         dst->def[i] = src->def[i];
 163         }
 164 }
 165
 166 static __attribute__((constructor)) void on_dso_load_tlv(void)
 167 {
 168         int i;
 169         for (i = 0; i < ARRAY_SIZE(tvlv_att_def.def); i++)
 170                 tvlv_att_def.def[i].type = TLV_TYPE_TvLV;
 171 }