diff --git a/src/drivers/boards/common/stm32/board_identity.c b/src/drivers/boards/common/stm32/board_identity.c index 4a5f66c589..5060f31adb 100644 --- a/src/drivers/boards/common/stm32/board_identity.c +++ b/src/drivers/boards/common/stm32/board_identity.c @@ -44,6 +44,8 @@ #define CPU_UUID_BYTE_FORMAT_ORDER {3, 2, 1, 0, 7, 6, 5, 4, 11, 10, 9, 8} #define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24)) +static const uint16_t soc_arch_id = PX4_SOC_ARCH_ID; + /* A type suitable for holding the reordering array for the byte format of the UUID */ @@ -124,3 +126,42 @@ int board_get_mfguid_formated(char *format_buffer, int size) return offset; } + +int board_get_px4_guid(px4_guid_t px4_guid) +{ + uint8_t *pb = (uint8_t *) &px4_guid[0]; + *pb++ = (soc_arch_id >> 8) & 0xff; + *pb++ = (soc_arch_id & 0xff); + + for (unsigned i = 0; i < PX4_GUID_BYTE_LENGTH - (sizeof(soc_arch_id) + PX4_CPU_UUID_BYTE_LENGTH); i++) { + *pb++ = 0; + } + + uint32_t *rv = (uint32_t *) pb; + uint32_t *chip_uuid = (uint32_t *) STM32_SYSMEM_UID; + + for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + *rv++ = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]); + } + + return PX4_GUID_BYTE_LENGTH; +} + +int board_get_px4_guid_formated(char *format_buffer, int size) +{ + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + int offset = 0; + + /* size should be 2 per byte + 1 for termination + * So it needs to be odd + */ + size = size & 1 ? size : size - 1; + + /* Discard from MSD */ + for (unsigned i = PX4_GUID_BYTE_LENGTH - size / 2; offset < size && i < PX4_GUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", px4_guid[i]); + } + + return offset; +}