85 lines · c
1// SPDX-License-Identifier: ISC2/*3 * Copyright (c) 2019 Broadcom4 */5 6#include <linux/unaligned.h>7 8#include <linux/math.h>9#include <linux/string.h>10#include <linux/bug.h>11 12#include "xtlv.h"13 14static int brcmf_xtlv_header_size(u16 opts)15{16 int len = (int)offsetof(struct brcmf_xtlv, data);17 18 if (opts & BRCMF_XTLV_OPTION_IDU8)19 --len;20 if (opts & BRCMF_XTLV_OPTION_LENU8)21 --len;22 23 return len;24}25 26int brcmf_xtlv_data_size(int dlen, u16 opts)27{28 int hsz;29 30 hsz = brcmf_xtlv_header_size(opts);31 if (opts & BRCMF_XTLV_OPTION_ALIGN32)32 return roundup(dlen + hsz, 4);33 34 return dlen + hsz;35}36 37void brcmf_xtlv_pack_header(struct brcmf_xtlv *xtlv, u16 id, u16 len,38 const u8 *data, u16 opts)39{40 u8 *data_buf;41 u16 mask = BRCMF_XTLV_OPTION_IDU8 | BRCMF_XTLV_OPTION_LENU8;42 43 if (!(opts & mask)) {44 u8 *idp = (u8 *)xtlv;45 u8 *lenp = idp + sizeof(xtlv->id);46 47 put_unaligned_le16(id, idp);48 put_unaligned_le16(len, lenp);49 data_buf = lenp + sizeof(u16);50 } else if ((opts & mask) == mask) { /* u8 id and u8 len */51 u8 *idp = (u8 *)xtlv;52 u8 *lenp = idp + 1;53 54 *idp = (u8)id;55 *lenp = (u8)len;56 data_buf = lenp + sizeof(u8);57 } else if (opts & BRCMF_XTLV_OPTION_IDU8) { /* u8 id, u16 len */58 u8 *idp = (u8 *)xtlv;59 u8 *lenp = idp + 1;60 61 *idp = (u8)id;62 put_unaligned_le16(len, lenp);63 data_buf = lenp + sizeof(u16);64 } else if (opts & BRCMF_XTLV_OPTION_LENU8) { /* u16 id, u8 len */65 u8 *idp = (u8 *)xtlv;66 u8 *lenp = idp + sizeof(u16);67 68 put_unaligned_le16(id, idp);69 *lenp = (u8)len;70 data_buf = lenp + sizeof(u8);71 } else {72 WARN(true, "Unexpected xtlv option");73 return;74 }75 76 if (opts & BRCMF_XTLV_OPTION_LENU8) {77 WARN_ON(len > 0x00ff);78 len &= 0xff;79 }80 81 if (data)82 memcpy(data_buf, data, len);83}84 85