drivers/nvmem/layouts/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
  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                 .owner = THIS_MODULE,
 251                 .name = "onie-tlv-layout",
 252                 .of_match_table = onie_tlv_of_match_table,
 253         },
 254         .probe = onie_tlv_probe,
 255         .remove = onie_tlv_remove,
 256 };
 257 module_nvmem_layout_driver(onie_tlv_layout);
 258
 259 MODULE_LICENSE("GPL");
 260 MODULE_AUTHOR("Miquel Raynal <miquel.raynal@bootlin.com>");
 261 MODULE_DESCRIPTION("NVMEM layout driver for Onie TLV table parsing");