bcmdhd.100.10.315.x/bcmxtlv.c

   1 /*
   2  * Driver O/S-independent utility routines
   3  *
   4  * Copyright (C) 1999-2019, Broadcom.
   5  *
   6  *      Unless you and Broadcom execute a separate written software license
   7  * agreement governing use of this software, this software is licensed to you
   8  * under the terms of the GNU General Public License version 2 (the "GPL"),
   9  * available at http://www.broadcom.com/licenses/GPLv2.php, with the
  10  * following added to such license:
  11  *
  12  *      As a special exception, the copyright holders of this software give you
  13  * permission to link this software with independent modules, and to copy and
  14  * distribute the resulting executable under terms of your choice, provided that
  15  * you also meet, for each linked independent module, the terms and conditions of
  16  * the license of that module.  An independent module is a module which is not
  17  * derived from this software.  The special exception does not apply to any
  18  * modifications of the software.
  19  *
  20  *      Notwithstanding the above, under no circumstances may you combine this
  21  * software in any way with any other Broadcom software provided under a license
  22  * other than the GPL, without Broadcom's express prior written consent.
  23  *
  24  *
  25  * <<Broadcom-WL-IPTag/Open:>>
  26  *
  27  * $Id: bcmxtlv.c 788740 2018-11-13 21:45:01Z $
  28  */
  29
  30 #include <bcm_cfg.h>
  31
  32 #include <typedefs.h>
  33 #include <bcmdefs.h>
  34
  35 #include <stdarg.h>
  36
  37 #ifdef BCMDRIVER
  38 #include <osl.h>
  39 #else /* !BCMDRIVER */
  40 #include <stdio.h>
  41 #include <string.h>
  42 #include <stdlib.h>
  43 #ifndef ASSERT
  44 #define ASSERT(exp)
  45 #endif // endif
  46 #endif /* !BCMDRIVER */
  47
  48 #include <bcmtlv.h>
  49 #include <bcmendian.h>
  50 #include <bcmutils.h>
  51
  52 int
  53 bcm_xtlv_hdr_size(bcm_xtlv_opts_t opts)
  54 {
  55         int len = (int)OFFSETOF(bcm_xtlv_t, data); /* nominal */
  56         if (opts & BCM_XTLV_OPTION_LENU8) --len;
  57         if (opts & BCM_XTLV_OPTION_IDU8) --len;
  58
  59         return len;
  60 }
  61
  62 bool
  63 bcm_valid_xtlv(const bcm_xtlv_t *elt, int buf_len, bcm_xtlv_opts_t opts)
  64 {
  65         return elt != NULL &&
  66                 buf_len >= bcm_xtlv_hdr_size(opts) &&
  67                 buf_len  >= bcm_xtlv_size(elt, opts);
  68 }
  69
  70 int
  71 bcm_xtlv_size_for_data(int dlen, bcm_xtlv_opts_t opts)
  72 {
  73         int hsz;
  74
  75         hsz = bcm_xtlv_hdr_size(opts);
  76         return ((opts & BCM_XTLV_OPTION_ALIGN32) ? ALIGN_SIZE(dlen + hsz, 4)
  77                 : (dlen + hsz));
  78 }
  79
  80 int
  81 bcm_xtlv_size(const bcm_xtlv_t *elt, bcm_xtlv_opts_t opts)
  82 {
  83         int size;       /* size including header, data, and any pad */
  84         int len;        /* length wthout padding */
  85
  86         len = BCM_XTLV_LEN_EX(elt, opts);
  87         size = bcm_xtlv_size_for_data(len, opts);
  88         return size;
  89 }
  90
  91 int
  92 bcm_xtlv_len(const bcm_xtlv_t *elt, bcm_xtlv_opts_t opts)
  93 {
  94         const uint8 *lenp;
  95         int len;
  96
  97         lenp = (const uint8 *)&elt->len; /* nominal */
  98         if (opts & BCM_XTLV_OPTION_IDU8) {
  99                 --lenp;
 100         }
 101
 102         if (opts & BCM_XTLV_OPTION_LENU8) {
 103                 len = *lenp;
 104         } else if (opts & BCM_XTLV_OPTION_LENBE) {
 105                 len = (uint32)hton16(elt->len);
 106         } else {
 107                 len = ltoh16_ua(lenp);
 108         }
 109
 110         return len;
 111 }
 112
 113 int
 114 bcm_xtlv_id(const bcm_xtlv_t *elt, bcm_xtlv_opts_t opts)
 115 {
 116         int id = 0;
 117         if (opts & BCM_XTLV_OPTION_IDU8) {
 118                 id =  *(const uint8 *)elt;
 119         } else if (opts & BCM_XTLV_OPTION_IDBE) {
 120                 id = (uint32)hton16(elt->id);
 121         } else {
 122                 id = ltoh16_ua((const uint8 *)elt);
 123         }
 124
 125         return id;
 126 }
 127
 128 bcm_xtlv_t *
 129 bcm_next_xtlv(const bcm_xtlv_t *elt, int *buflen, bcm_xtlv_opts_t opts)
 130 {
 131         int sz;
 132         /* advance to next elt */
 133         sz = BCM_XTLV_SIZE_EX(elt, opts);
 134         elt = (const bcm_xtlv_t*)((const uint8 *)elt + sz);
 135         *buflen -= sz;
 136
 137         /* validate next elt */
 138         if (!bcm_valid_xtlv(elt, *buflen, opts))
 139                 return NULL;
 140
 141         GCC_DIAGNOSTIC_PUSH_SUPPRESS_CAST();
 142         return (bcm_xtlv_t *)(elt);
 143         GCC_DIAGNOSTIC_POP();
 144 }
 145
 146 int
 147 bcm_xtlv_buf_init(bcm_xtlvbuf_t *tlv_buf, uint8 *buf, uint16 len, bcm_xtlv_opts_t opts)
 148 {
 149         if (!tlv_buf || !buf || !len)
 150                 return BCME_BADARG;
 151
 152         tlv_buf->opts = opts;
 153         tlv_buf->size = len;
 154         tlv_buf->head = buf;
 155         tlv_buf->buf  = buf;
 156         return BCME_OK;
 157 }
 158
 159 uint16
 160 bcm_xtlv_buf_len(bcm_xtlvbuf_t *tbuf)
 161 {
 162         uint16 len;
 163
 164         if (tbuf)
 165                 len = (uint16)(tbuf->buf - tbuf->head);
 166         else
 167                 len = 0;
 168
 169         return len;
 170 }
 171
 172 uint16
 173 bcm_xtlv_buf_rlen(bcm_xtlvbuf_t *tbuf)
 174 {
 175         uint16 rlen;
 176         if (tbuf)
 177                 rlen = tbuf->size - bcm_xtlv_buf_len(tbuf);
 178         else
 179                 rlen = 0;
 180
 181         return rlen;
 182 }
 183
 184 uint8 *
 185 bcm_xtlv_buf(bcm_xtlvbuf_t *tbuf)
 186 {
 187         return tbuf ? tbuf->buf : NULL;
 188 }
 189
 190 uint8 *
 191 bcm_xtlv_head(bcm_xtlvbuf_t *tbuf)
 192 {
 193         return tbuf ? tbuf->head : NULL;
 194 }
 195
 196 void
 197 bcm_xtlv_pack_xtlv(bcm_xtlv_t *xtlv, uint16 type, uint16 len, const uint8 *data,
 198         bcm_xtlv_opts_t opts)
 199 {
 200         uint8 *data_buf;
 201         bcm_xtlv_opts_t mask = BCM_XTLV_OPTION_IDU8 | BCM_XTLV_OPTION_LENU8;
 202
 203         if (!(opts & mask)) {           /* default */
 204                 uint8 *idp = (uint8 *)xtlv;
 205                 uint8 *lenp = idp + sizeof(xtlv->id);
 206                 htol16_ua_store(type, idp);
 207                 htol16_ua_store(len, lenp);
 208                 data_buf = lenp + sizeof(uint16);
 209         } else if ((opts & mask) == mask) { /* u8 id and u8 len */
 210                 uint8 *idp = (uint8 *)xtlv;
 211                 uint8 *lenp = idp + 1;
 212                 *idp = (uint8)type;
 213                 *lenp = (uint8)len;
 214                 data_buf = lenp + sizeof(uint8);
 215         } else if (opts & BCM_XTLV_OPTION_IDU8) { /* u8 id, u16 len */
 216                 uint8 *idp = (uint8 *)xtlv;
 217                 uint8 *lenp = idp + 1;
 218                 *idp = (uint8)type;
 219                 htol16_ua_store(len, lenp);
 220                 data_buf = lenp + sizeof(uint16);
 221         } else if (opts & BCM_XTLV_OPTION_LENU8) { /* u16 id, u8 len */
 222                 uint8 *idp = (uint8 *)xtlv;
 223                 uint8 *lenp = idp + sizeof(uint16);
 224                 htol16_ua_store(type, idp);
 225                 *lenp = (uint8)len;
 226                 data_buf = lenp + sizeof(uint8);
 227         } else {
 228                 bool Unexpected_xtlv_option = TRUE;
 229                 BCM_REFERENCE(Unexpected_xtlv_option);
 230                 ASSERT(!Unexpected_xtlv_option);
 231                 return;
 232         }
 233
 234         if (opts & BCM_XTLV_OPTION_LENU8) {
 235                 ASSERT(len <= 0x00ff);
 236                 len &= 0xff;
 237         }
 238
 239         if (data != NULL)
 240                 memcpy(data_buf, data, len);
 241 }
 242
 243 /* xtlv header is always packed in LE order */
 244 void
 245 bcm_xtlv_unpack_xtlv(const bcm_xtlv_t *xtlv, uint16 *type, uint16 *len,
 246         const uint8 **data, bcm_xtlv_opts_t opts)
 247 {
 248         if (type)
 249                 *type = (uint16)bcm_xtlv_id(xtlv, opts);
 250         if (len)
 251                 *len = (uint16)bcm_xtlv_len(xtlv, opts);
 252         if (data)
 253                 *data = (const uint8 *)xtlv + BCM_XTLV_HDR_SIZE_EX(opts);
 254 }
 255
 256 int
 257 bcm_xtlv_put_data(bcm_xtlvbuf_t *tbuf, uint16 type, const uint8 *data, int n)
 258 {
 259         bcm_xtlv_t *xtlv;
 260         int size;
 261
 262         if (tbuf == NULL)
 263                 return BCME_BADARG;
 264
 265         size = bcm_xtlv_size_for_data(n, tbuf->opts);
 266         if (bcm_xtlv_buf_rlen(tbuf) < size)
 267                 return BCME_NOMEM;
 268
 269         xtlv = (bcm_xtlv_t *)bcm_xtlv_buf(tbuf);
 270         bcm_xtlv_pack_xtlv(xtlv, type, (uint16)n, data, tbuf->opts);
 271         tbuf->buf += size; /* note: data may be NULL, reserves space */
 272         return BCME_OK;
 273 }
 274
 275 static int
 276 bcm_xtlv_put_int(bcm_xtlvbuf_t *tbuf, uint16 type, const uint8 *data, int n, int int_sz)
 277 {
 278         bcm_xtlv_t *xtlv;
 279         int xtlv_len;
 280         uint8 *xtlv_data;
 281         int err = BCME_OK;
 282
 283         if (tbuf == NULL) {
 284                 err = BCME_BADARG;
 285                 goto done;
 286         }
 287
 288         xtlv = (bcm_xtlv_t *)bcm_xtlv_buf(tbuf);
 289
 290         /* put type and length in xtlv and reserve data space */
 291         xtlv_len = n * int_sz;
 292         err = bcm_xtlv_put_data(tbuf, type, NULL, xtlv_len);
 293         if (err != BCME_OK)
 294                 goto done;
 295
 296         xtlv_data = (uint8 *)xtlv + bcm_xtlv_hdr_size(tbuf->opts);
 297
 298         /* write data w/ little-endianness into buffer - single loop, aligned access */
 299         for (; n != 0; --n, xtlv_data += int_sz, data += int_sz) {
 300                 switch (int_sz) {
 301                 case sizeof(uint8):
 302                         break;
 303                 case sizeof(uint16):
 304                         {
 305                                 uint16 v =  load16_ua(data);
 306                                 htol16_ua_store(v, xtlv_data);
 307                                 break;
 308                         }
 309                 case sizeof(uint32):
 310                         {
 311                                 uint32 v = load32_ua(data);
 312                                 htol32_ua_store(v, xtlv_data);
 313                                 break;
 314                         }
 315                 case sizeof(uint64):
 316                         {
 317                                 uint64 v = load64_ua(data);
 318                                 htol64_ua_store(v, xtlv_data);
 319                                 break;
 320                         }
 321                 default:
 322                         err = BCME_UNSUPPORTED;
 323                         goto done;
 324                 }
 325         }
 326
 327 done:
 328         return err;
 329 }
 330
 331 int
 332 bcm_xtlv_put16(bcm_xtlvbuf_t *tbuf, uint16 type, const uint16 *data, int n)
 333 {
 334         return bcm_xtlv_put_int(tbuf, type, (const uint8 *)data, n, sizeof(uint16));
 335 }
 336
 337 int
 338 bcm_xtlv_put32(bcm_xtlvbuf_t *tbuf, uint16 type, const uint32 *data, int n)
 339 {
 340         return bcm_xtlv_put_int(tbuf, type, (const uint8 *)data, n, sizeof(uint32));
 341 }
 342
 343 int
 344 bcm_xtlv_put64(bcm_xtlvbuf_t *tbuf, uint16 type, const uint64 *data, int n)
 345 {
 346         return bcm_xtlv_put_int(tbuf, type, (const uint8 *)data, n, sizeof(uint64));
 347 }
 348
 349 /*
 350  *  upacks xtlv record from buf checks the type
 351  *  copies data to callers buffer
 352  *  advances tlv pointer to next record
 353  *  caller's resposible for dst space check
 354  */
 355 int
 356 bcm_unpack_xtlv_entry(const uint8 **tlv_buf, uint16 xpct_type, uint16 xpct_len,
 357         uint8 *dst_data, bcm_xtlv_opts_t opts)
 358 {
 359         const bcm_xtlv_t *ptlv = (const bcm_xtlv_t *)*tlv_buf;
 360         uint16 len;
 361         uint16 type;
 362         const uint8 *data;
 363
 364         ASSERT(ptlv);
 365
 366         bcm_xtlv_unpack_xtlv(ptlv, &type, &len, &data, opts);
 367         if (len) {
 368                 if ((type != xpct_type) || (len > xpct_len))
 369                         return BCME_BADARG;
 370                 if (dst_data && data)
 371                         memcpy(dst_data, data, len); /* copy data to dst */
 372         }
 373
 374         *tlv_buf += BCM_XTLV_SIZE_EX(ptlv, opts);
 375         return BCME_OK;
 376 }
 377
 378 /*
 379  *  packs user data into tlv record and advances tlv pointer to next xtlv slot
 380  *  buflen is used for tlv_buf space check
 381  */
 382 int
 383 bcm_pack_xtlv_entry(uint8 **tlv_buf, uint16 *buflen, uint16 type, uint16 len,
 384         const uint8 *src_data, bcm_xtlv_opts_t opts)
 385 {
 386         bcm_xtlv_t *ptlv = (bcm_xtlv_t *)*tlv_buf;
 387         int size;
 388
 389         ASSERT(ptlv);
 390
 391         size = bcm_xtlv_size_for_data(len, opts);
 392
 393         /* copy data from tlv buffer to dst provided by user */
 394         if (size > *buflen)
 395                 return BCME_BADLEN;
 396
 397         bcm_xtlv_pack_xtlv(ptlv, type, len, src_data, opts);
 398
 399         /* advance callers pointer to tlv buff */
 400         *tlv_buf = (uint8*)(*tlv_buf) + size;
 401         /* decrement the len */
 402         *buflen -= (uint16)size;
 403         return BCME_OK;
 404 }
 405
 406 /*
 407  *  unpack all xtlv records from the issue a callback
 408  *  to set function one call per found tlv record
 409  */
 410 int
 411 bcm_unpack_xtlv_buf(void *ctx, const uint8 *tlv_buf, uint16 buflen, bcm_xtlv_opts_t opts,
 412         bcm_xtlv_unpack_cbfn_t *cbfn)
 413 {
 414         uint16 len;
 415         uint16 type;
 416         int res = BCME_OK;
 417         int size;
 418         const bcm_xtlv_t *ptlv;
 419         int sbuflen = buflen;
 420         const uint8 *data;
 421         int hdr_size;
 422
 423         ASSERT(!buflen || tlv_buf);
 424         ASSERT(!buflen || cbfn);
 425
 426         hdr_size = BCM_XTLV_HDR_SIZE_EX(opts);
 427         while (sbuflen >= hdr_size) {
 428                 ptlv = (const bcm_xtlv_t *)tlv_buf;
 429
 430                 bcm_xtlv_unpack_xtlv(ptlv, &type, &len, &data, opts);
 431                 size = bcm_xtlv_size_for_data(len, opts);
 432
 433                 sbuflen -= size;
 434                 if (sbuflen < 0) /* check for buffer overrun */
 435                         break;
 436
 437                 if ((res = cbfn(ctx, data, type, len)) != BCME_OK)
 438                         break;
 439                 tlv_buf += size;
 440         }
 441         return res;
 442 }
 443
 444 int
 445 bcm_pack_xtlv_buf(void *ctx, uint8 *tlv_buf, uint16 buflen, bcm_xtlv_opts_t opts,
 446         bcm_pack_xtlv_next_info_cbfn_t get_next, bcm_pack_xtlv_pack_next_cbfn_t pack_next,
 447         int *outlen)
 448 {
 449         int res = BCME_OK;
 450         uint16 tlv_id;
 451         uint16 tlv_len;
 452         uint8 *startp;
 453         uint8 *endp;
 454         uint8 *buf;
 455         bool more;
 456         int size;
 457         int hdr_size;
 458
 459         ASSERT(get_next && pack_next);
 460
 461         buf = tlv_buf;
 462         startp = buf;
 463         endp = (uint8 *)buf + buflen;
 464         more = TRUE;
 465         hdr_size = BCM_XTLV_HDR_SIZE_EX(opts);
 466
 467         while (more && (buf < endp)) {
 468                 more = get_next(ctx, &tlv_id, &tlv_len);
 469                 size = bcm_xtlv_size_for_data(tlv_len, opts);
 470                 if ((buf + size) > endp) {
 471                         res = BCME_BUFTOOSHORT;
 472                         goto done;
 473                 }
 474
 475                 bcm_xtlv_pack_xtlv((bcm_xtlv_t *)buf, tlv_id, tlv_len, NULL, opts);
 476                 pack_next(ctx, tlv_id, tlv_len, buf + hdr_size);
 477                 buf += size;
 478         }
 479
 480         if (more)
 481                 res = BCME_BUFTOOSHORT;
 482
 483 done:
 484         if (outlen) {
 485                 *outlen = (int)(buf - startp);
 486         }
 487         return res;
 488 }
 489
 490 /*
 491  *  pack xtlv buffer from memory according to xtlv_desc_t
 492  */
 493 int
 494 bcm_pack_xtlv_buf_from_mem(uint8 **tlv_buf, uint16 *buflen, const xtlv_desc_t *items,
 495         bcm_xtlv_opts_t opts)
 496 {
 497         int res = BCME_OK;
 498         uint8 *ptlv = *tlv_buf;
 499
 500         while (items->type != 0) {
 501                 if (items->len && items->ptr) {
 502                         res = bcm_pack_xtlv_entry(&ptlv, buflen, items->type,
 503                                 items->len, items->ptr, opts);
 504                         if (res != BCME_OK)
 505                                 break;
 506                 }
 507                 items++;
 508         }
 509
 510         *tlv_buf = ptlv; /* update the external pointer */
 511         return res;
 512 }
 513
 514 /*
 515  *  unpack xtlv buffer to memory according to xtlv_desc_t
 516  *
 517  */
 518 int
 519 bcm_unpack_xtlv_buf_to_mem(uint8 *tlv_buf, int *buflen, xtlv_desc_t *items,
 520         bcm_xtlv_opts_t opts)
 521 {
 522         int res = BCME_OK;
 523         bcm_xtlv_t *elt;
 524
 525         elt =  bcm_valid_xtlv((bcm_xtlv_t *)tlv_buf, *buflen, opts) ? (bcm_xtlv_t *)tlv_buf : NULL;
 526         if (!elt || !items) {
 527                 res = BCME_BADARG;
 528                 return res;
 529         }
 530
 531         for (; elt != NULL && res == BCME_OK; elt = bcm_next_xtlv(elt, buflen, opts)) {
 532                 /*  find matches in desc_t items  */
 533                 xtlv_desc_t *dst_desc = items;
 534                 uint16 len, type;
 535                 const uint8 *data;
 536
 537                 bcm_xtlv_unpack_xtlv(elt, &type, &len, &data, opts);
 538                 while (dst_desc->type != 0) {
 539                         if (type == dst_desc->type) {
 540                                 if (len != dst_desc->len) {
 541                                         res = BCME_BADLEN;
 542                                 } else {
 543                                         memcpy(dst_desc->ptr, data, len);
 544                                 }
 545                                 break;
 546                         }
 547                         dst_desc++;
 548                 }
 549         }
 550
 551         if (res == BCME_OK && *buflen != 0)
 552                 res =  BCME_BUFTOOSHORT;
 553
 554         return res;
 555 }
 556
 557 /*
 558  * return data pointer of a given ID from xtlv buffer.
 559  * If the specified xTLV ID is found, on return *datalen will contain
 560  * the the data length of the xTLV ID.
 561  */
 562 const uint8*
 563 bcm_get_data_from_xtlv_buf(const uint8 *tlv_buf, uint16 buflen, uint16 id,
 564         uint16 *datalen, bcm_xtlv_opts_t opts)
 565 {
 566         const uint8 *retptr = NULL;
 567         uint16 type, len;
 568         int size;
 569         const bcm_xtlv_t *ptlv;
 570         int sbuflen = buflen;
 571         const uint8 *data;
 572         int hdr_size;
 573
 574         hdr_size = BCM_XTLV_HDR_SIZE_EX(opts);
 575
 576         /* Init the datalength */
 577         if (datalen) {
 578                 *datalen = 0;
 579         }
 580         while (sbuflen >= hdr_size) {
 581                 ptlv = (const bcm_xtlv_t *)tlv_buf;
 582                 bcm_xtlv_unpack_xtlv(ptlv, &type, &len, &data, opts);
 583
 584                 size = bcm_xtlv_size_for_data(len, opts);
 585                 sbuflen -= size;
 586                 if (sbuflen < 0) /* buffer overrun? */
 587                         break;
 588
 589                 if (id == type) {
 590                         retptr = data;
 591                         if (datalen)
 592                                 *datalen = len;
 593                         break;
 594                 }
 595
 596                 tlv_buf += size;
 597         }
 598
 599         return retptr;
 600 }
 601
 602 bcm_xtlv_t*
 603 bcm_xtlv_bcopy(const bcm_xtlv_t *src, bcm_xtlv_t *dst,
 604         int src_buf_len, int dst_buf_len, bcm_xtlv_opts_t opts)
 605 {
 606         bcm_xtlv_t *dst_next = NULL;
 607         src =  (src && bcm_valid_xtlv(src, src_buf_len, opts)) ? src : NULL;
 608         if (src && dst) {
 609                 uint16 type;
 610                 uint16 len;
 611                 const uint8 *data;
 612                 int size;
 613                 bcm_xtlv_unpack_xtlv(src, &type, &len, &data, opts);
 614                 size = bcm_xtlv_size_for_data(len, opts);
 615                 if (size <= dst_buf_len) {
 616                         bcm_xtlv_pack_xtlv(dst, type, len, data, opts);
 617                         dst_next = (bcm_xtlv_t *)((uint8 *)dst + size);
 618                 }
 619         }
 620
 621         return dst_next;
 622 }