X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fflash%2Fmflash.c;h=5a9b7c355df1e7325b87033e06200147c3459909;hp=b5c7d46ba912171649bec9ef59eec0b88005d231;hb=cfc4d5c6b7b6f8f82dc5bbf3ee661c179814666e;hpb=ee9766f1db994754f47187d4f929b4ca1891d27d

diff --git a/src/flash/mflash.c b/src/flash/mflash.c
index b5c7d46ba9..5a9b7c355d 100644
--- a/src/flash/mflash.c
+++ b/src/flash/mflash.c
@@ -28,9 +28,9 @@
 
 
 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 s3c2440_set_gpio_output_val (mflash_gpio_num_t gpio, uint8_t 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 int pxa270_set_gpio_output_val (mflash_gpio_num_t gpio, uint8_t val);
 
 static command_t *mflash_cmd;
 
@@ -66,7 +66,7 @@ static mflash_gpio_drv_t *mflash_gpio[] =
 
 static int pxa270_set_gpio_to_output (mflash_gpio_num_t gpio)
 {
-	u32 addr, value, mask;
+	uint32_t addr, value, mask;
 	target_t *target = mflash_bank->target;
 	int ret;
 
@@ -101,9 +101,9 @@ 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 (mflash_gpio_num_t gpio, uint8_t val)
 {
-	u32 addr, value, mask;
+	uint32_t addr, value, mask;
 	target_t *target = mflash_bank->target;
 	int ret;
 
@@ -132,7 +132,7 @@ static int pxa270_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val)
 
 static int s3c2440_set_gpio_to_output (mflash_gpio_num_t gpio)
 {
-	u32 data, mask, gpio_con;
+	uint32_t data, mask, gpio_con;
 	target_t *target = mflash_bank->target;
 	int ret;
 
@@ -141,7 +141,7 @@ static int s3c2440_set_gpio_to_output (mflash_gpio_num_t gpio)
 	} else if (gpio.port[0] == 'j') {
 		gpio_con = S3C2440_GPJCON;
 	} else {
-		LOG_ERROR("invalid port %d%s", gpio.num, gpio.port);
+		LOG_ERROR("mflash: invalid port %d%s", gpio.num, gpio.port);
 		return ERROR_INVALID_ARGUMENTS;
 	}
 
@@ -162,9 +162,9 @@ 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 (mflash_gpio_num_t gpio, uint8_t val)
 {
-	u32 data, mask, gpio_dat;
+	uint32_t data, mask, gpio_dat;
 	target_t *target = mflash_bank->target;
 	int ret;
 
@@ -173,7 +173,7 @@ static int s3c2440_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val)
 	} else if (gpio.port[0] == 'j') {
 		gpio_dat = S3C2440_GPJDAT;
 	} else {
-		LOG_ERROR("invalid port %d%s", gpio.num, gpio.port);
+		LOG_ERROR("mflash: invalid port %d%s", gpio.num, gpio.port);
 		return ERROR_INVALID_ARGUMENTS;
 	}
 
@@ -191,41 +191,48 @@ 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)
 {
+	int ret;
 	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);
+	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, uint32_t time)
 {
-	u8 status, error;
+	uint8_t 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;
+	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) {
 
-		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)
 				return ERROR_OK;
 		} else {
-			switch(wait)
+			switch (wait)
 			{
 				case mg_io_wait_not_bsy:
 					return ERROR_OK;
@@ -244,19 +251,13 @@ 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);
+				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)
@@ -274,30 +275,31 @@ 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)
 			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;
+	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)
 		return ret;
 
-	if(on) {
+	if (on) {
 		value |= (mg_io_rbit_devc_srst);
 	} else {
 		value &= ~mg_io_rbit_devc_srst;
@@ -307,78 +309,89 @@ static int mg_dsk_srst(u8 on)
 	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;
+	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);
 
-	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;
+	uint32_t mg_buff = mflash_bank->base + MG_BUFFER_OFFSET;
+	int ret;
 
-	if ( mg_dsk_io_cmd(0, 1, mg_io_cmd_identify) != ERROR_OK)
-		return ERROR_FAIL;
+	if ((ret =  mg_dsk_io_cmd(0, 1, mg_io_cmd_identify)) != ERROR_OK)
+		return ret;
 
-	if ( mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL) != ERROR_OK)
-		return ERROR_FAIL;
+	if ((ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL)) != ERROR_OK)
+		return ret;
 
-	LOG_INFO("read drive info.");
+	LOG_INFO("mflash: read drive info");
 
 	if (! mflash_bank->drv_info)
 		mflash_bank->drv_info = malloc(sizeof(mg_drv_info_t));
 
-	target->type->read_memory(target, mg_buff, 2, sizeof(mg_io_type_drv_info) >> 1,
-			(u8 *)&mflash_bank->drv_info->drv_id);
+	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;
 
-	mflash_bank->drv_info->tot_sects = (u32)(mflash_bank->drv_info->drv_id.total_user_addressable_sectors_hi << 16)
+	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;
 
-	target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_read);
-
-	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);
+	if ((ret = mg_init_gpio()) != ERROR_OK)
+		return ret;
 
-	if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
-		return ERROR_FAIL;
+	if ((ret = mg_hdrst(0)) != ERROR_OK)
+		return ret;
 
-	mg_hdrst(1);
+	if ((ret = mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG)) != ERROR_OK)
+		return ret;
 
-	if (mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
-		return ERROR_FAIL;
+	if ((ret = mg_hdrst(1)) != ERROR_OK)
+		return ret;
+
+	if ((ret = mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG)) != ERROR_OK)
+		return ret;
 
-	mg_dsk_srst(1);
+	if ((ret = mg_dsk_srst(1)) != ERROR_OK)
+		return ret;
 
-	if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
-		return ERROR_FAIL;
+	if ((ret = mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG)) != ERROR_OK)
+		return ret;
 
-	mg_dsk_srst(0);
+	if ((ret = mg_dsk_srst(0)) != ERROR_OK)
+		return ret;
 
-	if (mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
-		return ERROR_FAIL;
+	if ((ret = mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG)) != ERROR_OK)
+		return ret;
 
 	LOG_INFO("mflash: reset ok");
 
@@ -387,129 +400,136 @@ static int mg_mflash_rst(void)
 
 static int mg_mflash_probe(void)
 {
-	if (mg_mflash_rst() != ERROR_OK)
-		return ERROR_FAIL;
+	int ret;
 
-	if (mg_dsk_drv_info() != ERROR_OK)
-		return ERROR_FAIL;
+	if ((ret = mg_mflash_rst()) != 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;
+	uint8_t *buff_ptr = buff;
 
-	if ( mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_read) != ERROR_OK )
-		return ERROR_FAIL;
+	if ((ret = mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_read)) != 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,
 		mg_io_type_cmd cmd)
 {
-	u32 i, address;
+	uint32_t i, address;
 	int ret;
 	target_t *target = mflash_bank->target;
-	u8 *buff_ptr = buff;
-	duration_t duration;
+	uint8_t *buff_ptr = buff;
 
-	if ( mg_dsk_io_cmd(sect_num, sect_cnt, cmd) != ERROR_OK )
-		return ERROR_FAIL;
+	if ((ret = mg_dsk_io_cmd(sect_num, sect_cnt, cmd)) != 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);
 		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 +541,61 @@ 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);
+			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);
+			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;
 		}
@@ -586,7 +612,8 @@ static int mg_mflash_read (u32 addr, u8 *buff, u32 len)
 		}
 
 		if (cnt)
-			mg_mflash_read_sects(buff_ptr, sect_num, cnt);
+			if ((ret = mg_mflash_read_sects(buff_ptr, sect_num, cnt)) != ERROR_OK)
+				return ret;
 
 		buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 		cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
@@ -594,47 +621,52 @@ 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);
+			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("copies %u byte", 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);
+			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);
+			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) {
@@ -648,7 +680,8 @@ static int mg_mflash_write(u32 addr, u8 *buff, u32 len)
 		}
 
 		if (cnt)
-			mg_mflash_write_sects(buff_ptr, sect_num, cnt);
+			if ((ret = mg_mflash_write_sects(buff_ptr, sect_num, cnt)) != ERROR_OK)
+				return ret;
 
 		buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 		cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
@@ -656,125 +689,165 @@ 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;
-	u8 *buffer;
-	/* TODO : multi-bank support, large file support */
+	uint32_t address, buf_cnt, cnt, res, i;
+	uint8_t *buffer;
 	fileio_t fileio;
-	duration_t duration;
-	char *duration_text;
 	int ret;
 
 	if (argc != 3) {
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
-	address = strtoul(args[2], NULL, 0);
-
-	if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK) {
-		return ERROR_FAIL;
-	}
+	COMMAND_PARSE_NUMBER(u32, args[2], address);
 
-	buffer = malloc(fileio.size);
+	ret = fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (fileio_read(&fileio, fileio.size, buffer, &buf_cnt) != ERROR_OK)
-	{
-		free(buffer);
+	buffer = malloc(MG_FILEIO_CHUNK);
+	if (!buffer) {
 		fileio_close(&fileio);
 		return ERROR_FAIL;
 	}
 
-	duration_start_measure(&duration);
-
-	ret = mg_mflash_write(address, buffer, (u32)fileio.size);
+	cnt = fileio.size / MG_FILEIO_CHUNK;
+	res = fileio.size % MG_FILEIO_CHUNK;
 
-	duration_stop_measure(&duration, &duration_text);
+	struct duration bench;
+	duration_start(&bench);
 
-	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)));
+	for (i = 0; i < cnt; i++) {
+		if ((ret = fileio_read(&fileio, MG_FILEIO_CHUNK, buffer, &buf_cnt)) !=
+				ERROR_OK)
+			goto mg_write_cmd_err;
+		if ((ret = mg_mflash_write(address, buffer, MG_FILEIO_CHUNK)) != ERROR_OK)
+			goto mg_write_cmd_err;
+		address += MG_FILEIO_CHUNK;
+	}
 
-	free(duration_text);
+	if (res) {
+		if ((ret = fileio_read(&fileio, res, buffer, &buf_cnt)) != ERROR_OK)
+			goto mg_write_cmd_err;
+		if ((ret = mg_mflash_write(address, buffer, res)) != ERROR_OK)
+			goto mg_write_cmd_err;
+	}
 
-	fileio_close(&fileio);
+	if (duration_measure(&bench) == ERROR_OK)
+	{
+		command_print(cmd_ctx, "wrote %lli byte from file %s "
+				"in %fs (%0.3f kB/s)", fileio.size, args[1],
+				duration_elapsed(&bench), duration_kbps(&bench, fileio.size));
+	}
 
 	free(buffer);
+	fileio_close(&fileio);
 
 	return ERROR_OK;
+
+mg_write_cmd_err:
+ 	free(buffer);
+	fileio_close(&fileio);
+
+	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;
-	u8 *buffer;
-	/* TODO : multi-bank support */
+	uint32_t address, size_written, size, cnt, res, i;
+	uint8_t *buffer;
 	fileio_t fileio;
-	duration_t duration;
-	char *duration_text;
+	int ret;
 
 	if (argc != 4) {
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
-	address = strtoul(args[2], NULL, 0);
-	size = strtoul(args[3], NULL, 0);
+	COMMAND_PARSE_NUMBER(u32, args[2], address);
+	COMMAND_PARSE_NUMBER(u32, args[3], size);
+
+	ret = fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY);
+	if (ret != ERROR_OK)
+		return ret;
 
-	if (fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK) {
+	buffer = malloc(MG_FILEIO_CHUNK);
+	if (!buffer) {
+		fileio_close(&fileio);
 		return ERROR_FAIL;
 	}
 
-	buffer = malloc(size);
+	cnt = size / MG_FILEIO_CHUNK;
+	res = size % MG_FILEIO_CHUNK;
 
-	duration_start_measure(&duration);
+	struct duration bench;
+	duration_start(&bench);
 
-	mg_mflash_read(address, buffer, size);
+	for (i = 0; i < cnt; i++) {
+		if ((ret = mg_mflash_read(address, buffer, MG_FILEIO_CHUNK)) != ERROR_OK)
+			goto mg_dump_cmd_err;
+		if ((ret = fileio_write(&fileio, MG_FILEIO_CHUNK, buffer, &size_written))
+				!= ERROR_OK)
+			goto mg_dump_cmd_err;
+		address += MG_FILEIO_CHUNK;
+	}
 
-	duration_stop_measure(&duration, &duration_text);
+	if (res) {
+		if ((ret = mg_mflash_read(address, buffer, res)) != ERROR_OK)
+			goto mg_dump_cmd_err;
+		if ((ret = fileio_write(&fileio, res, buffer, &size_written)) != ERROR_OK)
+			goto mg_dump_cmd_err;
+	}
 
-	fileio_write(&fileio, size, buffer, &size_written);
+	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, args[1],
+				duration_elapsed(&bench), duration_kbps(&bench, size));
+	}
 
-	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)));
+	free(buffer);
+	fileio_close(&fileio);
 
-	free(duration_text);
+	return ERROR_OK;
 
+mg_dump_cmd_err:
+ 	free(buffer);
 	fileio_close(&fileio);
 
-	free(buffer);
-
-	return ERROR_OK;
+	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;
+	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)
+	if ((ret = mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		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)
@@ -783,7 +856,7 @@ static int mg_is_valid_pll(double XIN, int N, double CLK_OUT, int NO)
 	double v2 = CLK_OUT * NO;
 
 	if (v1 <1000000 || v1 > 15000000 || v2 < 100000000 || v2 > 500000000)
-		return ERROR_FAIL;
+		return ERROR_MG_INVALID_PLL;
 
 	return ERROR_OK;
 }
@@ -792,7 +865,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;
@@ -813,7 +886,7 @@ 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;
@@ -844,7 +917,7 @@ static double mg_do_calc_pll(double XIN, mg_pll_t * p_pll_val, int is_approximat
 
 				CLK_OUT = XIN * ((double)M / N) / NO;
 
-				if ((int)((CLK_OUT+ROUND) / DIV)
+				if ((int)((CLK_OUT + ROUND) / DIV)
 						== (int)(MG_PLL_CLK_OUT / DIV))	{
 					if (mg_is_valid_pll(XIN, N, CLK_OUT, NO) == ERROR_OK)
 					{
@@ -877,33 +950,38 @@ 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;
+	uint16_t buff[MG_MFLASH_SECTOR_SIZE >> 1];
+	uint16_t i, j;
+	uint32_t address = mflash_bank->base + MG_BUFFER_OFFSET;
 	target_t *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] = {
@@ -1033,157 +1111,158 @@ static void mg_gen_ataid(mg_io_type_drv_info *pSegIdDrvInfo)
 
 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)
+	if ((ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		return ret;
 
 	mg_gen_ataid((mg_io_type_drv_info *)buff);
 
-	if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_stgdrvinfo)
+	if ((ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_stgdrvinfo))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		return ret;
 
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
+	if ((ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		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)
+	if ((ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		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 *)(buff + 4)) = 0;		/* XIP size */
 
-	if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_xipinfo)
+	if ((ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_xipinfo))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		return ret;
 
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
+	if ((ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		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)
+	if ((ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		return ret;
 
-	if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_wr_pll)
+	if ((ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_wr_pll))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		return ret;
 
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
+	if ((ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		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)
+	int ret;
+
+	if ((ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		return ret;
 
-	if (mg_mflash_do_write_sects(NULL, 0, 0, mg_vcmd_purge_nand)
+	if ((ret = mg_mflash_do_write_sects(NULL, 0, 0, mg_vcmd_purge_nand))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		return ret;
 
-	if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
+	if ((ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default))
 			!= ERROR_OK)
-		return ERROR_FAIL;
+		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;
+	if ((ret = mg_verify_interface()) != ERROR_OK)
+		return ret;
 
-	if (mg_mflash_rst() != ERROR_OK)
-		return ERROR_FAIL;
+	if ((ret = mg_mflash_rst()) != ERROR_OK)
+		return ret;
 
 	switch (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(args[1], "boot"))
+				return mg_boot_config();
+			else if (!strcmp(args[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);
+				unsigned long freq;
+				COMMAND_PARSE_NUMBER(ulong, args[2], freq);
+				fin = freq;
 
-				if (fin > MG_PLL_CLK_OUT)
-					return ERROR_FAIL;
+				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;
-
-				if (mg_set_pll(&pll) != ERROR_OK)
-					return ERROR_FAIL;
+				if ((ret = mg_erase_nand()) != ERROR_OK)
+					return ret;
 
-				return ERROR_OK;
+				return mg_set_pll(&pll);
 			} else
 				return ERROR_COMMAND_NOTFOUND;
 			break;
 		default:
 			return ERROR_COMMAND_SYNTAX_ERROR;
 	}
-
-	return ERROR_OK;
 }
 
 int mflash_init_drivers(struct command_context_s *cmd_ctx)
@@ -1201,10 +1280,9 @@ int mflash_init_drivers(struct command_context_s *cmd_ctx)
 	return ERROR_OK;
 }
 
-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;
 	int i;
 
 	if (argc < 4)
@@ -1219,10 +1297,12 @@ static int mg_bank_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args
 	}
 
 	mflash_bank = calloc(sizeof(mflash_bank_t), 1);
-	mflash_bank->base = strtoul(args[1], NULL, 0);
+	COMMAND_PARSE_NUMBER(u32, args[1], mflash_bank->base);
+	/// @todo Verify how this parsing should work, then document it.
+	char *str;
 	mflash_bank->rst_pin.num = strtoul(args[2], &str, 0);
 	if (*str)
-		mflash_bank->rst_pin.port[0] = (u16)tolower(str[0]);
+		mflash_bank->rst_pin.port[0] = (uint16_t)tolower(str[0]);
 
 	mflash_bank->target = target;
 
@@ -1234,7 +1314,7 @@ static int mg_bank_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args
 
 	if (! mflash_bank->gpio_drv) {
 		LOG_ERROR("%s is unsupported soc", args[0]);
-		return ERROR_INVALID_ARGUMENTS;
+		return ERROR_MG_UNSUPPORTED_SOC;
 	}
 
 	return ERROR_OK;