summaryrefslogtreecommitdiffstats
path: root/src/gsm/tlv_parser.c
blob: b1b1034037de2478524d4fa975273014f7015829 (plain)
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
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
/* (C) 2008-2010 by Harald Welte <laforge@gnumonks.org>
 *
 * All Rights Reserved
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <stdio.h>
#include <stdint.h>
#include <errno.h>
#include <osmocom/core/utils.h>
#include <osmocom/gsm/tlv.h>

/*! \addtogroup tlv
 *  @{
 *  Osmocom TLV Parser
 *
 *  The Osmocom TLV parser is intended to operate as a low-level C
 *  implementation without dynamic memory allocations.  Basically, it
 *  iterates over the IE (Information Elements) of the message and fills
 *  an array of pointers, indexed by the IEI (IE Identifier).  The
 *  parser output is thus an array of pointers to the start of the
 *  respective IE inside the message.
 *
 *  The TLV parser is configured by a TLV parser definition, which
 *  determines which if the IEIs for a given protocol are of which
 *  particular type.  Types are e.g. TV (Tag + single byte value), Tag +
 *  fixed-length value, TLV with 8bit length, TLV with 16bit length, TLV
 *  with variable-length length field, etc.
 *
 * \file tlv_parser.c */

struct tlv_definition tvlv_att_def;
struct tlv_definition vtvlv_gan_att_def;

/*! Dump pasred TLV structure to stdout */
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;
}

/*! Copy \ref tlv_parsed using given talloc context
 *  \param[in] tp_orig Parsed TLV structure
 *  \param[in] ctx Talloc context for allocations
 *  \returns NULL on errors, \ref tlv_parsed pointer otherwise
 */
struct tlv_parsed *osmo_tlvp_copy(const struct tlv_parsed *tp_orig, void *ctx)
{
	struct tlv_parsed *tp_out;
	size_t i, len;

	tp_out = talloc_zero(ctx, struct tlv_parsed);
	if (!tp_out)
		return NULL;

	/* if the original is NULL, return empty tlvp */
	if (!tp_orig)
		return tp_out;

	for (i = 0; i < ARRAY_SIZE(tp_orig->lv); i++) {
		len = tp_orig->lv[i].len;
		tp_out->lv[i].len = len;
		if (len && tp_out->lv[i].val) {
			tp_out->lv[i].val = talloc_zero_size(tp_out, len);
			if (!tp_out->lv[i].val) {
				talloc_free(tp_out);
				return NULL;
			}
			memcpy((uint8_t *)tp_out->lv[i].val, tp_orig->lv[i].val,
			       len);
		}
	}

	return tp_out;
}

/*! Merge all \ref tlv_parsed attributes of 'src' into 'dst'
 *  \param[in] dst Parsed TLV structure to merge into
 *  \param[in] src Parsed TLV structure to merge from
 *  \returns 0 on success, negative on error
 */
int osmo_tlvp_merge(struct tlv_parsed *dst, const struct tlv_parsed *src)
{
	size_t i, len;
	for (i = 0; i < ARRAY_SIZE(dst->lv); i++) {
		len = src->lv[i].len;
		if (len == 0 || src->lv[i].val == NULL)
			continue;
		if (dst->lv[i].val) {
			talloc_free((uint8_t *) dst->lv[i].val);
			dst->lv[i].len = 0;
		}
		dst->lv[i].val = talloc_zero_size(dst, len);
		if (!dst->lv[i].val)
			return -ENOMEM;
		memcpy((uint8_t *) dst->lv[i].val, src->lv[i].val, len);
	}
	return 0;
}

/*! Parse a single TLV encoded IE
 *  \param[out] o_tag the tag of the IE that was found
 *  \param[out] o_len length of the IE that was found
 *  \param[out] o_val pointer to the data of the IE that was found
 *  \param[in] def structure defining the valid TLV tags / configurations
 *  \param[in] buf the input data buffer to be parsed
 *  \param[in] buf_len length of the input data buffer
 *  \returns number of bytes consumed by the TLV entry / IE parsed
 */
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 known 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:
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_vTvLV_GAN:	/* 44.318 / 11.1.4 */
		/* FIXME: variable-length TAG! */
		if (*(buf+1) & 0x80) {
			/* like TL16Vbut without highest bit of len */
			if (2 > buf_len)
				return -1;
			*o_val = buf+3;
			*o_len = (*(buf+1) & 0x7F) << 8 | *(buf+2);
			len = *o_len + 3;
			if (len > buf_len)
				return -2;
		} else {
			/* like TLV */
			goto tlv;
		}
		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;
}

/*! Parse an entire buffer of TLV encoded Information Elements
 *  \param[out] dec caller-allocated pointer to \ref tlv_parsed
 *  \param[in] def structure defining the valid TLV tags / configurations
 *  \param[in] buf the input data buffer to be parsed
 *  \param[in] buf_len length of the input data buffer
 *  \param[in] lv_tag an initial LV tag at the start of the buffer
 *  \param[in] lv_tag2 a second initial LV tag following the \a lv_tag
 *  \returns number of bytes consumed by the TLV entry / IE parsed
 */
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'
 *  \param dst TLV parser definition that is to be patched
 *  \param[in] src TLV parser definition whose content is patched into \a 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;

	for (i = 0; i < ARRAY_SIZE(vtvlv_gan_att_def.def); i++)
		vtvlv_gan_att_def.def[i].type = TLV_TYPE_vTvLV_GAN;
}

/*! Advance the data pointer, subtract length and assign value pointer
 *  \param data pointer to the pointer to data
 *  \param data_len pointer to size_t containing \arg data length
 *  \param[in] len the length that we expect the fixed IE to hav
 *  \param[out] value pointer to pointer of value part of IE
 *  \returns length of IE value; negative in case of error
 */
int osmo_shift_v_fixed(uint8_t **data, size_t *data_len,
			size_t len, uint8_t **value)
{
	if (len > *data_len)
		goto fail;

	if (value)
		*value = *data;

	*data += len;
	*data_len -= len;

	return len;

fail:
	*data += *data_len;
	*data_len = 0;
	return -1;
}

/*! Match tag, check length and assign value pointer
 *  \param data pointer to the pointer to data
 *  \param data_len pointer to size_t containing \arg data length
 *  \param[in] tag the tag (IEI) that we expect at \arg data
 *  \param[in] len the length that we expect the fixed IE to have
 *  \param[out] value pointer to pointer of value part of IE
 *  \returns length of IE value; negative in case of error
 */
int osmo_match_shift_tv_fixed(uint8_t **data, size_t *data_len,
			      uint8_t tag, size_t len,
			      uint8_t **value)
{
	size_t ie_len;

	if (*data_len == 0)
		goto fail;

	if ((*data)[0] != tag)
		return 0;

	if (len > *data_len - 1)
		goto fail;

	if (value)
		*value = *data + 1;

	ie_len = len + 1;
	*data += ie_len;
	*data_len -= ie_len;

	return ie_len;

fail:
	*data += *data_len;
	*data_len = 0;
	return -1;
}

/*! Verify TLV header and advance data / subtract length
 *  \param data pointer to the pointer to data
 *  \param data_len pointer to size_t containing \arg data length
 *  \param[in] expected_tag the tag (IEI) that we expect at \arg data
 *  \param[out] value pointer to pointer of value part of IE
 *  \param[out] value_len pointer to length of \arg value
 *  \returns length of IE value; negative in case of error
 */
int osmo_match_shift_tlv(uint8_t **data, size_t *data_len,
			 uint8_t expected_tag, uint8_t **value,
			 size_t *value_len)
{
	int rc;
	uint8_t tag;
	uint8_t *old_data = *data;
	size_t old_data_len = *data_len;

	rc = osmo_shift_tlv(data, data_len, &tag, value, value_len);

	if (rc > 0 && tag != expected_tag) {
		*data = old_data;
		*data_len = old_data_len;
		return 0;
	}

	return rc;
}

/*! Extract TLV and advance data pointer + subtract length
 *  \param data pointer to the pointer to data
 *  \param data_len  pointer to size_t containing \arg data lengt
 *  \param[out] tag extract the tag (IEI) at start of \arg data
 *  \param[out] value extracted pointer to value part of TLV
 *  \param[out] value_len extracted length of \arg value
 *  \returns number of bytes subtracted
 */
int osmo_shift_tlv(uint8_t **data, size_t *data_len,
	      uint8_t *tag, uint8_t **value, size_t *value_len)
{
	size_t len;
	size_t ie_len;

	if (*data_len < 2)
		goto fail;

	len = (*data)[1];
	if (len > *data_len - 2)
		goto fail;

	if (tag)
		*tag = (*data)[0];
	if (value)
		*value = *data + 2;
	if (value_len)
		*value_len = len;

	ie_len = len + 2;

	*data += ie_len;
	*data_len -= ie_len;

	return ie_len;

fail:
	*data += *data_len;
	*data_len = 0;
	return -1;
}

/*! Extract LV and advance data pointer + subtract length
 *  \param data pointer to the pointer to data
 *  \param data_len  pointer to size_t containing \arg data lengt
 *  \param[out] value extracted pointer to value part of TLV
 *  \param[out] value_len extracted length of \arg value
 *  \returns number of bytes subtracted
 */
int osmo_shift_lv(uint8_t **data, size_t *data_len,
		  uint8_t **value, size_t *value_len)
{
	size_t len;
	size_t ie_len;

	if (*data_len < 1)
		goto fail;

	len = (*data)[0];
	if (len > *data_len - 1)
		goto fail;

	if (value)
		*value = *data + 1;
	if (value_len)
		*value_len = len;

	ie_len = len + 1;
	*data += ie_len;
	*data_len -= ie_len;

	return ie_len;

fail:
	*data += *data_len;
	*data_len = 0;
	return -1;
}

/*! @} */