| 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 <[email protected]> |
| 7 |
* Based on the nvmem driver written by: Vadym Kochan <[email protected]> |
| 8 |
* Inspired by the first layout written by: Rafał Miłecki <[email protected]> |
| 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 |
|
| 22 |
struct onie_tlv_hdr { |
| 23 |
u8 id[8]; |
| 24 |
u8 version; |
| 25 |
__be16 data_len; |
| 26 |
} __packed; |
| 27 |
|
| 28 |
struct onie_tlv { |
| 29 |
u8 type; |
| 30 |
u8 len; |
| 31 |
} __packed; |
| 32 |
|
| 33 |
static 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 |
|
| 77 |
static 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 |
|
| 86 |
static 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 |
|
| 98 |
static 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 |
|
| 143 |
static 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 |
|
| 158 |
static 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 |
|
| 185 |
static 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 |
|
| 230 |
static 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 |
|
| 237 |
static void onie_tlv_remove(struct nvmem_layout *layout) |
| 238 |
{ |
| 239 |
nvmem_layout_unregister(layout); |
| 240 |
} |
| 241 |
|
| 242 |
static const struct of_device_id onie_tlv_of_match_table[] = { |
| 243 |
{ .compatible = "onie,tlv-layout", }, |
| 244 |
{}, |
| 245 |
}; |
| 246 |
MODULE_DEVICE_TABLE(of, onie_tlv_of_match_table); |
| 247 |
|
| 248 |
static 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 |
}; |
| 256 |
module_nvmem_layout_driver(onie_tlv_layout); |
| 257 |
|
| 258 |
MODULE_LICENSE("GPL"); |
| 259 |
MODULE_AUTHOR("Miquel Raynal <[email protected]>"); |
| 260 |
MODULE_DESCRIPTION("NVMEM layout driver for Onie TLV table parsing"); |