1 /* SPDX-License-Identifier: GPL-2.0 */
2 /*
3 * Driver O/S-independent utility routines
4 *
5 * Copyright (C) 1999-2017, Broadcom Corporation
6 *
7 * Unless you and Broadcom execute a separate written software license
8 * agreement governing use of this software, this software is licensed to you
9 * under the terms of the GNU General Public License version 2 (the "GPL"),
10 * available at http://www.broadcom.com/licenses/GPLv2.php, with the
11 * following added to such license:
12 *
13 * As a special exception, the copyright holders of this software give you
14 * permission to link this software with independent modules, and to copy and
15 * distribute the resulting executable under terms of your choice, provided that
16 * you also meet, for each linked independent module, the terms and conditions of
17 * the license of that module. An independent module is a module which is not
18 * derived from this software. The special exception does not apply to any
19 * modifications of the software.
20 *
21 * Notwithstanding the above, under no circumstances may you combine this
22 * software in any way with any other Broadcom software provided under a license
23 * other than the GPL, without Broadcom's express prior written consent.
24 *
25 *
26 * <<Broadcom-WL-IPTag/Open:>>
27 *
28 * $Id: bcmxtlv.c 628611 2016-03-31 17:53:25Z $
29 */
30
31 #include <bcm_cfg.h>
32
33 #include <typedefs.h>
34 #include <bcmdefs.h>
35
36 #include <stdarg.h>
37
38 #ifdef BCMDRIVER
39 #include <osl.h>
40 #else /* !BCMDRIVER */
41 #include <stdlib.h> /* AS!!! */
42 #include <stdio.h>
43 #include <string.h>
44 #include <stdlib.h>
45 #ifndef ASSERT
46 #define ASSERT(exp)
47 #endif
MALLOCZ(void * o,size_t s)48 INLINE void* MALLOCZ(void *o, size_t s) { BCM_REFERENCE(o); return calloc(1, s); }
MFREE(void * o,void * p,size_t s)49 INLINE void MFREE(void *o, void *p, size_t s) { BCM_REFERENCE(o); BCM_REFERENCE(s); free(p); }
50 #endif /* !BCMDRIVER */
51
52 #include <bcmendian.h>
53 #include <bcmutils.h>
54
bcm_xtlv_size_for_data(int dlen,bcm_xtlv_opts_t opts)55 static INLINE int bcm_xtlv_size_for_data(int dlen, bcm_xtlv_opts_t opts)
56 {
57 return ((opts & BCM_XTLV_OPTION_ALIGN32) ? ALIGN_SIZE(dlen + BCM_XTLV_HDR_SIZE, 4)
58 : (dlen + BCM_XTLV_HDR_SIZE));
59 }
60
61 bcm_xtlv_t *
bcm_next_xtlv(bcm_xtlv_t * elt,int * buflen,bcm_xtlv_opts_t opts)62 bcm_next_xtlv(bcm_xtlv_t *elt, int *buflen, bcm_xtlv_opts_t opts)
63 {
64 int sz;
65 /* advance to next elt */
66 sz = BCM_XTLV_SIZE(elt, opts);
67 elt = (bcm_xtlv_t*)((uint8 *)elt + sz);
68 *buflen -= sz;
69
70 /* validate next elt */
71 if (!bcm_valid_xtlv(elt, *buflen, opts))
72 return NULL;
73
74 return elt;
75 }
76
77 int
bcm_xtlv_buf_init(bcm_xtlvbuf_t * tlv_buf,uint8 * buf,uint16 len,bcm_xtlv_opts_t opts)78 bcm_xtlv_buf_init(bcm_xtlvbuf_t *tlv_buf, uint8 *buf, uint16 len, bcm_xtlv_opts_t opts)
79 {
80 if (!tlv_buf || !buf || !len)
81 return BCME_BADARG;
82
83 tlv_buf->opts = opts;
84 tlv_buf->size = len;
85 tlv_buf->head = buf;
86 tlv_buf->buf = buf;
87 return BCME_OK;
88 }
89
90 uint16
bcm_xtlv_buf_len(bcm_xtlvbuf_t * tbuf)91 bcm_xtlv_buf_len(bcm_xtlvbuf_t *tbuf)
92 {
93 if (tbuf == NULL) return 0;
94 return (uint16)(tbuf->buf - tbuf->head);
95 }
96 uint16
bcm_xtlv_buf_rlen(bcm_xtlvbuf_t * tbuf)97 bcm_xtlv_buf_rlen(bcm_xtlvbuf_t *tbuf)
98 {
99 if (tbuf == NULL) return 0;
100 return tbuf->size - bcm_xtlv_buf_len(tbuf);
101 }
102 uint8 *
bcm_xtlv_buf(bcm_xtlvbuf_t * tbuf)103 bcm_xtlv_buf(bcm_xtlvbuf_t *tbuf)
104 {
105 if (tbuf == NULL) return NULL;
106 return tbuf->buf;
107 }
108 uint8 *
bcm_xtlv_head(bcm_xtlvbuf_t * tbuf)109 bcm_xtlv_head(bcm_xtlvbuf_t *tbuf)
110 {
111 if (tbuf == NULL) return NULL;
112 return tbuf->head;
113 }
114 int
bcm_xtlv_put_data(bcm_xtlvbuf_t * tbuf,uint16 type,const void * data,uint16 dlen)115 bcm_xtlv_put_data(bcm_xtlvbuf_t *tbuf, uint16 type, const void *data, uint16 dlen)
116 {
117 bcm_xtlv_t *xtlv;
118 int size;
119
120 if (tbuf == NULL)
121 return BCME_BADARG;
122 size = bcm_xtlv_size_for_data(dlen, tbuf->opts);
123 if (bcm_xtlv_buf_rlen(tbuf) < size)
124 return BCME_NOMEM;
125 xtlv = (bcm_xtlv_t *)bcm_xtlv_buf(tbuf);
126 xtlv->id = htol16(type);
127 xtlv->len = htol16(dlen);
128 memcpy(xtlv->data, data, dlen);
129 tbuf->buf += size;
130 return BCME_OK;
131 }
132 int
bcm_xtlv_put_8(bcm_xtlvbuf_t * tbuf,uint16 type,const int8 data)133 bcm_xtlv_put_8(bcm_xtlvbuf_t *tbuf, uint16 type, const int8 data)
134 {
135 bcm_xtlv_t *xtlv;
136 int size;
137
138 if (tbuf == NULL)
139 return BCME_BADARG;
140 size = bcm_xtlv_size_for_data(1, tbuf->opts);
141 if (bcm_xtlv_buf_rlen(tbuf) < size)
142 return BCME_NOMEM;
143 xtlv = (bcm_xtlv_t *)bcm_xtlv_buf(tbuf);
144 xtlv->id = htol16(type);
145 xtlv->len = htol16(sizeof(data));
146 xtlv->data[0] = data;
147 tbuf->buf += size;
148 return BCME_OK;
149 }
150 int
bcm_xtlv_put_16(bcm_xtlvbuf_t * tbuf,uint16 type,const int16 data)151 bcm_xtlv_put_16(bcm_xtlvbuf_t *tbuf, uint16 type, const int16 data)
152 {
153 bcm_xtlv_t *xtlv;
154 int size;
155
156 if (tbuf == NULL)
157 return BCME_BADARG;
158 size = bcm_xtlv_size_for_data(2, tbuf->opts);
159 if (bcm_xtlv_buf_rlen(tbuf) < size)
160 return BCME_NOMEM;
161
162 xtlv = (bcm_xtlv_t *)bcm_xtlv_buf(tbuf);
163 xtlv->id = htol16(type);
164 xtlv->len = htol16(sizeof(data));
165 htol16_ua_store(data, xtlv->data);
166 tbuf->buf += size;
167 return BCME_OK;
168 }
169 int
bcm_xtlv_put_32(bcm_xtlvbuf_t * tbuf,uint16 type,const int32 data)170 bcm_xtlv_put_32(bcm_xtlvbuf_t *tbuf, uint16 type, const int32 data)
171 {
172 bcm_xtlv_t *xtlv;
173 int size;
174
175 if (tbuf == NULL)
176 return BCME_BADARG;
177 size = bcm_xtlv_size_for_data(4, tbuf->opts);
178 if (bcm_xtlv_buf_rlen(tbuf) < size)
179 return BCME_NOMEM;
180 xtlv = (bcm_xtlv_t *)bcm_xtlv_buf(tbuf);
181 xtlv->id = htol16(type);
182 xtlv->len = htol16(sizeof(data));
183 htol32_ua_store(data, xtlv->data);
184 tbuf->buf += size;
185 return BCME_OK;
186 }
187
188 /*
189 * upacks xtlv record from buf checks the type
190 * copies data to callers buffer
191 * advances tlv pointer to next record
192 * caller's resposible for dst space check
193 */
194 int
bcm_unpack_xtlv_entry(uint8 ** tlv_buf,uint16 xpct_type,uint16 xpct_len,void * dst,bcm_xtlv_opts_t opts)195 bcm_unpack_xtlv_entry(uint8 **tlv_buf, uint16 xpct_type, uint16 xpct_len, void *dst,
196 bcm_xtlv_opts_t opts)
197 {
198 bcm_xtlv_t *ptlv = (bcm_xtlv_t *)*tlv_buf;
199 uint16 len;
200 uint16 type;
201
202 ASSERT(ptlv);
203 /* tlv headr is always packed in LE order */
204 len = ltoh16(ptlv->len);
205 type = ltoh16(ptlv->id);
206 if (len == 0) {
207 /* z-len tlv headers: allow, but don't process */
208 printf("z-len, skip unpack\n");
209 } else {
210 if ((type != xpct_type) ||
211 (len > xpct_len)) {
212 printf("xtlv_unpack Error: found[type:%d,len:%d] != xpct[type:%d,len:%d]\n",
213 type, len, xpct_type, xpct_len);
214 return BCME_BADARG;
215 }
216 /* copy tlv record to caller's buffer */
217 memcpy(dst, ptlv->data, ptlv->len);
218 }
219 *tlv_buf = (uint8*)(*tlv_buf) + BCM_XTLV_SIZE(ptlv, opts);
220 return BCME_OK;
221 }
222
223 /*
224 * packs user data into tlv record
225 * advances tlv pointer to next xtlv slot
226 * buflen is used for tlv_buf space check
227 */
228 int
bcm_pack_xtlv_entry(uint8 ** tlv_buf,uint16 * buflen,uint16 type,uint16 len,void * src,bcm_xtlv_opts_t opts)229 bcm_pack_xtlv_entry(uint8 **tlv_buf, uint16 *buflen, uint16 type, uint16 len, void *src,
230 bcm_xtlv_opts_t opts)
231 {
232 bcm_xtlv_t *ptlv = (bcm_xtlv_t *)*tlv_buf;
233 int size;
234
235 ASSERT(ptlv);
236 ASSERT(src);
237
238 size = bcm_xtlv_size_for_data(len, opts);
239
240 /* copy data from tlv buffer to dst provided by user */
241 if (size > *buflen) {
242 printf("bcm_pack_xtlv_entry: no space tlv_buf: requested:%d, available:%d\n",
243 size, *buflen);
244 return BCME_BADLEN;
245 }
246 ptlv->id = htol16(type);
247 ptlv->len = htol16(len);
248
249 /* copy callers data */
250 memcpy(ptlv->data, src, len);
251
252 /* advance callers pointer to tlv buff */
253 *tlv_buf = (uint8*)(*tlv_buf) + size;
254 /* decrement the len */
255 *buflen -= (uint16)size;
256 return BCME_OK;
257 }
258
259 /*
260 * unpack all xtlv records from the issue a callback
261 * to set function one call per found tlv record
262 */
263 int
bcm_unpack_xtlv_buf(void * ctx,uint8 * tlv_buf,uint16 buflen,bcm_xtlv_opts_t opts,bcm_xtlv_unpack_cbfn_t * cbfn)264 bcm_unpack_xtlv_buf(void *ctx, uint8 *tlv_buf, uint16 buflen, bcm_xtlv_opts_t opts,
265 bcm_xtlv_unpack_cbfn_t *cbfn)
266 {
267 uint16 len;
268 uint16 type;
269 int res = BCME_OK;
270 int size;
271 bcm_xtlv_t *ptlv;
272 int sbuflen = buflen;
273
274 ASSERT(!buflen || tlv_buf);
275 ASSERT(!buflen || cbfn);
276
277 while (sbuflen >= (int)BCM_XTLV_HDR_SIZE) {
278 ptlv = (bcm_xtlv_t *)tlv_buf;
279
280 /* tlv header is always packed in LE order */
281 len = ltoh16(ptlv->len);
282 type = ltoh16(ptlv->id);
283
284 size = bcm_xtlv_size_for_data(len, opts);
285
286 sbuflen -= size;
287 /* check for possible buffer overrun */
288 if (sbuflen < 0)
289 break;
290
291 if ((res = cbfn(ctx, ptlv->data, type, len)) != BCME_OK)
292 break;
293 tlv_buf = (uint8*)tlv_buf + size;
294 }
295 return res;
296 }
297
298 int
bcm_pack_xtlv_buf(void * ctx,void * tlv_buf,uint16 buflen,bcm_xtlv_opts_t opts,bcm_pack_xtlv_next_info_cbfn_t get_next,bcm_pack_xtlv_pack_next_cbfn_t pack_next,int * outlen)299 bcm_pack_xtlv_buf(void *ctx, void *tlv_buf, uint16 buflen, bcm_xtlv_opts_t opts,
300 bcm_pack_xtlv_next_info_cbfn_t get_next, bcm_pack_xtlv_pack_next_cbfn_t pack_next,
301 int *outlen)
302 {
303 int res = BCME_OK;
304 uint16 tlv_id;
305 uint16 tlv_len;
306 uint8 *startp;
307 uint8 *endp;
308 uint8 *buf;
309 bool more;
310 int size;
311
312 ASSERT(get_next && pack_next);
313
314 buf = (uint8 *)tlv_buf;
315 startp = buf;
316 endp = (uint8 *)buf + buflen;
317 more = TRUE;
318 while (more && (buf < endp)) {
319 more = get_next(ctx, &tlv_id, &tlv_len);
320 size = bcm_xtlv_size_for_data(tlv_len, opts);
321 if ((buf + size) > endp) {
322 res = BCME_BUFTOOSHORT;
323 goto done;
324 }
325
326 htol16_ua_store(tlv_id, buf);
327 htol16_ua_store(tlv_len, buf + sizeof(tlv_id));
328 pack_next(ctx, tlv_id, tlv_len, buf + BCM_XTLV_HDR_SIZE);
329 buf += size;
330 }
331
332 if (more)
333 res = BCME_BUFTOOSHORT;
334
335 done:
336 if (outlen) {
337 *outlen = (int)(buf - startp);
338 }
339 return res;
340 }
341
342 /*
343 * pack xtlv buffer from memory according to xtlv_desc_t
344 */
345 int
bcm_pack_xtlv_buf_from_mem(void ** tlv_buf,uint16 * buflen,xtlv_desc_t * items,bcm_xtlv_opts_t opts)346 bcm_pack_xtlv_buf_from_mem(void **tlv_buf, uint16 *buflen, xtlv_desc_t *items,
347 bcm_xtlv_opts_t opts)
348 {
349 int res = BCME_OK;
350 uint8 *ptlv = (uint8 *)*tlv_buf;
351
352 while (items->type != 0) {
353 if ((items->len > 0) && (res = bcm_pack_xtlv_entry(&ptlv,
354 buflen, items->type,
355 items->len, items->ptr, opts) != BCME_OK)) {
356 break;
357 }
358 items++;
359 }
360 *tlv_buf = ptlv; /* update the external pointer */
361 return res;
362 }
363
364 /*
365 * unpack xtlv buffer to memory according to xtlv_desc_t
366 *
367 */
368 int
bcm_unpack_xtlv_buf_to_mem(void * tlv_buf,int * buflen,xtlv_desc_t * items,bcm_xtlv_opts_t opts)369 bcm_unpack_xtlv_buf_to_mem(void *tlv_buf, int *buflen, xtlv_desc_t *items, bcm_xtlv_opts_t opts)
370 {
371 int res = BCME_OK;
372 bcm_xtlv_t *elt;
373
374 elt = bcm_valid_xtlv((bcm_xtlv_t *)tlv_buf, *buflen, opts) ? (bcm_xtlv_t *)tlv_buf : NULL;
375 if (!elt || !items) {
376 res = BCME_BADARG;
377 return res;
378 }
379
380 for (; elt != NULL && res == BCME_OK; elt = bcm_next_xtlv(elt, buflen, opts)) {
381 /* find matches in desc_t items */
382 xtlv_desc_t *dst_desc = items;
383 uint16 len = ltoh16(elt->len);
384
385 while (dst_desc->type != 0) {
386 if (ltoh16(elt->id) == dst_desc->type) {
387 if (len != dst_desc->len) {
388 res = BCME_BADLEN;
389 } else {
390 memcpy(dst_desc->ptr, elt->data, len);
391 }
392 break;
393 }
394 dst_desc++;
395 }
396 }
397
398 if (res == BCME_OK && *buflen != 0)
399 res = BCME_BUFTOOSHORT;
400
401 return res;
402 }
403
404 /*
405 * return data pointer of a given ID from xtlv buffer.
406 * If the specified xTLV ID is found, on return *data_len_out will contain
407 * the the data length of the xTLV ID.
408 */
409 void *
bcm_get_data_from_xtlv_buf(uint8 * tlv_buf,uint16 buflen,uint16 id,uint16 * datalen_out,bcm_xtlv_opts_t opts)410 bcm_get_data_from_xtlv_buf(uint8 *tlv_buf, uint16 buflen, uint16 id,
411 uint16 *datalen_out, bcm_xtlv_opts_t opts)
412 {
413 void *retptr = NULL;
414 uint16 type, len;
415 int size;
416 bcm_xtlv_t *ptlv;
417 int sbuflen = buflen;
418
419 while (sbuflen >= (int)BCM_XTLV_HDR_SIZE) {
420 ptlv = (bcm_xtlv_t *)tlv_buf;
421
422 /* tlv header is always packed in LE order */
423 type = ltoh16(ptlv->id);
424 len = ltoh16(ptlv->len);
425 size = bcm_xtlv_size_for_data(len, opts);
426
427 sbuflen -= size;
428 /* check for possible buffer overrun */
429 if (sbuflen < 0) {
430 printf("%s %d: Invalid sbuflen %d\n",
431 __FUNCTION__, __LINE__, sbuflen);
432 break;
433 }
434
435 if (id == type) {
436 retptr = ptlv->data;
437 if (datalen_out) {
438 *datalen_out = len;
439 }
440 break;
441 }
442 tlv_buf += size;
443 }
444
445 return retptr;
446 }
447
bcm_xtlv_size(const bcm_xtlv_t * elt,bcm_xtlv_opts_t opts)448 int bcm_xtlv_size(const bcm_xtlv_t *elt, bcm_xtlv_opts_t opts)
449 {
450 int size; /* entire size of the XTLV including header, data, and optional padding */
451 int len; /* XTLV's value real length wthout padding */
452
453 len = BCM_XTLV_LEN(elt);
454
455 size = bcm_xtlv_size_for_data(len, opts);
456
457 return size;
458 }
459