[PATCH 1/2] nvmem: add ONIE nvmem provider

[Vadym Kochan]

ONIE is a small operating system, pre-installed on bare metal network
switches, that provides an environment for automated provisioning.

This system requires that NVMEM (EEPROM) device holds various system
information (mac address, platform name, etc) in a special TLV layout.

The driver parses ONIE TLV attributes and registers them as NVMEM cells
which can be accessed by other platform driver. Also it allows to use
of_get_mac_address() to retrieve mac address for the netdev.

The provider differs from the normal NVMEM providers that it is not
depends on particular storage but uses other nvmem as accessor for
reading the cell, and proxies the reg_read() call to this device driver.

Signed-off-by: Vadym Kochan <vadym.kochan@xxxxxxxxxxx>
---
 drivers/nvmem/Kconfig  |   9 ++
 drivers/nvmem/Makefile |   2 +
 drivers/nvmem/onie.c   | 357 +++++++++++++++++++++++++++++++++++++++++
 3 files changed, 368 insertions(+)
 create mode 100644 drivers/nvmem/onie.c

diff --git a/drivers/nvmem/Kconfig b/drivers/nvmem/Kconfig
index 954d3b4a52ab..77387bd13105 100644
--- a/drivers/nvmem/Kconfig
+++ b/drivers/nvmem/Kconfig
@@ -270,4 +270,13 @@ config SPRD_EFUSE
 	  This driver can also be built as a module. If so, the module
 	  will be called nvmem-sprd-efuse.
 
+config NVMEM_ONIE
+	tristate "ONIE NVMEM support"
+	help
+	  This is a driver to provide cells from ONIE TLV structure stored
+	  on NVMEM device.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called nvmem-onie.
+
 endif
diff --git a/drivers/nvmem/Makefile b/drivers/nvmem/Makefile
index a7c377218341..fbf30c94447e 100644
--- a/drivers/nvmem/Makefile
+++ b/drivers/nvmem/Makefile
@@ -55,3 +55,5 @@ obj-$(CONFIG_NVMEM_ZYNQMP)	+= nvmem_zynqmp_nvmem.o
 nvmem_zynqmp_nvmem-y		:= zynqmp_nvmem.o
 obj-$(CONFIG_SPRD_EFUSE)	+= nvmem_sprd_efuse.o
 nvmem_sprd_efuse-y		:= sprd-efuse.o
+obj-$(CONFIG_NVMEM_ONIE)	+= nvmem-onie.o
+nvmem-onie-y			:= onie.o
diff --git a/drivers/nvmem/onie.c b/drivers/nvmem/onie.c
new file mode 100644
index 000000000000..5f280f227b44
--- /dev/null
+++ b/drivers/nvmem/onie.c
@@ -0,0 +1,357 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * ONIE NVMEM provider
+ *
+ * Author: Vadym Kochan <vadym.kochan@xxxxxxxxxxx>
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/nvmem-consumer.h>
+#include <linux/nvmem-provider.h>
+#include <linux/of_device.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define ONIE_NVMEM_TLV_MAX_LEN	2048
+
+#define ONIE_NVMEM_HDR_ID	"TlvInfo"
+
+struct onie_nvmem_hdr {
+	u8 id[8];
+	u8 version;
+	__be16 data_len;
+} __packed;
+
+struct onie_nvmem_tlv {
+	u8 type;
+	u8 len;
+	u8 val[0];
+} __packed;
+
+struct onie_nvmem_attr {
+	struct list_head head;
+	const char *name;
+	unsigned int offset;
+	unsigned int len;
+};
+
+struct onie_nvmem {
+	struct nvmem_cell_lookup *lookup;
+	struct nvmem_cell_info *cells;
+	int ncells;
+	struct nvmem_device *nvmem;
+	unsigned int attr_count;
+	struct list_head attrs;
+	struct device *dev;
+	u8 version;
+};
+
+static bool onie_nvmem_hdr_is_valid(struct onie_nvmem_hdr *hdr)
+{
+	if (memcmp(hdr->id, ONIE_NVMEM_HDR_ID, sizeof(hdr->id)) != 0)
+		return false;
+	if (hdr->version != 0x1)
+		return false;
+
+	return true;
+}
+
+static void onie_nvmem_attrs_free(struct onie_nvmem *onie)
+{
+	struct onie_nvmem_attr *attr, *tmp;
+
+	list_for_each_entry_safe(attr, tmp, &onie->attrs, head) {
+		list_del(&attr->head);
+		kfree(attr);
+	}
+}
+
+static const char *onie_nvmem_attr_name(u8 type)
+{
+	switch (type) {
+	case 0x21: return "product-name";
+	case 0x22: return "part-number";
+	case 0x23: return "serial-number";
+	case 0x24: return "mac-address";
+	case 0x25: return "manufacture-date";
+	case 0x26: return "device-version";
+	case 0x27: return "label-revision";
+	case 0x28: return "platforn-name";
+	case 0x29: return "onie-version";
+	case 0x2A: return "num-macs";
+	case 0x2B: return "manufacturer";
+	case 0x2C: return "country-code";
+	case 0x2D: return "vendor";
+	case 0x2E: return "diag-version";
+	case 0x2F: return "service-tag";
+	case 0xFD: return "vendor-extension";
+	case 0xFE: return "crc32";
+
+	default: return NULL;
+	}
+}
+
+static int onie_nvmem_tlv_parse(struct onie_nvmem *onie, u8 *data, u16 len)
+{
+	unsigned int hlen = sizeof(struct onie_nvmem_hdr);
+	unsigned int offset = 0;
+	int err;
+
+	onie->attr_count = 0;
+
+	while (offset < len) {
+		struct onie_nvmem_attr *attr;
+		struct onie_nvmem_tlv *tlv;
+		const char *name;
+
+		tlv = (struct onie_nvmem_tlv *)(data + offset);
+
+		if (offset + tlv->len >= len) {
+			dev_err(onie->dev, "TLV len is too big(0x%x) at 0x%x\n",
+				tlv->len, hlen + offset);
+
+			/* return success in case something was parsed */
+			return 0;
+		}
+
+		name = onie_nvmem_attr_name(tlv->type);
+		if (!name)
+			goto onie_next_tlv;
+
+		attr = kmalloc(sizeof(*attr), GFP_KERNEL);
+		if (!attr) {
+			err = -ENOMEM;
+			goto err_attr_alloc;
+		}
+
+		/* skip 'type' and 'len' */
+		attr->offset = hlen + offset + 2;
+		attr->len = tlv->len;
+		attr->name = name;
+
+		list_add(&attr->head, &onie->attrs);
+		onie->attr_count++;
+
+onie_next_tlv:
+		offset += sizeof(*tlv) + tlv->len;
+	}
+
+	if (!onie->attr_count)
+		return -EINVAL;
+
+	return 0;
+
+err_attr_alloc:
+	onie_nvmem_attrs_free(onie);
+	return err;
+}
+
+static int
+onie_nvmem_decode(struct onie_nvmem *onie, struct nvmem_device *nvmem)
+{
+	struct onie_nvmem_hdr hdr;
+	u8 *data;
+	u16 len;
+	int ret;
+
+	ret = nvmem_device_read(nvmem, 0, sizeof(hdr), &hdr);
+	if (ret < 0)
+		return ret;
+
+	if (!onie_nvmem_hdr_is_valid(&hdr)) {
+		dev_err(onie->dev, "invalid ONIE TLV header\n");
+		return -EINVAL;
+	}
+
+	onie->version = hdr.version;
+
+	len = be16_to_cpu(hdr.data_len);
+
+	if (len > ONIE_NVMEM_TLV_MAX_LEN)
+		len = ONIE_NVMEM_TLV_MAX_LEN;
+
+	data = kmalloc(len, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	ret = nvmem_device_read(nvmem, sizeof(hdr), len, data);
+	if (ret < 0)
+		goto err_data_read;
+
+	ret = onie_nvmem_tlv_parse(onie, data, len);
+	if (ret)
+		goto err_info_parse;
+
+	kfree(data);
+
+	return 0;
+
+err_info_parse:
+err_data_read:
+	kfree(data);
+	return ret;
+}
+
+static int onie_nvmem_cells_parse(struct onie_nvmem *onie,
+				  struct nvmem_device *nvmem)
+{
+	struct nvmem_cell_info *cells;
+	struct onie_nvmem_attr *attr;
+	unsigned int ncells = 0;
+	int err;
+
+	INIT_LIST_HEAD(&onie->attrs);
+	onie->attr_count = 0;
+
+	err = onie_nvmem_decode(onie, nvmem);
+	if (err)
+		return err;
+
+	cells = kmalloc_array(onie->attr_count, sizeof(*cells), GFP_KERNEL);
+	if (!cells) {
+		err = -ENOMEM;
+		goto err_cells_alloc;
+	}
+
+	onie->lookup = kmalloc_array(onie->attr_count,
+				     sizeof(struct nvmem_cell_lookup),
+				     GFP_KERNEL);
+	if (!onie->lookup) {
+		err = -ENOMEM;
+		goto err_lookup_alloc;
+	}
+
+	list_for_each_entry(attr, &onie->attrs, head) {
+		struct nvmem_cell_lookup *lookup;
+		struct nvmem_cell_info *cell;
+
+		cell = &cells[ncells];
+
+		cell->offset = attr->offset;
+		cell->name = attr->name;
+		cell->bytes = attr->len;
+		cell->bit_offset = 0;
+		cell->nbits = 0;
+
+		lookup = &onie->lookup[ncells];
+
+		lookup->nvmem_name = "onie0";
+		lookup->dev_id = dev_name(onie->dev);
+		lookup->cell_name = cell->name;
+		lookup->con_id = cell->name;
+
+		ncells++;
+	}
+
+	onie->ncells = ncells;
+	onie->cells = cells;
+
+	onie_nvmem_attrs_free(onie);
+
+	return 0;
+
+err_lookup_alloc:
+	kfree(cells);
+err_cells_alloc:
+	onie_nvmem_attrs_free(onie);
+
+	return err;
+}
+
+static int onie_nvmem_read(void *priv, unsigned int off, void *val, size_t count)
+{
+	struct onie_nvmem *onie = priv;
+	int rc;
+
+	rc = nvmem_device_read(onie->nvmem, off, count, val);
+	if (rc != count)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int onie_nvmem_probe(struct platform_device *pdev)
+{
+	struct nvmem_config nvmem_config = { };
+	struct device *dev = &pdev->dev;
+	struct onie_nvmem *onie;
+	int err;
+
+	onie = devm_kzalloc(dev, sizeof(*onie), GFP_KERNEL);
+	if (!onie)
+		return -ENOMEM;
+
+	onie->nvmem = of_nvmem_device_get(dev_of_node(dev), NULL);
+	if (IS_ERR(onie->nvmem))
+		return PTR_ERR(onie->nvmem);
+
+	onie->dev = dev;
+
+	dev_set_drvdata(dev, onie);
+
+	err = onie_nvmem_cells_parse(onie, onie->nvmem);
+	if (err) {
+		dev_err(onie->dev, "failed to parse ONIE attributes\n");
+		goto err_nvmem_put;
+	}
+
+	nvmem_add_cell_lookups(onie->lookup, onie->ncells);
+
+	nvmem_config.id = 0;
+	nvmem_config.name = "onie";
+	nvmem_config.base_dev = dev;
+	nvmem_config.dev = dev;
+	nvmem_config.owner = THIS_MODULE;
+	nvmem_config.reg_read = onie_nvmem_read;
+	nvmem_config.priv = onie;
+	nvmem_config.ncells = onie->ncells;
+	nvmem_config.cells = onie->cells;
+
+	err = PTR_ERR_OR_ZERO(devm_nvmem_register(dev, &nvmem_config));
+
+err_nvmem_put:
+	if (err)
+		nvmem_device_put(onie->nvmem);
+	else
+		dev_info(dev, "ONIE TLV v%u\n", onie->version);
+
+	return err;
+}
+
+static int onie_nvmem_remove(struct platform_device *pdev)
+{
+	struct onie_nvmem *onie = dev_get_drvdata(&pdev->dev);
+
+	nvmem_del_cell_lookups(onie->lookup, onie->attr_count);
+	nvmem_device_put(onie->nvmem);
+	kfree(onie->lookup);
+	kfree(onie->cells);
+	kfree(onie);
+
+	return 0;
+}
+
+static const struct of_device_id onie_nvmem_match[] = {
+	{
+		.compatible = "onie-nvmem",
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, onie_nvmem_match);
+
+static struct platform_driver onie_nvmem_driver = {
+	.probe = onie_nvmem_probe,
+	.remove = onie_nvmem_remove,
+	.driver = {
+		.name = "onie-nvmem",
+		.of_match_table = onie_nvmem_match,
+	},
+};
+module_platform_driver(onie_nvmem_driver);
+
+MODULE_AUTHOR("Vadym Kochan <vadym.kochan@xxxxxxxxxxx>");
+MODULE_DESCRIPTION("ONIE NVMEM driver");
+MODULE_LICENSE("GPL");
-- 
2.17.1