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