From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: afaulkner420 Date: Fri, 25 Mar 2022 20:18:18 +0000 Subject: Add sunxi-addr driver - Used to fix uwe5622 bluetooth MAC addresses --- drivers/misc/Kconfig | 1 + drivers/misc/Makefile | 1 + drivers/misc/sunxi-addr/Kconfig | 6 + drivers/misc/sunxi-addr/Makefile | 5 + drivers/misc/sunxi-addr/sha256.c | 178 +++++ drivers/misc/sunxi-addr/sunxi-addr.c | 357 ++++++++++ 6 files changed, 548 insertions(+) diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig index 57201f491f8d..26f2df508f70 100644 --- a/drivers/misc/Kconfig +++ b/drivers/misc/Kconfig @@ -593,4 +593,5 @@ source "drivers/misc/cardreader/Kconfig" source "drivers/misc/uacce/Kconfig" source "drivers/misc/pvpanic/Kconfig" source "drivers/misc/mchp_pci1xxxx/Kconfig" +source "drivers/misc/sunxi-addr/Kconfig" endmenu diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile index c5556e3008c1..9ba27862b564 100644 --- a/drivers/misc/Makefile +++ b/drivers/misc/Makefile @@ -69,3 +69,4 @@ obj-$(CONFIG_TMR_INJECT) += xilinx_tmr_inject.o obj-$(CONFIG_TPS6594_ESM) += tps6594-esm.o obj-$(CONFIG_TPS6594_PFSM) += tps6594-pfsm.o obj-$(CONFIG_MODEM_POWER) += modem-power.o +obj-$(CONFIG_SUNXI_ADDR_MGT) += sunxi-addr/ diff --git a/drivers/misc/sunxi-addr/Kconfig b/drivers/misc/sunxi-addr/Kconfig new file mode 100644 index 000000000000..801dd2c02a56 --- /dev/null +++ b/drivers/misc/sunxi-addr/Kconfig @@ -0,0 +1,6 @@ +config SUNXI_ADDR_MGT + tristate "Allwinner Network MAC Addess Manager" + depends on BT || ETHERNET || WLAN + depends on NVMEM_SUNXI_SID + help + allwinner network mac address management diff --git a/drivers/misc/sunxi-addr/Makefile b/drivers/misc/sunxi-addr/Makefile new file mode 100644 index 000000000000..f01fd4783566 --- /dev/null +++ b/drivers/misc/sunxi-addr/Makefile @@ -0,0 +1,5 @@ +# +# Makefile for wifi mac addr manager drivers +# +sunxi_addr-objs := sunxi-addr.o sha256.o +obj-$(CONFIG_SUNXI_ADDR_MGT) += sunxi_addr.o diff --git a/drivers/misc/sunxi-addr/sha256.c b/drivers/misc/sunxi-addr/sha256.c new file mode 100644 index 000000000000..78825810c53f --- /dev/null +++ b/drivers/misc/sunxi-addr/sha256.c @@ -0,0 +1,178 @@ +/* + * Local implement of sha256. + * + * Copyright (C) 2013 Allwinner. + * + * This file is licensed under the terms of the GNU General Public + * License version 2. This program is licensed "as is" without any + * warranty of any kind, whether express or implied. + */ +#include +#include + +/****************************** MACROS ******************************/ +#define ROTRIGHT(a, b) (((a) >> (b)) | ((a) << (32 - (b)))) +#define CH(x, y, z) (((x) & (y)) ^ (~(x) & (z))) +#define MAJ(x, y, z) (((x) & (y)) ^ ((x) & (z)) ^ ((y) & (z))) +#define EP0(x) (ROTRIGHT(x, 2) ^ ROTRIGHT(x, 13) ^ ROTRIGHT(x, 22)) +#define EP1(x) (ROTRIGHT(x, 6) ^ ROTRIGHT(x, 11) ^ ROTRIGHT(x, 25)) +#define SIG0(x) (ROTRIGHT(x, 7) ^ ROTRIGHT(x, 18) ^ ((x) >> 3)) +#define SIG1(x) (ROTRIGHT(x, 17) ^ ROTRIGHT(x, 19) ^ ((x) >> 10)) + +/**************************** VARIABLES *****************************/ +static const uint32_t k[64] = { + 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, + 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, + 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, + 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, + 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, + 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, + 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, + 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, + 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, + 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, + 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, + 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, + 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, + 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, + 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, + 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2 +}; + +struct sha256_ctx { + uint8_t data[64]; /* current 512-bit chunk of message data, just like a buffer */ + uint32_t datalen; /* sign the data length of current chunk */ + uint64_t bitlen; /* the bit length of the total message */ + uint32_t state[8]; /* store the middle state of hash abstract */ +}; + +/*********************** FUNCTION DEFINITIONS ***********************/ +static void sha256_transform(struct sha256_ctx *ctx, const uint8_t *data) +{ + uint32_t a, b, c, d, e, f, g, h, i, j, t1, t2, m[64]; + + /* initialization */ + for (i = 0, j = 0; i < 16; ++i, j += 4) + m[i] = (data[j] << 24) | (data[j + 1] << 16) | + (data[j + 2] << 8) | (data[j + 3]); + for ( ; i < 64; ++i) + m[i] = SIG1(m[i - 2]) + m[i - 7] + SIG0(m[i - 15]) + m[i - 16]; + + a = ctx->state[0]; + b = ctx->state[1]; + c = ctx->state[2]; + d = ctx->state[3]; + e = ctx->state[4]; + f = ctx->state[5]; + g = ctx->state[6]; + h = ctx->state[7]; + + for (i = 0; i < 64; ++i) { + t1 = h + EP1(e) + CH(e, f, g) + k[i] + m[i]; + t2 = EP0(a) + MAJ(a, b, c); + h = g; + g = f; + f = e; + e = d + t1; + d = c; + c = b; + b = a; + a = t1 + t2; + } + + ctx->state[0] += a; + ctx->state[1] += b; + ctx->state[2] += c; + ctx->state[3] += d; + ctx->state[4] += e; + ctx->state[5] += f; + ctx->state[6] += g; + ctx->state[7] += h; +} + +static void sha256_init(struct sha256_ctx *ctx) +{ + ctx->datalen = 0; + ctx->bitlen = 0; + ctx->state[0] = 0x6a09e667; + ctx->state[1] = 0xbb67ae85; + ctx->state[2] = 0x3c6ef372; + ctx->state[3] = 0xa54ff53a; + ctx->state[4] = 0x510e527f; + ctx->state[5] = 0x9b05688c; + ctx->state[6] = 0x1f83d9ab; + ctx->state[7] = 0x5be0cd19; +} + +static void sha256_update(struct sha256_ctx *ctx, const uint8_t *data, size_t len) +{ + uint32_t i; + + for (i = 0; i < len; ++i) { + ctx->data[ctx->datalen] = data[i]; + ctx->datalen++; + if (ctx->datalen == 64) { + /* 64 byte = 512 bit means the buffer ctx->data has + * fully stored one chunk of message, + * so do the sha256 hash map for the current chunk. + */ + sha256_transform(ctx, ctx->data); + ctx->bitlen += 512; + ctx->datalen = 0; + } + } +} + +static void sha256_final(struct sha256_ctx *ctx, uint8_t *hash) +{ + uint32_t i; + + i = ctx->datalen; + + /* Pad whatever data is left in the buffer. */ + if (ctx->datalen < 56) { + ctx->data[i++] = 0x80; /* pad 10000000 = 0x80 */ + while (i < 56) + ctx->data[i++] = 0x00; + } else { + ctx->data[i++] = 0x80; + while (i < 64) + ctx->data[i++] = 0x00; + sha256_transform(ctx, ctx->data); + memset(ctx->data, 0, 56); + } + + /* Append to the padding the total message's length in bits and transform. */ + ctx->bitlen += ctx->datalen * 8; + ctx->data[63] = ctx->bitlen; + ctx->data[62] = ctx->bitlen >> 8; + ctx->data[61] = ctx->bitlen >> 16; + ctx->data[60] = ctx->bitlen >> 24; + ctx->data[59] = ctx->bitlen >> 32; + ctx->data[58] = ctx->bitlen >> 40; + ctx->data[57] = ctx->bitlen >> 48; + ctx->data[56] = ctx->bitlen >> 56; + sha256_transform(ctx, ctx->data); + + /* copying the final state to the output hash(use big endian). */ + for (i = 0; i < 4; ++i) { + hash[i] = (ctx->state[0] >> (24 - i * 8)) & 0x000000ff; + hash[i + 4] = (ctx->state[1] >> (24 - i * 8)) & 0x000000ff; + hash[i + 8] = (ctx->state[2] >> (24 - i * 8)) & 0x000000ff; + hash[i + 12] = (ctx->state[3] >> (24 - i * 8)) & 0x000000ff; + hash[i + 16] = (ctx->state[4] >> (24 - i * 8)) & 0x000000ff; + hash[i + 20] = (ctx->state[5] >> (24 - i * 8)) & 0x000000ff; + hash[i + 24] = (ctx->state[6] >> (24 - i * 8)) & 0x000000ff; + hash[i + 28] = (ctx->state[7] >> (24 - i * 8)) & 0x000000ff; + } +} + +int hmac_sha256(const uint8_t *plaintext, ssize_t psize, uint8_t *output) +{ + struct sha256_ctx ctx; + + sha256_init(&ctx); + sha256_update(&ctx, plaintext, psize); + sha256_final(&ctx, output); + return 0; +} diff --git a/drivers/misc/sunxi-addr/sunxi-addr.c b/drivers/misc/sunxi-addr/sunxi-addr.c new file mode 100644 index 000000000000..f4f2dcecbb48 --- /dev/null +++ b/drivers/misc/sunxi-addr/sunxi-addr.c @@ -0,0 +1,357 @@ +/* + * The driver of SUNXI NET MAC ADDR Manager. + * + * Copyright (C) 2013 Allwinner. + * + * This file is licensed under the terms of the GNU General Public + * License version 2. This program is licensed "as is" without any + * warranty of any kind, whether express or implied. + */ +#define DEBUG + +#include +#include +#include +#include +#include + +#define ADDR_MGT_DBG(fmt, arg...) printk(KERN_DEBUG "[ADDR_MGT] %s: " fmt "\n",\ + __func__, ## arg) +#define ADDR_MGT_ERR(fmt, arg...) printk(KERN_ERR "[ADDR_MGT] %s: " fmt "\n",\ + __func__, ## arg) + +#define MODULE_CUR_VERSION "v1.0.9" + +#define MATCH_STR_LEN 20 +#define ADDR_VAL_LEN 6 +#define ADDR_STR_LEN 18 +#define ID_LEN 16 +#define HASH_LEN 32 + +#define TYPE_ANY 0 +#define TYPE_BURN 1 +#define TYPE_IDGEN 2 +#define TYPE_USER 3 +#define TYPE_RAND 4 + +#define ADDR_FMT_STR 0 +#define ADDR_FMT_VAL 1 + +#define IS_TYPE_INVALID(x) ((x < TYPE_ANY) || (x > TYPE_RAND)) + +#define ADDR_CLASS_ATTR_ADD(name) \ +static ssize_t addr_##name##_show(const struct class *class, \ + const struct class_attribute *attr, char *buffer) \ +{ \ + char addr[ADDR_STR_LEN]; \ + if (IS_TYPE_INVALID(get_addr_by_name(ADDR_FMT_STR, addr, #name))) \ + return 0; \ + return sprintf(buffer, "%.17s\n", addr); \ +} \ +static ssize_t addr_##name##_store(const struct class *class, \ + const struct class_attribute *attr, \ + const char *buffer, size_t count) \ +{ \ + if (count != ADDR_STR_LEN) { \ + ADDR_MGT_ERR("Length wrong."); \ + return -EINVAL; \ + } \ + set_addr_by_name(TYPE_USER, ADDR_FMT_STR, buffer, #name); \ + return count; \ +} \ +static CLASS_ATTR_RW(addr_##name); + +struct addr_mgt_info { + unsigned int type_def; + unsigned int type_cur; + unsigned int flag; + char *addr; + char *name; +}; + +static struct addr_mgt_info info[] = { + {TYPE_ANY, TYPE_ANY, 1, NULL, "wifi"}, + {TYPE_ANY, TYPE_ANY, 0, NULL, "bt" }, + {TYPE_ANY, TYPE_ANY, 1, NULL, "eth" }, +}; + +extern int hmac_sha256(const uint8_t *plaintext, ssize_t psize, uint8_t *output); +extern int sunxi_get_soc_chipid(unsigned char *chipid); + +static int addr_parse(int fmt, const char *addr, int check) +{ + char val_buf[ADDR_VAL_LEN]; + char cmp_buf[ADDR_VAL_LEN]; + int ret = ADDR_VAL_LEN; + + if (fmt == ADDR_FMT_STR) + ret = sscanf(addr, "%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx", + &val_buf[0], &val_buf[1], &val_buf[2], + &val_buf[3], &val_buf[4], &val_buf[5]); + else + memcpy(val_buf, addr, ADDR_VAL_LEN); + + if (ret != ADDR_VAL_LEN) + return -1; + + if (check && (val_buf[0] & 0x3)) + return -1; + + memset(cmp_buf, 0x00, ADDR_VAL_LEN); + if (memcmp(val_buf, cmp_buf, ADDR_VAL_LEN) == 0) + return -1; + + memset(cmp_buf, 0xFF, ADDR_VAL_LEN); + if (memcmp(val_buf, cmp_buf, ADDR_VAL_LEN) == 0) + return -1; + + return 0; +} + +static struct addr_mgt_info *addr_find_by_name(char *name) +{ + int i = 0; + for (i = 0; i < ARRAY_SIZE(info); i++) { + if (strcmp(info[i].name, name) == 0) + return &info[i]; + } + return NULL; +} + +static int get_addr_by_name(int fmt, char *addr, char *name) +{ + struct addr_mgt_info *t; + + t = addr_find_by_name(name); + if (t == NULL) { + ADDR_MGT_ERR("can't find addr named: %s", name); + return -1; + } + + if (IS_TYPE_INVALID(t->type_cur)) { + ADDR_MGT_ERR("addr type invalid"); + return -1; + } + + if (addr_parse(ADDR_FMT_VAL, t->addr, t->flag)) { + ADDR_MGT_ERR("addr parse fail(%s)", t->addr); + return -1; + } + + if (fmt == ADDR_FMT_STR) + sprintf(addr, "%02X:%02X:%02X:%02X:%02X:%02X", + t->addr[0], t->addr[1], t->addr[2], + t->addr[3], t->addr[4], t->addr[5]); + else + memcpy(addr, t->addr, ADDR_VAL_LEN); + + return t->type_cur; +} + +static int set_addr_by_name(int type, int fmt, const char *addr, char *name) +{ + struct addr_mgt_info *t; + + t = addr_find_by_name(name); + if (t == NULL) { + ADDR_MGT_ERR("can't find addr named: %s", name); + return -1; + } + + if (addr_parse(fmt, addr, t->flag)) { + ADDR_MGT_ERR("addr parse fail(%s)", addr); + return -1; + } + + t->type_cur = type; + if (fmt == ADDR_FMT_STR) + sscanf(addr, "%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx", + &t->addr[0], &t->addr[1], &t->addr[2], + &t->addr[3], &t->addr[4], &t->addr[5]); + else + memcpy(t->addr, addr, ADDR_VAL_LEN); + + return 0; +} + +int get_custom_mac_address(int fmt, char *name, char *addr) +{ + return get_addr_by_name(fmt, addr, name); +} +EXPORT_SYMBOL_GPL(get_custom_mac_address); + +static int addr_factory(struct device_node *np, + int idx, int type, char *mac, char *name) +{ + int ret, i; + char match[MATCH_STR_LEN]; + const char *p; + char id[ID_LEN], hash[HASH_LEN], cmp_buf[ID_LEN]; + struct timespec64 curtime; + + switch (type) { + case TYPE_BURN: + sprintf(match, "addr_%s", name); + ret = of_property_read_string_index(np, match, 0, &p); + if (ret) + return -1; + + ret = sscanf(p, "%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx", + &mac[0], &mac[1], &mac[2], + &mac[3], &mac[4], &mac[5]); + + if (ret != ADDR_VAL_LEN) + return -1; + break; + case TYPE_IDGEN: + if (idx > HASH_LEN / ADDR_VAL_LEN - 1) + return -1; + if (sunxi_get_soc_chipid(id)) + return -1; + memset(cmp_buf, 0x00, ID_LEN); + if (memcmp(id, cmp_buf, ID_LEN) == 0) + return -1; + if (hmac_sha256(id, ID_LEN, hash)) + return -1; + memcpy(mac, &hash[idx * ADDR_VAL_LEN], ADDR_VAL_LEN); + break; + case TYPE_RAND: + for (i = 0; i < ADDR_VAL_LEN; i++) { + ktime_get_real_ts64(&curtime); + mac[i] = (char)curtime.tv_nsec; + } + break; + default: + ADDR_MGT_ERR("unsupport type: %d", type); + return -1; + } + return 0; +} + +static int addr_init(struct platform_device *pdev) +{ + struct device_node *np = pdev->dev.of_node; + int type, i, j; + char match[MATCH_STR_LEN]; + char addr[ADDR_VAL_LEN]; + int type_tab[] = {TYPE_BURN, TYPE_IDGEN, TYPE_RAND}; + + /* init addr type and value */ + for (i = 0; i < ARRAY_SIZE(info); i++) { + sprintf(match, "type_addr_%s", info[i].name); + if (of_property_read_u32(np, match, &type)) { + ADDR_MGT_DBG("Failed to get type_def_%s, use default: %d", + info[i].name, info[i].type_def); + } else { + info[i].type_def = type; + info[i].type_cur = type; + } + + if (IS_TYPE_INVALID(info[i].type_def)) + return -1; + if (info[i].type_def != TYPE_ANY) { + if (addr_factory(np, i, info[i].type_def, addr, info[i].name)) + return -1; + } else { + for (j = 0; j < ARRAY_SIZE(type_tab); j++) { + if (!addr_factory(np, i, type_tab[j], addr, info[i].name)) { + info[i].type_cur = type_tab[j]; + break; + } + } + } + + if (info[i].flag) + addr[0] &= 0xFC; + + if (addr_parse(ADDR_FMT_VAL, addr, info[i].flag)) + return -1; + else { + info[i].addr = devm_kzalloc(&pdev->dev, ADDR_VAL_LEN, GFP_KERNEL); + memcpy(info[i].addr, addr, ADDR_VAL_LEN); + } + } + return 0; +} + +static ssize_t summary_show(const struct class *class, + const struct class_attribute *attr, char *buffer) +{ + int i = 0, ret = 0; + + ret += sprintf(&buffer[ret], "name cfg cur address\n"); + for (i = 0; i < ARRAY_SIZE(info); i++) { + ret += sprintf(&buffer[ret], + "%4s %d %d %02X:%02X:%02X:%02X:%02X:%02X\n", + info[i].name, info[i].type_def, info[i].type_cur, + info[i].addr[0], info[i].addr[1], info[i].addr[2], + info[i].addr[3], info[i].addr[4], info[i].addr[5]); + } + return ret; +} +static CLASS_ATTR_RO(summary); + +ADDR_CLASS_ATTR_ADD(wifi); +ADDR_CLASS_ATTR_ADD(bt); +ADDR_CLASS_ATTR_ADD(eth); + +static struct attribute *addr_class_attrs[] = { + &class_attr_summary.attr, + &class_attr_addr_wifi.attr, + &class_attr_addr_bt.attr, + &class_attr_addr_eth.attr, + NULL +}; +ATTRIBUTE_GROUPS(addr_class); + +static struct class addr_class = { + .name = "addr_mgt", + .class_groups = addr_class_groups, +}; + +static const struct of_device_id addr_mgt_ids[] = { + { .compatible = "allwinner,sunxi-addr_mgt" }, + { /* Sentinel */ } +}; + +static int addr_mgt_probe(struct platform_device *pdev) +{ + int status; + + ADDR_MGT_DBG("module version: %s", MODULE_CUR_VERSION); + status = class_register(&addr_class); + if (status < 0) { + ADDR_MGT_ERR("class register error, status: %d.", status); + return -1; + } + + if (addr_init(pdev)) { + ADDR_MGT_ERR("failed to init addr."); + class_unregister(&addr_class); + return -1; + } + ADDR_MGT_DBG("success."); + return 0; +} + +static int addr_mgt_remove(struct platform_device *pdev) +{ + class_unregister(&addr_class); + return 0; +} + +static struct platform_driver addr_mgt_driver = { + .probe = addr_mgt_probe, + .remove = addr_mgt_remove, + .driver = { + .owner = THIS_MODULE, + .name = "sunxi-addr-mgt", + .of_match_table = addr_mgt_ids, + }, +}; + +module_platform_driver_probe(addr_mgt_driver, addr_mgt_probe); + +MODULE_AUTHOR("Allwinnertech"); +MODULE_DESCRIPTION("Network MAC Addess Manager"); +MODULE_LICENSE("GPL"); -- Armbian