summaryrefslogtreecommitdiff
path: root/drivers/net/wireless/broadcom/brcm80211/brcmfmac/xtlv.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/net/wireless/broadcom/brcm80211/brcmfmac/xtlv.c')
-rw-r--r--drivers/net/wireless/broadcom/brcm80211/brcmfmac/xtlv.c82
1 files changed, 82 insertions, 0 deletions
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/xtlv.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/xtlv.c
new file mode 100644
index 000000000000..2f3c451148db
--- /dev/null
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/xtlv.c
@@ -0,0 +1,82 @@
+// SPDX-License-Identifier: ISC
+/*
+ * Copyright (c) 2019 Broadcom
+ */
+
+#include <asm/unaligned.h>
+#include <linux/string.h>
+#include <linux/bug.h>
+
+#include "xtlv.h"
+
+static int brcmf_xtlv_header_size(u16 opts)
+{
+ int len = (int)offsetof(struct brcmf_xtlv, data);
+
+ if (opts & BRCMF_XTLV_OPTION_IDU8)
+ --len;
+ if (opts & BRCMF_XTLV_OPTION_LENU8)
+ --len;
+
+ return len;
+}
+
+int brcmf_xtlv_data_size(int dlen, u16 opts)
+{
+ int hsz;
+
+ hsz = brcmf_xtlv_header_size(opts);
+ if (opts & BRCMF_XTLV_OPTION_ALIGN32)
+ return roundup(dlen + hsz, 4);
+
+ return dlen + hsz;
+}
+
+void brcmf_xtlv_pack_header(struct brcmf_xtlv *xtlv, u16 id, u16 len,
+ const u8 *data, u16 opts)
+{
+ u8 *data_buf;
+ u16 mask = BRCMF_XTLV_OPTION_IDU8 | BRCMF_XTLV_OPTION_LENU8;
+
+ if (!(opts & mask)) {
+ u8 *idp = (u8 *)xtlv;
+ u8 *lenp = idp + sizeof(xtlv->id);
+
+ put_unaligned_le16(id, idp);
+ put_unaligned_le16(len, lenp);
+ data_buf = lenp + sizeof(u16);
+ } else if ((opts & mask) == mask) { /* u8 id and u8 len */
+ u8 *idp = (u8 *)xtlv;
+ u8 *lenp = idp + 1;
+
+ *idp = (u8)id;
+ *lenp = (u8)len;
+ data_buf = lenp + sizeof(u8);
+ } else if (opts & BRCMF_XTLV_OPTION_IDU8) { /* u8 id, u16 len */
+ u8 *idp = (u8 *)xtlv;
+ u8 *lenp = idp + 1;
+
+ *idp = (u8)id;
+ put_unaligned_le16(len, lenp);
+ data_buf = lenp + sizeof(u16);
+ } else if (opts & BRCMF_XTLV_OPTION_LENU8) { /* u16 id, u8 len */
+ u8 *idp = (u8 *)xtlv;
+ u8 *lenp = idp + sizeof(u16);
+
+ put_unaligned_le16(id, idp);
+ *lenp = (u8)len;
+ data_buf = lenp + sizeof(u8);
+ } else {
+ WARN(true, "Unexpected xtlv option");
+ return;
+ }
+
+ if (opts & BRCMF_XTLV_OPTION_LENU8) {
+ WARN_ON(len > 0x00ff);
+ len &= 0xff;
+ }
+
+ if (data)
+ memcpy(data_buf, data, len);
+}
+