mxssb: Implement the MODE command

The last missing command is the MODE command. Implement this command
for the completeness of the command set.
Signed-off-by: Marek Vasut <marex@denx.de>

mxssb.c

@@ -100,6 +100,11 @@ struct sb_image_ctx {
  * FILL address pattern length
  * JUMP [HAB] address [r0_arg]
  * CALL [HAB] address [r0_arg]
+ * MODE mode
+ *      For i.MX23, mode = USB/I2C/SPI1_FLASH/SPI2_FLASH/NAND_BCH
+ *                         JTAG/SPI3_EEPROM/SD_SSP0/SD_SSP1
+ *      For i.MX28, mode = USB/I2C/SPI2_FLASH/SPI3_FLASH/NAND_BCH
+ *                         JTAG/SPI2_EEPROM/SD_SSP0/SD_SSP1
  */
 
 /* Blank image key. */
@@ -960,6 +965,108 @@ static int sb_build_command_call(struct sb_image_ctx *ictx, struct sb_cmd_list *
 	return sb_build_command_jump_call(ictx, cmd, 1);
 }
 
+static int sb_build_command_mode(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
+{
+	struct sb_section_ctx *sctx = ictx->sect_tail;
+	struct sb_cmd_ctx *cctx;
+	struct sb_command *ccmd;
+	char *tok;
+	int ret;
+	unsigned int i;
+	uint32_t mode = 0xffffffff;
+
+	/*
+	 * Most of the mode names are same or at least similar
+	 * on i.MX23 and i.MX28, but some of the mode names
+	 * differ. The "name" field represents the mode name
+	 * on i.MX28 as seen in Table 12-2 of the datasheet.
+	 * The "altname" field represents the differently named
+	 * fields on i.MX23 as seen in Table 35-3 of the
+	 * datasheet.
+	 */
+	const struct {
+		const char	*name;
+		const char	*altname;
+		const uint8_t	mode;
+	} modetable[] = {
+		{ "USB",		NULL,		0x00 },
+		{ "I2C",		NULL,		0x01 },
+		{ "SPI2_FLASH",		"SPI1_FLASH",	0x02 },
+		{ "SPI3_FLASH",		"SPI2_FLASH",	0x03 },
+		{ "NAND_BCH",		NULL,		0x04 },
+		{ "JTAG",		NULL,		0x06 },
+		{ "SPI3_EEPROM",	"SPI2_EEPROM",	0x08 },
+		{ "SD_SSP0",		NULL,		0x09 },
+		{ "SD_SSP1",		NULL,		0x0A }
+	};
+
+	cctx = calloc(1, sizeof(*cctx));
+	if (!cctx)
+		return -ENOMEM;
+
+	ccmd = &cctx->payload;
+
+	/*
+	 * Prepare the command.
+	 */
+	tok = strtok(cmd->cmd, " ");
+	if (!tok) {
+		fprintf(stderr, "#%i ERR: Missing MODE boot mode argument!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	for (i = 0; i < sizeof(modetable)/sizeof(modetable[0]); i++) {
+		if (!strcmp(tok, modetable[i].name)) {
+			mode = modetable[i].mode;
+			break;
+		}
+
+		if (!modetable[i].altname)
+			continue;
+
+		if (!strcmp(tok, modetable[i].altname)) {
+			mode = modetable[i].mode;
+			break;
+		}
+	}
+
+	if (mode == 0xffffffff) {
+		fprintf(stderr, "#%i ERR: Invalid MODE boot mode argument!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/*
+	 * Construct the command.
+	 */
+	ccmd->header.checksum	= 0x5a;
+	ccmd->header.tag	= ROM_MODE_CMD;
+
+	ccmd->mode.mode		= mode;
+
+	cctx->size = sizeof(*ccmd);
+
+	/*
+	 * Append the command to the last section.
+	 */
+	if (!sctx->cmd_head) {
+		sctx->cmd_head = cctx;
+		sctx->cmd_tail = cctx;
+	} else {
+		sctx->cmd_tail->cmd = cctx;
+		sctx->cmd_tail = cctx;
+	}
+
+	return 0;
+
+err:
+	free(cctx);
+	return ret;
+}
+
 static int sb_prefill_image_header(struct sb_image_ctx *ictx)
 {
 	struct sb_boot_image_header *hdr = &ictx->payload;
@@ -1133,6 +1240,8 @@ static int sb_parse_line(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
 		ret = sb_build_command_jump(ictx, cmd);
 	else if (!strcmp(tok, "CALL"))
 		ret = sb_build_command_call(ictx, cmd);
+	else if (!strcmp(tok, "MODE"))
+		ret = sb_build_command_mode(ictx, cmd);
 	else {
 		fprintf(stderr, "#%i ERR: Unsupported instruction '%s'!\n",
 			cmd->lineno, tok);