Line data Source code
1 : /*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/
2 :
3 : /***
4 : This file is part of systemd.
5 :
6 : Copyright (C) 2014 Tom Gundersen
7 : Copyright (C) 2014 Susant Sahani
8 :
9 : systemd is free software; you can redistribute it and/or modify it
10 : under the terms of the GNU Lesser General Public License as published by
11 : the Free Software Foundation; either version 2.1 of the License, or
12 : (at your option) any later version.
13 :
14 : systemd is distributed in the hope that it will be useful, but
15 : WITHOUT ANY WARRANTY; without even the implied warranty of
16 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 : Lesser General Public License for more details.
18 :
19 : You should have received a copy of the GNU Lesser General Public License
20 : along with systemd; If not, see <http://www.gnu.org/licenses/>.
21 : ***/
22 :
23 : #include <net/ethernet.h>
24 : #include <arpa/inet.h>
25 :
26 : #include "macro.h"
27 : #include "lldp-tlv.h"
28 :
29 12 : int tlv_section_new(tlv_section **ret) {
30 : tlv_section *s;
31 :
32 12 : s = new0(tlv_section, 1);
33 12 : if (!s)
34 0 : return -ENOMEM;
35 :
36 12 : *ret = s;
37 :
38 12 : return 0;
39 : }
40 :
41 12 : void tlv_section_free(tlv_section *m) {
42 :
43 12 : if (!m)
44 0 : return;
45 :
46 12 : free(m);
47 : }
48 :
49 1 : int tlv_packet_new(tlv_packet **ret) {
50 : tlv_packet *m;
51 :
52 1 : m = new0(tlv_packet, 1);
53 1 : if (!m)
54 0 : return -ENOMEM;
55 :
56 1 : LIST_HEAD_INIT(m->sections);
57 :
58 1 : *ret = m;
59 :
60 1 : return 0;
61 : }
62 :
63 1 : void tlv_packet_free(tlv_packet *m) {
64 : tlv_section *s, *n;
65 :
66 1 : if (!m)
67 0 : return;
68 :
69 11 : LIST_FOREACH_SAFE(section, s, n, m->sections)
70 10 : tlv_section_free(s);
71 :
72 1 : free(m);
73 : }
74 :
75 14 : int tlv_packet_append_bytes(tlv_packet *m, const void *data, size_t data_length) {
76 : uint8_t *p;
77 :
78 14 : assert_return(m, -EINVAL);
79 14 : assert_return(data, -EINVAL);
80 14 : assert_return(data_length, -EINVAL);
81 :
82 14 : if (m->length + data_length > ETHER_MAX_LEN)
83 0 : return -ENOMEM;
84 :
85 14 : p = m->pdu + m->length;
86 14 : memcpy(p, data, data_length);
87 14 : m->length += data_length;
88 :
89 14 : return 0;
90 : }
91 :
92 2 : int tlv_packet_append_u8(tlv_packet *m, uint8_t data) {
93 :
94 2 : assert_return(m, -EINVAL);
95 :
96 2 : return tlv_packet_append_bytes(m, &data, sizeof(uint8_t));
97 : }
98 :
99 7 : int tlv_packet_append_u16(tlv_packet *m, uint16_t data) {
100 : uint16_t type;
101 :
102 7 : assert_return(m, -EINVAL);
103 :
104 7 : type = htons(data);
105 :
106 7 : return tlv_packet_append_bytes(m, &type, sizeof(uint16_t));
107 : }
108 :
109 0 : int tlv_packet_append_u32(tlv_packet *m, uint32_t data) {
110 : uint32_t type;
111 :
112 0 : assert_return(m, -EINVAL);
113 :
114 0 : type = htonl(data);
115 :
116 0 : return tlv_packet_append_bytes(m, &type, sizeof(uint32_t));
117 : }
118 :
119 0 : int tlv_packet_append_string(tlv_packet *m, char *data, uint16_t size) {
120 :
121 0 : assert_return(m, -EINVAL);
122 :
123 0 : return tlv_packet_append_bytes(m, data, size);
124 : }
125 :
126 6 : int lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type) {
127 :
128 6 : assert_return(m, -EINVAL);
129 :
130 6 : m->container_pos = m->pdu + m->length;
131 :
132 6 : return tlv_packet_append_u16(m, type << 9);
133 : }
134 :
135 6 : int lldp_tlv_packet_close_container(tlv_packet *m) {
136 : uint16_t type;
137 :
138 6 : assert_return(m, -EINVAL);
139 6 : assert_return(m->container_pos, -EINVAL);
140 :
141 6 : memcpy(&type, m->container_pos, sizeof(uint16_t));
142 :
143 6 : type |= htons(((m->pdu + m->length) - (m->container_pos + 2)) & 0x01ff);
144 6 : memcpy(m->container_pos, &type, sizeof(uint16_t));
145 :
146 6 : return 0;
147 : }
148 :
149 7 : static inline int tlv_packet_read_internal(tlv_section *m, void **data) {
150 :
151 7 : assert_return(m->read_pos, -EINVAL);
152 :
153 7 : *data = m->read_pos;
154 :
155 7 : return 0;
156 : }
157 :
158 2 : int tlv_packet_read_u8(tlv_packet *m, uint8_t *data) {
159 2 : void *val = NULL;
160 : int r;
161 :
162 2 : assert_return(m, -EINVAL);
163 :
164 2 : r = tlv_packet_read_internal(m->container, &val);
165 2 : if (r < 0)
166 0 : return r;
167 :
168 2 : memcpy(data, val, sizeof(uint8_t));
169 :
170 2 : m->container->read_pos ++;
171 :
172 2 : return 0;
173 : }
174 :
175 1 : int tlv_packet_read_u16(tlv_packet *m, uint16_t *data) {
176 : uint16_t t;
177 1 : void *val = NULL;
178 : int r;
179 :
180 1 : assert_return(m, -EINVAL);
181 :
182 1 : r = tlv_packet_read_internal(m->container, &val);
183 1 : if (r < 0)
184 0 : return r;
185 :
186 1 : memcpy(&t, val, sizeof(uint16_t));
187 1 : *data = ntohs(t);
188 :
189 1 : m->container->read_pos += 2;
190 :
191 1 : return 0;
192 : }
193 :
194 0 : int tlv_packet_read_u32(tlv_packet *m, uint32_t *data) {
195 : uint32_t t;
196 : void *val;
197 : int r;
198 :
199 0 : assert_return(m, -EINVAL);
200 :
201 0 : r = tlv_packet_read_internal(m->container, &val);
202 0 : if (r < 0)
203 0 : return r;
204 :
205 0 : memcpy(&t, val, sizeof(uint32_t));
206 0 : *data = ntohl(t);
207 :
208 0 : m->container->read_pos += 4;
209 :
210 0 : return r;
211 : }
212 :
213 3 : int tlv_packet_read_string(tlv_packet *m, char **data, uint16_t *data_length) {
214 3 : void *val = NULL;
215 : int r;
216 :
217 3 : assert_return(m, -EINVAL);
218 :
219 3 : r = tlv_packet_read_internal(m->container, &val);
220 3 : if (r < 0)
221 0 : return r;
222 :
223 3 : *data = (char *) val;
224 3 : *data_length = m->container->length;
225 :
226 3 : m->container->read_pos += m->container->length;
227 :
228 3 : return 0;
229 : }
230 :
231 1 : int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length) {
232 1 : void *val = NULL;
233 : int r;
234 :
235 1 : assert_return(m, -EINVAL);
236 :
237 1 : r = tlv_packet_read_internal(m->container, &val);
238 1 : if (r < 0)
239 0 : return r;
240 :
241 1 : *data = (uint8_t *) val;
242 1 : *data_length = m->container->length;
243 :
244 1 : m->container->read_pos += m->container->length;
245 :
246 1 : return 0;
247 : }
248 :
249 : /* parse raw TLV packet */
250 2 : int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) {
251 : tlv_section *section, *tail;
252 : uint16_t t, l;
253 : uint8_t *p;
254 : int r;
255 :
256 2 : assert_return(m, -EINVAL);
257 2 : assert_return(size, -EINVAL);
258 :
259 2 : p = m->pdu;
260 :
261 : /* extract ethernet herader */
262 2 : memcpy(&m->mac, p, ETH_ALEN);
263 2 : p += sizeof(struct ether_header);
264 :
265 14 : for (l = 0; l <= size; ) {
266 12 : r = tlv_section_new(§ion);
267 12 : if (r < 0)
268 0 : return r;
269 :
270 12 : memcpy(&t, p, sizeof(uint16_t));
271 :
272 12 : section->type = ntohs(t) >> 9;
273 12 : section->length = ntohs(t) & 0x01ff;
274 :
275 12 : if (section->type == LLDP_TYPE_END || section->type >=_LLDP_TYPE_MAX) {
276 2 : tlv_section_free(section);
277 2 : break;
278 : }
279 :
280 10 : p += 2;
281 10 : section->data = p;
282 :
283 10 : LIST_FIND_TAIL(section, m->sections, tail);
284 10 : LIST_INSERT_AFTER(section, m->sections, tail, section);
285 :
286 10 : p += section->length;
287 10 : l += (section->length + 2);
288 : }
289 :
290 2 : return 0;
291 : }
292 :
293 5 : int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) {
294 : tlv_section *s;
295 :
296 5 : assert_return(m, -EINVAL);
297 :
298 15 : LIST_FOREACH(section, s, m->sections)
299 15 : if (s->type == type)
300 5 : break;
301 5 : if (!s)
302 0 : return -1;
303 :
304 5 : m->container = s;
305 :
306 5 : m->container->read_pos = s->data;
307 5 : if (!m->container->read_pos) {
308 0 : m->container = 0;
309 0 : return -1;
310 : }
311 :
312 5 : return 0;
313 : }
314 :
315 5 : int lldp_tlv_packet_exit_container(tlv_packet *m) {
316 5 : assert_return(m, -EINVAL);
317 :
318 5 : m->container = 0;
319 :
320 5 : return 0;
321 : }
|
