aboutsummaryrefslogtreecommitdiffstats
path: root/src/openbsc_nat/tlv_parser.c
blob: e44b3a10cfbd413a1612017c5efa2afe65ec77c3 (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
#include <stdio.h>
#include <openbsc_nat/tlv.h>

#ifndef ARRAY_SIZE
#define ARRAY_SIZE(x) (sizeof(x)/sizeof(x[0]))
#endif

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(u_int8_t *o_tag, u_int16_t *o_len, const u_int8_t **o_val,
		  const struct tlv_definition *def,
		  const u_int8_t *buf, int buf_len)
{
	u_int8_t tag;
	int len;

	tag = *buf;
	*o_tag = tag;

	/* 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 u_int8_t *buf, int buf_len, u_int8_t lv_tag,
	      u_int8_t lv_tag2)
{
	int ofs = 0, num_parsed = 0;
	u_int16_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;
		u_int8_t tag;
		const u_int8_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;
}

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;
}