diff --git a/mmc.c b/mmc.c
index 50c9c9e..4e850ed 100644
--- a/mmc.c
+++ b/mmc.c
@@ -211,8 +211,9 @@ static struct Command commands[] = {
 	  NULL
 	},
 	{ do_ffu, -2,
-	  "ffu", "<image name> <device>\n"
-		"Run Field Firmware Update with <image name> on <device>.\n",
+	  "ffu", "[-c] <image name> <device>\n"
+		"Run Field Firmware Update with <image name> on <device>.\n"
+		"'-c' is required to select CMD25 for FFU.\n",
 	  NULL
 	},
 	{ 0, 0, 0, 0 }
diff --git a/mmc.h b/mmc.h
index 285c1f1..9bfdd8a 100644
--- a/mmc.h
+++ b/mmc.h
@@ -25,6 +25,7 @@
 /* From kernel linux/mmc/mmc.h */
 #define MMC_SWITCH		6	/* ac	[31:0] See below	R1b */
 #define MMC_SEND_EXT_CSD	8	/* adtc				R1  */
+#define MMC_STOP_TRANSMISSION    12   /* ac                      R1b */
 #define MMC_SEND_STATUS		13	/* ac   [31:16] RCA        R1  */
 #define R1_SWITCH_ERROR   (1 << 7)  /* sx, c */
 #define MMC_SWITCH_MODE_WRITE_BYTE	0x03	/* Set target to value */
diff --git a/mmc_cmds.c b/mmc_cmds.c
index 19a9da1..0da6ad8 100644
--- a/mmc_cmds.c
+++ b/mmc_cmds.c
@@ -2403,6 +2403,62 @@ int do_cache_dis(int nargs, char **argv)
 	return do_cache_ctrl(0, nargs, argv);
 }
 
+static void prepare_ffu_multi_cmd(struct mmc_ioc_multi_cmd *multi_cmd,
+				  int enable_cmd25, __u32 arg, __u32 sect_size,
+				  __u32 blocks, __u8 *buf)
+{
+	/* prepare multi_cmd to be sent */
+	multi_cmd->num_of_cmds = enable_cmd25 ? 4 : 3;
+
+	/* put device into ffu mode */
+	multi_cmd->cmds[0].opcode = MMC_SWITCH;
+	multi_cmd->cmds[0].arg = (MMC_SWITCH_MODE_WRITE_BYTE << 24) |
+			(EXT_CSD_MODE_CONFIG << 16) |
+			(EXT_CSD_FFU_MODE << 8) |
+			EXT_CSD_CMD_SET_NORMAL;
+	multi_cmd->cmds[0].flags = MMC_RSP_SPI_R1B | MMC_RSP_R1B | MMC_CMD_AC;
+	multi_cmd->cmds[0].write_flag = 1;
+
+	/* send image chunk */
+	multi_cmd->cmds[1].opcode = enable_cmd25 ? MMC_WRITE_MULTIPLE_BLOCK
+		: MMC_WRITE_BLOCK;
+	multi_cmd->cmds[1].blksz = sect_size;
+	multi_cmd->cmds[1].blocks = blocks;
+	multi_cmd->cmds[1].arg = arg;
+	multi_cmd->cmds[1].flags = MMC_RSP_SPI_R1 | MMC_RSP_R1 | MMC_CMD_ADTC;
+	multi_cmd->cmds[1].write_flag = 1;
+	mmc_ioc_cmd_set_data(multi_cmd->cmds[1], buf);
+
+	if (enable_cmd25) {
+		/* stop the device after transmission */
+		multi_cmd->cmds[2].opcode = MMC_STOP_TRANSMISSION;
+		multi_cmd->cmds[2].arg = 0;
+		multi_cmd->cmds[2].flags = MMC_RSP_SPI_R1B |
+			MMC_RSP_R1B | MMC_CMD_AC;
+		multi_cmd->cmds[2].write_flag = 1;
+
+		/* return device into normal mode */
+		multi_cmd->cmds[3].opcode = MMC_SWITCH;
+		multi_cmd->cmds[3].arg = (MMC_SWITCH_MODE_WRITE_BYTE << 24) |
+				(EXT_CSD_MODE_CONFIG << 16) |
+				(EXT_CSD_NORMAL_MODE << 8) |
+				EXT_CSD_CMD_SET_NORMAL;
+		multi_cmd->cmds[3].flags = MMC_RSP_SPI_R1B |
+			MMC_RSP_R1B | MMC_CMD_AC;
+		multi_cmd->cmds[3].write_flag = 1;
+	} else {
+		/* return device into normal mode */
+		multi_cmd->cmds[2].opcode = MMC_SWITCH;
+		multi_cmd->cmds[2].arg = (MMC_SWITCH_MODE_WRITE_BYTE << 24) |
+				(EXT_CSD_MODE_CONFIG << 16) |
+				(EXT_CSD_NORMAL_MODE << 8) |
+				EXT_CSD_CMD_SET_NORMAL;
+		multi_cmd->cmds[2].flags = MMC_RSP_SPI_R1B |
+			MMC_RSP_R1B | MMC_CMD_AC;
+		multi_cmd->cmds[2].write_flag = 1;
+	}
+}
+
 int do_ffu(int nargs, char **argv)
 {
 #ifndef MMC_IOC_MULTI_CMD
@@ -2414,37 +2470,41 @@ int do_ffu(int nargs, char **argv)
 	int sect_done = 0, retry = 3, ret = -EINVAL;
 	unsigned int sect_size;
 	__u8 ext_csd[512];
-	__u8 *buf;
+	__u8 *buf = NULL;
 	__u32 arg;
 	off_t fw_size;
 	ssize_t chunk_size;
 	char *device;
-	struct mmc_ioc_multi_cmd *multi_cmd;
+	struct mmc_ioc_multi_cmd *multi_cmd = NULL;
+	int c = 0, enable_cmd25 = 0;
+	__u32 blocks = 1;
 
-	if (nargs != 3) {
-		fprintf(stderr, "Usage: ffu <image name> </path/to/mmcblkX> \n");
+	if (nargs < 3) {
+		fprintf(stderr, "Usage: ffu [-c] <image name> </path/to/mmcblkX>\n");
 		exit(1);
 	}
 
-	device = argv[2];
+	device = argv[nargs - 1];
 	dev_fd = open(device, O_RDWR);
 	if (dev_fd < 0) {
 		perror("device open failed");
 		exit(1);
 	}
-	img_fd = open(argv[1], O_RDONLY);
+	img_fd = open(argv[nargs - 2], O_RDONLY);
 	if (img_fd < 0) {
 		perror("image open failed");
 		close(dev_fd);
 		exit(1);
 	}
 
-	buf = malloc(512);
-	multi_cmd = calloc(1, sizeof(struct mmc_ioc_multi_cmd) +
-				3 * sizeof(struct mmc_ioc_cmd));
-	if (!buf || !multi_cmd) {
-		perror("failed to allocate memory");
-		goto out;
+	while ((c = getopt(nargs, argv, "c")) != -1) {
+		switch (c) {
+		case 'c':
+			enable_cmd25 = 1;
+			break;
+		default:
+			break;
+		}
 	}
 
 	ret = read_extcsd(dev_fd, ext_csd);
@@ -2471,58 +2531,47 @@ int do_ffu(int nargs, char **argv)
 	}
 
 	fw_size = lseek(img_fd, 0, SEEK_END);
-
 	if (fw_size == 0) {
 		fprintf(stderr, "Firmware image is empty");
 		goto out;
 	}
 
+	/* allocate maximum required */
+	buf = malloc(fw_size);
+	multi_cmd = calloc(1, sizeof(struct mmc_ioc_multi_cmd) +
+				4 * sizeof(struct mmc_ioc_cmd));
+	if (!buf || !multi_cmd) {
+		perror("failed to allocate memory");
+		goto out;
+	}
+
 	sect_size = (ext_csd[EXT_CSD_DATA_SECTOR_SIZE] == 0) ? 512 : 4096;
 	if (fw_size % sect_size) {
 		fprintf(stderr, "Firmware data size (%jd) is not aligned!\n", (intmax_t)fw_size);
 		goto out;
 	}
 
+	/* calculate required fw blocks for CMD25 */
+	if (enable_cmd25)
+		blocks = fw_size / sect_size;
+
 	/* set CMD ARG */
 	arg = ext_csd[EXT_CSD_FFU_ARG_0] |
 		ext_csd[EXT_CSD_FFU_ARG_1] << 8 |
 		ext_csd[EXT_CSD_FFU_ARG_2] << 16 |
 		ext_csd[EXT_CSD_FFU_ARG_3] << 24;
 
-	/* prepare multi_cmd to be sent */
-	multi_cmd->num_of_cmds = 3;
-
-	/* put device into ffu mode */
-	multi_cmd->cmds[0].opcode = MMC_SWITCH;
-	multi_cmd->cmds[0].arg = (MMC_SWITCH_MODE_WRITE_BYTE << 24) |
-			(EXT_CSD_MODE_CONFIG << 16) |
-			(EXT_CSD_FFU_MODE << 8) |
-			EXT_CSD_CMD_SET_NORMAL;
-	multi_cmd->cmds[0].flags = MMC_RSP_SPI_R1B | MMC_RSP_R1B | MMC_CMD_AC;
-	multi_cmd->cmds[0].write_flag = 1;
-
-	/* send image chunk */
-	multi_cmd->cmds[1].opcode = MMC_WRITE_BLOCK;
-	multi_cmd->cmds[1].blksz = sect_size;
-	multi_cmd->cmds[1].blocks = 1;
-	multi_cmd->cmds[1].arg = arg;
-	multi_cmd->cmds[1].flags = MMC_RSP_SPI_R1 | MMC_RSP_R1 | MMC_CMD_ADTC;
-	multi_cmd->cmds[1].write_flag = 1;
-	mmc_ioc_cmd_set_data(multi_cmd->cmds[1], buf);
-
-	/* return device into normal mode */
-	multi_cmd->cmds[2].opcode = MMC_SWITCH;
-	multi_cmd->cmds[2].arg = (MMC_SWITCH_MODE_WRITE_BYTE << 24) |
-			(EXT_CSD_MODE_CONFIG << 16) |
-			(EXT_CSD_NORMAL_MODE << 8) |
-			EXT_CSD_CMD_SET_NORMAL;
-	multi_cmd->cmds[2].flags = MMC_RSP_SPI_R1B | MMC_RSP_R1B | MMC_CMD_AC;
-	multi_cmd->cmds[2].write_flag = 1;
+	/* prepare multi_cmd for FFU based on cmd to be used */
+	prepare_ffu_multi_cmd(multi_cmd, enable_cmd25,
+			      arg, sect_size, blocks, buf);
 
 do_retry:
 	/* read firmware chunk */
 	lseek(img_fd, 0, SEEK_SET);
-	chunk_size = read(img_fd, buf, 512);
+	if (enable_cmd25)
+		chunk_size = read(img_fd, buf, fw_size);
+	else
+		chunk_size = read(img_fd, buf, 512);
 
 	while (chunk_size > 0) {
 		/* send ioctl with multi-cmd */
@@ -2531,7 +2580,10 @@ do_retry:
 		if (ret) {
 			perror("Multi-cmd ioctl");
 			/* In case multi-cmd ioctl failed before exiting from ffu mode */
-			ioctl(dev_fd, MMC_IOC_CMD, &multi_cmd->cmds[2]);
+			if (enable_cmd25)
+				ioctl(dev_fd, MMC_IOC_CMD, &multi_cmd->cmds[3]);
+			else
+				ioctl(dev_fd, MMC_IOC_CMD, &multi_cmd->cmds[2]);
 			goto out;
 		}
 
