[RFC v2 40/96] cl8k: add mac_addr.c

[viktor.barna@celeno.com]

From: Viktor Barna <viktor.barna@celeno.com>

(Part of the split. Please, take a look at the cover letter for more
details).

Signed-off-by: Viktor Barna <viktor.barna@celeno.com>
---
 drivers/net/wireless/celeno/cl8k/mac_addr.c | 418 ++++++++++++++++++++
 1 file changed, 418 insertions(+)
 create mode 100644 drivers/net/wireless/celeno/cl8k/mac_addr.c

diff --git a/drivers/net/wireless/celeno/cl8k/mac_addr.c b/drivers/net/wireless/celeno/cl8k/mac_addr.c
new file mode 100644
index 000000000000..be9080564773
--- /dev/null
+++ b/drivers/net/wireless/celeno/cl8k/mac_addr.c
@@ -0,0 +1,418 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-2-Clause
+/* Copyright(c) 2019-2022, Celeno Communications Ltd. */
+
+#include "chip.h"
+#include "mac_addr.h"
+
+/** DOC: MAC identifier generation
+ *
+ * The driver allows to works with multiple MAC indentifiers via one base MAC,
+ * which it gets from:
+ *  a) EEPROM;
+ *  b) %ce_phys_mac_addr parameter in chip config;
+ *  c) randomly generated address with Celeno's OUI.
+ *
+ * All other MAC are generated from base MAC by modyfing specific byte in each
+ * new address (starting from %ce_first_mask_bit). All addresses should be in
+ * 4-bit range from each other (HW requirement).
+ *
+ * Both tranceivers are sharing MAC addresses pool per same chip, and with LAM
+ * (=Locally Administered Mac) there is the ability to have up to 16 different
+ * addrs starting with any base MAC. Otherwise (without LAM), base MAC should
+ * allow to generate addresses within 4-bit difference at max.
+ *
+ * Max addresses count is configurable on per-TCV basis via %ci_max_bss_num
+ * variable.
+ *
+ * If LAM is enabled, TCV0 is getting original MAC for the first interface. All
+ * another interfaces of the TCV0 and all interfaces of the TCV1 are getting
+ * LAM-based addresses, which will have 0x02 in the first byte and will have
+ * dynamic last (depends on %ce_first_mask_bit) byte, which typically is being
+ * incremented for each new address, but not always (the logic tries to fit
+ * addresses in 4-bit range, for the sake of this rule new LAM-based address be
+ * reseted to start from 0.
+ *
+ * MAC examples
+ * Case 1: Typical (with LAM)
+ *  - 00:1C:51:BD:FB:00; -> base MAC (valid with and without LAM)
+ *  - 02:1C:51:BD:FB:00;
+ *  - 02:1C:51:BD:FB:01;
+ *
+ * Case 2: Typical (without LAM)
+ *  - 00:1C:51:BD:FB:00; -> base MAC (valid with and without LAM)
+ *  - 00:1C:51:BD:FB:01;
+ *  - 00:1C:51:BD:FB:02;
+ *
+ * Case 3: With reset to fit 4-bit rule (with LAM)
+ *  - 00:1C:51:BD:FB:CF; -> base MAC (valid only with LAM)
+ *  - 02:1C:51:BD:FB:CF;
+ *  - 02:1C:51:BD:FB:C0:
+ *  - 02:1C:51:BD:FB:C1;
+ */
+
+#define CL_LAM_INDICATION 0x02
+
+static int cl_set_mask_addr_without_zeroing_bits(struct cl_hw *cl_hw, u64 mac64, u8 bss_num,
+						 u8 first_mask_bit, u8 *mask_addr)
+{
+	u64 mask = mac64;
+	s8 idx = 0;
+
+	mask >>= first_mask_bit;
+	mask += (bss_num - 1);
+
+	/*
+	 * After the following line the mask will contain the changed
+	 * bits between the first BSS MAC and the last BSS MAC
+	 */
+	mask ^= (mac64 >> first_mask_bit);
+
+	/* Find leftmost set bit */
+	for (idx = 47 - first_mask_bit; (idx >= 0) && (!(mask & (1ULL << idx))); idx--)
+		;
+
+	if (idx < 0) {
+		cl_dbg_err(cl_hw, "Invalid mask (mac=0x%02llx, first_mask_bit=%u, bss_num=%u)\n",
+			   mac64, first_mask_bit, bss_num);
+		mask = 0;
+		eth_zero_addr(mask_addr);
+
+		return -1;
+	}
+
+	mask = (1ULL << idx);
+	mask |= (mask - 1);
+	mask <<= first_mask_bit;
+
+	for (idx = 0; idx < ETH_ALEN; idx++) {
+		u8 shift = (BITS_PER_BYTE * (ETH_ALEN - 1 - idx));
+
+		mask_addr[idx] = (mask & ((u64)0xff << shift)) >> shift;
+	}
+
+	return 0;
+}
+
+static int cl_mask_mac_by_bss_num(struct cl_hw *cl_hw, u8 *mac_addr, u8 *mask_addr,
+				  bool use_lam, bool random_mac)
+{
+	u8 bss_num = cl_hw->conf->ci_max_bss_num;
+	struct cl_hw *cl_hw_tcv0 = cl_hw->chip->cl_hw_tcv0;
+	u8 first_mask_bit = cl_hw->chip->conf->ce_first_mask_bit;
+	u8 i;
+	/* Determine the bits necessary to cover the number of BSSIDs. */
+	u8 num_bits_to_mask[ARRAY_SIZE(cl_hw->addresses) * TCV_MAX] = {
+		0, /* 0 : 00:1C:51:BD:FB:(0b 0000 0000) -> 0-bit diff (no LAM, original MAC) */
+		0, /* 1 : 02:1C:51:BD:FB:(0b 0000 0000) -> 0-bit diff (LAM) */
+
+		1, /* 2 : 02:1C:51:BD:FB:(0b 0000 0001) -> 1-bit diff (LAM, between address #1) */
+
+		2, /* 3 : 02:1C:51:BD:FB:(0b 0000 0011) -> 2-bit diff (LAM) */
+		2, /* 4 : 02:1C:51:BD:FB:(0b 0000 0010) -> 2-bit diff (LAM) */
+
+		3, /* 5 : 02:1C:51:BD:FB:(0b 0000 0111) -> 3-bit diff (LAM) */
+		3, /* 6 : 02:1C:51:BD:FB:(0b 0000 0100) -> 3-bit diff (LAM) */
+		3, /* 7 : 02:1C:51:BD:FB:(0b 0000 0101) -> 3-bit diff (LAM) */
+		3, /* 8 : 02:1C:51:BD:FB:(0b 0000 0110) -> 3-bit diff (LAM) */
+
+		4, /* 9 : 02:1C:51:BD:FB:(0b 0000 1111) -> 4-bit diff (LAM) */
+		4, /* 10: 02:1C:51:BD:FB:(0b 0000 1000) -> 4-bit diff (LAM) */
+		4, /* 11: 02:1C:51:BD:FB:(0b 0000 1001) -> 4-bit diff (LAM) */
+		4, /* 12: 02:1C:51:BD:FB:(0b 0000 1010) -> 4-bit diff (LAM) */
+		4, /* 13: 02:1C:51:BD:FB:(0b 0000 1100) -> 4-bit diff (LAM) */
+		4, /* 14: 02:1C:51:BD:FB:(0b 0000 1110) -> 4-bit diff (LAM) */
+		4, /* 15: 02:1C:51:BD:FB:(0b 0000 1011) -> 4-bit diff (LAM) */
+	};
+
+	u8 mask_size = 0;
+	u8 byte_num = ETH_ALEN - 1 - (first_mask_bit / BITS_PER_BYTE);
+	u8 bit_in_byte = first_mask_bit % BITS_PER_BYTE; /* Referring to the index of the bit */
+
+	if ((first_mask_bit + num_bits_to_mask[bss_num]) > BITS_PER_TYPE(struct mac_address)) {
+		cl_dbg_err(cl_hw, "Invalid combination of first_mask_bit + bss_num. "
+		       "must be lower than 48 bit in total\n");
+		return -EINVAL;
+	}
+
+	if (cl_hw_is_first_tcv(cl_hw)) {
+		mask_size = num_bits_to_mask[bss_num - 1];
+	} else {
+		u64 mac64 = ether_addr_to_u64(mac_addr);
+		u8 tcv0_bss_num = cl_hw_tcv0 ? cl_hw_tcv0->conf->ci_max_bss_num : 0;
+		u8 bit_mask = (1 << num_bits_to_mask[bss_num + tcv0_bss_num - 1]) - 1;
+
+		/*
+		 * If we need to zero bits due to lack of room for the MAC addresses
+		 * of all BSSs of TCV1, then the mask is the number of zeroed bits
+		 */
+		if (((u64)bit_mask - ((mac64 >> first_mask_bit) & (u64)bit_mask) + 1) < bss_num) {
+			mask_size = num_bits_to_mask[bss_num + tcv0_bss_num - 1];
+		} else {
+			/*
+			 * Otherwise the mask is the different bits between the
+			 * addresses of the first and the last BSSs
+			 */
+			cl_set_mask_addr_without_zeroing_bits(cl_hw, mac64, bss_num,
+							      first_mask_bit, mask_addr);
+			return 0;
+		}
+	}
+
+	/* Build mac and mask addr */
+	for (i = 0; i < mask_size; i++) {
+		/*
+		 * Build mask - Convert to "1" the relevant bits in the mask
+		 * addr in order to support the desired number of BSSIDs
+		 */
+		mask_addr[byte_num] |= (0x01 << bit_in_byte);
+
+		/*
+		 * Build mac -convert to "0" the relevant bits in the mac addr
+		 * in order to support the desired number of BSSIDs
+		 */
+		if (random_mac && !use_lam)
+			mac_addr[byte_num] &= ~(0x01 << bit_in_byte);
+
+		bit_in_byte++;
+
+		/* Support cases where the mask bits are not at the same byte. */
+		if (bit_in_byte == BITS_PER_BYTE) {
+			byte_num--;
+			bit_in_byte = 0;
+		}
+	}
+
+	if (use_lam) {
+		/* Mask LAM bit (Locally Administered Mac) */
+		if (cl_hw_is_first_tcv(cl_hw))
+			mask_addr[0] |= CL_LAM_INDICATION;
+	} else {
+		/*
+		 * When not using LAM we do not zero the MAC address of the second BSS,
+		 * so the mask (the modified bits between the first and last BSS) depends
+		 * on initial MAC
+		 */
+		u64 mac64 = ether_addr_to_u64(mac_addr);
+
+		cl_set_mask_addr_without_zeroing_bits(cl_hw, mac64, bss_num,
+						      first_mask_bit, mask_addr);
+	}
+
+	return 0;
+}
+
+#define MAC_FILTER_BITS 4
+#define MAC_FILTER_MASK ((1 << MAC_FILTER_BITS) - 1)
+
+static bool cl_is_valid_mac_addr(u64 mac64, u8 first_mask_bit, u8 bss_num)
+{
+	u8 mac_bits = (mac64 >> first_mask_bit) & MAC_FILTER_MASK;
+	u8 mac_diff = 0;
+	u8 i;
+
+	for (i = 0; i < bss_num; i++) {
+		mac_diff |= mac_bits;
+		mac_bits++;
+	}
+
+	return hweight8(mac_diff) <= MAC_FILTER_BITS;
+}
+
+static int cl_mac_addr_set_addresses(struct cl_hw *cl_hw, bool use_lam,
+				     u8 *mask_addr)
+{
+	u8 first_mask_bit = cl_hw->chip->conf->ce_first_mask_bit;
+	int i = 0;
+	u8 bss_num = min_t(u8, cl_hw->conf->ci_max_bss_num, ARRAY_SIZE(cl_hw->addresses));
+	u64 mac64 = ether_addr_to_u64(cl_hw->hw->wiphy->perm_addr);
+	u64 mask64 = 0;
+	u8 new_addr[ETH_ALEN] = {0};
+
+	if (!use_lam && !cl_is_valid_mac_addr(mac64, first_mask_bit, bss_num)) {
+		cl_dbg_err(cl_hw,
+			   "perm_addr %pM is invalid for bss_num %d without LAM\n",
+			   cl_hw->hw->wiphy->perm_addr, bss_num);
+		return -1;
+	}
+
+	cl_mac_addr_copy(cl_hw->addresses[i].addr,
+			 cl_hw->hw->wiphy->perm_addr);
+	for (i = 1; i < bss_num; i++) {
+		u8 *prev_addr = cl_hw->addresses[i - 1].addr;
+
+		if (use_lam) {
+			mac64 = ether_addr_to_u64(prev_addr);
+			mask64 = ether_addr_to_u64(mask_addr);
+			if (cl_hw_is_first_tcv(cl_hw)) {
+				if (i == 1)
+					mac64 &= ~mask64;
+				else
+					mac64 += 1 << first_mask_bit;
+				u64_to_ether_addr(mac64, new_addr);
+				new_addr[0] |= CL_LAM_INDICATION;
+			} else {
+				if ((mac64 & mask64) == mask64)
+					mac64 &= ~mask64;
+				else
+					mac64 += 1 << first_mask_bit;
+				u64_to_ether_addr(mac64, new_addr);
+			}
+			cl_mac_addr_copy(cl_hw->addresses[i].addr, new_addr);
+		} else {
+			mac64 = ether_addr_to_u64(prev_addr);
+			mac64 += 1 << first_mask_bit;
+			u64_to_ether_addr(mac64, cl_hw->addresses[i].addr);
+		}
+	}
+	cl_hw->n_addresses = bss_num;
+
+	return 0;
+}
+
+static int cl_mac_addr_set_first_tcv(struct cl_hw *cl_hw, u8 *dflt_mac,
+				     bool *random_mac)
+{
+	struct cl_chip *chip = cl_hw->chip;
+
+	if (!cl_mac_addr_is_zero(chip->conf->ce_phys_mac_addr)) {
+		/* Read MAC from NVRAM file */
+		cl_mac_addr_copy(dflt_mac, chip->conf->ce_phys_mac_addr);
+		cl_dbg_verbose(cl_hw, "Read MAC address from NVRAM [%pM]\n", dflt_mac);
+	} else {
+		/* Read MAC from EEPROM */
+		if (chip->eeprom_read_block(chip, ADDR_GEN_MAC_ADDR,
+					    ETH_ALEN, dflt_mac) != ETH_ALEN) {
+			CL_DBG_ERROR(cl_hw, "Error reading MAC address from EEPROM\n");
+			return -1;
+		}
+
+		cl_dbg_verbose(cl_hw, "Read MAC address from EEPROM [%pM]\n", dflt_mac);
+	}
+
+	/* Test if the new mac address is 00:00:00:00:00:00 or ff:ff:ff:ff:ff:ff */
+	if (cl_mac_addr_is_zero(dflt_mac) || cl_mac_addr_is_broadcast(dflt_mac)) {
+		/* Set celeno oui */
+		dflt_mac[0] = 0x00;
+		dflt_mac[1] = 0x1c;
+		dflt_mac[2] = 0x51;
+		get_random_bytes(&dflt_mac[3], 3);
+		cl_dbg_verbose(cl_hw, "Random MAC address [%pM]\n", dflt_mac);
+		*random_mac = true;
+	}
+
+	return 0;
+}
+
+static void cl_mac_addr_set_second_tcv(struct cl_hw *cl_hw, u8 *dflt_mac)
+{
+	struct cl_chip *chip = cl_hw->chip;
+	struct cl_hw *cl_hw_tcv0 = chip->cl_hw_tcv0;
+	u8 tcv0_bss_num = cl_hw_tcv0->conf->ci_max_bss_num;
+	u8 first_mask_bit = chip->conf->ce_first_mask_bit;
+	u64 mac64;
+	u8 idx;
+	u8 bss_num = cl_hw->conf->ci_max_bss_num;
+	u8 lam_bit_mask[ARRAY_SIZE(cl_hw->addresses) * TCV_MAX] = {
+		0b0000, /* 1 addr,  0-bit diff between MAC addrs, LAM is not affecting it */
+		0b0000, /* 2 addrs, 0-bit diff between MAC addrs, first differs by LAM !!! */
+		0b0001, /* 3 addrs, 1-bit diff */
+		0b0011, /* 4 addrs, 2-bit diff */
+		0b0011, /* 5 addrs, 2-bit diff */
+
+		0b0111, /* 6 addrs, 3-bit diff */
+		0b0111, /* 7 addrs, 3-bit diff */
+		0b0111, /* 8 addrs, 3-bit diff */
+		0b0111, /* 9 addrs, 3-bit diff */
+
+		0b1111, /* 10 addrs, 4-bit diff */
+		0b1111, /* 11 addrs, 4-bit diff */
+		0b1111, /* 12 addrs, 4-bit diff */
+		0b1111, /* 13 addrs, 4-bit diff */
+		0b1111, /* 14 addrs, 4-bit diff */
+		0b1111, /* 15 addrs, 4-bit diff */
+		0b1111, /* 16 addrs, 4-bit diff */
+	};
+
+	mac64 = ether_addr_to_u64(cl_hw_tcv0->hw->wiphy->perm_addr);
+
+	if (chip->conf->ce_lam_enable) {
+		/* Find the first address of TCV1 */
+		if (tcv0_bss_num == 1) {
+			/*
+			 * For tcv0 bss num = 1, we have to zero the necessary bits
+			 * since it hasn't been done in TCV0
+			 */
+			mac64 &= ~((u64)lam_bit_mask[bss_num] << first_mask_bit);
+		} else {
+			u8 total_bss_to_mask = bss_num + tcv0_bss_num - 1;
+
+			mac64 &= ~((u64)lam_bit_mask[tcv0_bss_num - 1] << first_mask_bit);
+			/*
+			 * Get the first MAC address of TCV1 by incrementing the MAC
+			 * address of the last BSS of TCV0.
+			 * After the instruction below mac64 will hold the MAC of TCV0's
+			 * last BSS.
+			 */
+			mac64 += ((u64)(tcv0_bss_num - 2) << first_mask_bit);
+			/*
+			 * If there is no more room for another address in TCV0's mask
+			 * address then we have to zero bits else increment the last
+			 * address of TCV0
+			 */
+			if (((mac64 >> first_mask_bit) & (u64)lam_bit_mask[total_bss_to_mask]) ==
+			    (u64)lam_bit_mask[total_bss_to_mask])
+				mac64 &= ~((u64)lam_bit_mask[total_bss_to_mask] << first_mask_bit);
+			else
+				mac64 += (1ULL << first_mask_bit);
+		}
+
+		/* Enable LAM bit */
+		mac64 += (0x2ULL << 40);
+	} else {
+		mac64 += ((u64)tcv0_bss_num << first_mask_bit);
+	}
+
+	for (idx = 0; idx < ETH_ALEN; idx++) {
+		u8 shift = (8 * (ETH_ALEN - 1 - idx));
+
+		dflt_mac[idx] = (mac64 & ((u64)0xFF << shift)) >> shift;
+	}
+}
+
+int cl_mac_addr_set(struct cl_hw *cl_hw)
+{
+	bool random_mac = false;
+	u8 dflt_mac[ETH_ALEN] = {0x00, 0x1c, 0x51, 0x51, 0x51, 0x51};
+	u8 dflt_mask[ETH_ALEN] = {0};
+	bool use_lam = cl_hw->chip->conf->ce_lam_enable;
+	struct wiphy *wiphy = cl_hw->hw->wiphy;
+
+	if (cl_hw_is_first_tcv(cl_hw)) {
+		if (cl_mac_addr_set_first_tcv(cl_hw, dflt_mac, &random_mac))
+			return -1;
+	} else {
+		cl_mac_addr_set_second_tcv(cl_hw, dflt_mac);
+	}
+
+	if (cl_mask_mac_by_bss_num(cl_hw, dflt_mac, dflt_mask, use_lam, random_mac))
+		return -1;
+
+	/* Permanent address MAC (the MAC of the first BSS) */
+	SET_IEEE80211_PERM_ADDR(cl_hw->hw, dflt_mac);
+
+	/*
+	 * Addresses count must be power of 2
+	 * mac80211 doesn't handle non-contiguous masks
+	 */
+	BUILD_BUG_ON_NOT_POWER_OF_2(ARRAY_SIZE(cl_hw->addresses));
+
+	cl_mac_addr_array_to_nxmac(dflt_mask, &cl_hw->mask_low, &cl_hw->mask_hi);
+
+	if (cl_mac_addr_set_addresses(cl_hw, use_lam, dflt_mask))
+		return -1;
+
+	wiphy->addresses = cl_hw->addresses;
+	wiphy->n_addresses = cl_hw->n_addresses;
+
+	return 0;
+}
-- 
2.36.1