X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fflash%2Fmflash.c;h=b6999554279114c6967e971f99568fa86218e6eb;hp=f2cfd4a111f49a053a40fc48bd85b9c13e85f61a;hb=bcaf775fc10d88d2c63c06bafada141895318b34;hpb=3d79669c5ef4648e19d5153edf0cb9a30c717c35

diff --git a/src/flash/mflash.c b/src/flash/mflash.c
index f2cfd4a111..b699955427 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,42 +189,47 @@ static int s3c2440_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val)
 	return ret;
 }
 
-static int mg_hdrst(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 mg_init_gpio (void)
+static int mg_init_gpio(void)
 {
-	mflash_gpio_drv_t *gpio_drv = mflash_bank->gpio_drv;
+	int ret;
+	struct mflash_gpio_drv *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);
+	ret = gpio_drv->set_gpio_to_output(mflash_bank->rst_pin);
+	if (ret != ERROR_OK)
+		return ret;
 
-	return ERROR_OK;
+	ret = gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, 1);
+
+	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:
@@ -242,25 +245,17 @@ 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;
@@ -274,111 +269,138 @@ static int mg_dsk_wait(mg_io_type_wait wait, u32 time)
 			}
 		}
 
-		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_rst(void)
 {
-	mg_init_gpio();
+	int ret;
 
-	mg_hdrst(0);
+	ret = mg_init_gpio();
+	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(0);
+	if (ret != ERROR_OK)
+		return ret;
 
-	mg_hdrst(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_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_hdrst(1);
+	if (ret != ERROR_OK)
+		return ret;
 
-	mg_dsk_srst(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_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(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_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_dsk_srst(0);
+	if (ret != ERROR_OK)
+		return ret;
+
+	ret = mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
+	if (ret != ERROR_OK)
+		return ret;
 
 	LOG_INFO("mflash: reset ok");
 
@@ -387,129 +409,145 @@ static int mg_mflash_rst(void)
 
 static int mg_mflash_probe(void)
 {
-	if (mg_mflash_rst() != ERROR_OK)
-		return ERROR_FAIL;
-
-	if (mg_dsk_drv_info() != ERROR_OK)
-		return ERROR_FAIL;
+	int ret = mg_mflash_rst();
+	if (ret != ERROR_OK)
+		return ret;
 
-	return ERROR_OK;
+	return mg_dsk_drv_info();
 }
 
-static int mg_probe_cmd(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,
-		mg_io_type_cmd cmd)
+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, cmd) != ERROR_OK )
-		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");
+			return ret;
 
-		LOG_DEBUG("%u (0x%8.8x) sector write", sect_num + i, (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
+		LOG_DEBUG("mflash: %" PRIu32 " (0x%8.8" PRIx32 ") sector write", sect_num + i,
+			(sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 
-		duration_stop_measure(&duration, NULL);
-
-		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);
 		}
 	}
 
@@ -521,55 +559,71 @@ static int mg_mflash_do_write_sects(void *buff, u32 sect_num, u32 sect_cnt,
 	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, mg_io_cmd_write);
+		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, mg_io_cmd_write);
+		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;
 		}
@@ -585,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;
@@ -594,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) {
@@ -647,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;
@@ -656,166 +730,176 @@ 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 mg_write_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+COMMAND_HANDLER(mg_write_cmd)
 {
-	u32 address, buf_cnt, cnt, res, i;
-	u8 *buffer;
-	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;
-	}
 
-	address = strtoul(args[2], NULL, 0);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], address);
 
-	if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK) {
-		return ERROR_FAIL;
-	}
+	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);
+		fileio_close(fileio);
 		return ERROR_FAIL;
 	}
+	int retval = fileio_size(fileio, &filesize);
+	if (retval != ERROR_OK) {
+		fileio_close(fileio);
+		free(buffer);
+		return retval;
+	}
 
-	cnt = fileio.size / MG_FILEIO_CHUNK;
-	res = fileio.size % MG_FILEIO_CHUNK;
+	cnt = filesize / MG_FILEIO_CHUNK;
+	res = filesize % MG_FILEIO_CHUNK;
 
-	duration_start_measure(&duration);
+	struct duration bench;
+	duration_start(&bench);
 
+	size_t buf_cnt;
 	for (i = 0; i < cnt; i++) {
-		if (fileio_read(&fileio, MG_FILEIO_CHUNK, buffer, &buf_cnt) !=
-				ERROR_OK)
+		ret = fileio_read(fileio, MG_FILEIO_CHUNK, buffer, &buf_cnt);
+		if (ret != ERROR_OK)
 			goto mg_write_cmd_err;
-		if (mg_mflash_write(address, buffer, MG_FILEIO_CHUNK) != ERROR_OK)
+		ret = mg_mflash_write(address, buffer, MG_FILEIO_CHUNK);
+		if (ret != ERROR_OK)
 			goto mg_write_cmd_err;
 		address += MG_FILEIO_CHUNK;
 	}
- 
+
 	if (res) {
-		if (fileio_read(&fileio, res, buffer, &buf_cnt) != ERROR_OK)
-			goto mg_write_cmd_err;			
-		if (mg_mflash_write(address, buffer, res) != ERROR_OK)
+		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;
 	}
 
-	duration_stop_measure(&duration, &duration_text);
-
-	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 (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));
+	}
 
-	free(duration_text);
 	free(buffer);
-	fileio_close(&fileio);
+	fileio_close(fileio);
 
 	return ERROR_OK;
 
 mg_write_cmd_err:
-	duration_stop_measure(&duration, &duration_text);
-	free(duration_text);
- 	free(buffer);
-	fileio_close(&fileio);
+	free(buffer);
+	fileio_close(fileio);
 
-	return ERROR_FAIL;
+	return ret;
 }
 
-static int mg_dump_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+COMMAND_HANDLER(mg_dump_cmd)
 {
-	u32 address, size_written, size, cnt, res, i;
-	u8 *buffer;
-	fileio_t fileio;
-	duration_t duration;
-	char *duration_text;
+	uint32_t address, size, cnt, res, i;
+	uint8_t *buffer;
+	struct fileio *fileio;
+	int ret;
 
-	if (argc != 4) {
+	if (CMD_ARGC != 4)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
-	address = strtoul(args[2], NULL, 0);
-	size = strtoul(args[3], NULL, 0);
+	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;
 
-	if (fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK) {
-		return ERROR_FAIL;
-	}
- 
 	buffer = malloc(MG_FILEIO_CHUNK);
 	if (!buffer) {
-		fileio_close(&fileio);
+		fileio_close(fileio);
 		return ERROR_FAIL;
 	}
 
 	cnt = size / MG_FILEIO_CHUNK;
 	res = size % MG_FILEIO_CHUNK;
- 
-	duration_start_measure(&duration);
 
+	struct duration bench;
+	duration_start(&bench);
+
+	size_t size_written;
 	for (i = 0; i < cnt; i++) {
-		if (mg_mflash_read(address, buffer, MG_FILEIO_CHUNK) != ERROR_OK)
+		ret = mg_mflash_read(address, buffer, MG_FILEIO_CHUNK);
+		if (ret != ERROR_OK)
 			goto mg_dump_cmd_err;
-		if (fileio_write(&fileio, MG_FILEIO_CHUNK, buffer, &size_written)
-				!= ERROR_OK)
+		ret = fileio_write(fileio, MG_FILEIO_CHUNK, buffer, &size_written);
+		if (ret != ERROR_OK)
 			goto mg_dump_cmd_err;
 		address += MG_FILEIO_CHUNK;
 	}
- 
+
 	if (res) {
-		if (mg_mflash_read(address, buffer, res) != ERROR_OK)
+		ret = mg_mflash_read(address, buffer, res);
+		if (ret != ERROR_OK)
 			goto mg_dump_cmd_err;
-		if (fileio_write(&fileio, res, buffer, &size_written) != ERROR_OK)
+		ret = fileio_write(fileio, res, buffer, &size_written);
+		if (ret != ERROR_OK)
 			goto mg_dump_cmd_err;
 	}
 
-	duration_stop_measure(&duration, &duration_text);
-
-	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)));
+	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:
-	duration_stop_measure(&duration, &duration_text);
-	free(duration_text);
- 	free(buffer);
-	fileio_close(&fileio);
- 
-	return ERROR_FAIL;	
+	free(buffer);
+	fileio_close(fileio);
+
+	return ret;
 }
 
 static int mg_set_feature(mg_feature_id feature, mg_feature_val config)
 {
-	target_t *target = mflash_bank->target;
-	u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
+	struct target *target = mflash_bank->target;
+	uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
+	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;
 
-	target_write_u8(target, mg_task_reg + MG_REG_FEATURE, feature);
-	target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, config);
-	target_write_u8(target, mg_task_reg + MG_REG_COMMAND,
+	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 ERROR_OK;
+	return ret;
 }
 
 static int mg_is_valid_pll(double XIN, int N, double CLK_OUT, int NO)
@@ -823,8 +907,8 @@ 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_FAIL;
+	if (v1 < 1000000 || v1 > 15000000 || v2 < 100000000 || v2 > 500000000)
+		return ERROR_MG_INVALID_PLL;
 
 	return ERROR_OK;
 }
@@ -833,7 +917,7 @@ static int mg_pll_get_M(unsigned short feedback_div)
 {
 	int i, M;
 
-	for (i = 1, M=0; i < 512; i <<= 1, feedback_div >>= 1)
+	for (i = 1, M = 0; i < 512; i <<= 1, feedback_div >>= 1)
 		M += (feedback_div & 1) * i;
 
 	return M + 2;
@@ -849,22 +933,22 @@ static int mg_pll_get_N(unsigned char input_div)
 	return N + 2;
 }
 
-static int mg_pll_get_NO(unsigned char  output_div)
+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)
+		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)
+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;
+	unsigned char j, k;
+	int M, N, NO;
 	double CLK_OUT;
 	double DIV = 1;
 	double ROUND = 0;
@@ -874,22 +958,23 @@ static double mg_do_calc_pll(double XIN, mg_pll_t * p_pll_val, int is_approximat
 		ROUND = 500000;
 	}
 
-	for (i = 0; i < MG_PLL_MAX_FEEDBACKDIV_VAL ; ++i) {
+	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) {
+		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) {
+			for (k = 0; k < MG_PLL_MAX_OUTPUTDIV_VAL; ++k) {
 				NO = mg_pll_get_NO(k);
 
 				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);
+				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;
@@ -918,373 +1003,447 @@ static double mg_calc_pll(double XIN, mg_pll_t *p_pll_val)
 
 static int mg_verify_interface(void)
 {
-	u16 buff[MG_MFLASH_SECTOR_SIZE >> 1];
-	u16 i, j;
-	u32 address = mflash_bank->base + MG_BUFFER_OFFSET;
-	target_t *target = mflash_bank->target;
+	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;
 
-		target->type->write_memory(target, address, 2,
-				MG_MFLASH_SECTOR_SIZE / 2, (u8 *)buff);
+		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);
 
-		target->type->read_memory(target, address, 2,
-				MG_MFLASH_SECTOR_SIZE / 2, (u8 *)buff);
+		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_FAIL;
+				return ERROR_MG_INTERFACE;
 			}
 		}
 	}
 
 	LOG_INFO("mflash: verify interface ok");
-	return ERROR_OK;
+	return ret;
 }
 
 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
+	'G', 'm', 'n', 'i', '-', 'e', 'e', 'S', 'g', 'a', 'e', 'l',
+	0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20
 };
 
 static const char g_strSEG_FWRev[8] = {
-	'F','X','L','T','2','v','0','.'
+	'F', 'X', 'L', 'T', '2', 'v', '0', '.'
 };
 
 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
+	'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
 };
 
 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;
+	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, (void *)g_strSEG_SerialNum,20);
+	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;
+	pSegIdDrvInfo->buffer_type                      = 0x2;
 	/* buffer size : 2KB */
-	pSegIdDrvInfo->buffer_sector_size		= 0x800;
-	pSegIdDrvInfo->number_of_ecc_bytes		= 0;
+	pSegIdDrvInfo->buffer_sector_size               = 0x800;
+	pSegIdDrvInfo->number_of_ecc_bytes              = 0;
 
-	memcpy(pSegIdDrvInfo->firmware_revision, (void *)g_strSEG_FWRev,8);
+	memcpy(pSegIdDrvInfo->firmware_revision, g_strSEG_FWRev, 8);
 
-	memcpy(pSegIdDrvInfo->model_number, (void *)g_strSEG_ModelNum,40);
+	memcpy(pSegIdDrvInfo->model_number, g_strSEG_ModelNum, 40);
 
-	pSegIdDrvInfo->maximum_block_transfer		= 0x4;
-	pSegIdDrvInfo->vendor_unique2   		= 0x0;
-	pSegIdDrvInfo->dword_io				= 0x00;
+	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->capabilities                     = 0x1 << 9;
 
-	pSegIdDrvInfo->reserved2			= 0x4000;
-	pSegIdDrvInfo->vendor_unique3			= 0x00;
+	pSegIdDrvInfo->reserved2                        = 0x4000;
+	pSegIdDrvInfo->vendor_unique3                   = 0x00;
 	/* PIOMode-2 support */
-	pSegIdDrvInfo->pio_cycle_timing_mode		= 0x02;
-	pSegIdDrvInfo->vendor_unique4			= 0x00;
+	pSegIdDrvInfo->pio_cycle_timing_mode            = 0x02;
+	pSegIdDrvInfo->vendor_unique4                   = 0x00;
 	/* MultiWord-2 support */
-	pSegIdDrvInfo->dma_cycle_timing_mode		= 0x00;
+	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;
+	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;
+	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);
+	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);
+	pSegIdDrvInfo->multi_dma_transfer_active                = (0x1 << 0);
 	/* b0 : PIO Mode-3 support, b1 : PIO Mode-4 support */
-	pSegIdDrvInfo->adv_pio_mode				= 0x00;
+	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((void *)pSegIdDrvInfo->reserved3, 0x00, 22);
+	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;
+	pSegIdDrvInfo->major_ver_num                    = 0x7E;
 	/* 0x1C : ATA/ATAPI-6 T13 1532D revision1 */
-	pSegIdDrvInfo->minor_ver_num			= 0x19;
+	pSegIdDrvInfo->minor_ver_num                    = 0x19;
 	/* NOP/READ BUFFER/WRITE BUFFER/Power management feature set support */
-	pSegIdDrvInfo->feature_cmd_set_suprt0		= 0x7068;
+	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;
+	pSegIdDrvInfo->feature_cmd_set_suprt1           = 0x400C;
+	pSegIdDrvInfo->feature_cmd_set_suprt2           = 0x4000;
 	/* READ/WRITE BUFFER/PWR Management enable */
-	pSegIdDrvInfo->feature_cmd_set_en0		= 0x7000;
+	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;
+	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;
+	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((void *)pSegIdDrvInfo->reserved6, 0x00, 68);
+	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((void *)pSegIdDrvInfo->vendor_uniq_bytes, 0x00, 62);
+	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((void *)pSegIdDrvInfo->reserved7, 0x00, 190);
+	pSegIdDrvInfo->cfa_pwr_mode                     = 0x0100;
+	memset(pSegIdDrvInfo->reserved7, 0x00, 190);
 }
 
 static int mg_storage_config(void)
 {
-	u8 buff[512];
+	uint8_t buff[512];
+	int ret;
 
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
+	if (ret != ERROR_OK)
+		return ret;
 
-	mg_gen_ataid((mg_io_type_drv_info *)buff);
+	mg_gen_ataid((mg_io_type_drv_info *)(void *)buff);
 
-	if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_stgdrvinfo)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_stgdrvinfo);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	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 ERROR_OK;
+	return ret;
 }
 
 static int mg_boot_config(void)
 {
-	u8 buff[512];
+	uint8_t buff[512];
+	int ret;
 
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	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 */
-	*((u32 *)(buff + 4)) = 0;		/* XIP size */
+	*((uint32_t *)(void *)(buff + 4)) = 0;		/* XIP size */
 
-	if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_xipinfo)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_xipinfo);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	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 ERROR_OK;
+	return ret;
 }
 
 static int mg_set_pll(mg_pll_t *pll)
 {
-	u8 buff[512];
+	uint8_t buff[512];
+	int ret;
 
 	memset(buff, 0xff, 512);
-	*((u32 *)&buff[0]) = pll->lock_cyc;	/* PLL Lock cycle */
-	*((u16 *)&buff[4]) = pll->feedback_div;	/* PLL Feedback 9bit Divider */
+	/* 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 */
 
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_wr_pll)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_wr_pll);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	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 ERROR_OK;
+	return ret;
 }
 
 static int mg_erase_nand(void)
 {
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	int ret;
 
-	if (mg_mflash_do_write_sects(NULL, 0, 0, mg_vcmd_purge_nand)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
-			!= ERROR_OK)
-		return ERROR_FAIL;
+	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 ERROR_OK;
+	return ret;
 }
 
-int mg_config_cmd(struct command_context_s *cmd_ctx, char *cmd,
-		char **args, int argc)
+COMMAND_HANDLER(mg_config_cmd)
 {
 	double fin, fout;
 	mg_pll_t pll;
+	int ret;
 
-	if (mg_verify_interface() != ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_verify_interface();
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (mg_mflash_rst() != ERROR_OK)
-		return ERROR_FAIL;
+	ret = mg_mflash_rst();
+	if (ret != ERROR_OK)
+		return ret;
 
-	switch (argc) {
+	switch (CMD_ARGC) {
 		case 2:
-			if (!strcmp(args[1], "boot")) {
-				if (mg_boot_config() != ERROR_OK)
-					return ERROR_FAIL;
-
-				return ERROR_OK;
-			} else if (!strcmp(args[1], "storage")) {
-				if (mg_storage_config() != ERROR_OK)
-					return ERROR_FAIL;
-
-				return ERROR_OK;
-			} else
+			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(args[1], "pll")) {
-				fin = strtoul(args[2], NULL, 0);
-
-				if (fin > MG_PLL_CLK_OUT)
-					return ERROR_FAIL;
+			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;
+				}
 
 				fout = mg_calc_pll(fin, &pll);
 
-				if (!fout)
-					return ERROR_FAIL;
+				if (!fout) {
+					LOG_ERROR("mflash: cannot generate valid pll");
+					return ERROR_MG_INVALID_PLL;
+				}
 
-				LOG_INFO("mflash: Fout=%u Hz, feedback=%u," 
+				LOG_INFO("mflash: Fout=%" PRIu32 " Hz, feedback=%u,"
 						"indiv=%u, outdiv=%u, lock=%u",
-						(u32)fout, pll.feedback_div,
+						(uint32_t)fout, pll.feedback_div,
 						pll.input_div, pll.output_div,
 						pll.lock_cyc);
 
-				if (mg_erase_nand() != ERROR_OK)
-					return ERROR_FAIL;
+				ret = mg_erase_nand();
+				if (ret != ERROR_OK)
+					return ret;
 
-				if (mg_set_pll(&pll) != ERROR_OK)
-					return ERROR_FAIL;
-
-				return ERROR_OK;
+				return mg_set_pll(&pll);
 			} else
 				return ERROR_COMMAND_NOTFOUND;
 			break;
 		default:
 			return ERROR_COMMAND_SYNTAX_ERROR;
 	}
+}
 
-	return ERROR_OK;
+static const struct command_registration mflash_exec_command_handlers[] = {
+	{
+		.name = "probe",
+		.handler = mg_probe_cmd,
+		.mode = COMMAND_EXEC,
+		.help = "Detect bank configuration information",
+	},
+	{
+		.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);
 }
 
-int mflash_init_drivers(struct command_context_s *cmd_ctx)
+COMMAND_HANDLER(handle_mflash_init_command)
 {
-	if (mflash_bank) {
-		register_command(cmd_ctx, mflash_cmd, "probe", mg_probe_cmd, COMMAND_EXEC, NULL);
-		register_command(cmd_ctx, mflash_cmd, "write", mg_write_cmd, COMMAND_EXEC,
-				"mflash write <num> <file> <address>");
-		register_command(cmd_ctx, mflash_cmd, "dump", mg_dump_cmd, COMMAND_EXEC,
-						"mflash dump <num> <file> <address> <size>");
-		register_command(cmd_ctx, mflash_cmd, "config", mg_config_cmd,
-				COMMAND_EXEC, "mflash config <num> <stage>");
+	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;
 
-	return ERROR_OK;
+	LOG_DEBUG("Initializing mflash devices...");
+	return mflash_init_drivers(CMD_CTX);
 }
 
-static int mg_bank_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+COMMAND_HANDLER(mg_bank_cmd)
 {
-	target_t *target;
-	char *str;
+	struct target *target;
 	int i;
 
-	if (argc < 4)
-	{
+	if (CMD_ARGC < 4)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
-	if ((target = get_target(args[3])) == NULL)
-	{
-		LOG_ERROR("target '%s' not defined", args[3]);
+	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->rst_pin.num = strtoul(args[2], &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->rst_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", mg_bank_cmd, COMMAND_CONFIG,
-			"mflash bank <soc> <base> <RST pin> <target #>");
-	return ERROR_OK;
+	return register_commands(cmd_ctx, NULL, mflash_command_handler);
 }