1// SPDX-License-Identifier: ISC
2/*
3 * Copyright (c) 2019 Broadcom
4 */
5
6#include <asm/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