onie-tlv.c

  1// SPDX-License-Identifier: GPL-2.0-only
  2/*
  3 * ONIE tlv NVMEM cells provider
  4 *
  5 * Copyright (C) 2022 Open Compute Group ONIE
  6 * Author: Miquel Raynal <miquel.raynal@bootlin.com>
  7 * Based on the nvmem driver written by: Vadym Kochan <vadym.kochan@plvision.eu>
  8 * Inspired by the first layout written by: Rafał Miłecki <rafal@milecki.pl>
  9 */
 10
 11#include <linux/crc32.h>
 12#include <linux/etherdevice.h>
 13#include <linux/nvmem-consumer.h>
 14#include <linux/nvmem-provider.h>
 15#include <linux/of.h>
 16
 17#define ONIE_TLV_MAX_LEN 2048
 18#define ONIE_TLV_CRC_FIELD_SZ 6
 19#define ONIE_TLV_CRC_SZ 4
 20#define ONIE_TLV_HDR_ID	"TlvInfo"
 21
 22struct onie_tlv_hdr {
 23	u8 id[8];
 24	u8 version;
 25	__be16 data_len;
 26} __packed;
 27
 28struct onie_tlv {
 29	u8 type;
 30	u8 len;
 31} __packed;
 32
 33static const char *onie_tlv_cell_name(u8 type)
 34{
 35	switch (type) {
 36	case 0x21:
 37		return "product-name";
 38	case 0x22:
 39		return "part-number";
 40	case 0x23:
 41		return "serial-number";
 42	case 0x24:
 43		return "mac-address";
 44	case 0x25:
 45		return "manufacture-date";
 46	case 0x26:
 47		return "device-version";
 48	case 0x27:
 49		return "label-revision";
 50	case 0x28:
 51		return "platform-name";
 52	case 0x29:
 53		return "onie-version";
 54	case 0x2A:
 55		return "num-macs";
 56	case 0x2B:
 57		return "manufacturer";
 58	case 0x2C:
 59		return "country-code";
 60	case 0x2D:
 61		return "vendor";
 62	case 0x2E:
 63		return "diag-version";
 64	case 0x2F:
 65		return "service-tag";
 66	case 0xFD:
 67		return "vendor-extension";
 68	case 0xFE:
 69		return "crc32";
 70	default:
 71		break;
 72	}
 73
 74	return NULL;
 75}
 76
 77static int onie_tlv_mac_read_cb(void *priv, const char *id, int index,
 78				unsigned int offset, void *buf,
 79				size_t bytes)
 80{
 81	eth_addr_add(buf, index);
 82
 83	return 0;
 84}
 85
 86static nvmem_cell_post_process_t onie_tlv_read_cb(u8 type, u8 *buf)
 87{
 88	switch (type) {
 89	case 0x24:
 90		return &onie_tlv_mac_read_cb;
 91	default:
 92		break;
 93	}
 94
 95	return NULL;
 96}
 97
 98static int onie_tlv_add_cells(struct device *dev, struct nvmem_device *nvmem,
 99			      size_t data_len, u8 *data)
100{
101	struct nvmem_cell_info cell = {};
102	struct device_node *layout;
103	struct onie_tlv tlv;
104	unsigned int hdr_len = sizeof(struct onie_tlv_hdr);
105	unsigned int offset = 0;
106	int ret;
107
108	layout = of_nvmem_layout_get_container(nvmem);
109	if (!layout)
110		return -ENOENT;
111
112	while (offset < data_len) {
113		memcpy(&tlv, data + offset, sizeof(tlv));
114		if (offset + tlv.len >= data_len) {
115			dev_err(dev, "Out of bounds field (0x%x bytes at 0x%x)\n",
116				tlv.len, hdr_len + offset);
117			break;
118		}
119
120		cell.name = onie_tlv_cell_name(tlv.type);
121		if (!cell.name)
122			continue;
123
124		cell.offset = hdr_len + offset + sizeof(tlv.type) + sizeof(tlv.len);
125		cell.bytes = tlv.len;
126		cell.np = of_get_child_by_name(layout, cell.name);
127		cell.read_post_process = onie_tlv_read_cb(tlv.type, data + offset + sizeof(tlv));
128
129		ret = nvmem_add_one_cell(nvmem, &cell);
130		if (ret) {
131			of_node_put(layout);
132			return ret;
133		}
134
135		offset += sizeof(tlv) + tlv.len;
136	}
137
138	of_node_put(layout);
139
140	return 0;
141}
142
143static bool onie_tlv_hdr_is_valid(struct device *dev, struct onie_tlv_hdr *hdr)
144{
145	if (memcmp(hdr->id, ONIE_TLV_HDR_ID, sizeof(hdr->id))) {
146		dev_err(dev, "Invalid header\n");
147		return false;
148	}
149
150	if (hdr->version != 0x1) {
151		dev_err(dev, "Invalid version number\n");
152		return false;
153	}
154
155	return true;
156}
157
158static bool onie_tlv_crc_is_valid(struct device *dev, size_t table_len, u8 *table)
159{
160	struct onie_tlv crc_hdr;
161	u32 read_crc, calc_crc;
162	__be32 crc_be;
163
164	memcpy(&crc_hdr, table + table_len - ONIE_TLV_CRC_FIELD_SZ, sizeof(crc_hdr));
165	if (crc_hdr.type != 0xfe || crc_hdr.len != ONIE_TLV_CRC_SZ) {
166		dev_err(dev, "Invalid CRC field\n");
167		return false;
168	}
169
170	/* The table contains a JAMCRC, which is XOR'ed compared to the original
171	 * CRC32 implementation as known in the Ethernet world.
172	 */
173	memcpy(&crc_be, table + table_len - ONIE_TLV_CRC_SZ, ONIE_TLV_CRC_SZ);
174	read_crc = be32_to_cpu(crc_be);
175	calc_crc = crc32(~0, table, table_len - ONIE_TLV_CRC_SZ) ^ 0xFFFFFFFF;
176	if (read_crc != calc_crc) {
177		dev_err(dev, "Invalid CRC read: 0x%08x, expected: 0x%08x\n",
178			read_crc, calc_crc);
179		return false;
180	}
181
182	return true;
183}
184
185static int onie_tlv_parse_table(struct nvmem_layout *layout)
186{
187	struct nvmem_device *nvmem = layout->nvmem;
188	struct device *dev = &layout->dev;
189	struct onie_tlv_hdr hdr;
190	size_t table_len, data_len, hdr_len;
191	u8 *table, *data;
192	int ret;
193
194	ret = nvmem_device_read(nvmem, 0, sizeof(hdr), &hdr);
195	if (ret < 0)
196		return ret;
197
198	if (!onie_tlv_hdr_is_valid(dev, &hdr)) {
199		dev_err(dev, "Invalid ONIE TLV header\n");
200		return -EINVAL;
201	}
202
203	hdr_len = sizeof(hdr.id) + sizeof(hdr.version) + sizeof(hdr.data_len);
204	data_len = be16_to_cpu(hdr.data_len);
205	table_len = hdr_len + data_len;
206	if (table_len > ONIE_TLV_MAX_LEN) {
207		dev_err(dev, "Invalid ONIE TLV data length\n");
208		return -EINVAL;
209	}
210
211	table = devm_kmalloc(dev, table_len, GFP_KERNEL);
212	if (!table)
213		return -ENOMEM;
214
215	ret = nvmem_device_read(nvmem, 0, table_len, table);
216	if (ret != table_len)
217		return ret;
218
219	if (!onie_tlv_crc_is_valid(dev, table_len, table))
220		return -EINVAL;
221
222	data = table + hdr_len;
223	ret = onie_tlv_add_cells(dev, nvmem, data_len, data);
224	if (ret)
225		return ret;
226
227	return 0;
228}
229
230static int onie_tlv_probe(struct nvmem_layout *layout)
231{
232	layout->add_cells = onie_tlv_parse_table;
233
234	return nvmem_layout_register(layout);
235}
236
237static void onie_tlv_remove(struct nvmem_layout *layout)
238{
239	nvmem_layout_unregister(layout);
240}
241
242static const struct of_device_id onie_tlv_of_match_table[] = {
243	{ .compatible = "onie,tlv-layout", },
244	{},
245};
246MODULE_DEVICE_TABLE(of, onie_tlv_of_match_table);
247
248static struct nvmem_layout_driver onie_tlv_layout = {
249	.driver = {
250		.name = "onie-tlv-layout",
251		.of_match_table = onie_tlv_of_match_table,
252	},
253	.probe = onie_tlv_probe,
254	.remove = onie_tlv_remove,
255};
256module_nvmem_layout_driver(onie_tlv_layout);
257
258MODULE_LICENSE("GPL");
259MODULE_AUTHOR("Miquel Raynal <miquel.raynal@bootlin.com>");
260MODULE_DESCRIPTION("NVMEM layout driver for Onie TLV table parsing");