[RFC v2 1/3] lib: add tlv_eeprom library
Josua Mayer
josua at solid-run.com
Tue May 16 13:41:51 CEST 2023
Create a new tlv library by rewriting parts, and reusing other parts of
the tlv_eeprom command. This library is intended to simplify reading tlv
data during system initialisation from board files, as well as increase
maintainability by defining a clear API and functionally decouple from
the command implementation.
Signed-off-by: Josua Mayer <josua at solid-run.com>
Cc: Stefan Roese <sr at denx.de>
Cc: Baruch Siach <baruch at tkos.co.il>
Cc: Heinrich Schuchardt <xypron.glpk at gmx.de>
---
cmd/Kconfig | 2 +
include/tlv_eeprom.h | 272 ++++++++++++++++++++
lib/Kconfig | 2 +
lib/Makefile | 2 +
lib/tlv/Kconfig | 18 ++
lib/tlv/Makefile | 4 +
lib/tlv/tlv_eeprom.c | 599 +++++++++++++++++++++++++++++++++++++++++++
7 files changed, 899 insertions(+)
create mode 100644 lib/tlv/Kconfig
create mode 100644 lib/tlv/Makefile
create mode 100644 lib/tlv/tlv_eeprom.c
diff --git a/cmd/Kconfig b/cmd/Kconfig
index 365371fb511..9c2149dc881 100644
--- a/cmd/Kconfig
+++ b/cmd/Kconfig
@@ -191,6 +191,7 @@ config CMD_REGINFO
config CMD_TLV_EEPROM
bool "tlv_eeprom"
depends on I2C_EEPROM
+ depends on !EEPROM_TLV_LIB
select CRC32
help
Display and program the system EEPROM data block in ONIE Tlvinfo
@@ -199,6 +200,7 @@ config CMD_TLV_EEPROM
config SPL_CMD_TLV_EEPROM
bool "tlv_eeprom for SPL"
depends on SPL_I2C_EEPROM
+ depends on !SPL_EEPROM_TLV_LIB
select SPL_DRIVERS_MISC
select SPL_CRC32
help
diff --git a/include/tlv_eeprom.h b/include/tlv_eeprom.h
index fd45e5f6ebb..b08c98a5833 100644
--- a/include/tlv_eeprom.h
+++ b/include/tlv_eeprom.h
@@ -7,6 +7,8 @@
#ifndef __TLV_EEPROM_H_
#define __TLV_EEPROM_H_
+#if !defined(CONFIG_EEPROM_TLV_LIB) && !defined(CONFIG_SPL_EEPROM_TLV_LIB)
+
/*
* The Definition of the TlvInfo EEPROM format can be found at onie.org or
* github.com/onie
@@ -150,4 +152,274 @@ static inline bool is_valid_tlvinfo_header(struct tlvinfo_header *hdr)
(be16_to_cpu(hdr->totallen) <= TLV_TOTAL_LEN_MAX));
}
+#else
+
+/*
+ * The Definition of the TlvInfo EEPROM format can be found at onie.org or
+ * github.com/onie
+ */
+
+#include <dm/device.h>
+#include <i2c_eeprom.h>
+#include <stdbool.h>
+
+/* tlv library internal state, per each tlv structure */
+struct tlvinfo_priv;
+
+/*
+ * TlvInfo header: Layout of the header for the TlvInfo format
+ *
+ * See the end of this file for details of this eeprom format
+ */
+struct __attribute__ ((__packed__)) tlvinfo_header {
+ char signature[8]; /* 0x00 - 0x07 EEPROM Tag "TlvInfo" */
+ u8 version; /* 0x08 Structure version */
+ u16 totallen; /* 0x09 - 0x0A Length of all data which follows */
+};
+
+// Header Field Constants
+#define TLV_INFO_HEADER_SIZE sizeof(struct tlvinfo_header)
+#define TLV_INFO_ID_STRING "TlvInfo"
+#define TLV_INFO_VERSION 0x01
+#define TLV_INFO_MAX_LEN 2048
+#define TLV_TOTAL_LEN_MAX (TLV_INFO_MAX_LEN - TLV_INFO_HEADER_SIZE)
+
+/*
+ * TlvInfo TLV: Layout of a TLV field
+ */
+struct __attribute__ ((__packed__)) tlvinfo_tlv {
+ u8 type;
+ u8 length;
+ u8 value[];
+};
+
+#define TLV_INFO_ENTRY_SIZE sizeof(struct tlvinfo_tlv)
+/* Maximum length of a TLV value in bytes */
+#define TLV_VALUE_MAX_LEN 255
+
+/**
+ * The TLV Types.
+ *
+ * Keep these in sync with tlv_code_list in cmd/tlv_eeprom.c
+ */
+#define TLV_CODE_PRODUCT_NAME 0x21
+#define TLV_CODE_PART_NUMBER 0x22
+#define TLV_CODE_SERIAL_NUMBER 0x23
+#define TLV_CODE_MAC_BASE 0x24
+#define TLV_CODE_MANUF_DATE 0x25
+#define TLV_CODE_DEVICE_VERSION 0x26
+#define TLV_CODE_LABEL_REVISION 0x27
+#define TLV_CODE_PLATFORM_NAME 0x28
+#define TLV_CODE_ONIE_VERSION 0x29
+#define TLV_CODE_MAC_SIZE 0x2A
+#define TLV_CODE_MANUF_NAME 0x2B
+#define TLV_CODE_MANUF_COUNTRY 0x2C
+#define TLV_CODE_VENDOR_NAME 0x2D
+#define TLV_CODE_DIAG_VERSION 0x2E
+#define TLV_CODE_SERVICE_TAG 0x2F
+#define TLV_CODE_VENDOR_EXT 0xFD
+#define TLV_CODE_CRC_32 0xFE
+
+/* how many EEPROMs can be used */
+#define MAX_TLV_DEVICES 2
+
+/*
+ * EEPROM<->TLV API
+ */
+
+/**
+ * Find EEPROM device by index.
+ *
+ * @index: index of eeprom in the system, 0 = first.
+ * @return: handle to eeprom device on success, error pointer otherwise.
+ */
+struct udevice *tlv_eeprom_get_by_index(unsigned int index);
+
+/**
+ * Read TLV formatted data from EEPROM.
+ *
+ * @dev: EEPROM device handle.
+ * @offset: Offset into EEPROM to read from.
+ * @buffer: Buffer for storing TLV structure.
+ * @buffer_size: Size of the buffer.
+ * @return: Buffer initialised with TLV structure, or error pointer.
+ */
+struct tlvinfo_priv *tlv_eeprom_read(struct udevice *dev, int offset, u8 *buffer, size_t buffer_size);
+
+/**
+ * Write TLV formatted data to EEPROM.
+ *
+ * @dev: EEPROM device handle.
+ * @offset: Offset into EEPROM to write to.
+ * @tlv: Pointer to TLV structure.
+ * @return: Status code.
+ */
+int tlv_eeprom_write(struct udevice *dev, int offset, struct tlvinfo_priv *priv);
+
+/*
+ * TLV API
+ */
+
+/**
+ * Initialise new TLV structure.
+ *
+ * @buffer: Buffer for storing TLV structure.
+ * @buffer_size: Size of the buffer.
+ * @return: Buffer initialised with TLV structure, or error pointer.
+ */
+struct tlvinfo_priv *tlv_init(u8 *buffer, size_t buffer_size);
+
+/**
+ * Access TLV Header
+ */
+struct tlvinfo_header *tlv_header_get(struct tlvinfo_priv *priv);
+
+/**
+ * Add new entry to TLV structure.
+ *
+ * @priv: Pointer to TLV structure.
+ * @offset: Pointer inside TLV structure where to insert new element. May be NULL to insert at the end, otherwise the new entry shall be created before the specified offset, moving existing elements back.
+ * @code: TLV code number for this entry.
+ * @size: Data size for this entry.
+ * @return: Pointer to TLV entry, or error pointer.
+ */
+struct tlvinfo_tlv *tlv_entry_add(struct tlvinfo_priv *priv, struct tlvinfo_tlv *offset, u8 code, u8 size);
+
+/**
+ * Remove entry from TLV structure.
+ *
+ * @tlv: Pointer to TLV structure.
+ * @entry: Pointer to TLV entry.
+ * @return: Status code.
+ */
+int tlv_entry_remove(struct tlvinfo_priv *priv, struct tlvinfo_tlv *entry);
+
+/**
+ * Get the next TLV entry.
+ *
+ * @tlv: Pointer to TLV structure.
+ * @offset: Start search after this entry; Pass NULL to search from the beginning.
+ * @return: Pointer to TLV entry, or error code.
+ */
+struct tlvinfo_tlv *tlv_entry_next(struct tlvinfo_priv *priv, struct tlvinfo_tlv *offset);
+
+/**
+ * Get the next TLV entry by code.
+ *
+ * @tlv: Pointer to TLV structure.
+ * @code: TLV code number to search.
+ * @offset: Start search after this entry; Pass NULL to search from the beginning.
+ * @return: Pointer to TLV entry, or error code.
+ */
+struct tlvinfo_tlv *tlv_entry_next_by_code(struct tlvinfo_priv *priv, struct tlvinfo_tlv *offset, u8 code);
+
+/*
+ * TLV data get/set API
+ * (Convenience wrappers around struct tlvinfo_tlv)
+ */
+
+/**
+ * Get TLV entry binary data.
+ *
+ * @entry: Pointer to TLV entry.
+ * @buffer: Destination buffer for data.
+ * @size: Size of the buffer.
+ * @return: Status code.
+ */
+int tlv_entry_get_raw(struct tlvinfo_tlv *entry, u8 *buffer, size_t size);
+
+/**
+ * Set TLV entry binary data.
+ *
+ * @entry: Pointer to TLV entry.
+ * @values: Source buffer with data.
+ * @size: Length of the data.
+ * @return: Status code.
+ *
+ * Note: The size of entries can not be changed!
+ */
+int tlv_entry_set_raw(struct tlvinfo_tlv *entry, const u8 *values, size_t length);
+
+/**
+ * Get TLV entry data as null-terminated string.
+ *
+ * @entry: Pointer to TLV entry.
+ * @buffer: Destination buffer for string.
+ * @size: Size of the buffer.
+ * @return: Status code.
+ */
+int tlv_entry_get_string(struct tlvinfo_tlv *entry, char *buffer, size_t size);
+
+/**
+ * Set TLV entry data from null-terminated string.
+ *
+ * @entry: Pointer to TLV entry.
+ * @values: Source buffer with string.
+ * @return: Status code.
+ *
+ * Note: The size of entries can not be changed!
+ */
+int tlv_entry_set_string(struct tlvinfo_tlv *entry, const char *string);
+
+/**
+ * Get TLV entry data as uint8 value.
+ *
+ * @entry: Pointer to TLV entry.
+ * @buffer: Destination buffer for data.
+ * @return: Status code.
+ */
+int tlv_entry_get_uint8(struct tlvinfo_tlv *entry, u8 *buffer);
+
+/**
+ * Set TLV entry data from uint8 value.
+ *
+ * @entry: Pointer to TLV entry.
+ * @value: Source value.
+ * @return: Status code.
+ *
+ * Note: The size of entries can not be changed!
+ */
+int tlv_entry_set_uint8(struct tlvinfo_tlv *entry, const u8 value);
+
+/**
+ * Get TLV entry data as uint16 value.
+ *
+ * @entry: Pointer to TLV entry.
+ * @buffer: Destination buffer for data.
+ * @return: Status code.
+ */
+int tlv_entry_get_uint16(struct tlvinfo_tlv *entry, u16 *buffer);
+
+/**
+ * Set TLV entry data from uint16 value.
+ *
+ * @entry: Pointer to TLV entry.
+ * @value: Source value.
+ * @return: Status code.
+ *
+ * Note: The size of fields can not be changed!
+ */
+int tlv_entry_set_uint16(struct tlvinfo_tlv *entry, const u16 value);
+
+/**
+ * Get TLV entry data as uint32 value.
+ *
+ * @entry: Pointer to TLV entry.
+ * @buffer: Destination buffer for data.
+ * @return: Status code.
+ */
+int tlv_entry_get_uint32(struct tlvinfo_tlv *entry, u32 *buffer);
+
+/**
+ * Set TLV entry data from uint16 value.
+ *
+ * @entry: Pointer to TLV entry.
+ * @value: Source value.
+ * @return: Status code.
+ *
+ * Note: The size of fields can not be changed!
+ */
+int tlv_entry_set_uint32(struct tlvinfo_tlv *entry, const u32 value);
+
+#endif
#endif /* __TLV_EEPROM_H_ */
diff --git a/lib/Kconfig b/lib/Kconfig
index c8b3ec1ec9c..5e8bdb66c7e 100644
--- a/lib/Kconfig
+++ b/lib/Kconfig
@@ -1111,3 +1111,5 @@ menu "FWU Multi Bank Updates"
source lib/fwu_updates/Kconfig
endmenu
+
+source lib/tlv/Kconfig
diff --git a/lib/Makefile b/lib/Makefile
index 8d8ccc8bbc3..21c0d18bb8d 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -149,6 +149,8 @@ obj-$(CONFIG_LIB_ELF) += elf.o
obj-$(CONFIG_$(SPL_TPL_)SEMIHOSTING) += semihosting.o
+obj-y += tlv/
+
#
# Build a fast OID lookup registry from include/linux/oid_registry.h
#
diff --git a/lib/tlv/Kconfig b/lib/tlv/Kconfig
new file mode 100644
index 00000000000..688b0317a5d
--- /dev/null
+++ b/lib/tlv/Kconfig
@@ -0,0 +1,18 @@
+config EEPROM_TLV_LIB
+ bool "Enable EEPROM TLV library"
+ depends on I2C_EEPROM
+ select CRC32
+ help
+ Selecting this option will enable the shared EEPROM TLV library code.
+ It provides functions for reading, writing and parsing of
+ TLV-encoded data from EEPROMs.
+
+config SPL_EEPROM_TLV_LIB
+ bool "Enable EEPROM TLV library for SPL"
+ depends on SPL_I2C_EEPROM
+ select SPL_CRC32
+ select SPL_DRIVERS_MISC
+ help
+ Selecting this option will enable the shared EEPROM TLV library code.
+ It provides functions for reading, writing and parsing of
+ TLV-encoded data from EEPROMs.
diff --git a/lib/tlv/Makefile b/lib/tlv/Makefile
new file mode 100644
index 00000000000..93b8042d449
--- /dev/null
+++ b/lib/tlv/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0+
+#
+
+obj-$(CONFIG_$(SPL_)EEPROM_TLV_LIB) += tlv_eeprom.o
diff --git a/lib/tlv/tlv_eeprom.c b/lib/tlv/tlv_eeprom.c
new file mode 100644
index 00000000000..8f79941280b
--- /dev/null
+++ b/lib/tlv/tlv_eeprom.c
@@ -0,0 +1,599 @@
+// SPDX-License-Identifier: GPL-2.0+
+
+#include <compiler.h>
+#include <dm/uclass.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <string.h>
+#include <tlv_eeprom.h>
+#include <u-boot/crc.h>
+
+/*
+ * internal state specific to each TLV structure
+ */
+struct __attribute__ ((__packed__)) tlvinfo_priv {
+ size_t size;
+ size_t length;
+ struct tlvinfo_header header;
+ struct tlvinfo_tlv entries[];
+};
+
+#define TLVINFO_PRIV_SIZE sizeof(struct tlvinfo_priv)
+
+/*
+ * Calculate offset in bytes relative to end of header
+ */
+static inline ssize_t tlv_offset(struct tlvinfo_priv *priv, struct tlvinfo_tlv *entry)
+{
+ return entry - priv->entries;
+}
+
+/*
+ * Access tlv entry at specific offset in bytes relative to header
+ */
+static inline struct tlvinfo_tlv *tlv_at_offset(struct tlvinfo_priv *priv, size_t offset)
+{
+ u8 *buffer = (void *)priv->entries;
+ return (void *)&buffer[offset];
+}
+
+/*
+ * Move TLV data between offsets
+ */
+static inline void tlv_move(struct tlvinfo_priv *priv, size_t dest_offset, size_t src_offset) {
+ u8 *buffer = (void *)priv->entries;
+ memmove(&buffer[dest_offset], &buffer[src_offset], priv->length - src_offset);
+}
+
+/**
+ * is_valid_tlvinfo_header
+ *
+ * Perform sanity checks on the first 11 bytes of the TlvInfo EEPROM
+ * data pointed to by the parameter:
+ * 1. First 8 bytes contain null-terminated ASCII string "TlvInfo"
+ * 2. Version byte is 1
+ * 3. Total length bytes contain value which is less than or equal
+ * to the allowed maximum (2048-11)
+ */
+static inline bool is_valid_tlvinfo_header(struct tlvinfo_header *hdr)
+{
+ return ((strcmp(hdr->signature, TLV_INFO_ID_STRING) == 0) &&
+ (hdr->version == TLV_INFO_VERSION) &&
+ (be16_to_cpu(hdr->totallen) <= TLV_TOTAL_LEN_MAX));
+}
+
+/**
+ * Calculate TLV Checksum
+ */
+static inline uint32_t tlvinfo_calc_crc(struct tlvinfo_header *header)
+{
+ uint32_t crc;
+
+ /* calculate crc32 for complete structure, excluding final 4 byte (crc location) */
+ crc = crc32(0, (void *)header, TLV_INFO_HEADER_SIZE + be16_to_cpu(header->totallen) - 4);
+
+ return crc;
+}
+
+/**
+ * Validate the checksum in the provided TlvInfo EEPROM data. First,
+ * verify that the TlvInfo header is valid, then make sure the last
+ * TLV is a CRC-32 TLV. Then calculate the CRC over the EEPROM data
+ * and compare it to the value stored in the EEPROM CRC-32 TLV.
+ */
+static bool tlvinfo_check_crc(struct tlvinfo_priv *priv)
+{
+ struct tlvinfo_tlv *entry;
+ unsigned int calc_crc;
+ unsigned int stored_crc;
+
+ // Is the eeprom header valid?
+ if (!is_valid_tlvinfo_header(&priv->header)) {
+ pr_debug("%s:%d: tlv header is invalid\n", __FILE__, __LINE__);
+ return false;
+ }
+ printf("header was valid\n");
+
+ /* read existing CRC entry */
+ entry = tlv_entry_next_by_code(priv, NULL, TLV_CODE_CRC_32);
+ if (IS_ERR(entry)) {
+ pr_debug("%s:%d: have no tlv entry with type crc\n", __FILE__, __LINE__);
+ return false;
+ }
+ printf("found crc entry\n");
+
+ /* ensure crc entry size is correct */
+ if (entry->length != 4) {
+ pr_debug("%s:%d: crc tlv entry has illegal length: Have %u, expect 4\n", __FILE__, __LINE__, entry->length);
+ return false;
+ }
+ printf("crc entry size was correct\n");
+
+ /* ensure crc is final entry */
+ if (!IS_ERR(tlv_entry_next(priv, entry))) {
+ pr_debug("%s:%d: crc tlv entry is not last\n", __FILE__, __LINE__);
+ return false;
+ }
+ printf("crc was final entry\n");
+
+ /* copy stored crc value */
+ tlv_entry_get_uint32(entry, &stored_crc);
+ printf("got uint32\n");
+
+ /* calculate crc from data */
+ calc_crc = tlvinfo_calc_crc(&priv->header);
+ printf("calculated crc\n");
+
+ /* compare */
+ return calc_crc == be32_to_cpu(stored_crc);
+}
+
+struct udevice *tlv_eeprom_get_by_index(unsigned int index)
+{
+ int ret;
+ int count_dev = 0;
+ struct udevice *dev;
+
+ for (ret = uclass_first_device_check(UCLASS_I2C_EEPROM, &dev);
+ dev;
+ ret = uclass_next_device_check(&dev)) {
+ if (ret == 0 && count_dev++ == index)
+ return dev;
+ if (count_dev >= MAX_TLV_DEVICES)
+ break;
+ }
+
+ return ERR_PTR(-ENODEV);
+
+
+/* struct uclass *uc;
+ struct udevice *dev;
+ int i;
+
+ uclass_id_foreach_dev(UCLASS_I2C_EEPROM, dev, uc) {
+ pr_debug("%s:%d: have eeprom device at index %u\n", __FILE__, __LINE__, i);
+ if (i++ == index)
+ return dev;
+ }
+
+ pr_debug("%s:%d: couldn't find eeprom device index %u\n", __FILE__, __LINE__, index);
+ return ERR_PTR(-ENODEV);*/
+}
+
+struct tlvinfo_priv *tlv_eeprom_read(struct udevice *dev, int offset, u8 *buffer, size_t buffer_size)
+{
+ int ret;
+ struct tlvinfo_priv *priv;
+
+ if (!dev) {
+ pr_debug("%s:%d: device handle is NULL\n", __FILE__, __LINE__);
+ return ERR_PTR(-EINVAL);
+ } else if (IS_ERR(dev)) {
+ pr_debug("%s:%d: device handle is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(dev));
+ return (void *)dev;
+ }
+
+ /* check device type */
+ if (device_get_uclass_id(dev) != UCLASS_I2C_EEPROM) {
+ pr_debug("%s:%d: device handle is not an eeprom\n", __FILE__, __LINE__);
+ return ERR_PTR(-EINVAL);
+ }
+
+ /* initialise private data */
+ priv = tlv_init(buffer, buffer_size);
+ if (IS_ERR(priv)) {
+ pr_debug("%s:%d: failed to initialise in-memory tlv structure: %i\n", __FILE__, __LINE__, (int)PTR_ERR(priv));
+ return priv;
+ }
+
+ printf("Fresh TLV Header: \"%s\" %u %u\n", priv->header.signature, priv->header.version, priv->header.totallen);
+
+ /* read header */
+ ret = i2c_eeprom_read(dev, offset, (void *)&priv->header, TLV_INFO_HEADER_SIZE);
+ if (ret) {
+ pr_debug("%s:%d: failed to read from eeprom: %i\n", __FILE__, __LINE__, ret);
+ printf("%s:%d: failed to read from eeprom: %i\n", __FILE__, __LINE__, ret);
+ return ERR_PTR(ret);
+ }
+
+ printf("EEPROM TLV Header: \"%s\" %u %u\n", priv->header.signature, priv->header.version, priv->header.totallen);
+
+ /* validate header */
+ if (!is_valid_tlvinfo_header(&priv->header)) {
+ pr_warn("TLV header is invalid!\n");
+ return ERR_PTR(-EINVAL);
+ }
+
+ /* copy length from header */
+ priv->length = be16_to_cpu(priv->header.totallen);
+ printf("EEPROM TLV Header decoded length: %zu\n", priv->length);
+
+ /* check buffer is sufficient for complete tlv data */
+ if (priv->size < priv->length) {
+ pr_warn("buffer too small for TLV data: Have %zu, need %zu\n", buffer_size, TLVINFO_PRIV_SIZE + TLV_INFO_HEADER_SIZE + priv->length);
+ return ERR_PTR(-ENOBUFS);
+ }
+
+ /* read complete tlv data according to size indicated by header */
+ ret = i2c_eeprom_read(dev, offset + TLV_INFO_HEADER_SIZE, (void *)&priv->entries, priv->length);
+ if (ret) {
+ pr_debug("%s:%d: failed to read from eeprom: %i\n", __FILE__, __LINE__, ret);
+ return ERR_PTR(ret);
+ }
+
+ /* validate checksum */
+ if (!tlvinfo_check_crc(priv)) {
+ pr_err("TLV Checksum is invalid or missing!\n");
+ /* ignore this error to allow inspecting data */
+ }
+
+ /* return data */
+ return priv;
+}
+
+int tlv_eeprom_write(struct udevice *dev, int offset, struct tlvinfo_priv *priv)
+{
+ int ret;
+
+ if (!dev) {
+ pr_debug("%s:%d: device handle is NULL\n", __FILE__, __LINE__);
+ return -EINVAL;
+ } else if (IS_ERR(dev)) {
+ pr_debug("%s:%d: device handle is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(dev));
+ return PTR_ERR(dev);
+ }
+
+ ret = i2c_eeprom_write(dev, offset, (void *)&priv->header, TLV_INFO_HEADER_SIZE + priv->length);
+ if (ret)
+ pr_debug("%s:%d: failed to write to eeprom: %i\n", __FILE__, __LINE__, ret);
+
+ return ret;
+}
+
+struct tlvinfo_priv *tlv_init(u8 *buffer, size_t buffer_size)
+{
+ struct tlvinfo_priv *priv;
+ struct tlvinfo_tlv *entry;
+ uint32_t crc;
+
+ /* check buffer is sufficient for private data & header */
+ if (!buffer || buffer_size < TLVINFO_PRIV_SIZE + TLV_INFO_HEADER_SIZE) {
+ pr_debug("%s:%d: buffer insufficient for private data and tlv header: Have %zu, need %zu\n", __FILE__, __LINE__, buffer_size, TLVINFO_PRIV_SIZE + TLV_INFO_HEADER_SIZE);
+ return ERR_PTR(-ENOBUFS);
+ }
+
+ /* initialise private structure */
+ priv = (void *)buffer;
+ priv->size = buffer_size - TLVINFO_PRIV_SIZE - TLV_INFO_HEADER_SIZE;
+ //priv->header = (void *)(buffer + TLVINFO_PRIV_SIZE);
+ //priv->entries = (void *)(buffer + TLVINFO_PRIV_SIZE + TLV_INFO_HEADER_SIZE);
+ priv->length = 0;
+
+ /* initialise header */
+ strcpy(priv->header.signature, TLV_INFO_ID_STRING);
+ priv->header.version = TLV_INFO_VERSION;
+ priv->header.totallen = cpu_to_be16(0);
+
+ /* add crc entry */
+ entry = tlv_entry_add(priv, NULL, TLV_CODE_CRC_32, 4);
+ if (IS_ERR(entry)) {
+ pr_debug("%s:%d: failed to create crc entry: %i\n", __FILE__, __LINE__, (int)PTR_ERR(entry));
+ return (void *)entry;
+ }
+
+ /* calculate crc */
+ crc = cpu_to_be32(tlvinfo_calc_crc(&priv->header));
+ tlv_entry_set_uint32(entry, crc);
+
+ return priv;
+}
+
+struct tlvinfo_header *tlv_header_get(struct tlvinfo_priv *priv)
+{
+ if (!priv) {
+ pr_debug("%s:%d: private handle is NULL\n", __FILE__, __LINE__);
+ return ERR_PTR(-EINVAL);
+ } else if (IS_ERR(priv)) {
+ pr_debug("%s:%d: private handle is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(priv));
+ return (void *)PTR_ERR(priv);
+ }
+
+ return &priv->header;
+}
+
+struct tlvinfo_tlv *tlv_entry_add(struct tlvinfo_priv *priv, struct tlvinfo_tlv *_offset, u8 code, u8 size)
+{
+ struct tlvinfo_tlv *entry;
+ ssize_t offset;
+
+ if (!priv) {
+ pr_debug("%s:%d: private handle is NULL\n", __FILE__, __LINE__);
+ return ERR_PTR(-EINVAL);
+ } else if (IS_ERR(priv)) {
+ pr_debug("%s:%d: private handle is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(priv));
+ return (void *)PTR_ERR(priv);
+ }
+
+ if (IS_ERR(_offset)) {
+ pr_debug("%s:%d: offset is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(_offset));
+ return (void *)_offset;
+ }
+
+ /* calculate internal offset */
+ offset = priv->length;
+ if (_offset) {
+ offset = tlv_offset(priv, _offset);
+
+ /* check offset element is inside structure */
+ if (offset < 0 ||
+ offset + TLV_INFO_ENTRY_SIZE > priv->length ||
+ offset + TLV_INFO_ENTRY_SIZE + _offset->length > priv->length) {
+ pr_debug("%s:%d: reference element at offset %zd outside tlv structure\n", __FILE__, __LINE__, offset);
+ return ERR_PTR(-EINVAL);
+ }
+ }
+
+ /* check buffer is sufficient for new entry */
+ if (priv->size < priv->length + TLV_INFO_ENTRY_SIZE + size) {
+ pr_debug("%s:%d: buffer insufficient for additional tlv entry: Have %zu, need %zu\n", __FILE__, __LINE__, TLVINFO_PRIV_SIZE + TLV_INFO_HEADER_SIZE + priv->size, TLVINFO_PRIV_SIZE + TLV_INFO_HEADER_SIZE + priv->length + TLV_INFO_ENTRY_SIZE + size);
+ return ERR_PTR(-ENOBUFS);
+ }
+
+ /* move existing data to make space */
+ tlv_move(priv, offset + TLV_INFO_ENTRY_SIZE + entry->length, offset);
+
+ /* initialise new entry */
+ entry = priv->entries + offset;
+ entry->type = code;
+ entry->length = size;
+ memset(entry->value, 0, size);
+
+ /* update total length */
+ priv->length += TLV_INFO_ENTRY_SIZE + size;
+ priv->header.totallen = cpu_to_be16(priv->length);
+
+ // TODO: update CRC?!
+
+ return entry;
+}
+
+int tlv_entry_remove(struct tlvinfo_priv *priv, struct tlvinfo_tlv *entry)
+{
+ ssize_t offset;
+ size_t end;
+
+ if (!priv) {
+ pr_debug("%s:%d: private handle is NULL\n", __FILE__, __LINE__);
+ return -EINVAL;
+ } else if (IS_ERR(priv)) {
+ pr_debug("%s:%d: private handle is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(priv));
+ return PTR_ERR(priv);
+ }
+
+ if (!entry) {
+ pr_debug("%s:%d: entry is NULL\n", __FILE__, __LINE__);
+ return -EINVAL;
+ } else if (IS_ERR(entry)) {
+ pr_debug("%s:%d: entry is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(entry));
+ return PTR_ERR(entry);
+ }
+
+ /* calculate internal offset */
+ offset = tlv_offset(priv, entry);
+
+ /* check entry within structure */
+ if (offset < 0 ||
+ offset + TLV_INFO_ENTRY_SIZE > priv->length ||
+ offset + TLV_INFO_ENTRY_SIZE + entry->length > priv->length) {
+ pr_debug("%s:%d: element at offset %zd outside tlv structure\n", __FILE__, __LINE__, offset);
+ return -EINVAL;
+ }
+
+ /* move existing data from end into gap */
+ end = offset + TLV_INFO_ENTRY_SIZE + entry->length;
+ tlv_move(priv, offset, end);
+
+ /* update total length */
+ priv->length -= end - offset;
+ priv->header.totallen = cpu_to_be16(priv->length);
+
+ return 0;
+}
+
+struct tlvinfo_tlv *tlv_entry_next(struct tlvinfo_priv *priv, struct tlvinfo_tlv *_offset)
+{
+ ssize_t offset = 0;
+ struct tlvinfo_tlv *entry;
+
+ if (!priv) {
+ pr_debug("%s:%d: private handle is NULL\n", __FILE__, __LINE__);
+ return ERR_PTR(-EINVAL);
+ } else if (IS_ERR(priv)) {
+ pr_debug("%s:%d: private handle is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(priv));
+ return (void *)PTR_ERR(priv);
+ }
+
+ if (IS_ERR(_offset)) {
+ pr_debug("%s:%d: reference element is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(_offset));
+ return (void *)_offset;
+ }
+
+ /* by default search from beginning */
+ entry = priv->entries;
+
+ /* search after reference entry, if any */
+ if (_offset) {
+ offset = tlv_offset(priv, _offset);
+
+ /* check offset element is inside structure */
+ if (offset < 0 ||
+ offset + TLV_INFO_ENTRY_SIZE > priv->length ||
+ offset + TLV_INFO_ENTRY_SIZE + _offset->length > priv->length) {
+ pr_debug("%s:%d: reference element at offset %zd outside tlv structure\n", __FILE__, __LINE__, offset);
+ return ERR_PTR(-EINVAL);
+ }
+
+ /* seek beyond reference element */
+ offset += _offset->length + TLV_INFO_ENTRY_SIZE;
+ entry = tlv_at_offset(priv, offset);
+
+ pr_debug("%s:%d: searching at offset %zd relative to end of header\n", __FILE__, __LINE__, offset);
+ }
+
+ /* check for end of tlv data */
+ if (offset == priv->length) {
+ pr_debug("%s:%d: reached end of tlv data at offset %zu\n", __FILE__, __LINE__, offset);
+ return ERR_PTR(-ENOENT);
+ }
+
+ /* check element is inside structure */
+ if (offset + TLV_INFO_ENTRY_SIZE > priv->length ||
+ offset + TLV_INFO_ENTRY_SIZE + entry->length > priv->length) {
+ pr_debug("%s:%d: found element at offset %zd outside tlv structure\n", __FILE__, __LINE__, offset);
+ return ERR_PTR(-EINVAL);
+ }
+
+ return entry;
+}
+
+struct tlvinfo_tlv *tlv_entry_next_by_code(struct tlvinfo_priv *priv, struct tlvinfo_tlv *offset, u8 code)
+{
+ struct tlvinfo_tlv *entry;
+
+ for (entry = tlv_entry_next(priv, offset);
+ !IS_ERR(entry);
+ entry = tlv_entry_next(priv, entry))
+ if (entry->type == code)
+ return entry;
+
+ return ERR_PTR(-ENOENT);
+}
+
+int tlv_entry_get_raw(struct tlvinfo_tlv *entry, u8 *buffer, size_t size)
+{
+ if (!entry) {
+ pr_debug("%s:%d: tlv entry is NULL\n", __FILE__, __LINE__);
+ return -EINVAL;
+ } else if (IS_ERR(entry)) {
+ pr_debug("%s:%d: tlv entry is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(entry));
+ return PTR_ERR(entry);
+ }
+
+ if (!buffer || entry->length > size) {
+ pr_debug("%s:%d: buffer insufficient: Have %zu, need %u\n", __FILE__, __LINE__, size, entry->length);
+ return -ENOBUFS;
+ }
+
+ memcpy(buffer, &entry->value[0], entry->length);
+
+ return 0;
+}
+
+int tlv_entry_set_raw(struct tlvinfo_tlv *entry, const u8 *values, size_t length)
+{
+ if (!entry) {
+ pr_debug("%s:%d: tlv entry is NULL\n", __FILE__, __LINE__);
+ return -EINVAL;
+ } else if (IS_ERR(entry)) {
+ pr_debug("%s:%d: tlv entry is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(entry));
+ return PTR_ERR(entry);
+ }
+
+ if (entry->length < length) {
+ pr_debug("%s:%d: tlv entry too small: Have %u, need %zu\n", __FILE__, __LINE__, entry->length, length);
+ return -ENOBUFS;
+ }
+
+ memcpy(&entry->value[0], values, length);
+
+ return 0;
+}
+
+int tlv_entry_get_string(struct tlvinfo_tlv *entry, char *buffer, size_t size)
+{
+ if (!entry) {
+ pr_debug("%s:%d: tlv entry is NULL\n", __FILE__, __LINE__);
+ return -EINVAL;
+ } else if (IS_ERR(entry)) {
+ pr_debug("%s:%d: tlv entry is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(entry));
+ return PTR_ERR(entry);
+ }
+
+ if (!buffer || size <= entry->length) {
+ pr_debug("%s:%d: buffer insufficient: Have %zu, need %u\n", __FILE__, __LINE__, size, entry->length + 1);
+ return -ENOBUFS;
+ }
+
+ memcpy(buffer, &entry->value[0], entry->length);
+ buffer[entry->length] = '\0';
+ return 0;
+}
+
+int tlv_entry_set_string(struct tlvinfo_tlv *entry, const char *string)
+{
+ if (!entry) {
+ pr_debug("%s:%d: tlv entry is NULL\n", __FILE__, __LINE__);
+ return -EINVAL;
+ } else if (IS_ERR(entry)) {
+ pr_debug("%s:%d: tlv entry is error: %i\n", __FILE__, __LINE__, (int)PTR_ERR(entry));
+ return PTR_ERR(entry);
+ }
+
+ if (!string) {
+ pr_debug("%s:%d: string is NULL\n", __FILE__, __LINE__);
+ return -EINVAL;
+ }
+
+ strncpy(&entry->value[0], string, entry->length);
+ return 0;
+}
+
+int tlv_entry_get_uint8(struct tlvinfo_tlv *entry, u8 *buffer)
+{
+ return tlv_entry_get_raw(entry, buffer, sizeof(uint8_t));
+}
+
+int tlv_entry_set_uint8(struct tlvinfo_tlv *entry, const u8 value)
+{
+ return tlv_entry_set_raw(entry, &value, sizeof(value));
+}
+
+int tlv_entry_get_uint16(struct tlvinfo_tlv *entry, u16 *buffer)
+{
+ int ret;
+ uint16_t val;
+
+ ret = tlv_entry_get_raw(entry, (uint8_t *)&val, sizeof(val));
+ if (ret)
+ return ret;
+
+ *buffer = be16_to_cpu(val);
+ return 0;
+}
+
+int tlv_entry_set_uint16(struct tlvinfo_tlv *entry, const u16 value)
+{
+ uint16_t raw = cpu_to_be16(value);
+
+ return tlv_entry_set_raw(entry, (uint8_t *)&raw, sizeof(raw));
+}
+
+int tlv_entry_get_uint32(struct tlvinfo_tlv *entry, u32 *buffer)
+{
+ int ret;
+ uint32_t val;
+
+ ret = tlv_entry_get_raw(entry, (uint8_t *)&val, sizeof(val));
+ if (ret)
+ return ret;
+
+ *buffer = be32_to_cpu(val);
+ return 0;
+}
+
+int tlv_entry_set_uint32(struct tlvinfo_tlv *entry, const u32 value)
+{
+ uint32_t raw = cpu_to_be32(value);
+
+ return tlv_entry_set_raw(entry, (uint8_t *)&raw, sizeof(raw));
+}
--
2.35.3
More information about the U-Boot
mailing list