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