+
+ if (!offset || !config_block)
+ return -EINVAL;
+
+ tag = (struct toradex_tag *)(config_block + *offset);
+ tag->id = tag_id;
+ tag->flags = TAG_FLAG_VALID;
+ /* len is provided as number of 32bit values after the tag */
+ tag->len = (tag_data_size + sizeof(u32) - 1) / sizeof(u32);
+ *offset += sizeof(struct toradex_tag);
+ if (tag_data && tag_data_size) {
+ memcpy(config_block + *offset, tag_data,
+ tag_data_size);
+ *offset += tag_data_size;
+ }
+
+ return 0;
+}
+
+#ifdef CONFIG_TDX_CFG_BLOCK_EXTRA
+int read_tdx_cfg_block_carrier(void)
+{
+ int ret = 0;
+ u8 *config_block = NULL;
+ struct toradex_tag *tag;
+ size_t size = TDX_CFG_BLOCK_EXTRA_MAX_SIZE;
+ int offset;
+
+ /* Allocate RAM area for carrier config block */
+ config_block = memalign(ARCH_DMA_MINALIGN, size);
+ if (!config_block) {
+ printf("Not enough malloc space available!\n");
+ return -ENOMEM;
+ }
+
+ memset(config_block, 0, size);
+
+ ret = read_tdx_eeprom_data(TDX_EEPROM_ID_CARRIER, 0x0, config_block,
+ size);
+ if (ret)
+ return ret;
+
+ /* Expect a valid tag first */
+ tag = (struct toradex_tag *)config_block;
+ if (tag->flags != TAG_FLAG_VALID || tag->id != TAG_VALID) {
+ valid_cfgblock_carrier = false;
+ ret = -EINVAL;
+ goto out;
+ }
+ valid_cfgblock_carrier = true;
+ offset = 4;
+
+ while (offset + sizeof(struct toradex_tag) +
+ sizeof(struct toradex_hw) < TDX_CFG_BLOCK_MAX_SIZE) {
+ tag = (struct toradex_tag *)(config_block + offset);
+ offset += 4;
+ if (tag->id == TAG_INVALID)
+ break;
+
+ if (tag->flags == TAG_FLAG_VALID) {
+ switch (tag->id) {
+ case TAG_CAR_SERIAL:
+ memcpy(&tdx_car_serial, config_block + offset,
+ sizeof(tdx_car_serial));
+ break;
+ case TAG_HW:
+ memcpy(&tdx_car_hw_tag, config_block +
+ offset, 8);
+ break;
+ }
+ }
+
+ /* Get to next tag according to current tags length */
+ offset += tag->len * 4;
+ }
+out:
+ free(config_block);
+ return ret;
+}
+
+int check_pid8_sanity(char *pid8)
+{
+ char s_carrierid_verdin_dev[5];
+ char s_carrierid_dahlia[5];
+
+ sprintf(s_carrierid_verdin_dev, "0%d", VERDIN_DEVELOPMENT_BOARD);
+ sprintf(s_carrierid_dahlia, "0%d", DAHLIA);
+
+ /* sane value check, first 4 chars which represent carrier id */
+ if (!strncmp(pid8, s_carrierid_verdin_dev, 4))
+ return 0;
+
+ if (!strncmp(pid8, s_carrierid_dahlia, 4))
+ return 0;
+
+ return -EINVAL;
+}
+
+int try_migrate_tdx_cfg_block_carrier(void)
+{
+ char pid8[8];
+ int offset = 0;
+ int ret = CMD_RET_SUCCESS;
+ size_t size = TDX_CFG_BLOCK_EXTRA_MAX_SIZE;
+ u8 *config_block;
+
+ memset(pid8, 0x0, 8);
+ ret = read_tdx_eeprom_data(TDX_EEPROM_ID_CARRIER, 0x0, (u8 *)pid8, 8);
+ if (ret)
+ return ret;
+
+ if (check_pid8_sanity(pid8))
+ return -EINVAL;
+
+ /* Allocate RAM area for config block */
+ config_block = memalign(ARCH_DMA_MINALIGN, size);
+ if (!config_block) {
+ printf("Not enough malloc space available!\n");
+ return CMD_RET_FAILURE;
+ }
+
+ memset(config_block, 0xff, size);
+ /* we try parse PID8 concatenating zeroed serial number */
+ tdx_car_hw_tag.ver_major = pid8[4] - '0';
+ tdx_car_hw_tag.ver_minor = pid8[5] - '0';
+ tdx_car_hw_tag.ver_assembly = pid8[7] - '0';
+
+ pid8[4] = '\0';
+ tdx_car_hw_tag.prodid = simple_strtoul(pid8, NULL, 10);
+
+ /* Valid Tag */
+ write_tag(config_block, &offset, TAG_VALID, NULL, 0);
+
+ /* Product Tag */
+ write_tag(config_block, &offset, TAG_HW, (u8 *)&tdx_car_hw_tag,
+ sizeof(tdx_car_hw_tag));
+
+ /* Serial Tag */
+ write_tag(config_block, &offset, TAG_CAR_SERIAL, (u8 *)&tdx_car_serial,
+ sizeof(tdx_car_serial));
+
+ memset(config_block + offset, 0, 32 - offset);
+ ret = write_tdx_eeprom_data(TDX_EEPROM_ID_CARRIER, 0x0, config_block,
+ size);
+ if (ret) {
+ printf("Failed to write Toradex Extra config block: %d\n",
+ ret);
+ ret = CMD_RET_FAILURE;
+ goto out;
+ }
+
+ printf("Successfully migrated to Toradex Config Block from PID8\n");
+
+out:
+ free(config_block);
+ return ret;
+}
+
+static int get_cfgblock_carrier_interactive(void)
+{
+ char message[CONFIG_SYS_CBSIZE];
+ int len;
+
+ printf("Supported carrier boards:\n");
+ printf("CARRIER BOARD NAME\t\t [ID]\n");
+ for (int i = 0; i < sizeof(toradex_carrier_boards) /
+ sizeof(toradex_carrier_boards[0]); i++)
+ if (toradex_carrier_boards[i])
+ printf("%s \t\t [%d]\n", toradex_carrier_boards[i], i);
+
+ sprintf(message, "Choose your carrier board (provide ID): ");
+ len = cli_readline(message);
+ tdx_car_hw_tag.prodid = simple_strtoul(console_buffer, NULL, 10);
+
+ do {
+ sprintf(message, "Enter carrier board version (e.g. V1.1B): V");
+ len = cli_readline(message);
+ } while (len < 4);
+
+ tdx_car_hw_tag.ver_major = console_buffer[0] - '0';
+ tdx_car_hw_tag.ver_minor = console_buffer[2] - '0';
+ tdx_car_hw_tag.ver_assembly = console_buffer[3] - 'A';
+
+ while (len < 8) {
+ sprintf(message, "Enter carrier board serial number: ");
+ len = cli_readline(message);
+ }
+
+ tdx_car_serial = simple_strtoul(console_buffer, NULL, 10);
+
+ return 0;
+}
+
+static int do_cfgblock_carrier_create(struct cmd_tbl *cmdtp, int flag, int argc,
+ char * const argv[])
+{
+ u8 *config_block;
+ size_t size = TDX_CFG_BLOCK_EXTRA_MAX_SIZE;