X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fflash%2Fmflash.c;h=1ad7290fa5850095b889823a5e51c03d8aec6c4c;hp=8913e1662aea1da556d4cd6588c945f843c5499d;hb=b61e454869c988e7fafc1c16982ccfec04415b51;hpb=fbcb57baf842a84898dcc1cbe9a8b56d2a0028e1

diff --git a/src/flash/mflash.c b/src/flash/mflash.c
index 8913e1662a..1ad7290fa5 100644
--- a/src/flash/mflash.c
+++ b/src/flash/mflash.c
@@ -12,9 +12,7 @@
  *   GNU General Public License for more details.                          *
  *                                                                         *
  *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  ***************************************************************************/
 
 #ifdef HAVE_CONFIG_H
@@ -22,37 +20,34 @@
 #endif
 
 #include "mflash.h"
-#include "time_support.h"
-#include "fileio.h"
-#include "log.h"
+#include <target/target.h>
+#include <helper/time_support.h>
+#include <helper/fileio.h>
+#include <helper/log.h>
 
+static int s3c2440_set_gpio_to_output(struct mflash_gpio_num gpio);
+static int s3c2440_set_gpio_output_val(struct mflash_gpio_num gpio, uint8_t val);
+static int pxa270_set_gpio_to_output(struct mflash_gpio_num gpio);
+static int pxa270_set_gpio_output_val(struct mflash_gpio_num gpio, uint8_t val);
 
-static int s3c2440_set_gpio_to_output (mflash_gpio_num_t gpio);
-static int s3c2440_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val);
-static int pxa270_set_gpio_to_output (mflash_gpio_num_t gpio);
-static int pxa270_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val);
+static struct mflash_bank *mflash_bank;
 
-static command_t *mflash_cmd;
-
-static mflash_bank_t *mflash_bank;
-
-static mflash_gpio_drv_t pxa270_gpio = {
+static struct mflash_gpio_drv pxa270_gpio = {
 	.name = "pxa270",
 	.set_gpio_to_output = pxa270_set_gpio_to_output,
 	.set_gpio_output_val = pxa270_set_gpio_output_val
 };
 
-static mflash_gpio_drv_t s3c2440_gpio = {
+static struct mflash_gpio_drv s3c2440_gpio = {
 	.name = "s3c2440",
 	.set_gpio_to_output = s3c2440_set_gpio_to_output,
 	.set_gpio_output_val = s3c2440_set_gpio_output_val
 };
 
-static mflash_gpio_drv_t *mflash_gpio[] =
-{
-		&pxa270_gpio,
-		&s3c2440_gpio,
-		NULL
+static struct mflash_gpio_drv *mflash_gpio[] = {
+	&pxa270_gpio,
+	&s3c2440_gpio,
+	NULL
 };
 
 #define PXA270_GAFR0_L 0x40E00054
@@ -64,10 +59,10 @@ static mflash_gpio_drv_t *mflash_gpio[] =
 #define PXA270_GPSR0 0x40E00018
 #define PXA270_GPCR0 0x40E00024
 
-static int pxa270_set_gpio_to_output (mflash_gpio_num_t gpio)
+static int pxa270_set_gpio_to_output(struct mflash_gpio_num gpio)
 {
-	u32 addr, value, mask;
-	target_t *target = mflash_bank->target;
+	uint32_t addr, value, mask;
+	struct target *target = mflash_bank->target;
 	int ret;
 
 	/* remove alternate function. */
@@ -75,14 +70,16 @@ static int pxa270_set_gpio_to_output (mflash_gpio_num_t gpio)
 
 	addr = PXA270_GAFR0_L + (gpio.num >> 4) * 4;
 
-	if ((ret = target_read_u32(target, addr, &value)) != ERROR_OK)
+	ret = target_read_u32(target, addr, &value);
+	if (ret != ERROR_OK)
 		return ret;
 
 	value &= ~mask;
 	if (addr == PXA270_GAFR3_U)
 		value &= ~PXA270_GAFR3_U_RESERVED_BITS;
 
-	if ((ret = target_write_u32(target, addr, value)) != ERROR_OK)
+	ret = target_write_u32(target, addr, value);
+	if (ret != ERROR_OK)
 		return ret;
 
 	/* set direction to output */
@@ -90,7 +87,8 @@ static int pxa270_set_gpio_to_output (mflash_gpio_num_t gpio)
 
 	addr = PXA270_GPDR0 + (gpio.num >> 5) * 4;
 
-	if ((ret = target_read_u32(target, addr, &value)) != ERROR_OK)
+	ret = target_read_u32(target, addr, &value);
+	if (ret != ERROR_OK)
 		return ret;
 
 	value |= mask;
@@ -101,21 +99,21 @@ static int pxa270_set_gpio_to_output (mflash_gpio_num_t gpio)
 	return ret;
 }
 
-static int pxa270_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val)
+static int pxa270_set_gpio_output_val(struct mflash_gpio_num gpio, uint8_t val)
 {
-	u32 addr, value, mask;
-	target_t *target = mflash_bank->target;
+	uint32_t addr, value, mask;
+	struct target *target = mflash_bank->target;
 	int ret;
 
 	mask = 0x1u << (gpio.num & 0x1F);
 
-	if (val) {
+	if (val)
 		addr = PXA270_GPSR0 + (gpio.num >> 5) * 4;
-	} else {
+	else
 		addr = PXA270_GPCR0 + (gpio.num >> 5) * 4;
-	}
 
-	if ((ret = target_read_u32(target, addr, &value)) != ERROR_OK)
+	ret = target_read_u32(target, addr, &value);
+	if (ret != ERROR_OK)
 		return ret;
 
 	value |= mask;
@@ -130,19 +128,19 @@ static int pxa270_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val)
 #define S3C2440_GPJCON 0x560000d0
 #define S3C2440_GPJDAT 0x560000d4
 
-static int s3c2440_set_gpio_to_output (mflash_gpio_num_t gpio)
+static int s3c2440_set_gpio_to_output(struct mflash_gpio_num gpio)
 {
-	u32 data, mask, gpio_con;
-	target_t *target = mflash_bank->target;
+	uint32_t data, mask, gpio_con;
+	struct target *target = mflash_bank->target;
 	int ret;
 
-	if (gpio.port[0] >= 'a' && gpio.port[0] <= 'h') {
+	if (gpio.port[0] >= 'a' && gpio.port[0] <= 'h')
 		gpio_con = S3C2440_GPACON + (gpio.port[0] - 'a') * 0x10;
-	} else if (gpio.port[0] == 'j') {
+	else if (gpio.port[0] == 'j')
 		gpio_con = S3C2440_GPJCON;
-	} else {
-		LOG_ERROR("invalid port %d%s", gpio.num, gpio.port);
-		return ERROR_INVALID_ARGUMENTS;
+	else {
+		LOG_ERROR("mflash: invalid port %d%s", gpio.num, gpio.port);
+		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
 	ret = target_read_u32(target, gpio_con, &data);
@@ -162,19 +160,19 @@ static int s3c2440_set_gpio_to_output (mflash_gpio_num_t gpio)
 	return ret;
 }
 
-static int s3c2440_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val)
+static int s3c2440_set_gpio_output_val(struct mflash_gpio_num gpio, uint8_t val)
 {
-	u32 data, mask, gpio_dat;
-	target_t *target = mflash_bank->target;
+	uint32_t data, mask, gpio_dat;
+	struct target *target = mflash_bank->target;
 	int ret;
 
-	if (gpio.port[0] >= 'a' && gpio.port[0] <= 'h') {
+	if (gpio.port[0] >= 'a' && gpio.port[0] <= 'h')
 		gpio_dat = S3C2440_GPADAT + (gpio.port[0] - 'a') * 0x10;
-	} else if (gpio.port[0] == 'j') {
+	else if (gpio.port[0] == 'j')
 		gpio_dat = S3C2440_GPJDAT;
-	} else {
-		LOG_ERROR("invalid port %d%s", gpio.num, gpio.port);
-		return ERROR_INVALID_ARGUMENTS;
+	else {
+		LOG_ERROR("mflash: invalid port %d%s", gpio.num, gpio.port);
+		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
 	ret = target_read_u32(target, gpio_dat, &data);
@@ -191,52 +189,47 @@ static int s3c2440_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val)
 	return ret;
 }
 
-static int mflash_rst(u8 level)
+static int mg_hdrst(uint8_t level)
 {
 	return mflash_bank->gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, level);
 }
 
-static int mflash_init_gpio (void)
+static int mg_init_gpio(void)
 {
-	mflash_gpio_drv_t *gpio_drv = mflash_bank->gpio_drv;
-
-	gpio_drv->set_gpio_to_output(mflash_bank->rst_pin);
-	gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, 1);
+	int ret;
+	struct mflash_gpio_drv *gpio_drv = mflash_bank->gpio_drv;
 
-	if (mflash_bank->wp_pin.num != -1) {
-		gpio_drv->set_gpio_to_output(mflash_bank->wp_pin);
-		gpio_drv->set_gpio_output_val(mflash_bank->wp_pin, 1);
-	}
+	ret = gpio_drv->set_gpio_to_output(mflash_bank->rst_pin);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (mflash_bank->dpd_pin.num != -1) {
-		gpio_drv->set_gpio_to_output(mflash_bank->dpd_pin);
-		gpio_drv->set_gpio_output_val(mflash_bank->dpd_pin, 1);
-	}
+	ret = gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, 1);
 
-	return ERROR_OK;
+	return ret;
 }
 
-static int mg_dsk_wait(mg_io_type_wait wait, u32 time)
+static int mg_dsk_wait(mg_io_type_wait wait_local, uint32_t time_var)
 {
-	u8 status, error;
-	target_t *target = mflash_bank->target;
-	u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
-	duration_t duration;
-	long long t=0;
+	uint8_t status, error;
+	struct target *target = mflash_bank->target;
+	uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
+	int ret;
+	long long t = 0;
 
-	duration_start_measure(&duration);
+	struct duration bench;
+	duration_start(&bench);
 
-	while (time) {
+	while (time_var) {
 
-		target_read_u8(target, mg_task_reg + MG_REG_STATUS, &status);
+		ret = target_read_u8(target, mg_task_reg + MG_REG_STATUS, &status);
+		if (ret != ERROR_OK)
+			return ret;
 
-		if (status & mg_io_rbit_status_busy)
-		{
-			if (wait == mg_io_wait_bsy)
+		if (status & mg_io_rbit_status_busy) {
+			if (wait_local == mg_io_wait_bsy)
 				return ERROR_OK;
 		} else {
-			switch(wait)
-			{
+			switch (wait_local) {
 				case mg_io_wait_not_bsy:
 					return ERROR_OK;
 				case mg_io_wait_rdy_noerr:
@@ -252,328 +245,385 @@ static int mg_dsk_wait(mg_io_type_wait wait, u32 time)
 			}
 
 			/* Now we check the error condition! */
-			if (status & mg_io_rbit_status_error)
-			{
-				target_read_u8(target, mg_task_reg + MG_REG_ERROR, &error);
+			if (status & mg_io_rbit_status_error) {
+				ret = target_read_u8(target, mg_task_reg + MG_REG_ERROR, &error);
+				if (ret != ERROR_OK)
+					return ret;
 
-				if (error & mg_io_rbit_err_bad_sect_num) {
-					LOG_ERROR("sector not found");
-					return ERROR_FAIL;
-				}
-				else if (error & (mg_io_rbit_err_bad_block | mg_io_rbit_err_uncorrectable)) {
-					LOG_ERROR("bad block");
-					return ERROR_FAIL;
-				} else {
-					LOG_ERROR("disk operation fail");
-					return ERROR_FAIL;
-				}
+				LOG_ERROR("mflash: io error 0x%02x", error);
+
+				return ERROR_MG_IO;
 			}
 
-			switch (wait)
-			{
+			switch (wait_local) {
 				case mg_io_wait_rdy:
 					if (status & mg_io_rbit_status_ready)
 						return ERROR_OK;
-
+					/* fallthrough */
 				case mg_io_wait_drq:
 					if (status & mg_io_rbit_status_data_req)
 						return ERROR_OK;
-
+					/* fallthrough */
 				default:
 					break;
 			}
 		}
 
-		duration_stop_measure(&duration, NULL);
-
-		t=duration.duration.tv_usec/1000;
-		t+=duration.duration.tv_sec*1000;
+		ret = duration_measure(&bench);
+		if (ERROR_OK == ret)
+			t = duration_elapsed(&bench) * 1000.0;
+		else
+			LOG_ERROR("mflash: duration measurement failed: %d", ret);
 
-		if (t > time)
+		if (t > time_var)
 			break;
 	}
 
-	LOG_ERROR("timeout occured");
-	return ERROR_FAIL;
+	LOG_ERROR("mflash: timeout occured");
+	return ERROR_MG_TIMEOUT;
 }
 
-static int mg_dsk_srst(u8 on)
+static int mg_dsk_srst(uint8_t on)
 {
-	target_t *target = mflash_bank->target;
-	u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
-	u8 value;
+	struct target *target = mflash_bank->target;
+	uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
+	uint8_t value;
 	int ret;
 
-	if ((ret = target_read_u8(target, mg_task_reg + MG_REG_DRV_CTRL, &value)) != ERROR_OK)
+	ret = target_read_u8(target, mg_task_reg + MG_REG_DRV_CTRL, &value);
+	if (ret != ERROR_OK)
 		return ret;
 
-	if(on) {
+	if (on)
 		value |= (mg_io_rbit_devc_srst);
-	} else {
+	else
 		value &= ~mg_io_rbit_devc_srst;
-	}
 
 	ret = target_write_u8(target, mg_task_reg + MG_REG_DRV_CTRL, value);
 	return ret;
 }
 
-static int mg_dsk_io_cmd(u32 sect_num, u32 cnt, u8 cmd)
+static int mg_dsk_io_cmd(uint32_t sect_num, uint32_t cnt, uint8_t cmd)
 {
-	target_t *target = mflash_bank->target;
-	u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
-	u8 value;
+	struct target *target = mflash_bank->target;
+	uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
+	uint8_t value;
+	int ret;
 
-	if (mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL) != ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL);
+	if (ret != ERROR_OK)
+		return ret;
 
-	value = mg_io_rval_dev_drv_master | mg_io_rval_dev_lba_mode |((sect_num >> 24) & 0xf);
+	value = mg_io_rval_dev_drv_master | mg_io_rval_dev_lba_mode | ((sect_num >> 24) & 0xf);
 
-	target_write_u8(target, mg_task_reg + MG_REG_DRV_HEAD, value);
-	target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, (u8)cnt);
-	target_write_u8(target, mg_task_reg + MG_REG_SECT_NUM, (u8)sect_num);
-	target_write_u8(target, mg_task_reg + MG_REG_CYL_LOW, (u8)(sect_num >> 8));
-	target_write_u8(target, mg_task_reg + MG_REG_CYL_HIGH, (u8)(sect_num >> 16));
+	ret = target_write_u8(target, mg_task_reg + MG_REG_DRV_HEAD, value);
+	ret |= target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, (uint8_t)cnt);
+	ret |= target_write_u8(target, mg_task_reg + MG_REG_SECT_NUM, (uint8_t)sect_num);
+	ret |= target_write_u8(target, mg_task_reg + MG_REG_CYL_LOW, (uint8_t)(sect_num >> 8));
+	ret |= target_write_u8(target, mg_task_reg + MG_REG_CYL_HIGH, (uint8_t)(sect_num >> 16));
 
-	target_write_u8(target, mg_task_reg + MG_REG_COMMAND, cmd);
+	if (ret != ERROR_OK)
+		return ret;
 
-	return ERROR_OK;
+	return target_write_u8(target, mg_task_reg + MG_REG_COMMAND, cmd);
 }
 
 static int mg_dsk_drv_info(void)
 {
-	target_t *target = mflash_bank->target;
-	u32 mg_buff = mflash_bank->base + MG_BUFFER_OFFSET;
-
-	if ( mg_dsk_io_cmd(0, 1, mg_io_cmd_identify) != ERROR_OK)
-		return ERROR_FAIL;
+	struct target *target = mflash_bank->target;
+	uint32_t mg_buff = mflash_bank->base + MG_BUFFER_OFFSET;
+	int ret;
 
-	if ( mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL) != ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_dsk_io_cmd(0, 1, mg_io_cmd_identify);
+	if (ret != ERROR_OK)
+		return ret;
 
-	LOG_INFO("read drive info.");
+	ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (! mflash_bank->drv_info)
-		mflash_bank->drv_info = malloc(sizeof(mg_drv_info_t));
+	LOG_INFO("mflash: read drive info");
 
-	target->type->read_memory(target, mg_buff, 2, sizeof(mg_io_type_drv_info) >> 1,
-			(u8 *)&mflash_bank->drv_info->drv_id);
+	if (!mflash_bank->drv_info)
+		mflash_bank->drv_info = malloc(sizeof(struct mg_drv_info));
 
-	mflash_bank->drv_info->tot_sects = (u32)(mflash_bank->drv_info->drv_id.total_user_addressable_sectors_hi << 16)
-									+ mflash_bank->drv_info->drv_id.total_user_addressable_sectors_lo;
+	ret = target_read_memory(target, mg_buff, 2,
+			sizeof(mg_io_type_drv_info) >> 1,
+			(uint8_t *)&mflash_bank->drv_info->drv_id);
+	if (ret != ERROR_OK)
+		return ret;
 
-	target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_read);
+	mflash_bank->drv_info->tot_sects =
+		(uint32_t)(mflash_bank->drv_info->drv_id.total_user_addressable_sectors_hi << 16)
+		+ mflash_bank->drv_info->drv_id.total_user_addressable_sectors_lo;
 
-	return ERROR_OK;
+	return target_write_u8(target,
+		mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND,
+		mg_io_cmd_confirm_read);
 }
 
-static int mg_mflash_probe(void)
+static int mg_mflash_rst(void)
 {
-	mflash_bank->proved = 0;
+	int ret;
 
-	mflash_init_gpio();
+	ret = mg_init_gpio();
+	if (ret != ERROR_OK)
+		return ret;
 
-	LOG_INFO("reset mflash");
+	ret = mg_hdrst(0);
+	if (ret != ERROR_OK)
+		return ret;
 
-	mflash_rst(0);
+	ret = mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_hdrst(1);
+	if (ret != ERROR_OK)
+		return ret;
 
-	mflash_rst(1);
+	ret = mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_dsk_srst(1);
+	if (ret != ERROR_OK)
+		return ret;
 
-	mg_dsk_srst(1);
+	ret = mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_dsk_srst(0);
+	if (ret != ERROR_OK)
+		return ret;
 
-	mg_dsk_srst(0);
+	ret = mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
-		return ERROR_FAIL;
+	LOG_INFO("mflash: reset ok");
 
-	if (mg_dsk_drv_info() != ERROR_OK)
-		return ERROR_FAIL;
+	return ERROR_OK;
+}
 
-	mflash_bank->proved = 1;
+static int mg_mflash_probe(void)
+{
+	int ret = mg_mflash_rst();
+	if (ret != ERROR_OK)
+		return ret;
 
-	return ERROR_OK;
+	return mg_dsk_drv_info();
 }
 
-static int mflash_probe_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+COMMAND_HANDLER(mg_probe_cmd)
 {
 	int ret;
 
 	ret = mg_mflash_probe();
 
 	if (ret == ERROR_OK) {
-		command_print(cmd_ctx, "mflash (total %u sectors) found at 0x%8.8x",
-				mflash_bank->drv_info->tot_sects, mflash_bank->base );
+		command_print(CMD_CTX,
+			"mflash (total %" PRIu32 " sectors) found at 0x%8.8" PRIx32 "",
+			mflash_bank->drv_info->tot_sects,
+			mflash_bank->base);
 	}
 
 	return ret;
 }
 
-static int mg_mflash_do_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
+static int mg_mflash_do_read_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt)
 {
-	u32 i, address;
+	uint32_t i, address;
 	int ret;
-	target_t *target = mflash_bank->target;
-	u8 *buff_ptr = buff;
-	duration_t duration;
+	struct target *target = mflash_bank->target;
+	uint8_t *buff_ptr = buff;
 
-	if ( mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_read) != ERROR_OK )
-		return ERROR_FAIL;
+	ret = mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_read);
+	if (ret != ERROR_OK)
+		return ret;
 
 	address = mflash_bank->base + MG_BUFFER_OFFSET;
 
-	duration_start_measure(&duration);
+	struct duration bench;
+	duration_start(&bench);
 
 	for (i = 0; i < sect_cnt; i++) {
-		mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
+		ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
+		if (ret != ERROR_OK)
+			return ret;
 
-		target->type->read_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
-		buff_ptr += MG_MFLASH_SECTOR_SIZE;
+		ret = target_read_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
+		if (ret != ERROR_OK)
+			return ret;
 
-		target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_read);
+		buff_ptr += MG_MFLASH_SECTOR_SIZE;
 
-		LOG_DEBUG("%u (0x%8.8x) sector read", sect_num + i, (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
+		ret = target_write_u8(target,
+				mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND,
+				mg_io_cmd_confirm_read);
+		if (ret != ERROR_OK)
+			return ret;
 
-		duration_stop_measure(&duration, NULL);
+		LOG_DEBUG("mflash: %" PRIu32 " (0x%8.8" PRIx32 ") sector read", sect_num + i,
+			(sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 
-		if ((duration.duration.tv_sec * 1000 + duration.duration.tv_usec / 1000) > 3000) {
-			LOG_INFO("read %u'th sectors", sect_num + i);
-			duration_start_measure(&duration);
+		ret = duration_measure(&bench);
+		if ((ERROR_OK == ret) && (duration_elapsed(&bench) > 3)) {
+			LOG_INFO("mflash: read %" PRIu32 "'th sectors", sect_num + i);
+			duration_start(&bench);
 		}
 	}
 
-	ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
-
-	return ret;
+	return mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
 }
 
-static int mg_mflash_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
+static int mg_mflash_read_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt)
 {
-	u32 quotient, residue, i;
-	u8 *buff_ptr = buff;
+	uint32_t quotient, residue, i;
+	uint8_t *buff_ptr = buff;
+	int ret = ERROR_OK;
 
 	quotient = sect_cnt >> 8;
 	residue = sect_cnt % 256;
 
 	for (i = 0; i < quotient; i++) {
-		LOG_DEBUG("sect num : %u buff : 0x%0lx", sect_num, 
-			(unsigned long)buff_ptr);
-		mg_mflash_do_read_sects(buff_ptr, sect_num, 256);
+		LOG_DEBUG("mflash: sect num : %" PRIu32 " buff : %p",
+			sect_num, buff_ptr);
+		ret = mg_mflash_do_read_sects(buff_ptr, sect_num, 256);
+		if (ret != ERROR_OK)
+			return ret;
+
 		sect_num += 256;
 		buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 	}
 
 	if (residue) {
-		LOG_DEBUG("sect num : %u buff : %0lx", sect_num, 
-			(unsigned long)buff_ptr);
-		mg_mflash_do_read_sects(buff_ptr, sect_num, residue);
+		LOG_DEBUG("mflash: sect num : %" PRIx32 " buff : %p",
+			sect_num, buff_ptr);
+		return mg_mflash_do_read_sects(buff_ptr, sect_num, residue);
 	}
 
-	return ERROR_OK;
+	return ret;
 }
 
-static int mg_mflash_do_write_sects(void *buff, u32 sect_num, u32 sect_cnt)
+static int mg_mflash_do_write_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt,
+	uint8_t cmd)
 {
-	u32 i, address;
+	uint32_t i, address;
 	int ret;
-	target_t *target = mflash_bank->target;
-	u8 *buff_ptr = buff;
-	duration_t duration;
+	struct target *target = mflash_bank->target;
+	uint8_t *buff_ptr = buff;
 
-	if ( mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_write) != ERROR_OK ) {
-		LOG_ERROR("mg_io_cmd_write fail");
-		return ERROR_FAIL;
-	}
+	ret = mg_dsk_io_cmd(sect_num, sect_cnt, cmd);
+	if (ret != ERROR_OK)
+		return ret;
 
 	address = mflash_bank->base + MG_BUFFER_OFFSET;
 
-	duration_start_measure(&duration);
+	struct duration bench;
+	duration_start(&bench);
 
 	for (i = 0; i < sect_cnt; i++) {
 		ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 		if (ret != ERROR_OK)
-			LOG_ERROR("mg_io_wait_drq time out");
+			return ret;
 
-		ret = target->type->write_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
+		ret = target_write_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
 		if (ret != ERROR_OK)
-			LOG_ERROR("mem write error");
+			return ret;
+
 		buff_ptr += MG_MFLASH_SECTOR_SIZE;
 
-		ret = target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_write);
+		ret = target_write_u8(target,
+				mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND,
+				mg_io_cmd_confirm_write);
 		if (ret != ERROR_OK)
-			LOG_ERROR("mg_io_cmd_confirm_write error");
-
-		LOG_DEBUG("%u (0x%8.8x) sector write", sect_num + i, (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
+			return ret;
 
-		duration_stop_measure(&duration, NULL);
+		LOG_DEBUG("mflash: %" PRIu32 " (0x%8.8" PRIx32 ") sector write", sect_num + i,
+			(sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 
-		if ((duration.duration.tv_sec * 1000 + duration.duration.tv_usec / 1000) > 3000) {
-			LOG_INFO("wrote %u'th sectors", sect_num + i);
-			duration_start_measure(&duration);
+		ret = duration_measure(&bench);
+		if ((ERROR_OK == ret) && (duration_elapsed(&bench) > 3)) {
+			LOG_INFO("mflash: wrote %" PRIu32 "'th sectors", sect_num + i);
+			duration_start(&bench);
 		}
 	}
 
-	ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
+	if (cmd == mg_io_cmd_write)
+		ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
+	else
+		ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_LONG);
 
 	return ret;
 }
 
-static int mg_mflash_write_sects(void *buff, u32 sect_num, u32 sect_cnt)
+static int mg_mflash_write_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt)
 {
-	u32 quotient, residue, i;
-	u8 *buff_ptr = buff;
+	uint32_t quotient, residue, i;
+	uint8_t *buff_ptr = buff;
+	int ret = ERROR_OK;
 
 	quotient = sect_cnt >> 8;
 	residue = sect_cnt % 256;
 
 	for (i = 0; i < quotient; i++) {
-		LOG_DEBUG("sect num : %u buff : %0lx", sect_num, 
-			(unsigned long)buff_ptr);
-		mg_mflash_do_write_sects(buff_ptr, sect_num, 256);
+		LOG_DEBUG("mflash: sect num : %" PRIu32 "buff : %p", sect_num,
+			buff_ptr);
+		ret = mg_mflash_do_write_sects(buff_ptr, sect_num, 256, mg_io_cmd_write);
+		if (ret != ERROR_OK)
+			return ret;
+
 		sect_num += 256;
 		buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 	}
 
 	if (residue) {
-		LOG_DEBUG("sect num : %u buff : %0lx", sect_num, 
-			(unsigned long)buff_ptr);
-		mg_mflash_do_write_sects(buff_ptr, sect_num, residue);
+		LOG_DEBUG("mflash: sect num : %" PRIu32 " buff : %p", sect_num,
+			buff_ptr);
+		return mg_mflash_do_write_sects(buff_ptr, sect_num, residue, mg_io_cmd_write);
 	}
 
-	return ERROR_OK;
+	return ret;
 }
 
-static int mg_mflash_read (u32 addr, u8 *buff, u32 len)
+static int mg_mflash_read(uint32_t addr, uint8_t *buff, uint32_t len)
 {
-	u8 *sect_buff, *buff_ptr = buff;
-	u32 cur_addr, next_sec_addr, end_addr, cnt, sect_num;
+	uint8_t *buff_ptr = buff;
+	uint8_t sect_buff[MG_MFLASH_SECTOR_SIZE];
+	uint32_t cur_addr, next_sec_addr, end_addr, cnt, sect_num;
+	int ret = ERROR_OK;
 
 	cnt = 0;
 	cur_addr = addr;
 	end_addr = addr + len;
 
-	sect_buff = malloc(MG_MFLASH_SECTOR_SIZE);
-
 	if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 
 		next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 		sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
-		mg_mflash_read_sects(sect_buff, sect_num, 1);
+		ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
+		if (ret != ERROR_OK)
+			return ret;
 
 		if (end_addr < next_sec_addr) {
-			memcpy(buff_ptr, sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), end_addr - cur_addr);
-			LOG_DEBUG("copies %u byte from sector offset 0x%8.8x", end_addr - cur_addr, cur_addr);
+			memcpy(buff_ptr,
+				sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
+				end_addr - cur_addr);
+			LOG_DEBUG(
+				"mflash: copies %" PRIu32 " byte from sector offset 0x%8.8" PRIx32 "",
+				end_addr - cur_addr,
+				cur_addr);
 			cur_addr = end_addr;
 		} else {
-			memcpy(buff_ptr, sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), next_sec_addr - cur_addr);
-			LOG_DEBUG("copies %u byte from sector offset 0x%8.8x", next_sec_addr - cur_addr, cur_addr);
+			memcpy(buff_ptr,
+				sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
+				next_sec_addr - cur_addr);
+			LOG_DEBUG(
+				"mflash: copies %" PRIu32 " byte from sector offset 0x%8.8" PRIx32 "",
+				next_sec_addr - cur_addr,
+				cur_addr);
 			buff_ptr += (next_sec_addr - cur_addr);
 			cur_addr = next_sec_addr;
 		}
@@ -589,8 +639,11 @@ static int mg_mflash_read (u32 addr, u8 *buff, u32 len)
 			next_sec_addr += MG_MFLASH_SECTOR_SIZE;
 		}
 
-		if (cnt)
-			mg_mflash_read_sects(buff_ptr, sect_num, cnt);
+		if (cnt) {
+			ret = mg_mflash_read_sects(buff_ptr, sect_num, cnt);
+			if (ret != ERROR_OK)
+				return ret;
+		}
 
 		buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 		cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
@@ -598,47 +651,61 @@ static int mg_mflash_read (u32 addr, u8 *buff, u32 len)
 		if (cur_addr < end_addr) {
 
 			sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
-			mg_mflash_read_sects(sect_buff, sect_num, 1);
-			memcpy(buff_ptr, sect_buff, end_addr - cur_addr);
-			LOG_DEBUG("copies %u byte", end_addr - cur_addr);
+			ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
+			if (ret != ERROR_OK)
+				return ret;
 
+			memcpy(buff_ptr, sect_buff, end_addr - cur_addr);
+			LOG_DEBUG("mflash: copies %u byte", (unsigned)(end_addr - cur_addr));
 		}
 	}
 
-	free(sect_buff);
-
-	return ERROR_OK;
+	return ret;
 }
 
-static int mg_mflash_write(u32 addr, u8 *buff, u32 len)
+static int mg_mflash_write(uint32_t addr, uint8_t *buff, uint32_t len)
 {
-	u8 *sect_buff, *buff_ptr = buff;
-	u32 cur_addr, next_sec_addr, end_addr, cnt, sect_num;
+	uint8_t *buff_ptr = buff;
+	uint8_t sect_buff[MG_MFLASH_SECTOR_SIZE];
+	uint32_t cur_addr, next_sec_addr, end_addr, cnt, sect_num;
+	int ret = ERROR_OK;
 
 	cnt = 0;
 	cur_addr = addr;
 	end_addr = addr + len;
 
-	sect_buff = malloc(MG_MFLASH_SECTOR_SIZE);
-
 	if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 
 		next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 		sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
-		mg_mflash_read_sects(sect_buff, sect_num, 1);
+		ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
+		if (ret != ERROR_OK)
+			return ret;
 
 		if (end_addr < next_sec_addr) {
-			memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), buff_ptr, end_addr - cur_addr);
-			LOG_DEBUG("copies %u byte to sector offset 0x%8.8x", end_addr - cur_addr, cur_addr);
+			memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
+				buff_ptr,
+				end_addr - cur_addr);
+			LOG_DEBUG(
+				"mflash: copies %" PRIu32 " byte to sector offset 0x%8.8" PRIx32 "",
+				end_addr - cur_addr,
+				cur_addr);
 			cur_addr = end_addr;
 		} else {
-			memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), buff_ptr, next_sec_addr - cur_addr);
-			LOG_DEBUG("copies %u byte to sector offset 0x%8.8x", next_sec_addr - cur_addr, cur_addr);
+			memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
+				buff_ptr,
+				next_sec_addr - cur_addr);
+			LOG_DEBUG(
+				"mflash: copies %" PRIu32 " byte to sector offset 0x%8.8" PRIx32 "",
+				next_sec_addr - cur_addr,
+				cur_addr);
 			buff_ptr += (next_sec_addr - cur_addr);
 			cur_addr = next_sec_addr;
 		}
 
-		mg_mflash_write_sects(sect_buff, sect_num, 1);
+		ret = mg_mflash_write_sects(sect_buff, sect_num, 1);
+		if (ret != ERROR_OK)
+			return ret;
 	}
 
 	if (cur_addr < end_addr) {
@@ -651,8 +718,11 @@ static int mg_mflash_write(u32 addr, u8 *buff, u32 len)
 			next_sec_addr += MG_MFLASH_SECTOR_SIZE;
 		}
 
-		if (cnt)
-			mg_mflash_write_sects(buff_ptr, sect_num, cnt);
+		if (cnt) {
+			ret = mg_mflash_write_sects(buff_ptr, sect_num, cnt);
+			if (ret != ERROR_OK)
+				return ret;
+		}
 
 		buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 		cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
@@ -660,181 +730,721 @@ static int mg_mflash_write(u32 addr, u8 *buff, u32 len)
 		if (cur_addr < end_addr) {
 
 			sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
-			mg_mflash_read_sects(sect_buff, sect_num, 1);
+			ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
+			if (ret != ERROR_OK)
+				return ret;
+
 			memcpy(sect_buff, buff_ptr, end_addr - cur_addr);
-			LOG_DEBUG("copies %u byte", end_addr - cur_addr);
-			mg_mflash_write_sects(sect_buff, sect_num, 1);
+			LOG_DEBUG("mflash: copies %" PRIu32 " byte", end_addr - cur_addr);
+			ret = mg_mflash_write_sects(sect_buff, sect_num, 1);
 		}
 	}
 
-	free(sect_buff);
-
-	return ERROR_OK;
+	return ret;
 }
 
-static int mflash_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+COMMAND_HANDLER(mg_write_cmd)
 {
-	u32 address, buf_cnt;
-	u8 *buffer;
-	/* TODO : multi-bank support, large file support */
-	fileio_t fileio;
-	duration_t duration;
-	char *duration_text;
+	uint32_t address, cnt, res, i;
+	uint8_t *buffer;
+	struct fileio *fileio;
 	int ret;
 
-	if (argc != 3) {
+	if (CMD_ARGC != 3)
 		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], address);
+
+	ret = fileio_open(&fileio, CMD_ARGV[1], FILEIO_READ, FILEIO_BINARY);
+	if (ret != ERROR_OK)
+		return ret;
+
+	size_t filesize;
+	buffer = malloc(MG_FILEIO_CHUNK);
+	if (!buffer) {
+		fileio_close(fileio);
+		return ERROR_FAIL;
 	}
+	int retval = fileio_size(fileio, &filesize);
+	if (retval != ERROR_OK) {
+		fileio_close(fileio);
+		free(buffer);
+		return retval;
+	}
+
+	cnt = filesize / MG_FILEIO_CHUNK;
+	res = filesize % MG_FILEIO_CHUNK;
 
-	address = strtoul(args[2], NULL, 0);
+	struct duration bench;
+	duration_start(&bench);
 
-	if (! mflash_bank->proved ) {
-		mg_mflash_probe();
+	size_t buf_cnt;
+	for (i = 0; i < cnt; i++) {
+		ret = fileio_read(fileio, MG_FILEIO_CHUNK, buffer, &buf_cnt);
+		if (ret != ERROR_OK)
+			goto mg_write_cmd_err;
+		ret = mg_mflash_write(address, buffer, MG_FILEIO_CHUNK);
+		if (ret != ERROR_OK)
+			goto mg_write_cmd_err;
+		address += MG_FILEIO_CHUNK;
 	}
 
-	if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK) {
-		return ERROR_FAIL;
+	if (res) {
+		ret = fileio_read(fileio, res, buffer, &buf_cnt);
+		if (ret != ERROR_OK)
+			goto mg_write_cmd_err;
+		ret = mg_mflash_write(address, buffer, res);
+		if (ret != ERROR_OK)
+			goto mg_write_cmd_err;
+	}
+
+	if (duration_measure(&bench) == ERROR_OK) {
+		command_print(CMD_CTX, "wrote %zu bytes from file %s "
+			"in %fs (%0.3f kB/s)", filesize, CMD_ARGV[1],
+			duration_elapsed(&bench), duration_kbps(&bench, filesize));
 	}
 
-	buffer = malloc(fileio.size);
+	free(buffer);
+	fileio_close(fileio);
 
-	if (fileio_read(&fileio, fileio.size, buffer, &buf_cnt) != ERROR_OK)
-	{
-		free(buffer);
-		fileio_close(&fileio);
+	return ERROR_OK;
+
+mg_write_cmd_err:
+	free(buffer);
+	fileio_close(fileio);
+
+	return ret;
+}
+
+COMMAND_HANDLER(mg_dump_cmd)
+{
+	uint32_t address, size, cnt, res, i;
+	uint8_t *buffer;
+	struct fileio *fileio;
+	int ret;
+
+	if (CMD_ARGC != 4)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], address);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[3], size);
+
+	ret = fileio_open(&fileio, CMD_ARGV[1], FILEIO_WRITE, FILEIO_BINARY);
+	if (ret != ERROR_OK)
+		return ret;
+
+	buffer = malloc(MG_FILEIO_CHUNK);
+	if (!buffer) {
+		fileio_close(fileio);
 		return ERROR_FAIL;
 	}
 
-	duration_start_measure(&duration);
+	cnt = size / MG_FILEIO_CHUNK;
+	res = size % MG_FILEIO_CHUNK;
 
-	ret = mg_mflash_write(address, buffer, (u32)fileio.size);
+	struct duration bench;
+	duration_start(&bench);
 
-	duration_stop_measure(&duration, &duration_text);
+	size_t size_written;
+	for (i = 0; i < cnt; i++) {
+		ret = mg_mflash_read(address, buffer, MG_FILEIO_CHUNK);
+		if (ret != ERROR_OK)
+			goto mg_dump_cmd_err;
+		ret = fileio_write(fileio, MG_FILEIO_CHUNK, buffer, &size_written);
+		if (ret != ERROR_OK)
+			goto mg_dump_cmd_err;
+		address += MG_FILEIO_CHUNK;
+	}
 
-	command_print(cmd_ctx, "wrote %lli byte from file %s in %s (%f kB/s)",
-		fileio.size, args[1], duration_text,
-		(float)fileio.size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
+	if (res) {
+		ret = mg_mflash_read(address, buffer, res);
+		if (ret != ERROR_OK)
+			goto mg_dump_cmd_err;
+		ret = fileio_write(fileio, res, buffer, &size_written);
+		if (ret != ERROR_OK)
+			goto mg_dump_cmd_err;
+	}
+
+	if (duration_measure(&bench) == ERROR_OK) {
+		command_print(CMD_CTX, "dump image (address 0x%8.8" PRIx32 " "
+			"size %" PRIu32 ") to file %s in %fs (%0.3f kB/s)",
+			address, size, CMD_ARGV[1],
+			duration_elapsed(&bench), duration_kbps(&bench, size));
+	}
 
-	free(duration_text);
+	free(buffer);
+	fileio_close(fileio);
 
-	fileio_close(&fileio);
+	return ERROR_OK;
 
+mg_dump_cmd_err:
 	free(buffer);
+	fileio_close(fileio);
+
+	return ret;
+}
+
+static int mg_set_feature(mg_feature_id feature, mg_feature_val config)
+{
+	struct target *target = mflash_bank->target;
+	uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
+	int ret;
+
+	ret = mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL);
+	if (ret != ERROR_OK)
+		return ret;
+
+	ret = target_write_u8(target, mg_task_reg + MG_REG_FEATURE, feature);
+	ret |= target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, config);
+	ret |= target_write_u8(target, mg_task_reg + MG_REG_COMMAND,
+			mg_io_cmd_set_feature);
+
+	return ret;
+}
+
+static int mg_is_valid_pll(double XIN, int N, double CLK_OUT, int NO)
+{
+	double v1 = XIN / N;
+	double v2 = CLK_OUT * NO;
+
+	if (v1 < 1000000 || v1 > 15000000 || v2 < 100000000 || v2 > 500000000)
+		return ERROR_MG_INVALID_PLL;
 
 	return ERROR_OK;
 }
 
-static int mflash_dump_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int mg_pll_get_M(unsigned short feedback_div)
 {
-	u32 address, size_written, size;
-	u8 *buffer;
-	/* TODO : multi-bank support */
-	fileio_t fileio;
-	duration_t duration;
-	char *duration_text;
+	int i, M;
 
-	if (argc != 4) {
-		return ERROR_COMMAND_SYNTAX_ERROR;
+	for (i = 1, M = 0; i < 512; i <<= 1, feedback_div >>= 1)
+		M += (feedback_div & 1) * i;
+
+	return M + 2;
+}
+
+static int mg_pll_get_N(unsigned char input_div)
+{
+	int i, N;
+
+	for (i = 1, N = 0; i < 32; i <<= 1, input_div >>= 1)
+		N += (input_div & 1) * i;
+
+	return N + 2;
+}
+
+static int mg_pll_get_NO(unsigned char output_div)
+{
+	int i, NO;
+
+	for (i = 0, NO = 1; i < 2; ++i, output_div >>= 1)
+		if (output_div & 1)
+			NO = NO << 1;
+
+	return NO;
+}
+
+static double mg_do_calc_pll(double XIN, mg_pll_t *p_pll_val, int is_approximate)
+{
+	unsigned short i;
+	unsigned char j, k;
+	int M, N, NO;
+	double CLK_OUT;
+	double DIV = 1;
+	double ROUND = 0;
+
+	if (is_approximate) {
+		DIV   = 1000000;
+		ROUND = 500000;
 	}
 
-	address = strtoul(args[2], NULL, 0);
-	size = strtoul(args[3], NULL, 0);
+	for (i = 0; i < MG_PLL_MAX_FEEDBACKDIV_VAL; ++i) {
+		M  = mg_pll_get_M(i);
+
+		for (j = 0; j < MG_PLL_MAX_INPUTDIV_VAL; ++j) {
+			N  = mg_pll_get_N(j);
+
+			for (k = 0; k < MG_PLL_MAX_OUTPUTDIV_VAL; ++k) {
+				NO = mg_pll_get_NO(k);
 
-	if (! mflash_bank->proved ) {
-			mg_mflash_probe();
+				CLK_OUT = XIN * ((double)M / N) / NO;
+
+				if ((int)((CLK_OUT + ROUND) / DIV)
+				    == (int)(MG_PLL_CLK_OUT / DIV)) {
+					if (mg_is_valid_pll(XIN, N, CLK_OUT, NO) == ERROR_OK) {
+						p_pll_val->lock_cyc =
+							(int)(XIN * MG_PLL_STD_LOCKCYCLE /
+							      MG_PLL_STD_INPUTCLK);
+						p_pll_val->feedback_div = i;
+						p_pll_val->input_div = j;
+						p_pll_val->output_div = k;
+
+						return CLK_OUT;
+					}
+				}
+			}
+		}
 	}
 
-	if (fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK) {
-		return ERROR_FAIL;
+	return 0;
+}
+
+static double mg_calc_pll(double XIN, mg_pll_t *p_pll_val)
+{
+	double CLK_OUT;
+
+	CLK_OUT = mg_do_calc_pll(XIN, p_pll_val, 0);
+
+	if (!CLK_OUT)
+		return mg_do_calc_pll(XIN, p_pll_val, 1);
+	else
+		return CLK_OUT;
+}
+
+static int mg_verify_interface(void)
+{
+	uint16_t buff[MG_MFLASH_SECTOR_SIZE >> 1];
+	uint16_t i, j;
+	uint32_t address = mflash_bank->base + MG_BUFFER_OFFSET;
+	struct target *target = mflash_bank->target;
+	int ret;
+
+	for (j = 0; j < 10; j++) {
+		for (i = 0; i < MG_MFLASH_SECTOR_SIZE >> 1; i++)
+			buff[i] = i;
+
+		ret = target_write_memory(target, address, 2,
+				MG_MFLASH_SECTOR_SIZE / 2, (uint8_t *)buff);
+		if (ret != ERROR_OK)
+			return ret;
+
+		memset(buff, 0xff, MG_MFLASH_SECTOR_SIZE);
+
+		ret = target_read_memory(target, address, 2,
+				MG_MFLASH_SECTOR_SIZE / 2, (uint8_t *)buff);
+		if (ret != ERROR_OK)
+			return ret;
+
+		for (i = 0; i < MG_MFLASH_SECTOR_SIZE >> 1; i++) {
+			if (buff[i] != i) {
+				LOG_ERROR("mflash: verify interface fail");
+				return ERROR_MG_INTERFACE;
+			}
+		}
 	}
 
-	buffer = malloc(size);
+	LOG_INFO("mflash: verify interface ok");
+	return ret;
+}
 
-	duration_start_measure(&duration);
+static const char g_strSEG_SerialNum[20] = {
+	'G', 'm', 'n', 'i', '-', 'e', 'e', 'S', 'g', 'a', 'e', 'l',
+	0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20
+};
 
-	mg_mflash_read(address, buffer, size);
+static const char g_strSEG_FWRev[8] = {
+	'F', 'X', 'L', 'T', '2', 'v', '0', '.'
+};
 
-	duration_stop_measure(&duration, &duration_text);
+static const char g_strSEG_ModelNum[40] = {
+	'F', 'X', 'A', 'L', 'H', 'S', '2', 0x20, '0', '0', 's', '7',
+	0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20
+};
 
-	fileio_write(&fileio, size, buffer, &size_written);
+static void mg_gen_ataid(mg_io_type_drv_info *pSegIdDrvInfo)
+{
+	/* b15 is ATA device(0)	, b7 is Removable Media Device */
+	pSegIdDrvInfo->general_configuration            = 0x045A;
+	/* 128MB :   Cylinder=> 977 , Heads=> 8 ,  Sectors=> 32
+	 * 256MB :   Cylinder=> 980 , Heads=> 16 , Sectors=> 32
+	 * 384MB :   Cylinder=> 745 , Heads=> 16 , Sectors=> 63
+	 */
+	pSegIdDrvInfo->number_of_cylinders              = 0x02E9;
+	pSegIdDrvInfo->reserved1                        = 0x0;
+	pSegIdDrvInfo->number_of_heads                  = 0x10;
+	pSegIdDrvInfo->unformatted_bytes_per_track      = 0x0;
+	pSegIdDrvInfo->unformatted_bytes_per_sector     = 0x0;
+	pSegIdDrvInfo->sectors_per_track                = 0x3F;
+	pSegIdDrvInfo->vendor_unique1[0]                = 0x000B;
+	pSegIdDrvInfo->vendor_unique1[1]                = 0x7570;
+	pSegIdDrvInfo->vendor_unique1[2]                = 0x8888;
+
+	memcpy(pSegIdDrvInfo->serial_number, g_strSEG_SerialNum, 20);
+	/* 0x2 : dual buffer */
+	pSegIdDrvInfo->buffer_type                      = 0x2;
+	/* buffer size : 2KB */
+	pSegIdDrvInfo->buffer_sector_size               = 0x800;
+	pSegIdDrvInfo->number_of_ecc_bytes              = 0;
+
+	memcpy(pSegIdDrvInfo->firmware_revision, g_strSEG_FWRev, 8);
+
+	memcpy(pSegIdDrvInfo->model_number, g_strSEG_ModelNum, 40);
+
+	pSegIdDrvInfo->maximum_block_transfer           = 0x4;
+	pSegIdDrvInfo->vendor_unique2                   = 0x0;
+	pSegIdDrvInfo->dword_io                         = 0x00;
+	/* b11 : IORDY support(PIO Mode 4), b10 : Disable/Enbale IORDY
+	 * b9  : LBA support, b8 : DMA mode support
+	 */
+	pSegIdDrvInfo->capabilities                     = 0x1 << 9;
+
+	pSegIdDrvInfo->reserved2                        = 0x4000;
+	pSegIdDrvInfo->vendor_unique3                   = 0x00;
+	/* PIOMode-2 support */
+	pSegIdDrvInfo->pio_cycle_timing_mode            = 0x02;
+	pSegIdDrvInfo->vendor_unique4                   = 0x00;
+	/* MultiWord-2 support */
+	pSegIdDrvInfo->dma_cycle_timing_mode            = 0x00;
+	/* b1 : word64~70 is valid
+	 * b0 : word54~58 are valid and reflect the current numofcyls,heads,sectors
+	 * b2 : If device supports Ultra DMA , set to one to vaildate word88
+	 */
+	pSegIdDrvInfo->translation_fields_valid         = (0x1 << 1) | (0x1 << 0);
+	pSegIdDrvInfo->number_of_current_cylinders      = 0x02E9;
+	pSegIdDrvInfo->number_of_current_heads          = 0x10;
+	pSegIdDrvInfo->current_sectors_per_track        = 0x3F;
+	pSegIdDrvInfo->current_sector_capacity_lo       = 0x7570;
+	pSegIdDrvInfo->current_sector_capacity_hi       = 0x000B;
+
+	pSegIdDrvInfo->multi_sector_count                       = 0x04;
+	/* b8 : Multiple secotr setting valid , b[7:0] num of secotrs per block */
+	pSegIdDrvInfo->multi_sector_setting_valid               = 0x01;
+	pSegIdDrvInfo->total_user_addressable_sectors_lo        = 0x7570;
+	pSegIdDrvInfo->total_user_addressable_sectors_hi        = 0x000B;
+	pSegIdDrvInfo->single_dma_modes_supported               = 0x00;
+	pSegIdDrvInfo->single_dma_transfer_active               = 0x00;
+	/* b2 :Multi-word DMA mode 2, b1 : Multi-word DMA mode 1 */
+	pSegIdDrvInfo->multi_dma_modes_supported                = (0x1 << 0);
+	/* b2 :Multi-word DMA mode 2, b1 : Multi-word DMA mode 1 */
+	pSegIdDrvInfo->multi_dma_transfer_active                = (0x1 << 0);
+	/* b0 : PIO Mode-3 support, b1 : PIO Mode-4 support */
+	pSegIdDrvInfo->adv_pio_mode                             = 0x00;
+	/* 480(0x1E0)nsec for Multi-word DMA mode0
+	 * 150(0x96) nsec for Multi-word DMA mode1
+	 * 120(0x78) nsec for Multi-word DMA mode2
+	 */
+	pSegIdDrvInfo->min_dma_cyc                      = 0x1E0;
+	pSegIdDrvInfo->recommend_dma_cyc                = 0x1E0;
+	pSegIdDrvInfo->min_pio_cyc_no_iordy             = 0x1E0;
+	pSegIdDrvInfo->min_pio_cyc_with_iordy           = 0x1E0;
+	memset(pSegIdDrvInfo->reserved3, 0x00, 22);
+	/* b7 : ATA/ATAPI-7 ,b6 : ATA/ATAPI-6 ,b5 : ATA/ATAPI-5,b4 : ATA/ATAPI-4 */
+	pSegIdDrvInfo->major_ver_num                    = 0x7E;
+	/* 0x1C : ATA/ATAPI-6 T13 1532D revision1 */
+	pSegIdDrvInfo->minor_ver_num                    = 0x19;
+	/* NOP/READ BUFFER/WRITE BUFFER/Power management feature set support */
+	pSegIdDrvInfo->feature_cmd_set_suprt0           = 0x7068;
+	/* Features/command set is valid/Advanced Pwr management/CFA feature set
+	 * not support
+	 */
+	pSegIdDrvInfo->feature_cmd_set_suprt1           = 0x400C;
+	pSegIdDrvInfo->feature_cmd_set_suprt2           = 0x4000;
+	/* READ/WRITE BUFFER/PWR Management enable */
+	pSegIdDrvInfo->feature_cmd_set_en0              = 0x7000;
+	/* CFA feature is disabled / Advancde power management disable */
+	pSegIdDrvInfo->feature_cmd_set_en1              = 0x0;
+	pSegIdDrvInfo->feature_cmd_set_en2              = 0x4000;
+	pSegIdDrvInfo->reserved4                        = 0x0;
+	/* 0x1 * 2minutes */
+	pSegIdDrvInfo->req_time_for_security_er_done    = 0x19;
+	pSegIdDrvInfo->req_time_for_enhan_security_er_done      = 0x19;
+	/* Advanced power management level 1 */
+	pSegIdDrvInfo->adv_pwr_mgm_lvl_val                      = 0x0;
+	pSegIdDrvInfo->reserved5                        = 0x0;
+	memset(pSegIdDrvInfo->reserved6, 0x00, 68);
+	/* Security mode feature is disabled */
+	pSegIdDrvInfo->security_stas                    = 0x0;
+	memset(pSegIdDrvInfo->vendor_uniq_bytes, 0x00, 62);
+	/* CFA power mode 1 support in maximum 200mA */
+	pSegIdDrvInfo->cfa_pwr_mode                     = 0x0100;
+	memset(pSegIdDrvInfo->reserved7, 0x00, 190);
+}
 
-	command_print(cmd_ctx, "dump image (address 0x%8.8x size %u) to file %s in %s (%f kB/s)",
-				address, size, args[1], duration_text,
-				(float)size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
+static int mg_storage_config(void)
+{
+	uint8_t buff[512];
+	int ret;
 
-	free(duration_text);
+	ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
+	if (ret != ERROR_OK)
+		return ret;
 
-	fileio_close(&fileio);
+	mg_gen_ataid((mg_io_type_drv_info *)(void *)buff);
 
-	free(buffer);
+	ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_stgdrvinfo);
+	if (ret != ERROR_OK)
+		return ret;
 
-	return ERROR_OK;
+	ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
+	if (ret != ERROR_OK)
+		return ret;
+
+	LOG_INFO("mflash: storage config ok");
+	return ret;
 }
 
-int mflash_init_drivers(struct command_context_s *cmd_ctx)
+static int mg_boot_config(void)
 {
-	if (mflash_bank) {
-		register_command(cmd_ctx, mflash_cmd, "probe", mflash_probe_command, COMMAND_EXEC, NULL);
-		register_command(cmd_ctx, mflash_cmd, "write", mflash_write_command, COMMAND_EXEC,
-				"mflash write <num> <file> <address>");
-		register_command(cmd_ctx, mflash_cmd, "dump", mflash_dump_command, COMMAND_EXEC,
-						"mflash dump <num> <file> <address> <size>");
-	}
+	uint8_t buff[512];
+	int ret;
 
-	return ERROR_OK;
+	ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
+	if (ret != ERROR_OK)
+		return ret;
+
+	memset(buff, 0xff, 512);
+
+	buff[0] = mg_op_mode_snd;		/* operation mode */
+	buff[1] = MG_UNLOCK_OTP_AREA;
+	buff[2] = 4;				/* boot size */
+	*((uint32_t *)(void *)(buff + 4)) = 0;		/* XIP size */
+
+	ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_xipinfo);
+	if (ret != ERROR_OK)
+		return ret;
+
+	ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
+	if (ret != ERROR_OK)
+		return ret;
+
+	LOG_INFO("mflash: boot config ok");
+	return ret;
 }
 
-static int mflash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int mg_set_pll(mg_pll_t *pll)
 {
-	target_t *target;
-	char *str;
-	int i;
+	uint8_t buff[512];
+	int ret;
+
+	memset(buff, 0xff, 512);
+	/* PLL Lock cycle and Feedback 9bit Divider */
+	memcpy(buff, &pll->lock_cyc, sizeof(uint32_t));
+	memcpy(buff + 4, &pll->feedback_div, sizeof(uint16_t));
+	buff[6] = pll->input_div;		/* PLL Input 5bit Divider */
+	buff[7] = pll->output_div;		/* PLL Output Divider */
+
+	ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
+	if (ret != ERROR_OK)
+		return ret;
+
+	ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_wr_pll);
+	if (ret != ERROR_OK)
+		return ret;
+
+	ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
+	if (ret != ERROR_OK)
+		return ret;
+
+	LOG_INFO("mflash: set pll ok");
+	return ret;
+}
+
+static int mg_erase_nand(void)
+{
+	int ret;
+
+	ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
+	if (ret != ERROR_OK)
+		return ret;
+
+	ret = mg_mflash_do_write_sects(NULL, 0, 0, mg_vcmd_purge_nand);
+	if (ret != ERROR_OK)
+		return ret;
+
+	ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
+	if (ret != ERROR_OK)
+		return ret;
+
+	LOG_INFO("mflash: erase nand ok");
+	return ret;
+}
+
+COMMAND_HANDLER(mg_config_cmd)
+{
+	double fin, fout;
+	mg_pll_t pll;
+	int ret;
+
+	ret = mg_verify_interface();
+	if (ret != ERROR_OK)
+		return ret;
+
+	ret = mg_mflash_rst();
+	if (ret != ERROR_OK)
+		return ret;
+
+	switch (CMD_ARGC) {
+		case 2:
+			if (!strcmp(CMD_ARGV[1], "boot"))
+				return mg_boot_config();
+			else if (!strcmp(CMD_ARGV[1], "storage"))
+				return mg_storage_config();
+			else
+				return ERROR_COMMAND_NOTFOUND;
+			break;
+		case 3:
+			if (!strcmp(CMD_ARGV[1], "pll")) {
+				unsigned long freq;
+				COMMAND_PARSE_NUMBER(ulong, CMD_ARGV[2], freq);
+				fin = freq;
+
+				if (fin > MG_PLL_CLK_OUT) {
+					LOG_ERROR("mflash: input freq. is too large");
+					return ERROR_MG_INVALID_OSC;
+				}
 
-	if (argc < 8)
+				fout = mg_calc_pll(fin, &pll);
+
+				if (!fout) {
+					LOG_ERROR("mflash: cannot generate valid pll");
+					return ERROR_MG_INVALID_PLL;
+				}
+
+				LOG_INFO("mflash: Fout=%" PRIu32 " Hz, feedback=%u,"
+						"indiv=%u, outdiv=%u, lock=%u",
+						(uint32_t)fout, pll.feedback_div,
+						pll.input_div, pll.output_div,
+						pll.lock_cyc);
+
+				ret = mg_erase_nand();
+				if (ret != ERROR_OK)
+					return ret;
+
+				return mg_set_pll(&pll);
+			} else
+				return ERROR_COMMAND_NOTFOUND;
+			break;
+		default:
+			return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+}
+
+static const struct command_registration mflash_exec_command_handlers[] = {
+	{
+		.name = "probe",
+		.handler = mg_probe_cmd,
+		.mode = COMMAND_EXEC,
+		.help = "Detect bank configuration information",
+		.usage = "",
+	},
 	{
+		.name = "write",
+		.handler = mg_write_cmd,
+		.mode = COMMAND_EXEC,
+		/* FIXME bank_num is unused */
+		.usage = "bank_num filename address",
+		.help = "Write binary file at the specified address.",
+	},
+	{
+		.name = "dump",
+		.handler = mg_dump_cmd,
+		.mode = COMMAND_EXEC,
+		/* FIXME bank_num is unused */
+		.usage = "bank_num filename address size",
+		.help = "Write specified number of bytes from a binary file "
+			"to the specified, address.",
+	},
+	{
+		.name = "config",
+		.handler = mg_config_cmd,
+		.mode = COMMAND_EXEC,
+		.help = "Configure MFLASH options.",
+		.usage = "('boot'|'storage'|'pll' frequency)",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static int mflash_init_drivers(struct command_context *cmd_ctx)
+{
+	if (!mflash_bank)
+		return ERROR_OK;
+	return register_commands(cmd_ctx, NULL, mflash_exec_command_handlers);
+}
+
+COMMAND_HANDLER(handle_mflash_init_command)
+{
+	if (CMD_ARGC != 0)
 		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	static bool mflash_initialized;
+	if (mflash_initialized) {
+		LOG_INFO("'mflash init' has already been called");
+		return ERROR_OK;
 	}
+	mflash_initialized = true;
 
-	if ((target = get_target_by_num(strtoul(args[7], NULL, 0))) == NULL)
-	{
-		LOG_ERROR("target %lu not defined", strtoul(args[7], NULL, 0));
+	LOG_DEBUG("Initializing mflash devices...");
+	return mflash_init_drivers(CMD_CTX);
+}
+
+COMMAND_HANDLER(mg_bank_cmd)
+{
+	struct target *target;
+	int i;
+
+	if (CMD_ARGC < 4)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	target = get_target(CMD_ARGV[3]);
+	if (target == NULL) {
+		LOG_ERROR("target '%s' not defined", CMD_ARGV[3]);
 		return ERROR_FAIL;
 	}
 
-	mflash_bank = calloc(sizeof(mflash_bank_t), 1);
-	mflash_bank->base = strtoul(args[1], NULL, 0);
-	mflash_bank->chip_width = strtoul(args[2], NULL, 0);
-	mflash_bank->bus_width = strtoul(args[3], NULL, 0);
-	mflash_bank->rst_pin.num = strtoul(args[4], &str, 0);
-	if (*str)
-		mflash_bank->rst_pin.port[0] = (u16)tolower(str[0]);
-	mflash_bank->wp_pin.num = strtol(args[5], &str, 0);
-	if (*str)
-		mflash_bank->wp_pin.port[0] = (u16)tolower(str[0]);
-	mflash_bank->dpd_pin.num = strtol(args[6], &str, 0);
+	mflash_bank = calloc(sizeof(struct mflash_bank), 1);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], mflash_bank->base);
+	/** @todo Verify how this parsing should work, then document it. */
+	char *str;
+	mflash_bank->rst_pin.num = strtoul(CMD_ARGV[2], &str, 0);
 	if (*str)
-		mflash_bank->dpd_pin.port[0] = (u16)tolower(str[0]);
+		mflash_bank->rst_pin.port[0] = (uint16_t)
+			tolower((unsigned)str[0]);
 
 	mflash_bank->target = target;
 
-	for (i = 0; mflash_gpio[i] ; i++) {
-		if (! strcmp(mflash_gpio[i]->name, args[0])) {
+	for (i = 0; mflash_gpio[i]; i++) {
+		if (!strcmp(mflash_gpio[i]->name, CMD_ARGV[0]))
 			mflash_bank->gpio_drv = mflash_gpio[i];
-		}
 	}
 
-	if (! mflash_bank->gpio_drv) {
-		LOG_ERROR("%s is unsupported soc", args[0]);
-		return ERROR_INVALID_ARGUMENTS;
+	if (!mflash_bank->gpio_drv) {
+		LOG_ERROR("%s is unsupported soc", CMD_ARGV[0]);
+		return ERROR_MG_UNSUPPORTED_SOC;
 	}
 
 	return ERROR_OK;
 }
 
-int mflash_register_commands(struct command_context_s *cmd_ctx)
+static const struct command_registration mflash_config_command_handlers[] = {
+	{
+		.name = "bank",
+		.handler = mg_bank_cmd,
+		.mode = COMMAND_CONFIG,
+		.help = "configure a mflash device bank",
+		.usage = "soc_type base_addr pin_id target",
+	},
+	{
+		.name = "init",
+		.mode = COMMAND_CONFIG,
+		.handler = handle_mflash_init_command,
+		.help = "initialize mflash devices",
+		.usage = ""
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration mflash_command_handler[] = {
+	{
+		.name = "mflash",
+		.mode = COMMAND_ANY,
+		.help = "mflash command group",
+		.usage = "",
+		.chain = mflash_config_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+int mflash_register_commands(struct command_context *cmd_ctx)
 {
-	mflash_cmd = register_command(cmd_ctx, NULL, "mflash", NULL, COMMAND_ANY, NULL);
-	register_command(cmd_ctx, mflash_cmd, "bank", mflash_bank_command, COMMAND_CONFIG,
-			"mflash bank <soc> <base> <chip_width> <bus_width> <RST pin> <WP pin> <DPD pin> <target #>");
-	return ERROR_OK;
+	return register_commands(cmd_ctx, NULL, mflash_command_handler);
 }