diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt index 3a466715b1..f926bf71b0 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt @@ -34,3 +34,4 @@ px4_add_library(arch_board_hw_info board_hw_rev_ver.c ) +target_link_libraries(arch_board_hw_info PRIVATE arch_adc systemlib) \ No newline at end of file diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c index bb85fc47d3..45aea44bd3 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c @@ -40,10 +40,14 @@ #include #include #include +#include #include +#include #include +#include #include +#include #include #if defined(BOARD_HAS_HW_VERSIONING) @@ -339,17 +343,184 @@ __EXPORT int board_get_hw_revision() int board_determine_hw_info() { + + // Read ADC jumpering hw_info int rv = determine_hw_info(&hw_revision, &hw_version); if (rv == OK) { - if (rv == OK) { + // MFT supported? + const char *path; + int rvmft = px4_mtd_query("MTD_MFT_VER", NULL, &path); - snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision); + if (rvmft == OK && path != NULL && hw_version == HW_ID_EEPROM) { + mtd_mft_v0_t mtd_mft = {MTD_MFT_v0}; + rv = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft); + + if (rv == OK) { + hw_version = mtd_mft.hw_extended_id; + } } + + path = NULL; + rvmft = px4_mtd_query("MTD_MFT_REV", NULL, &path); + + if (rvmft == OK && path != NULL && hw_revision == HW_ID_EEPROM) { + + mtd_mft_v0_t mtd_mft = {MTD_MFT_v0}; + rv = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft); + + if (rv == OK) { + hw_revision = mtd_mft.hw_extended_id; + } + } + } + + if (rv == OK) { + snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision); } return rv; } + +/************************************************************************************ + * Name: board_set_eeprom_hw_info + * + * Description: + * Function for writing hardware info to EEPROM + * + * Input Parameters: + * *mtd_mft_unk - pointer to mtd_mft to write hw_info + * + * Returned Value: + * 0 - Successful storing to EEPROM + * -1 - Error while storing to EEPROM + * + ************************************************************************************/ + +int board_set_eeprom_hw_info(const char *path, mtd_mft_t *mtd_mft_unk) +{ + if (mtd_mft_unk == NULL || path == NULL) { + return -EINVAL; + } + + // Later this will be a demux on type + if (mtd_mft_unk->id != MTD_MFT_v0) { + printf("Version 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_id < HW_EEPROM_ID_MIN) { + printf("hardware version for EEPROM must be greater than %x\n", HW_EEPROM_ID_MIN); + return -EINVAL; + } + + int fd = open(path, O_WRONLY); + + if (fd < 0) { + return -errno; + } + + int ret_val = OK; + + 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) != write(fd, mtd_mft, sizeof(*mtd_mft))) + ) { + ret_val = -errno; + } + + close(fd); + + return ret_val; +} + +/************************************************************************************ + * Name: board_get_eeprom_hw_info + * + * Description: + * Function for reading hardware info from EEPROM + * + * Output Parameters: + * *mtd_mft - pointer to mtd_mft to read hw_info + * + * Returned Value: + * 0 - Successful reading from EEPROM + * -1 - Error while reading from EEPROM + * + ************************************************************************************/ +__EXPORT int board_get_eeprom_hw_info(const char *path, mtd_mft_t *mtd_mft) +{ + if (mtd_mft == NULL || path == NULL) { + return -EINVAL; + } + + int fd = open(path, O_RDONLY); + + if (fd < 0) { + return -errno; + } + + int ret_val = OK; + mtd_mft_t format_version = {-1}; + + if ( + (MTD_MFT_OFFSET != lseek(fd, MTD_MFT_OFFSET, SEEK_SET)) || + (sizeof(format_version) != read(fd, &format_version, sizeof(format_version))) + ) { + ret_val = -errno; + + } else if (format_version.id != mtd_mft->id) { + ret_val = -EPROTO; + + } else { + + uint16_t mft_size = 0; + + switch (format_version.id) { + case MTD_MFT_v0: mft_size = sizeof(mtd_mft_v0_t); break; + + case MTD_MFT_v1: mft_size = sizeof(mtd_mft_v1_t); break; + + default: + printf("[boot] Error, unknown version %d of mtd_mft in EEPROM\n", format_version.id); + ret_val = -1; + break; + } + + if (ret_val == OK) { + + if ( + (MTD_MFT_OFFSET != lseek(fd, MTD_MFT_OFFSET, SEEK_SET)) || + (mft_size != read(fd, mtd_mft, mft_size)) + ) { + ret_val = -errno; + + } else { + + union { + uint16_t w; + uint8_t b[2]; + } 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)]; + + if (!(crc.b[0] == eeprom_crc[0] && crc.b[1] == eeprom_crc[1])) { + ret_val = -1; + } + } + } + } + + close(fd); + return ret_val; +} + #endif