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         /* single octet TV IE */
  40         if (def->def[tag & 0xf0].type == TLV_TYPE_SINGLE_TV) {
  41                 *o_tag = tag & 0xf0;
  42                 *o_val = buf;
  43                 *o_len = 1;
  44                 return 1;
  45         }
  46
  47         /* FIXME: use tables for knwon IEI */
  48         switch (def->def[tag].type) {
  49         case TLV_TYPE_T:
  50                 /* GSM TS 04.07 11.2.4: Type 1 TV or Type 2 T */
  51                 *o_val = buf;
  52                 *o_len = 0;
  53                 len = 1;
  54                 break;
  55         case TLV_TYPE_TV:
  56                 *o_val = buf+1;
  57                 *o_len = 1;
  58                 len = 2;
  59                 break;
  60         case TLV_TYPE_FIXED:
  61                 *o_val = buf+1;
  62                 *o_len = def->def[tag].fixed_len;
  63                 len = def->def[tag].fixed_len + 1;
  64                 break;
  65         case TLV_TYPE_TLV:
  66                 /* GSM TS 04.07 11.2.4: Type 4 TLV */
  67                 if (buf + 1 > buf + buf_len)
  68                         return -1;
  69                 *o_val = buf+2;
  70                 *o_len = *(buf+1);
  71                 len = *o_len + 2;
  72                 if (len > buf_len)
  73                         return -2;
  74                 break;
  75         case TLV_TYPE_TvLV:
  76                 if (*(buf+1) & 0x80) {
  77                         /* like TLV, but without highest bit of len */
  78                         if (buf + 1 > buf + buf_len)
  79                                 return -1;
  80                         *o_val = buf+2;
  81                         *o_len = *(buf+1) & 0x7f;
  82                         len = *o_len + 2;
  83                         if (len > buf_len)
  84                                 return -2;
  85                         break;
  86                 }
  87                 /* like TL16V, fallthrough */
  88         case TLV_TYPE_TL16V:
  89                 if (2 > buf_len)
  90                         return -1;
  91                 *o_val = buf+3;
  92                 *o_len = *(buf+1) << 8 | *(buf+2);
  93                 len = *o_len + 3;
  94                 if (len > buf_len)
  95                         return -2;
  96                 break;
  97         default:
  98                 return -3;
  99         }
 100
 101         return len;
 102 }
 103
 104 /* dec:    output: a caller-allocated pointer to a struct tlv_parsed,
 105  * def:     input: a structure defining the valid TLV tags / configurations
 106  * buf:     input: the input data buffer to be parsed
 107  * buf_len: input: the length of the input data buffer
 108  * lv_tag:  input: an initial LV tag at the start of the buffer
 109  * lv_tag2: input: a second initial LV tag following lv_tag
 110  */
 111 int tlv_parse(struct tlv_parsed *dec, const struct tlv_definition *def,
 112               const uint8_t *buf, int buf_len, uint8_t lv_tag,
 113               uint8_t lv_tag2)
 114 {
 115         int ofs = 0, num_parsed = 0;
 116         uint16_t len;
 117
 118         memset(dec, 0, sizeof(*dec));
 119
 120         if (lv_tag) {
 121                 if (ofs > buf_len)
 122                         return -1;
 123                 dec->lv[lv_tag].val = &buf[ofs+1];
 124                 dec->lv[lv_tag].len = buf[ofs];
 125                 len = dec->lv[lv_tag].len + 1;
 126                 if (ofs + len > buf_len)
 127                         return -2;
 128                 num_parsed++;
 129                 ofs += len;
 130         }
 131         if (lv_tag2) {
 132                 if (ofs > buf_len)
 133                         return -1;
 134                 dec->lv[lv_tag2].val = &buf[ofs+1];
 135                 dec->lv[lv_tag2].len = buf[ofs];
 136                 len = dec->lv[lv_tag2].len + 1;
 137                 if (ofs + len > buf_len)
 138                         return -2;
 139                 num_parsed++;
 140                 ofs += len;
 141         }
 142
 143         while (ofs < buf_len) {
 144                 int rv;
 145                 uint8_t tag;
 146                 const uint8_t *val;
 147
 148                 rv = tlv_parse_one(&tag, &len, &val, def,
 149                                    &buf[ofs], buf_len-ofs);
 150                 if (rv < 0)
 151                         return rv;
 152                 dec->lv[tag].val = val;
 153                 dec->lv[tag].len = len;
 154                 ofs += rv;
 155                 num_parsed++;
 156         }
 157         //tlv_dump(dec);
 158         return num_parsed;
 159 }
 160
 161 /* take a master (src) tlvdev and fill up all empty slots in 'dst' */
 162 void tlv_def_patch(struct tlv_definition *dst, const struct tlv_definition *src)
 163 {
 164         int i;
 165
 166         for (i = 0; i < ARRAY_SIZE(dst->def); i++) {
 167                 if (src->def[i].type == TLV_TYPE_NONE)
 168                         continue;
 169                 if (dst->def[i].type == TLV_TYPE_NONE)
 170                         dst->def[i] = src->def[i];
 171         }
 172 }
 173
 174 static __attribute__((constructor)) void on_dso_load_tlv(void)
 175 {
 176         int i;
 177         for (i = 0; i < ARRAY_SIZE(tvlv_att_def.def); i++)
 178                 tvlv_att_def.def[i].type = TLV_TYPE_TvLV;
 179 }