From 00e82c906040d4902868ebeb0d8679205f15df47 Mon Sep 17 00:00:00 2001
From: David Sidrane <David.Sidrane@NscDg.com>
Date: Wed, 5 Jan 2022 15:12:48 +0100
Subject: [PATCH] board_hw_eeprom_rev_ver:Versioning hierarchy

---
 .../px4_platform/board_hw_eeprom_rev_ver.h    | 22 +++--
 .../board_hw_info/board_hw_rev_ver.c          | 84 +++++++++----------
 2 files changed, 55 insertions(+), 51 deletions(-)

diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_hw_eeprom_rev_ver.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_hw_eeprom_rev_ver.h
index dd5f168abb..fdd44cc80b 100644
--- a/platforms/nuttx/src/px4/common/include/px4_platform/board_hw_eeprom_rev_ver.h
+++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_hw_eeprom_rev_ver.h
@@ -38,22 +38,22 @@
 #pragma pack(push, 1)
 
 typedef struct {
-	uint16_t format_version;
+	uint16_t id;
+} mtd_mft_t;
+
+typedef struct {
+	mtd_mft_t version;
 	uint16_t hw_extended_ver;
 	uint16_t crc;
 } mtd_mft_v0_t;
 
 typedef struct {
-	uint16_t format_version;
+	mtd_mft_t  version;
 	uint16_t hw_extended_ver;
 	//{device tree overlay}
 	uint16_t crc;
 } mtd_mft_v1_t;
 
-typedef struct {
-	uint16_t hw_extended_ver;
-	//{device tree overlay}
-} mtd_mft_t;
 
 #pragma pack(pop)
 
@@ -62,6 +62,8 @@ typedef struct {
 
 #define MTD_MFT_OFFSET 0 //<! Offset in EEPROM where mtd_mft data starts
 
+__BEGIN_DECLS
+
 /************************************************************************************
   * Name: board_set_eeprom_hw_info
  *
@@ -69,7 +71,7 @@ typedef struct {
  * Function for writing hardware info to EEPROM
  *
  * Input Parameters:
- *   *mtd_mft - pointer to mtd_mft to write hw_info
+ *   *mtd_mft_unk - pointer to mtd_mft to write hw_info
  *
  * Returned Value:
  *    0    - Successful storing to EEPROM
@@ -78,9 +80,9 @@ typedef struct {
  ************************************************************************************/
 
 #if !defined(BOARD_HAS_SIMPLE_HW_VERSIONING) && defined(BOARD_HAS_VERSIONING)
-__EXPORT int board_set_eeprom_hw_info(mtd_mft_t *mtd_mft);
+__EXPORT int board_set_eeprom_hw_info(mtd_mft_t *mtd_mft_unk);
 #else
-static inline int board_set_eeprom_hw_info(mtd_mft_t *mtd_mft) { return -ENOSYS; }
+static inline int board_set_eeprom_hw_info(mtd_mft_t *mtd_mft_unk) { return -ENOSYS; }
 #endif
 
 /************************************************************************************
@@ -103,3 +105,5 @@ __EXPORT int board_get_eeprom_hw_info(mtd_mft_t *mtd_mft);
 #else
 static inline int board_get_eeprom_hw_info(mtd_mft_t *mtd_mft) { return -ENOSYS; }
 #endif
+
+__END_DECLS
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c
index ca0e9b2734..85a21e37f2 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c
+++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c
@@ -66,7 +66,6 @@ static int hw_version = 0;
 static int hw_revision = 0;
 static char hw_info[HW_INFO_SIZE] = {0};
 
-static const uint16_t mtd_mft_version = MTD_MFT_v0;	//< Current version of structure in EEPROM
 static const char *mtd_mft_path = "/fs/mtd_mft";
 
 /****************************************************************************
@@ -454,8 +453,8 @@ int board_determine_hw_info()
 		/* EEPROM hw version range: {0x10 - 0xFFFF} */
 		if (hw_version == HW_VERSION_EEPROM) {
 
-			mtd_mft_t mtd_mft;
-			rv = board_get_eeprom_hw_info(&mtd_mft);
+			mtd_mft_v0_t mtd_mft = {MTD_MFT_v0};
+			rv = board_get_eeprom_hw_info((mtd_mft_t *)&mtd_mft);
 
 			if (rv == OK) {
 				hw_version = mtd_mft.hw_extended_ver;
@@ -483,7 +482,7 @@ int board_determine_hw_info()
  * Function for writing hardware info to EEPROM
  *
  * Input Parameters:
- *   *mtd_mft - pointer to mtd_mft to write hw_info
+ *   *mtd_mft_unk - pointer to mtd_mft to write hw_info
  *
  * Returned Value:
  *    0    - Successful storing to EEPROM
@@ -491,12 +490,20 @@ int board_determine_hw_info()
  *
  ************************************************************************************/
 
-__EXPORT int board_set_eeprom_hw_info(mtd_mft_t *mtd_mft)
+int board_set_eeprom_hw_info(mtd_mft_t *mtd_mft_unk)
 {
-	if (mtd_mft == NULL) {
+	if (mtd_mft_unk == NULL) {
 		return -1;
 	}
 
+	// Later this will be a demux on type
+	if (mtd_mft_unk->id != MTD_MFT_v0) {
+		printf("Verson is: %d, Only mft version %d is supported\n", mtd_mft_unk->id, MTD_MFT_v0);
+		return -EINVAL;
+	}
+
+	mtd_mft_v0_t *mtd_mft = (mtd_mft_v0_t *)mtd_mft_unk;
+
 	if (mtd_mft->hw_extended_ver < HW_EEPROM_VERSION_MIN) {
 		printf("hardware version for EEPROM must be greater than %x\n", HW_EEPROM_VERSION_MIN);
 		return -EINVAL;
@@ -510,24 +517,16 @@ __EXPORT int board_set_eeprom_hw_info(mtd_mft_t *mtd_mft)
 
 	int ret_val = OK;
 
-	mtd_mft_v0_t mtd_mft_v0 = {
-		.format_version = mtd_mft_version,
-		.hw_extended_ver = mtd_mft->hw_extended_ver
-	};
-
-	mtd_mft_v0.crc = crc16_signature(CRC16_INITIAL, sizeof(mtd_mft_v0) - sizeof(mtd_mft_v0.crc), (uint8_t *)&mtd_mft_v0);
+	mtd_mft->crc = crc16_signature(CRC16_INITIAL, sizeof(*mtd_mft) - sizeof(mtd_mft->crc), (uint8_t *) mtd_mft);
 
 	if (
 		(MTD_MFT_OFFSET != lseek(fd, MTD_MFT_OFFSET, SEEK_SET)) ||
-		(sizeof(mtd_mft_v0) != write(fd, &mtd_mft_v0, sizeof(mtd_mft_v0)))
+		(sizeof(*mtd_mft) != write(fd, mtd_mft, sizeof(*mtd_mft)))
 	) {
 		ret_val = -errno;
 	}
 
-	if (fd >= 0) {
-		close(fd);
-		fd = -1;
-	}
+	close(fd);
 
 	return ret_val;
 }
@@ -548,14 +547,18 @@ __EXPORT int board_set_eeprom_hw_info(mtd_mft_t *mtd_mft)
  ************************************************************************************/
 __EXPORT int board_get_eeprom_hw_info(mtd_mft_t *mtd_mft)
 {
+	if (mtd_mft == NULL) {
+		return -EINVAL;
+	}
+
 	int fd = open(mtd_mft_path, O_RDONLY);
 
-	if ((fd < 0) || (mtd_mft == NULL)) {
+	if (fd < 0) {
 		return -errno;
 	}
 
 	int ret_val = OK;
-	uint16_t format_version = 0;
+	mtd_mft_t format_version = {-1};
 
 	if (
 		(MTD_MFT_OFFSET != lseek(fd, MTD_MFT_OFFSET, SEEK_SET)) ||
@@ -563,54 +566,51 @@ __EXPORT int board_get_eeprom_hw_info(mtd_mft_t *mtd_mft)
 	) {
 		ret_val = -errno;
 
+	} else if (format_version.id != mtd_mft->id) {
+		ret_val = -EPROTO;
+
 	} else {
 
-		uint16_t buffer_size = 0;
+		uint16_t mft_size = 0;
 
-		if (MTD_MFT_v0 == format_version) {
-			buffer_size = sizeof(mtd_mft_v0_t);
+		switch (format_version.id) {
+		case MTD_MFT_v0: mft_size = sizeof(mtd_mft_v0_t); break;
 
-		} else if (MTD_MFT_v1 == format_version) {
-			buffer_size = sizeof(mtd_mft_v1_t);
+		case MTD_MFT_v1: mft_size = sizeof(mtd_mft_v1_t); break;
 
-		} else {
-			printf("[boot] Error, unknown version %d of mtd_mft in EEPROM\n", format_version);
+		default:
+			printf("[boot] Error, unknown version %d of mtd_mft in EEPROM\n", format_version.id);
 			ret_val = -1;
+			break;
 		}
 
 		if (ret_val == OK) {
 
-			uint8_t buffer[buffer_size];
-			memset(buffer, 0u, buffer_size);
-
 			if (
 				(MTD_MFT_OFFSET != lseek(fd, MTD_MFT_OFFSET, SEEK_SET)) ||
-				(buffer_size != read(fd, buffer, buffer_size))
+				(mft_size != read(fd, mtd_mft, mft_size))
 			) {
 				ret_val = -errno;
 
 			} else {
 
-				uint16_t crc = 0, eeprom_crc = 0;
-				crc = crc16_signature(CRC16_INITIAL, buffer_size - sizeof(crc), buffer);
-
-				eeprom_crc = (uint16_t)((buffer[buffer_size - 1] << 8)  | buffer[buffer_size - 2]);
+				union {
+					uint16_t w;
+					uint8_t  b[2];
+				} crc;
 
-				if (crc == eeprom_crc) {
-					memcpy(mtd_mft, &buffer[sizeof(format_version)], buffer_size - sizeof(format_version) - sizeof(crc));
+				uint8_t *bytes = (uint8_t *) mtd_mft;
+				crc.w = crc16_signature(CRC16_INITIAL, mft_size - sizeof(crc), bytes);
+				uint8_t *eeprom_crc = &bytes[mft_size - sizeof(crc)];
 
-				} else {
+				if (!(crc.b[0] == eeprom_crc[0] && crc.b[1] == eeprom_crc[1])) {
 					ret_val = -1;
 				}
 			}
 		}
 	}
 
-	if (fd >= 0) {
-		close(fd);
-		fd = -1;
-	}
-
+	close(fd);
 	return ret_val;
 }