diff --git a/usbboot/src/Makefile.am b/usbboot/src/Makefile.am
index 90e256a..97789ae 100644
--- a/usbboot/src/Makefile.am
+++ b/usbboot/src/Makefile.am
@@ -1,4 +1,4 @@
-AM_CFLAGS = -pedantic -Wall -W -O1 -g3 -std=gnu99 -lusb -lconfuse
+AM_CFLAGS = -pedantic -Wall -Werror -O1 -g3 -std=gnu99 -lusb -lconfuse
 
 xburst-tools_version.h:
 	echo -e '#ifndef XBURST_TOOLS_VERSION' \
@@ -6,9 +6,12 @@ xburst-tools_version.h:
 	'\n#endif' > xburst-tools_version.h
 BUILT_SOURCES = xburst-tools_version.h
 
-bin_PROGRAMS = usbboot
-usbboot_SOURCES = cmd.c  command_line.c  ingenic_cfg.c  \
-	ingenic_usb.c  main.c
+bin_PROGRAMS = usbboot usbboot_server
+usbboot_SOURCES = cmd.c command_line.c ingenic_cfg.c \
+	ingenic_usb.c main.c nand.c mem.c
+
+usbboot_server_SOURCES = cmd.c ingenic_cfg.c \
+	ingenic_usb.c server.c nand.c mem.c
 
 prefix = /usr
 datadir = /usr/share/xburst-tools
diff --git a/usbboot/src/cmd.c b/usbboot/src/cmd.c
index e985556..67cdbec 100644
--- a/usbboot/src/cmd.c
+++ b/usbboot/src/cmd.c
@@ -31,26 +31,13 @@
 #include "ingenic_usb.h"
 #include "usb_boot_defines.h"
 
-extern int com_argc;
-extern char com_argv[MAX_ARGC][MAX_COMMAND_LENGTH];
-
-struct ingenic_dev ingenic_dev;
 struct hand hand;
 struct sdram_in sdram_in;
-struct nand_in nand_in;
-static struct nand_out nand_out;
 
 unsigned int total_size;
-unsigned char code_buf[4 * 512 * 1024];
-unsigned char check_buf[4 * 512 * 1024];
-unsigned char cs[16];
-unsigned char ret[8];
+static char code_buf[4 * 512 * 1024];
+static char ret[8];
 
-static const char IMAGE_TYPE[][30] = {
-	"with oob and ecc",
-	"with oob and without ecc",
-	"without oob",
-};
 
 static int load_file(struct ingenic_dev *ingenic_dev, const char *file_path)
 {
@@ -78,7 +65,7 @@ static int load_file(struct ingenic_dev *ingenic_dev, const char *file_path)
 
 	status = read(fd, ingenic_dev->file_buff, ingenic_dev->file_len);
 
-	if (status < ingenic_dev->file_len) {
+	if (status < (int)ingenic_dev->file_len) {
 		fprintf(stderr, "Error - can't read file '%s': %s\n", 
 			file_path, strerror(errno));
 		goto close;
@@ -97,22 +84,20 @@ out:
 }
 
 /* after upload stage2. must init device */
-void init_cfg()
+void init_cfg(struct ingenic_dev *dev)
 {
-	if (usb_get_ingenic_cpu(&ingenic_dev) < 3) {
+	if (usb_get_ingenic_cpu(dev) < 3) {
 		printf(" XBurst CPU not booted yet, boot it first!\n");
 		return;
 	}
 
-	ingenic_dev.file_buff = &hand;
-	ingenic_dev.file_len = sizeof(hand);
-	if (usb_send_data_to_ingenic(&ingenic_dev) != 1)
+	if (usb_send_data_to_ingenic(dev, (char*)&hand, sizeof(hand)) != 1)
 		goto xout;
 
-	if (usb_ingenic_configration(&ingenic_dev, DS_hand) != 1)
+	if (usb_ingenic_configration(dev, DS_hand) != 1)
 		goto xout;
 
-	if (usb_read_data_from_ingenic(&ingenic_dev, ret, 8) != 1)
+	if (usb_read_data_from_ingenic(dev, ret, 8) != 1)
 		goto xout;
 
 	printf(" Configuring XBurst CPU succeeded.\n");
@@ -121,10 +106,11 @@ xout:
 	printf("Configuring XBurst CPU failed.\n");
 }
 
-int boot(char *stage1_path, char *stage2_path){
+int boot(struct ingenic_dev *dev, const char *stage1_path, const char *stage2_path)
+{
 	int status;
 
-	status = usb_get_ingenic_cpu(&ingenic_dev);
+	status = usb_get_ingenic_cpu(dev);
 	switch (status)	{
 	case 1:            /* Jz4740v1 */
 		status = 0;
@@ -154,596 +140,36 @@ int boot(char *stage1_path, char *stage2_path){
 
 		/* now we upload the boot stage1 */
 		printf(" Loading stage1 from '%s'\n", stage1_path);
-		if (load_file(&ingenic_dev, stage1_path) < 1)
+		if (load_file(dev, stage1_path) < 1)
 			return -1;
 
-		if (usb_ingenic_upload(&ingenic_dev, 1) < 1)
+		if (usb_ingenic_upload(dev, 1) < 1)
 			return -1;
 
 		/* now we upload the boot stage2 */
 		usleep(100);
 		printf(" Loading stage2 from '%s'\n", stage2_path);
-		if (load_file(&ingenic_dev, stage2_path) < 1)
+		if (load_file(dev, stage2_path) < 1)
 			return -1;
 
-		if (usb_ingenic_upload(&ingenic_dev, 2) < 1)
+		if (usb_ingenic_upload(dev, 2) < 1)
 			return -1;
 
 		printf(" Booted successfully!\n");
 	}
 	usleep(100);
-	init_cfg();
+	init_cfg(dev);
 	return 1;
 }
 
-/* nand function  */
-int error_check(unsigned char *org,unsigned char * obj,unsigned int size)
+
+int debug_memory(struct ingenic_dev *dev, int obj, unsigned int start, unsigned int size)
 {
-	unsigned int i;
-	printf(" Comparing %d bytes - ", size);
-	for (i = 0; i < size; i++) {
-		if (org[i] != obj[i]) {
-			unsigned int s = (i < 8) ? i : i - 8; // start_dump
-			printf("FAIL at off %d, wrote 0x%x, read 0x%x\n", i, org[i], obj[i]);
-			printf("  off %d write: %02x %02x %02x %02x %02x %02x %02x %02x"
-			       " %02x %02x %02x %02x %02x %02x %02x %02x\n", s,
-				org[s], org[s+1], org[s+2], org[s+3], org[s+4], org[s+5], org[s+6], org[s+7],
-				org[s+8], org[s+9], org[s+10], org[s+11], org[s+12], org[s+13], org[s+14], org[s+15]);
-			printf("  off %d read:  %02x %02x %02x %02x %02x %02x %02x %02x"
-			       " %02x %02x %02x %02x %02x %02x %02x %02x\n", s,
-				obj[s], obj[s+1], obj[s+2], obj[s+3], obj[s+4], obj[s+5], obj[s+6], obj[s+7],
-				obj[s+8], obj[s+9], obj[s+10], obj[s+11], obj[s+12], obj[s+13], obj[s+14], obj[s+15]);
-			return 0;
-		}
-	}
-	printf("SUCCESS\n");
-	return 1;
-}
+	char buffer[8], tmp;
 
-int nand_markbad(struct nand_in *nand_in)
-{
-	if (usb_get_ingenic_cpu(&ingenic_dev) < 3) {
-		printf(" Device unboot! Boot it first!\n");
-		return -1;
-	}
-	printf(" mark bad block : %d\n",nand_in->start);
-	usb_send_data_address_to_ingenic(&ingenic_dev, nand_in->start);
-	usb_ingenic_nand_ops(&ingenic_dev, NAND_MARK_BAD);
-	usb_read_data_from_ingenic(&ingenic_dev, ret, 8);
-	printf(" Mark bad block at %d\n",((ret[3] << 24) | 
-					   (ret[2] << 16) | 
-					   (ret[1] << 8)  | 
-					   (ret[0] << 0)) / hand.nand_ppb);
-	return 0;
-}
+    (void)obj;
 
-int nand_program_check(struct nand_in *nand_in,
-		       struct nand_out *nand_out,
-		       unsigned int *start_page)
-{
-	unsigned int i, page_num, cur_page = -1;
-	unsigned short temp;
-
-	printf(" Writing NAND page %d len %d...\n", nand_in->start, nand_in->length);
-	if (nand_in->length > (unsigned int)MAX_TRANSFER_SIZE) {
-		printf(" Buffer size too long!\n");
-		return -1;
-	}
-
-#ifdef CONFIG_NAND_OUT
-	unsigned char status_buf[32];
-	nand_out->status = status_buf;
-	for (i = 0; i < nand_in->max_chip; i++)
-		(nand_out->status)[i] = 0; /* set all status to fail */
-#endif
-
-	if (usb_get_ingenic_cpu(&ingenic_dev) < 3) {
-		printf(" Device unboot! Boot it first!\n");
-		return -1;
-	}
-	ingenic_dev.file_buff = nand_in->buf;
-	ingenic_dev.file_len = nand_in->length;
-	usb_send_data_to_ingenic(&ingenic_dev);
-	for (i = 0; i < nand_in->max_chip; i++) {
-		if ((nand_in->cs_map)[i]==0) 
-			continue;
-		if (nand_in->option == NO_OOB) {
-			page_num = nand_in->length / hand.nand_ps;
-			if ((nand_in->length % hand.nand_ps) !=0) 
-				page_num++;
-		} else {
-			page_num = nand_in->length / 
-				(hand.nand_ps + hand.nand_os);
-			if ((nand_in->length% (hand.nand_ps + hand.nand_os)) !=0)
-				page_num++;
-		}
-		temp = ((nand_in->option << 12) & 0xf000)  + 
-			((i<<4) & 0xff0) + NAND_PROGRAM;	
-		usb_send_data_address_to_ingenic(&ingenic_dev, nand_in->start);
-		usb_send_data_length_to_ingenic(&ingenic_dev, page_num);
-		usb_ingenic_nand_ops(&ingenic_dev, temp);
-
-		usb_read_data_from_ingenic(&ingenic_dev, ret, 8);
-		printf(" Finish! (len %d start_page %d page_num %d)\n", 
-		       nand_in->length, nand_in->start, page_num);
-
-		usb_send_data_address_to_ingenic(&ingenic_dev, nand_in->start);
-		/* Read back to check! */
-		usb_send_data_length_to_ingenic(&ingenic_dev, page_num);
-
-		switch (nand_in->option) {
-		case OOB_ECC:
-			temp = ((OOB_ECC << 12) & 0xf000) +
-				((i << 4) & 0xff0) + NAND_READ;
-			usb_ingenic_nand_ops(&ingenic_dev, temp);
-			printf(" Checking %d bytes...", nand_in->length);
-			usb_read_data_from_ingenic(&ingenic_dev, check_buf, 
-						   page_num * (hand.nand_ps + hand.nand_os));
-			usb_read_data_from_ingenic(&ingenic_dev, ret, 8);
-			break;
-		case OOB_NO_ECC:	/* do not support data verify */
-			temp = ((OOB_NO_ECC << 12) & 0xf000) + 
-				((i << 4) & 0xff0) + NAND_READ;
-			usb_ingenic_nand_ops(&ingenic_dev, temp);
-			printf(" Checking %d bytes...", nand_in->length);
-			usb_read_data_from_ingenic(&ingenic_dev, check_buf, 
-						   page_num * (hand.nand_ps + hand.nand_os));
-			usb_read_data_from_ingenic(&ingenic_dev, ret, 8);
-			break;
-		case NO_OOB:
-			temp = ((NO_OOB << 12) & 0xf000) + 
-				((i << 4) & 0xff0) + NAND_READ;
-			usb_ingenic_nand_ops(&ingenic_dev, temp);
-			printf(" Checking %d bytes...", nand_in->length);
-			usb_read_data_from_ingenic(&ingenic_dev, check_buf, 
-						   page_num * hand.nand_ps);
-			usb_read_data_from_ingenic(&ingenic_dev, ret, 8);
-			break;
-		default:
-			;
-		}
-
-		cur_page = (ret[3] << 24) | (ret[2] << 16) |  (ret[1] << 8) | 
-			(ret[0] << 0);
-
-#ifdef CONFIG_NAND_OUT
-		(nand_out->status)[i] = 1;
-#endif
-
-		if (nand_in->start < 1 && 
-		    hand.nand_ps == 4096 && 
-		    hand.fw_args.cpu_id == 0x4740) {
-			printf(" no check! End at Page: %d\n", cur_page);
-			continue;
-		}
-
-		if (!nand_in->check(nand_in->buf, check_buf, nand_in->length)) {
-#ifdef CONFIG_NAND_OUT
-			(nand_out->status)[i] = 0;
-#endif
-			struct nand_in bad;
-			// tbd: doesn't the other side skip bad blocks too? Can we just deduct 1 from cur_page?
-			// tbd: why do we only mark a block as bad if the last page in the block was written?
-			bad.start = (cur_page - 1) / hand.nand_ppb;
-			if (cur_page % hand.nand_ppb == 0)
-				nand_markbad(&bad);
-		}
-
-		printf(" End at Page: %d\n",cur_page);
-	}
-
-	*start_page = cur_page;
-	return 0;
-}
-
-int nand_erase(struct nand_in *nand_in)
-{
-	unsigned int start_blk, blk_num, end_block;
-	int i;
-
-	start_blk = nand_in->start;
-	blk_num = nand_in->length;
-	if (start_blk > (unsigned int)NAND_MAX_BLK_NUM)  {
-		printf(" Start block number overflow!\n");
-		return -1;
-	}
-	if (blk_num > (unsigned int)NAND_MAX_BLK_NUM) {
-		printf(" Length block number overflow!\n");
-		return -1;
-	}
-
-	if (usb_get_ingenic_cpu(&ingenic_dev) < 3) {
-		printf(" Device unboot! Boot it first!\n");
-		return -1;
-	}
-
-	for (i = 0; i < nand_in->max_chip; i++) {
-		if ((nand_in->cs_map)[i]==0) 
-			continue;
-		printf(" Erasing No.%d device No.%d flash (start_blk %u blk_num %u)......\n",
-		       nand_in->dev, i, start_blk, blk_num);
-
-		usb_send_data_address_to_ingenic(&ingenic_dev, start_blk);
-		usb_send_data_length_to_ingenic(&ingenic_dev, blk_num);
-
-		unsigned short temp = ((i << 4) & 0xff0) + NAND_ERASE;
-		usb_ingenic_nand_ops(&ingenic_dev, temp);
-
-		usb_read_data_from_ingenic(&ingenic_dev, ret, 8);
-		printf(" Finish!");
-	}
-	end_block = ((ret[3] << 24) |
-		     (ret[2] << 16) |
-		     (ret[1] << 8)  | 
-		     (ret[0] << 0)) / hand.nand_ppb;
-	printf(" Return: %02x %02x %02x %02x %02x %02x %02x %02x (position %d)\n", 
-	       ret[0], ret[1], ret[2], ret[3], ret[4], ret[5], ret[6], ret[7], end_block);
-	if (!hand.nand_force_erase) {	
-	/* not force erase, show bad block infomation */
-		printf(" There are marked bad blocks: %d\n", 
-		       end_block - start_blk - blk_num );
-	} else {
-	/* force erase, no bad block infomation can show */
-		printf(" Force erase, no bad block infomation!\n" );
-	}
-
-	return 1;
-}
-
-int nand_program_file(struct nand_in *nand_in,
-		      struct nand_out *nand_out,
-		      char *fname)
-{
-
-	int flen, m, j, k;
-	unsigned int start_page = 0, page_num, code_len, offset, transfer_size;
-	int fd, status;
-	struct stat fstat;
-	struct nand_in n_in;
-	struct nand_out n_out;
-
-#ifdef CONFIG_NAND_OUT
-	unsigned char status_buf[32];
-	nand_out->status = status_buf;
-	for (i=0; i<nand_in->max_chip; i++)
-		(nand_out->status)[i] = 0; /* set all status to fail */
-#endif
-	status = stat(fname, &fstat);
-
-	if (status < 0) {
-		fprintf(stderr, "Error - can't get file size from '%s': %s\n",
-			fname, strerror(errno));
-		return -1;
-	}
-	flen = fstat.st_size;
-
-	fd = open(fname, O_RDONLY);
-	if (fd < 0) {
-		fprintf(stderr, "Error - can't open file '%s': %s\n", 
-			fname, strerror(errno));
-		return -1;
-	}
-
-	printf(" Programing No.%d device, flen %d, start page %d...\n",nand_in->dev, flen, nand_in->start);
-	n_in.start = nand_in->start / hand.nand_ppb; 
-	if (nand_in->option == NO_OOB) {
-		if (flen % (hand.nand_ppb * hand.nand_ps) == 0) 
-			n_in.length = flen / (hand.nand_ps * hand.nand_ppb);
-		else
-			n_in.length = flen / (hand.nand_ps * hand.nand_ppb) + 1;
-	} else {
-		if (flen % (hand.nand_ppb * (hand.nand_ps + hand.nand_os)) == 0) 
-			n_in.length = flen / 
-				((hand.nand_ps + hand.nand_os) * hand.nand_ppb);
-		else
-			n_in.length = flen / 
-				((hand.nand_ps + hand.nand_os) * hand.nand_ppb)
-				+ 1;
-	}
-	/* printf(" length %d flen %d\n", n_in.length, flen); */
-	n_in.cs_map = nand_in->cs_map;
-	n_in.dev = nand_in->dev;
-	n_in.max_chip = nand_in->max_chip;
-	if (nand_erase(&n_in) != 1)
-		return -1;
-	if (nand_in->option == NO_OOB)
-		transfer_size = (hand.nand_ppb * hand.nand_ps);
-	else
-		transfer_size = (hand.nand_ppb * (hand.nand_ps + hand.nand_os));
-
-	m = flen / transfer_size;
-	j = flen % transfer_size;
-	printf(" Size to send %d, transfer_size %d\n", flen, transfer_size);
-	printf(" Image type : %s\n", IMAGE_TYPE[nand_in->option]);
-	printf(" It will cause %d times buffer transfer.\n", j == 0 ? m : m + 1);
-
-#ifdef CONFIG_NAND_OUT
-	for (i = 0; i < nand_in->max_chip; i++)
-		(nand_out->status)[i] = 1; /* set all status to success! */
-#endif
-
-	offset = 0; 
-	for (k = 0; k < m; k++)	{
-		if (nand_in->option == NO_OOB)
-			page_num = transfer_size / hand.nand_ps;
-		else
-			page_num = transfer_size / (hand.nand_ps + hand.nand_os);
-
-		code_len = transfer_size;
-		status = read(fd, code_buf, code_len);
-		if (status < code_len) {
-			fprintf(stderr, "Error - can't read file '%s': %s\n", 
-				fname, strerror(errno));
-			return -1;
-		}
-
-		nand_in->length = code_len; /* code length,not page number! */
-		nand_in->buf = code_buf;
-		if (nand_program_check(nand_in, &n_out, &start_page) == -1)
-			return -1;
-
-		if (start_page - nand_in->start > hand.nand_ppb)
-			printf(" Skip a old bad block !\n");
-		nand_in->start = start_page;
-
-#ifdef CONFIG_NAND_OUT
-		for (i = 0; i < nand_in->max_chip; i++) {
-			(nand_out->status)[i] = (nand_out->status)[i] * 
-				(n_out.status)[i];
-		}
-#endif
-		offset += code_len ;
-	}
-
-	if (j) {
-		code_len = j;
-		if (j % hand.nand_ps)
-			j += hand.nand_ps - (j % hand.nand_ps);
-		memset(code_buf, 0, j);		/* set all to null */
-
-		status = read(fd, code_buf, code_len);
-
-		if (status < code_len) {
-			fprintf(stderr, "Error - can't read file '%s': %s\n", 
-				fname, strerror(errno));
-			return -1;
-		}
-
-		nand_in->length = j;
-		nand_in->buf = code_buf;
-		if (nand_program_check(nand_in, &n_out, &start_page) == -1) 
-			return -1;
-
-		if (start_page - nand_in->start > hand.nand_ppb)
-			printf(" Skip a old bad block !");
-
-#ifdef CONFIG_NAND_OUT
-		for (i=0; i < nand_in->max_chip; i++) {
-			(nand_out->status)[i] = (nand_out->status)[i] *
-				(n_out.status)[i];
-		}
-#endif
-	}
-	
-	close(fd);
-	return 1;
-}
-
-int nand_program_file_planes(struct nand_in *nand_in,
-		      struct nand_out *nand_out,
-		      char *fname)
-{
-	printf("  not implement yet !\n");
-	return -1;
-}
-
-int init_nand_in(void)
-{
-	nand_in.buf = code_buf;
-	nand_in.check = error_check;
-	nand_in.dev = 0;
-	nand_in.cs_map = cs;
-	memset(nand_in.cs_map, 0, MAX_DEV_NUM);
-
-	nand_in.max_chip = 16;
-	return 0;
-}
-
-int nand_prog(void)
-{
-	char *image_file;
-	char *help = " Usage: nprog (1) (2) (3) (4) (5)\n"
-		" (1)\tstart page number\n"
-		" (2)\timage file name\n"
-		" (3)\tdevice index number\n"
-		" (4)\tflash index number\n"
-		" (5) image type must be:\n"
-		" \t-n:\tno oob\n"
-		" \t-o:\twith oob no ecc\n"
-		" \t-e:\twith oob and ecc\n";
-
-	if (com_argc != 6) {
-		printf(" not enough argument.\n");
-		printf("%s", help);
-		return 0;
-	}
-
-	init_nand_in();
-
-	nand_in.start = atoi(com_argv[1]);
-	image_file = com_argv[2];
-	nand_in.dev = atoi(com_argv[3]);
-	(nand_in.cs_map)[atoi(com_argv[4])] = 1;
-	if (!strcmp(com_argv[5], "-e")) 
-		nand_in.option = OOB_ECC;
-	else if (!strcmp(com_argv[5], "-o")) 
-		nand_in.option = OOB_NO_ECC;
-	else if (!strcmp(com_argv[5], "-n")) 
-		nand_in.option = NO_OOB;
-	else
-		printf("%s", help);
-
-	if (hand.nand_plane > 1)
-		nand_program_file_planes(&nand_in, &nand_out, image_file);
-	else
-		nand_program_file(&nand_in, &nand_out, image_file);
-
-#ifdef CONFIG_NAND_OUT
-	printf(" Flash check result:\n");
-	int i;
-	for (i = 0; i < 16; i++)
-		printf(" %d", (nand_out.status)[i]);
-#endif
-
-	return 1;
-}
-
-int nand_query(void)
-{
-	int i;
-	unsigned char csn;
-
-	if (com_argc < 3) {
-		printf(" Usage: nquery (1) (2)\n"
-		       " (1):device index number\n"
-		       " (2):flash index number\n"); 
-		return -1;
-	}
-	init_nand_in();
-
-	nand_in.dev = atoi(com_argv[1]);
-	(nand_in.cs_map)[atoi(com_argv[2])] = 1;
-
-	for (i = 0; i < nand_in.max_chip; i++) {
-		if ((nand_in.cs_map)[i] != 0) 
-			break;
-	}
-	if (i >= nand_in.max_chip) 
-		return -1;
-
-	if (usb_get_ingenic_cpu(&ingenic_dev) < 3) {
-		printf(" Device unboot! Boot it first!\n");
-		return -1;
-	}
-
-	csn = i;
-	printf(" ID of No.%d device No.%d flash: \n", nand_in.dev, csn);
-
-	unsigned short ops = ((csn << 4) & 0xff0) + NAND_QUERY;
-	usb_ingenic_nand_ops(&ingenic_dev, ops);
-	usb_read_data_from_ingenic(&ingenic_dev, ret, 8);
-	printf(" Vendor ID    :0x%x \n",(unsigned char)ret[0]);
-	printf(" Product ID   :0x%x \n",(unsigned char)ret[1]);
-	printf(" Chip ID      :0x%x \n",(unsigned char)ret[2]);
-	printf(" Page ID      :0x%x \n",(unsigned char)ret[3]);
-	printf(" Plane ID     :0x%x \n",(unsigned char)ret[4]);
-
-	usb_read_data_from_ingenic(&ingenic_dev, ret, 8);
-	printf(" Operation status: Success!\n");
-
-	return 1;
-}
-
-int nand_read(int mode)
-{
-	unsigned int i,j;
-	unsigned int start_addr, length, page_num;
-	unsigned char csn;
-	unsigned short temp = 0;
-	unsigned ram_addr = 0;
-
-	if (com_argc < 5) {
-		printf(" Usage: nread (1) (2) (3) (4)\n"
-		       " 1:start page number\n"
-		       " 2:length in byte\n"
-		       " 3:device index number\n"
-		       " 4:flash index number\n"
-		       " 5:start SDRAM address\n");
-		return -1;
-	}
-	init_nand_in();
-
-	if (atoi(com_argv[4]) >= MAX_DEV_NUM) {
-		printf(" Flash index number overflow!\n");
-		return -1;
-	}
-	(nand_in.cs_map)[atoi(com_argv[4])] = 1;
-	nand_in.start = atoi(com_argv[1]);
-	nand_in.length= atoi(com_argv[2]);
-	nand_in.dev = atoi(com_argv[3]);
-
-	if (com_argc = 6) {
-		ram_addr = strtoul(com_argv[5], NULL, 0);
-		printf("==%s==", com_argv[5]);
-	}
-	start_addr = nand_in.start;
-	length = nand_in.length;
-
-	if (start_addr > NAND_MAX_PAGE_NUM || length > NAND_MAX_PAGE_NUM ) {
-		printf(" Page number overflow!\n");
-		return -1;
-	}
-	if (usb_get_ingenic_cpu(&ingenic_dev) < 3) {
-		printf(" Device unboot! Boot it first!\n");
-		return -1;
-	}
-	for (i = 0; i < nand_in.max_chip; i++) 
-		if ((nand_in.cs_map)[i] != 0) 
-			break;
-	if (i >= nand_in.max_chip) return 1;
-	csn = i;
-	printf(" Reading from No.%d device No.%d flash....\n",nand_in.dev,csn);
-
-	page_num = length / hand.nand_ps +1;
-
-	switch(mode) {
-	case NAND_READ:
-		temp = ((NO_OOB<<12) & 0xf000) + ((csn<<4) & 0xff0) + NAND_READ;
-		break;
-	case NAND_READ_OOB:
-		temp = ((csn<<4) & 0xff0) + NAND_READ_OOB;
-		break;
-	case NAND_READ_RAW:
-		temp = ((NO_OOB<<12) & 0xf000) + ((csn<<4) & 0xff0) + 
-			NAND_READ_RAW;
-		break;
-	case NAND_READ_TO_RAM:
-		temp = ((NO_OOB<<12) & 0xf000) + ((csn<<4) & 0xff0) + 
-			NAND_READ_TO_RAM;
-		printf(" Reading nand to RAM: 0x%x\n", ram_addr);
-		usb_ingenic_start(&ingenic_dev, VR_PROGRAM_START1, ram_addr);
-		break;
-	default:
-		printf(" unknow mode!\n");
-		return -1;
-	}
-
-	usb_send_data_address_to_ingenic(&ingenic_dev, start_addr);
-	usb_send_data_length_to_ingenic(&ingenic_dev, page_num);
-
-	usb_ingenic_nand_ops(&ingenic_dev, temp);
-
-	usb_read_data_from_ingenic(&ingenic_dev, nand_in.buf, page_num * hand.nand_ps);
-
-	for (j = 0; j < length; j++) {
-		if (j % 16 == 0)
-		printf("\n 0x%08x : ",j);
-		printf("%02x ",(nand_in.buf)[j]);
-	}
-	printf("\n");
-
-	usb_read_data_from_ingenic(&ingenic_dev, ret, 8);
-	printf(" Operation end position : %d \n",
-	       (ret[3]<<24)|(ret[2]<<16)|(ret[1]<<8)|(ret[0]<<0));
-
-	return 1;
-}
-
-int debug_memory(int obj, unsigned int start, unsigned int size)
-{
-	unsigned int buffer[8],tmp;
-
-	tmp = usb_get_ingenic_cpu(&ingenic_dev);
+	tmp = usb_get_ingenic_cpu(dev);
 	if (tmp  > 2) {
 		printf(" This command only run under UNBOOT state!\n");
 		return -1;
@@ -771,13 +197,13 @@ int debug_memory(int obj, unsigned int start, unsigned int size)
 	printf(" Now test memory from 0x%x to 0x%x: \n",
 	       start, start + hand.fw_args.size);
 
-	if (load_file(&ingenic_dev, STAGE1_FILE_PATH) < 1)
+	if (load_file(dev, STAGE1_FILE_PATH) < 1)
 		return -1;
-	if (usb_ingenic_upload(&ingenic_dev, 1) < 1)
+	if (usb_ingenic_upload(dev, 1) < 1)
 		return -1;
 
 	usleep(100);
-	usb_read_data_from_ingenic(&ingenic_dev, buffer, 8);
+	usb_read_data_from_ingenic(dev, buffer, 8);
 	if (buffer[0] != 0)
 		printf(" Test memory fail! Last error address is 0x%x !\n",
 		       buffer[0]);
@@ -787,75 +213,30 @@ int debug_memory(int obj, unsigned int start, unsigned int size)
 	return 1;
 }
 
-int debug_gpio(int obj, unsigned char ops, unsigned char pin)
-{
-	unsigned int tmp;
-
-	tmp = usb_get_ingenic_cpu(&ingenic_dev);
-	if (tmp  > 2) {
-		printf(" This command only run under UNBOOT state!\n");
-		return -1;
-	}
-
-	switch (tmp) {
-	case 1:
-		tmp = 0;
-		hand.fw_args.cpu_id = 0x4740;
-		if (pin > 124) {
-			printf(" Jz4740 has 124 GPIO pin in all!\n");
-			return -1;
-		}
-		break;
-	case 2:
-		tmp = 0;
-		hand.fw_args.cpu_id = 0x4750;
-		if (pin > 178) {
-			printf(" Jz4750 has 178 GPIO pin in all!\n");
-			return -1;
-		}
-		break;
-	}
-
-	hand.fw_args.debug_ops = ops;/* tell device it's memory debug */
-	hand.fw_args.pin_num = pin;
-
-	if (ops == 2)
-		printf(" GPIO %d set!\n",pin);
-	else
-		printf(" GPIO %d clear!\n",pin);
-
-	if (load_file(&ingenic_dev, STAGE1_FILE_PATH) < 1)
-		return -1;
-	if (usb_ingenic_upload(&ingenic_dev, 1) < 1)
-		return -1;
-
-	return 0;
-}
-
-int debug_go(void)
+int debug_go(struct ingenic_dev *dev, size_t argc, char *argv[])
 {
 	unsigned int addr,obj;
-	if (com_argc<3) {
+	if (argc < 3) {
 		printf(" Usage: go (1) (2) \n"
 		       " 1:start SDRAM address\n"
 		       " 2:device index number\n");
 		return 0;
 	}
 
-	addr = strtoul(com_argv[1], NULL, 0);
-	obj = atoi(com_argv[2]);
+	addr = strtoul(argv[1], NULL, 0);
+	obj = atoi(argv[2]);
 
 	printf(" Executing No.%d device at address 0x%x\n", obj, addr);
 
-	if (usb_ingenic_start(&ingenic_dev, VR_PROGRAM_START2, addr) < 1)
+	if (usb_ingenic_start(dev, VR_PROGRAM_START2, addr) < 1)
 		return -1;
 
 	return 1;
 }
 
-int sdram_load(struct sdram_in *sdram_in)
+int sdram_load(struct ingenic_dev *dev, struct sdram_in *sdram_in)
 {
-	if (usb_get_ingenic_cpu(&ingenic_dev) < 3) {
+	if (usb_get_ingenic_cpu(dev) < 3) {
 		printf(" Device unboot! Boot it first!\n");
 		return -1;
 	}
@@ -865,21 +246,19 @@ int sdram_load(struct sdram_in *sdram_in)
 		return -1;
 	}
 
-	ingenic_dev.file_buff = sdram_in->buf;
-	ingenic_dev.file_len = sdram_in->length;
-	usb_send_data_to_ingenic(&ingenic_dev);
-	usb_send_data_address_to_ingenic(&ingenic_dev, sdram_in->start);
-	usb_send_data_length_to_ingenic(&ingenic_dev, sdram_in->length);
-	usb_ingenic_sdram_ops(&ingenic_dev, sdram_in);
+	usb_send_data_to_ingenic(dev, sdram_in->buf, sdram_in->length);
+	usb_send_data_address_to_ingenic(dev, sdram_in->start);
+	usb_send_data_length_to_ingenic(dev, sdram_in->length);
+/*	usb_ingenic_sdram_ops(dev, sdram_in);*/
 
-	usb_read_data_from_ingenic(&ingenic_dev, ret, 8);
+	usb_read_data_from_ingenic(dev, ret, 8);
 	printf(" Load last address at 0x%x\n",
 	       ((ret[3]<<24)|(ret[2]<<16)|(ret[1]<<8)|(ret[0]<<0)));
 
 	return 1;
 }
 
-int sdram_load_file(struct sdram_in *sdram_in, char *file_path)
+int sdram_load_file(struct ingenic_dev *dev, struct sdram_in *sdram_in, char *file_path)
 {
 	struct stat fstat;
 	unsigned int flen,m,j,offset,k;
@@ -895,7 +274,7 @@ int sdram_load_file(struct sdram_in *sdram_in, char *file_path)
 
 	fd = open(file_path, O_RDONLY);
 	if (fd < 0) {
-		fprintf(stderr, "Error - can't open file '%s': %s\n", 
+		fprintf(stderr, "Error - can't open file '%s': %s\n",
 			file_path, strerror(errno));
 		goto out;
 	}
@@ -910,13 +289,13 @@ int sdram_load_file(struct sdram_in *sdram_in, char *file_path)
 	for (k = 0; k < m; k++) {
 		status = read(fd, sdram_in->buf, MAX_LOAD_SIZE);
 		if (status < MAX_LOAD_SIZE) {
-			fprintf(stderr, "Error - can't read file '%s': %s\n", 
+			fprintf(stderr, "Error - can't read file '%s': %s\n",
 				file_path, strerror(errno));
 			goto close;
 		}
 
 		sdram_in->length = MAX_LOAD_SIZE;
-		if (sdram_load(sdram_in) < 1)
+		if (sdram_load(dev, sdram_in) < 1)
 			goto close;
 
 		sdram_in->start += MAX_LOAD_SIZE;
@@ -925,17 +304,17 @@ int sdram_load_file(struct sdram_in *sdram_in, char *file_path)
 	}
 
 	if (j) {
-		if (j % 4 !=0) 
+		if (j % 4 !=0)
 			j += 4 - (j % 4);
 		status = read(fd, sdram_in->buf, j);
-		if (status < j) {
-			fprintf(stderr, "Error - can't read file '%s': %s\n", 
+		if (status < (int)j) {
+			fprintf(stderr, "Error - can't read file '%s': %s\n",
 				file_path, strerror(errno));
 			goto close;
 		}
 
 		sdram_in->length = j;
-		if (sdram_load(sdram_in) < 1)
+		if (sdram_load(dev, sdram_in) < 1)
 			goto close;
 	}
 
diff --git a/usbboot/src/cmd.h b/usbboot/src/cmd.h
index 0b1d613..8c01926 100644
--- a/usbboot/src/cmd.h
+++ b/usbboot/src/cmd.h
@@ -1,7 +1,7 @@
 /*
  * Copyright(C) 2009  Qi Hardware Inc.,
  * Authors: Xiangfu Liu <xiangfu@qi-hardware.com>
- *
+
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation, either version 3 of the License, or
@@ -20,18 +20,17 @@
 #define __CMD_H__
 
 #include "usb_boot_defines.h"
+#include <stdint.h>
 
 #define COMMAND_NUM 31
 #define MAX_ARGC	10
 #define MAX_COMMAND_LENGTH	100
 
-int boot(char *stage1_path, char *stage2_path);
-int init_nand_in();
-int nand_prog(void);
-int nand_query(void);
-int nand_erase(struct nand_in *nand_in);
-int debug_memory(int obj, unsigned int start, unsigned int size);
-int debug_gpio(int obj, unsigned char ops, unsigned char pin);
-int debug_go(void);
+struct ingenic_dev;
+
+int boot(struct ingenic_dev *dev, const char *stage1_path, const char *stage2_path);
+int debug_memory(struct ingenic_dev *dev, int obj, unsigned int start, unsigned int size);
+int debug_go(struct ingenic_dev *dev, size_t argc, char *argv[]);
+int sdram_load_file(struct ingenic_dev *dev, struct sdram_in *sdram_in, char *file_path);
 
 #endif  /* __CMD_H__ */
diff --git a/usbboot/src/command_line.c b/usbboot/src/command_line.c
index b2cbb36..3209f42 100644
--- a/usbboot/src/command_line.c
+++ b/usbboot/src/command_line.c
@@ -19,40 +19,34 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <ctype.h>
 #include "usb_boot_defines.h"
 #include "ingenic_usb.h"
 #include "cmd.h"
 #include "xburst-tools_version.h"
- 
+#include "nand.h"
+#include "mem.h"
+
 extern struct nand_in nand_in;
 extern struct sdram_in sdram_in;
-extern unsigned char code_buf[4 * 512 * 1024];
+static char code_buf[4 * 512 * 1024];
+extern struct ingenic_dev ingenic_dev;
+typedef int (*command_callback_t)(size_t argc, char *argv[]);
 
-int com_argc;
-char com_argv[MAX_ARGC][MAX_COMMAND_LENGTH];
+struct command {
+	const char *name;
+	command_callback_t callback;
+};
 
-static const char COMMAND[][COMMAND_NUM]=
+#define COMMAND(_name, _callback) {\
+	.name = _name, \
+	.callback = (command_callback_t)_callback, \
+}
+
+static const char COMMANDS[][COMMAND_NUM]=
 {
-	"",
-	"query",
-	"querya",
-	"erase",
-	"read",
-	"prog",
-	"nquery",
-	"nerase",
-	"nread",
-	"nreadraw",
-	"nreadoob", /* index 10 */
-	"nprog",
-	"help",
-	"version",
 	"go",
 	"fconfig",
-	"exit",
-	"readnand",
-	"gpios",
-	"gpioc",
 	"boot", /* index 20 */
 	"list",
 	"select",
@@ -63,10 +57,69 @@ static const char COMMAND[][COMMAND_NUM]=
 	"nmake",
 	"load",
 	"memtest",
-	"run"
+	"run",
 };
 
-static int handle_help(void)
+static unsigned long parse_number(const char *s, int *err)
+{
+	unsigned long val = 0;
+	unsigned int base = 10;
+	char *endptr;
+
+	if (s == 0 || *s == 0) {
+	    if (err)
+	        *err = 1;
+	    return 0;
+	}
+
+	if (*s == '0') {
+	    ++s;
+	    if (*s == 'x') {
+	        base = 16;
+	        ++s;
+	    } else if (*s != 0) {
+	        base = 8;
+	    }
+	} else if (*s == 'b') {
+	    ++s;
+	    base = 2;
+	}
+
+	val = strtoul(s, &endptr, base);
+
+	if (*endptr) {
+	    if (err)
+	        *err = 1;
+	    return 0;
+	}
+
+	if (err)
+	    *err = 0;
+
+	return val;
+}
+
+static unsigned long parse_number_print_error(const char *s, int *err)
+{
+	unsigned long value;
+	int err2 = 0;
+	value = parse_number(s, &err2);
+	if (err2) {
+	    fprintf(stderr, "Error: %s is not a number\n", s);
+	    if (err)
+	        ++err;
+	}
+
+	return value;
+}
+
+static int handle_exit()
+{
+	exit(0);
+	return 0;
+}
+
+static int handle_help()
 {
 	printf(" command support in current version:\n"
 	/* " query" */
@@ -74,253 +127,436 @@ static int handle_help(void)
 	/* " erase" */
 	/* " read" */
 	/* " prog" */
-	" nquery     query NAND flash info\n"
-	" nerase     erase NAND flash\n"
-	" nread      read NAND flash data with checking bad block and ECC\n"
-	" nreadraw   read NAND flash data without checking bad block and ECC\n"
-	" nreadoo    read NAND flash oob without checking bad block and ECC\n" /* index 10 */
-	" nprog      program NAND flash with data and ECC\n"
-	" help       print this help\n"
-	" version    show current USB Boot software version\n"
-	" go         execute program in SDRAM\n"
-	" fconfig    set USB Boot config file(not implement)\n"
-	" exit       quit from telnet session\n"
-	" readnand   read data from nand flash and store to SDRAM\n"
-	" gpios      set one GPIO to high level\n"
-	" gpioc      set one GPIO to low level\n"
-	" boot       boot device and make it in stage2\n" /* index 20 */
-	" list       show current device number can connect(not implement)\n"
+	" nquery        query NAND flash info\n"
+	" nerase        erase NAND flash\n"
+	" nread         read NAND flash data with checking bad block and ECC\n"
+	" nreadraw      read NAND flash data without checking bad block and ECC\n"
+	" nreadoo       read NAND flash oob without checking bad block and ECC\n" /* index 10 */
+	" nprog         program NAND flash with data and ECC\n"
+	" ndump         dump NAND flash data to file\n"
+	" help          print this help\n"
+	" version       show current USB Boot software version\n"
+	" go            execute program in SDRAM\n"
+	" fconfig       set USB Boot config file(not implement)\n"
+	" exit          quit from current session\n"
+	" readnand      read data from nand flash and store to SDRAM\n"
+	" boot          boot device and make it in stage2\n" /* index 20 */
+	" list          show current device number can connect(not implement)\n"
 	/* " select" */
 	/* " unselect" */
 	/* " chip" */
 	/* " unchip" */
-	" nmark      mark a bad block in NAND flash\n"
-	" nmake      read all data from nand flash and store to file(not implement)\n"
-	" load       load file data to SDRAM\n"
-	" memtest    do SDRAM test\n"
-	" run        run command script in file(implement by -c args)\n"
-	" sdprog     program SD card(not implement)\n"
-	" sdread     read data from SD card(not implement)\n");
-	return 1;
+	" nmark        mark a bad block in NAND flash\n"
+	" nmake        read all data from nand flash and store to file(not implement)\n"
+	" load         load file data to SDRAM\n"
+	" memtest      do SDRAM test\n"
+	" run          run command script in file(implement by -c args)\n"
+	" sdprog       program SD card(not implement)\n"
+	" sdread       read data from SD card(not implement)\n");
+
+	return 0;
 }
 
-static int handle_version(void)
+static int handle_version()
 {
-	printf(" USB Boot Software current version: %s\n", XBURST_TOOLS_VERSION);
-	return 1;
+	printf("USB Boot Software current version: %s\n", XBURST_TOOLS_VERSION);
+
+	return 0;
 }
 
-/* need transfer two para :blk_num ,start_blk */
-int handle_nerase(void)
+static int handle_boot()
 {
-	if (com_argc < 5) {
+	boot(&ingenic_dev, STAGE1_FILE_PATH, STAGE2_FILE_PATH);
+
+	return 0;
+}
+
+static int handle_nand_erase(size_t argc, char *argv[])
+{
+	uint32_t start_block, num_blocks;
+	unsigned int device_idx;
+	uint8_t nand_idx;
+	int err;
+
+	if (argc < 5) {
 		printf(" Usage: nerase (1) (2) (3) (4)\n"
-		       " 1:start block number\n"
-		       " 2:block length\n"
-		       " 3:device index number\n"
-		       " 4:flash chip index number\n");
+			   " 1:start block number\n"
+			   " 2:block length\n"
+			   " 3:device index number\n"
+			   " 4:flash chip index number\n");
 		return -1;
 	}
 
-	init_nand_in();
+	start_block = parse_number_print_error(argv[1], &err);
+	num_blocks =  parse_number_print_error(argv[2], &err);
+	device_idx =  parse_number_print_error(argv[3], &err);
+	nand_idx =    parse_number_print_error(argv[4], &err);
+	if (err)
+	    return err;
 
-	nand_in.start = atoi(com_argv[1]);
-	nand_in.length = atoi(com_argv[2]);
-	nand_in.dev = atoi(com_argv[3]);
-	if (atoi(com_argv[4]) >= MAX_DEV_NUM) {
+	if (nand_idx >= MAX_DEV_NUM) {
 		printf(" Flash index number overflow!\n");
 		return -1;
 	}
-	(nand_in.cs_map)[atoi(com_argv[4])] = 1;
 
-	if (nand_erase(&nand_in) < 1)
+	if (nand_erase(&ingenic_dev, nand_idx, start_block, num_blocks))
 		return -1;
 
-	return 1;
+	return 0;
 }
 
-int handle_nmark(void)
+static int handle_nand_mark(size_t argc, char *argv[])
 {
-	if (com_argc < 4) {
-		printf(" Usage: nerase (1) (2) (3)\n"
-		       " 1:bad block number\n"
-		       " 2:device index number\n"
-		       " 3:flash chip index number\n");
+	uint32_t block;
+	unsigned int device_idx;
+	uint8_t nand_idx;
+	int err = 0;
+
+	if (argc < 4) {
+		printf("Usage: nerase (1) (2) (3)\n"
+			   "1: bad block number\n"
+			   "2: device index number\n"
+			   "3: flash chip index number\n");
 		return -1;
 	}
-	init_nand_in();
 
-	nand_in.start = atoi(com_argv[1]);
-	nand_in.dev = atoi(com_argv[2]);
+	block =      parse_number_print_error(argv[1], &err);
+	device_idx = parse_number_print_error(argv[2], &err);
+	nand_idx =   parse_number_print_error(argv[3], &err);
+	if (err)
+	    return err;
 
-	if (atoi(com_argv[3])>=MAX_DEV_NUM) {
-		printf(" Flash index number overflow!\n");
+	if (nand_idx >= MAX_DEV_NUM) {
+		printf("Flash index number overflow!\n");
 		return -1;
 	}
-	(nand_in.cs_map)[atoi(com_argv[3])] = 1;
 
-	nand_markbad(&nand_in);
-	return 1;
+	nand_markbad(&ingenic_dev, nand_idx, block);
+
+	return 0;
 }
 
-int handle_memtest(void)
+static int handle_memtest(size_t argc, char *argv[])
 {
+	unsigned int device_idx;
 	unsigned int start, size;
-	if (com_argc != 2 && com_argc != 4)
+	int err = 0;
+
+	if (argc != 2 && argc != 4)
 	{
 		printf(" Usage: memtest (1) [2] [3]\n"
-		       " 1:device index number\n"
-		       " 2:SDRAM start address\n"
-		       " 3:test size\n");
+			   " 1: device index number\n"
+			   " 2: SDRAM start address\n"
+			   " 3: test size\n");
 		return -1;
 	}
 
-	if (com_argc == 4) {
-		start = strtoul(com_argv[2], NULL, 0);
-		size = strtoul(com_argv[3], NULL, 0);
+	if (argc == 4) {
+	    start = parse_number_print_error(argv[2], &err);
+	    size =  parse_number_print_error(argv[3], &err);
 	} else {
 		start = 0;
 		size = 0;
 	}
-	debug_memory(atoi(com_argv[1]), start, size);
-	return 1;
+	device_idx = parse_number_print_error(argv[1], &err);
+	if (err)
+	    return err;
+
+	debug_memory(&ingenic_dev, device_idx, start, size);
+	return 0;
 }
 
-int handle_gpio(int mode)
+static int handle_load(size_t argc, char *argv[])
 {
-	if (com_argc < 3) {
+	if (argc != 4) {
 		printf(" Usage:"
-		       " gpios (1) (2)\n"
-		       " 1:GPIO pin number\n"
-		       " 2:device index number\n");
-		return -1;
-	}
-
-	debug_gpio(atoi(com_argv[2]), mode, atoi(com_argv[1]));
-	return 1;
-}
-
-int handle_load(void)
-{
-	if (com_argc<4) {
-		printf(" Usage:"
-		       " load (1) (2) (3) \n"
-		       " 1:SDRAM start address\n"
-		       " 2:image file name\n"
-		       " 3:device index number\n");
+			   " load (1) (2) (3) \n"
+			   " 1:SDRAM start address\n"
+			   " 2:image file name\n"
+			   " 3:device index number\n");
 
 		return -1;
 	}
 
-	sdram_in.start=strtoul(com_argv[1], NULL, 0);
+	sdram_in.start=strtoul(argv[1], NULL, 0);
 	printf(" start:::::: 0x%x\n", sdram_in.start);
 
-	sdram_in.dev = atoi(com_argv[3]);
+	sdram_in.dev = atoi(argv[3]);
 	sdram_in.buf = code_buf;
-	sdram_load_file(&sdram_in, com_argv[2]);
-	return 1;
+	sdram_load_file(&ingenic_dev, &sdram_in, argv[2]);
+	return 0;
 }
 
-int command_interpret(char * com_buf)
+static size_t command_parse(char *cmd, char *argv[])
 {
-	char *buf = com_buf;
-	int k, L, i = 0, j = 0;
-	
-	L = (int)strlen(buf);
-	buf[L]=' ';
+	size_t argc = 0;
 
-	if (buf[0] == '\n')
-		return 0;
+	if (cmd == 0 || *cmd == 0)
+	    return 0;
 
-	for (k = 0; k <= L; k++) {
-		if (*buf == ' ' || *buf == '\n') {
-			while ( *(++buf) == ' ' );
-			com_argv[i][j] = '\0';
-			i++;
-			if (i > MAX_ARGC)
-				return COMMAND_NUM + 1;
-			j = 0;
-			continue;
-		} else {
-			com_argv[i][j] = *buf;
-			j++;
-			if (j > MAX_COMMAND_LENGTH)
-				return COMMAND_NUM + 1;
-		}
-		buf++;
+
+	while (isspace(*cmd)) {
+	    ++cmd;
 	}
 
-	com_argc = i;
+	argv[0] = cmd;
+	argc = 1;
 
-	for (i = 1; i <= COMMAND_NUM; i++) 
-		if (!strcmp(COMMAND[i], com_argv[0])) 
-			return i;
-	return COMMAND_NUM + 1;
+	while (*cmd) {
+	    if (isspace(*cmd)) {
+	        *cmd = 0;
+
+	        do {
+	            ++cmd;
+	        } while (isspace(*cmd));
+
+	        if (*cmd == 0 || argc >= MAX_ARGC)
+	            break;
+
+	        argv[argc] = cmd;
+	        ++argc;
+	    }
+
+	    ++cmd;
+	}
+
+	return argc;
 }
 
+static int handle_nand_read(size_t argc, char *argv[])
+{
+	int mode;
+	uint32_t start_page, num_pages;
+	unsigned int device_idx;
+	uint8_t nand_idx;
+	int err = 0;
+
+	if (argc != 5) {
+	    printf("Usage: %s <start page> <length> <device index> "
+	                "<nand chip index>\n", argv[0]);
+	    return -1;
+	}
+
+	if (strcmp(argv[0], "nread") == 0) {
+	    mode = NAND_READ;
+	} else if (strcmp(argv[0], "nreadraw") == 0) {
+	    mode = NAND_READ_RAW;
+	} else if (strcmp(argv[0], "nreadoob") == 0) {
+	    mode = NAND_READ_OOB;
+	} else {
+	    return -1;
+	}
+
+	start_page =  parse_number_print_error(argv[1], &err);
+	num_pages =   parse_number_print_error(argv[2], &err);
+	device_idx =  parse_number_print_error(argv[3], &err);
+	nand_idx =    parse_number_print_error(argv[4], &err);
+	if (err)
+	    return err;
+
+	return nand_read(&ingenic_dev, nand_idx, mode, start_page, num_pages, 0);
+}
+
+static int handle_nand_dump(size_t argc, char *argv[])
+{
+	int mode;
+	uint32_t start_page, num_pages;
+	unsigned int device_idx;
+	uint8_t nand_idx;
+	int err = 0;
+
+	if (argc != 5) {
+	    printf("Usage: %s <start page> <length> <filename> <mode>\n", argv[0]);
+	    return -1;
+	}
+
+	if (strcmp(argv[5], "-n") == 0) {
+	    mode = NAND_READ;
+	} else if (strcmp(argv[5], "-e") == 0) {
+	    mode = NAND_READ_RAW;
+	} else if (strcmp(argv[5], "-o") == 0) {
+	    mode = NAND_READ_OOB;
+	} else {
+	    return -1;
+	}
+
+	start_page =  parse_number_print_error(argv[1], &err);
+	num_pages =   parse_number_print_error(argv[2], &err);
+	device_idx =  parse_number_print_error(argv[3], &err);
+	nand_idx =    parse_number_print_error(argv[4], &err);
+	if (err)
+	    return err;
+
+	return nand_read(&ingenic_dev, nand_idx, mode, start_page, num_pages, 0);
+
+}
+
+static int handle_nand_query(size_t argc, char *argv[])
+{
+	unsigned int device_idx;
+	uint8_t nand_idx;
+	int err = 0;
+
+	if (argc != 3) {
+	    printf("Usage: %s <device index> <nand chip index>\n", argv[0]);
+	    return -1;
+	}
+
+	device_idx =  parse_number_print_error(argv[1], &err);
+	nand_idx =    parse_number_print_error(argv[2], &err);
+	if (err)
+	    return err;
+
+	return nand_query(&ingenic_dev, nand_idx);
+}
+
+static int handle_nand_prog(size_t argc, char *argv[])
+{
+	uint32_t start_page;
+	unsigned int device_idx;
+	uint8_t nand_idx, mode = -1;
+	int err = 0;
+
+	if (argc != 5) {
+		printf("Usage: %s <start page> <filename> <device index> <nand chip index> <mode>\n", argv[0]);
+		return -1;
+	}
+
+	start_page = parse_number_print_error(argv[1], &err);
+	device_idx = parse_number_print_error(argv[3], &err);
+	nand_idx =   parse_number_print_error(argv[4], &err);
+
+	if (argv[5][0] == '-') {
+		switch (argv[5][1]) {
+		case 'e':
+			mode = NO_OOB;
+			break;
+		case 'o':
+			mode = OOB_NO_ECC;
+			break;
+		case 'r':
+			mode = OOB_ECC;
+			break;
+		default:
+			break;
+		}
+	}
+	if (mode == -1) {
+		printf("%s: Invalid mode '%s'\n", argv[0], argv[5]);
+		err = -1;
+	}
+
+	if (err)
+		return err;
+
+
+	nand_prog(&ingenic_dev, nand_idx, start_page, argv[2], mode);
+
+	return 0;
+}
+
+static int handle_mem_read(size_t argc, char *argv[])
+{
+	uint32_t addr;
+	uint32_t val;
+	int err = 0;
+	if (argc != 2)
+	    printf("Usage: %s <addr>\n", argv[0]);
+
+	addr = parse_number_print_error(argv[1], &err);
+	if (err)
+	    return err;
+
+	switch (argv[0][7]) {
+	case '8':
+		val = mem_read8(&ingenic_dev, addr);
+		break;
+	case '1':
+		val = mem_read16(&ingenic_dev, addr);
+		break;
+	default:
+		val = mem_read32(&ingenic_dev, addr);
+		break;
+	}
+
+	printf("0x%x = 0x%x\n", addr, val);
+
+	return 0;
+}
+
+static int handle_mem_write(size_t argc, char *argv[])
+{
+	uint32_t addr;
+	uint32_t val;
+	int err = 0;
+
+	if (argc != 3)
+	    printf("Usage: %s <addr> <value>\n", argv[0]);
+
+	addr = parse_number_print_error(argv[1], &err);
+	val = parse_number_print_error(argv[2], &err);
+	if (err)
+	    return err;
+
+	switch (argv[0][8]) {
+	case '8':
+		mem_write8(&ingenic_dev, addr, val);
+		break;
+	case '1':
+		mem_write16(&ingenic_dev, addr, val);
+		break;
+	default:
+		mem_write32(&ingenic_dev, addr, val);
+		break;
+	}
+
+	printf("0x%x = 0x%x\n", addr, val);
+
+	return 0;
+}
+
+
+static const struct command commands[] = {
+	COMMAND("version",	handle_version),
+	COMMAND("help",		handle_help),
+	COMMAND("nquery",	handle_nand_query),
+	COMMAND("nerase",	handle_nand_erase),
+	COMMAND("nread",	handle_nand_read),
+	COMMAND("nreadraw",	handle_nand_read),
+	COMMAND("nreadoo",	handle_nand_read),
+	COMMAND("nprog",	handle_nand_prog),
+	COMMAND("nwrite",	handle_nand_prog),
+	COMMAND("nmark",	handle_nand_mark),
+	COMMAND("ndump",	handle_nand_dump),
+	COMMAND("exit",		handle_exit),
+	COMMAND("boot",		handle_boot),
+	COMMAND("memread",	handle_mem_read),
+	COMMAND("memwrite",	handle_mem_write),
+	COMMAND("memread16",	handle_mem_read),
+	COMMAND("memwrite16",	handle_mem_write),
+	COMMAND("memread8",	handle_mem_read),
+	COMMAND("memwrite8",	handle_mem_write),
+	COMMAND("memtest",		handle_memtest),
+	COMMAND("load",		handle_load),
+};
+
 int command_handle(char *buf)
 {
-	int cmd = command_interpret(buf); /* get the command index */
+	size_t argc;
+	char *argv[MAX_ARGC];
+	size_t i;
 
-	switch (cmd) {
-	case 0:
-		break;
-	case 6:
-		nand_query();
-		break;
-	case 7:	
-		handle_nerase();
-		break;
-	case 8:	/* nread */
-		nand_read(NAND_READ);
-		break;
-	case 9:	/* nreadraw */
-		nand_read(NAND_READ_RAW);
-		break;
-	case 10: /* nreadoob */
-		nand_read(NAND_READ_OOB);
-		break;
-	case 11:
-		nand_prog();
-		break;
-	case 12:
-		handle_help();
-		break;
-	case 13:
-		handle_version();
-		break;
-	case 14:
-		debug_go();
-		break;
-	case 16:		/* exit */
-		printf(" exiting usbboot software\n");
-		return -1;	/* return -1 to break the main.c while
-				 * then run usb_ingenic_cleanup*/
-		/*case 17:
-		nand_read(NAND_READ_TO_RAM); */
-		break;
-	case 18:
-		handle_gpio(2);
-		break;
-	case 19:
-		handle_gpio(3);
-		break;
-	case 20:
-		boot(STAGE1_FILE_PATH, STAGE2_FILE_PATH);
-		break;
-	case 26:
-		handle_nmark();
-		break;
-	case 28:
-		handle_load();
-		break;
-	case 29:
-		handle_memtest();
-		break;
-	default:
-		printf(" command not support or input error!\n");
-		break;
+	argc = command_parse(buf, argv);
+
+	if (argc == 0)
+	    return 0;
+
+	for (i = 0; i < ARRAY_SIZE(commands); ++i) {
+	    if (strcmp(commands[i].name, argv[0]) == 0)
+	        return commands[i].callback(argc, argv);
 	}
 
-	return 1;
+	printf("Unknow command \"%s\"\n", argv[0]);
+
+	return -1;
 }
diff --git a/usbboot/src/ingenic_cfg.c b/usbboot/src/ingenic_cfg.c
index 366479a..26eed78 100644
--- a/usbboot/src/ingenic_cfg.c
+++ b/usbboot/src/ingenic_cfg.c
@@ -18,7 +18,7 @@
 
 #include <errno.h>
 #include <confuse.h>
-#include <unistd.h>   
+#include <unistd.h>
 #include <string.h>
 #include "ingenic_cfg.h"
 #include "usb_boot_defines.h"
@@ -94,7 +94,7 @@ int check_dump_cfg(struct hand *hand)
 	/* check NAND */
 	if ( hand->nand_ps < 2048 && hand->nand_os > 16 ) {
 		printf(" PAGESIZE or OOBSIZE setting invalid!\n");
-		printf(" PAGESIZE is %d,\t OOBSIZE is %d\n", 
+		printf(" PAGESIZE is %d,\t OOBSIZE is %d\n",
 		       hand->nand_ps, hand->nand_os);
 		return 0;
 	}
@@ -105,7 +105,7 @@ int check_dump_cfg(struct hand *hand)
 
 	if ( hand->nand_ps > 512 && hand->nand_os <= 16 ) {
 		printf(" PAGESIZE or OOBSIZE setting invalid!\n");
-		printf(" PAGESIZE is %d,\t OOBSIZE is %d\n", 
+		printf(" PAGESIZE is %d,\t OOBSIZE is %d\n",
 		       hand->nand_ps, hand->nand_os);
 		return 0;
 	}
@@ -123,7 +123,7 @@ int check_dump_cfg(struct hand *hand)
 		((unsigned int)hand->fw_args.cpu_speed * hand->fw_args.ext_clk) / hand->fw_args.phm_div);
 
 	printf("SDRAM Total size is %d MB, work in %d bank and %d bit mode\n",
-		total_size / 0x100000, 2 * (hand->fw_args.bank_num + 1), 
+		total_size / 0x100000, 2 * (hand->fw_args.bank_num + 1),
 	       16 * (2 - hand->fw_args.bus_width));
 
 	printf("Nand page per block %d, "
@@ -143,18 +143,20 @@ int check_dump_cfg(struct hand *hand)
 
 int parse_configure(struct hand *hand, char * file_path)
 {
+    int cpu_speed;
 	if (access(file_path, F_OK)) {
 		fprintf(stderr, "Error - can't read configure file %s.\n",
 			file_path);
 		return -1;
 	}
 
+
 	hand_init_def(hand);
 
 	cfg_opt_t opts[] = {
 		CFG_INT("BOUDRATE", 57600, CFGF_NONE),
 		CFG_SIMPLE_INT("EXTCLK", &hand->fw_args.ext_clk),
-		CFG_SIMPLE_INT("CPUSPEED", &hand->fw_args.cpu_speed),
+		CFG_SIMPLE_INT("CPUSPEED", &cpu_speed),
 		CFG_SIMPLE_INT("PHMDIV", &hand->fw_args.phm_div),
 		CFG_SIMPLE_INT("USEUART", &hand->fw_args.use_uart),
 
@@ -200,12 +202,12 @@ int parse_configure(struct hand *hand, char * file_path)
 		hand->fw_args.bus_width = 0;
 	else
 		hand->fw_args.bus_width = 1;
-	hand->fw_args.bank_num = hand->fw_args.bank_num / 4; 
-	hand->fw_args.cpu_speed = hand->fw_args.cpu_speed / hand->fw_args.ext_clk;
-	
+	hand->fw_args.bank_num = hand->fw_args.bank_num / 4;
+	hand->fw_args.cpu_speed = cpu_speed / hand->fw_args.ext_clk;
+
 	total_size = (unsigned int)
-		(2 << (hand->fw_args.row_addr + hand->fw_args.col_addr - 1)) * 2 
-		* (hand->fw_args.bank_num + 1) * 2 
+		(2 << (hand->fw_args.row_addr + hand->fw_args.col_addr - 1)) * 2
+		* (hand->fw_args.bank_num + 1) * 2
 		* (2 - hand->fw_args.bus_width);
 
 	if (check_dump_cfg(hand) < 1)
diff --git a/usbboot/src/ingenic_usb.c b/usbboot/src/ingenic_usb.c
index 6b622cd..70e5fa5 100644
--- a/usbboot/src/ingenic_usb.c
+++ b/usbboot/src/ingenic_usb.c
@@ -2,7 +2,7 @@
  * Copyright(C) 2009  Qi Hardware Inc.,
  * Authors: Xiangfu Liu <xiangfu@qi-hardware.com>
  *          Marek Lindner <lindner_marek@yahoo.de>
- * 
+ *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation, either version 3 of the License, or
@@ -34,13 +34,14 @@ static int get_ingenic_device(struct ingenic_dev *ingenic_dev)
 	usb_busses = usb_get_busses();
 
 	for (usb_bus = usb_busses; usb_bus != NULL; usb_bus = usb_bus->next) {
-		for (usb_dev = usb_bus->devices; usb_dev != NULL; 
+		for (usb_dev = usb_bus->devices; usb_dev != NULL;
 		     usb_dev = usb_dev->next) {
 
 			if ((usb_dev->descriptor.idVendor == VENDOR_ID) &&
 				(usb_dev->descriptor.idProduct == PRODUCT_ID)) {
 				ingenic_dev->usb_dev = usb_dev;
 				count++;
+                break;
 			}
 
 		}
@@ -56,15 +57,15 @@ static int get_ingenic_interface(struct ingenic_dev *ingenic_dev)
 	struct usb_interface *usb_if;
 	int config_index, if_index, alt_index;
 
-	for (config_index = 0; 
-	     config_index < ingenic_dev->usb_dev->descriptor.bNumConfigurations; 
+	for (config_index = 0;
+	     config_index < ingenic_dev->usb_dev->descriptor.bNumConfigurations;
 	     config_index++) {
 		usb_config_desc = &ingenic_dev->usb_dev->config[config_index];
 
 		if (!usb_config_desc)
 			return 0;
 
-		for (if_index = 0; if_index < usb_config_desc->bNumInterfaces; 
+		for (if_index = 0; if_index < usb_config_desc->bNumInterfaces;
 		     if_index++) {
 			usb_if = &usb_config_desc->interface[if_index];
 
@@ -80,7 +81,7 @@ static int get_ingenic_interface(struct ingenic_dev *ingenic_dev)
 
 				if ((usb_if_desc->bInterfaceClass == 0xff) &&
 					(usb_if_desc->bInterfaceSubClass == 0)) {
-					ingenic_dev->interface = 
+					ingenic_dev->interface =
 						usb_if_desc->bInterfaceNumber;
 					return 1;
 				}
@@ -144,7 +145,7 @@ int usb_get_ingenic_cpu(struct ingenic_dev *ingenic_dev)
 {
 	int status;
 
-	memset(&ingenic_dev->cpu_info_buff, 0, 
+	memset(&ingenic_dev->cpu_info_buff, 0,
 	       ARRAY_SIZE(ingenic_dev->cpu_info_buff));
 
 	sleep(1);
@@ -195,9 +196,10 @@ int usb_ingenic_flush_cache(struct ingenic_dev *ingenic_dev)
 	return 1;
 }
 
-int usb_send_data_length_to_ingenic(struct ingenic_dev *ingenic_dev, int len)
+int usb_send_data_length_to_ingenic(struct ingenic_dev *ingenic_dev, unsigned int len)
 {
 	int status;
+
 	/* tell the device the length of the file to be uploaded */
 	status = usb_control_msg(ingenic_dev->usb_handle,
           /* bmRequestType */ USB_ENDPOINT_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
@@ -217,16 +219,15 @@ int usb_send_data_length_to_ingenic(struct ingenic_dev *ingenic_dev, int len)
 	return 1;
 }
 
-int usb_send_data_address_to_ingenic(struct ingenic_dev *ingenic_dev, 
-				     unsigned int stage_addr)
+int usb_send_data_address_to_ingenic(struct ingenic_dev *ingenic_dev, uint32_t addr)
 {
 	int status;
 	/* tell the device the RAM address to store the file */
 	status = usb_control_msg(ingenic_dev->usb_handle,
           /* bmRequestType */ USB_ENDPOINT_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
           /* bRequest      */ VR_SET_DATA_ADDRESS,
-          /* wValue        */ STAGE_ADDR_MSB(stage_addr),
-          /* wIndex        */ STAGE_ADDR_LSB(stage_addr),
+          /* wValue        */ STAGE_ADDR_MSB(addr),
+          /* wIndex        */ STAGE_ADDR_LSB(addr),
           /* Data          */ 0,
           /* wLength       */ 0,
                               USB_TIMEOUT);
@@ -240,15 +241,16 @@ int usb_send_data_address_to_ingenic(struct ingenic_dev *ingenic_dev,
 	return 1;
 }
 
-int usb_send_data_to_ingenic(struct ingenic_dev *ingenic_dev)
+int usb_send_data_to_ingenic(struct ingenic_dev *ingenic_dev, const char *data,
+                             int size)
 {
 	int status;
 	status = usb_bulk_write(ingenic_dev->usb_handle,
 	/* endpoint         */ INGENIC_OUT_ENDPOINT,
-	/* bulk data        */ ingenic_dev->file_buff,
-	/* bulk data length */ ingenic_dev->file_len,
+	/* bulk data        */ data,
+	/* bulk data length */ size,
 				USB_TIMEOUT);
-	if (status < ingenic_dev->file_len) {
+	if (status < size) {
 		fprintf(stderr, "Error - "
 			"can't send bulk data to Ingenic CPU: %i\n", status);
 		return -1;
@@ -258,15 +260,15 @@ int usb_send_data_to_ingenic(struct ingenic_dev *ingenic_dev)
 }
 
 int usb_read_data_from_ingenic(struct ingenic_dev *ingenic_dev,
-			       unsigned char *buff, unsigned int len)
+			       char *data, int size)
 {
 	int status;
 	status = usb_bulk_read(ingenic_dev->usb_handle,
         /* endpoint         */ INGENIC_IN_ENDPOINT,
-	/* bulk data        */ buff,
-	/* bulk data length */ len,
+	/* bulk data        */ data,
+	/* bulk data length */ size,
 				USB_TIMEOUT);
-	if (status < len) {
+	if (status < size) {
 		fprintf(stderr, "Error - "
 			"can't read bulk data from Ingenic device:%i\n", status);
 		return -1;
@@ -299,8 +301,6 @@ int usb_ingenic_start(struct ingenic_dev *ingenic_dev, int rqst, int stage_addr)
 
 int usb_ingenic_upload(struct ingenic_dev *ingenic_dev, int stage)
 {
-	int status;
-
 	unsigned int stage2_addr;
 	stage2_addr = total_size + 0x80000000;
 	stage2_addr -= CODE_SIZE;
@@ -309,12 +309,13 @@ int usb_ingenic_upload(struct ingenic_dev *ingenic_dev, int stage)
 	int rqst = VR_PROGRAM_START1;
 
 	usb_send_data_address_to_ingenic(ingenic_dev, stage_addr);
-	printf(" Download stage %d program and execute at 0x%08x\n", 
+	printf(" Download stage %d program and execute at 0x%08x\n",
 	       stage, (stage_addr));
-	usb_send_data_to_ingenic(ingenic_dev);
+	usb_send_data_to_ingenic(ingenic_dev, ingenic_dev->file_buff,
+    ingenic_dev->file_len);
 
 	if (stage == 2) {
-		if (usb_get_ingenic_cpu(ingenic_dev) < 1) 
+		if (usb_get_ingenic_cpu(ingenic_dev) < 1)
 			return -1;
 		usb_ingenic_flush_cache(ingenic_dev);
 		rqst = VR_PROGRAM_START2;
@@ -331,7 +332,7 @@ int usb_ingenic_upload(struct ingenic_dev *ingenic_dev, int stage)
 void usb_ingenic_cleanup(struct ingenic_dev *ingenic_dev)
 {
 	if ((ingenic_dev->usb_handle) && (ingenic_dev->interface))
-		usb_release_interface(ingenic_dev->usb_handle, 
+		usb_release_interface(ingenic_dev->usb_handle,
 				      ingenic_dev->interface);
 
 	if (ingenic_dev->usb_handle)
@@ -359,6 +360,28 @@ int usb_ingenic_nand_ops(struct ingenic_dev *ingenic_dev, int ops)
 	return 1;
 }
 
+int usb_ingenic_mem_ops(struct ingenic_dev *ingenic_dev, int ops)
+{
+	int status;
+	status = usb_control_msg(ingenic_dev->usb_handle,
+          /* bmRequestType */ USB_ENDPOINT_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+          /* bRequest      */ VR_MEM_OPS,
+          /* wValue        */ ops & 0xffff,
+          /* wIndex        */ 0,
+          /* Data          */ 0,
+          /* wLength       */ 0,
+                              USB_TIMEOUT);
+
+	if (status != 0) {
+		fprintf(stderr, "Error - "
+			"can't set Ingenic device nand ops: %i\n", status);
+		return -1;
+	}
+
+	return 1;
+}
+
+
 int usb_ingenic_configration(struct ingenic_dev *ingenic_dev, int ops)
 {
 	int status;
diff --git a/usbboot/src/ingenic_usb.h b/usbboot/src/ingenic_usb.h
index 9af7b72..664c360 100644
--- a/usbboot/src/ingenic_usb.h
+++ b/usbboot/src/ingenic_usb.h
@@ -35,7 +35,8 @@
 #define VR_NAND_OPS		0x07
 #define VR_SDRAM_OPS		0x08
 #define VR_CONFIGRATION		0x09
-#define VR_GET_NUM		0x0a
+#define VR_MEM_OPS		0x0a
+#define VR_GET_NUM		0x0b
 
 #define STAGE_ADDR_MSB(addr) ((addr) >> 16)
 #define STAGE_ADDR_LSB(addr) ((addr) & 0xffff)
@@ -57,20 +58,26 @@ struct ingenic_dev {
 	uint8_t interface;
 	char cpu_info_buff[9];
 	char *file_buff;
-	unsigned int file_len;
+	int file_len;
 };
 
 int usb_ingenic_init(struct ingenic_dev *ingenic_dev);
 int usb_get_ingenic_cpu(struct ingenic_dev *ingenic_dev);
 int usb_ingenic_upload(struct ingenic_dev *ingenic_dev, int stage);
 void usb_ingenic_cleanup(struct ingenic_dev *ingenic_dev);
-int usb_send_data_address_to_ingenic(struct ingenic_dev *ingenic_dev, 
-				     unsigned int stage_addr);
-int usb_send_data_to_ingenic(struct ingenic_dev *ingenic_dev);
-int usb_send_data_length_to_ingenic(struct ingenic_dev *ingenic_dev,
-				    int len);
 int usb_ingenic_nand_ops(struct ingenic_dev *ingenic_dev, int ops);
-int usb_read_data_from_ingenic(struct ingenic_dev *ingenic_dev,unsigned char *buff, unsigned int len);
+int usb_ingenic_mem_ops(struct ingenic_dev *ingenic_dev, int ops);
+int usb_send_data_address_to_ingenic(struct ingenic_dev *ingenic_dev,
+				     unsigned int stage_addr);
+int usb_send_data_length_to_ingenic(struct ingenic_dev *ingenic_dev,
+                                    uint32_t len);
+int usb_send_data_to_ingenic(struct ingenic_dev *ingenic_dev, const char *data,
+                             int size);
+int usb_read_data_from_ingenic(struct ingenic_dev *ingenic_dev, char *buffer,
+                               int size);
+int usb_ingenic_start(struct ingenic_dev *ingenic_dev, int rqst, int stage_addr);
+int usb_ingenic_sdram_ops(struct ingenic_dev *ingenic_dev, int ops);
+int usb_ingenic_configration(struct ingenic_dev *ingenic_dev, int ops);
 
 #endif	/* __INGENIC_USB_H__ */
 
diff --git a/usbboot/src/main.c b/usbboot/src/main.c
index d34f583..13a09c2 100644
--- a/usbboot/src/main.c
+++ b/usbboot/src/main.c
@@ -28,7 +28,7 @@
 
 #define CONFIG_FILE_PATH "/etc/xburst-tools/usbboot.cfg"
 
-extern struct ingenic_dev ingenic_dev;
+struct ingenic_dev ingenic_dev;
 extern struct hand hand;
 
 static void help(void)
@@ -58,10 +58,9 @@ static struct option opts[] = {
 
 int main(int argc, char **argv)
 {
-	int command = 0;
 	char *cptr;
 	char com_buf[256] = {0};
-	char *cmdpt;
+	char *cmdpt = NULL;
 	char *cfgpath = CONFIG_FILE_PATH;
 
 	printf("usbboot - Ingenic XBurst USB Boot Utility\n"
@@ -83,7 +82,6 @@ int main(int argc, char **argv)
 			print_version();
 			exit(EXIT_SUCCESS);
 		case 'c':
-			command = 1;
 			cmdpt = optarg;
 			break;
 		case 'f':
@@ -95,10 +93,10 @@ int main(int argc, char **argv)
 		}
 	}
 
-	if ((getuid()) || (getgid())) {
+/*	if ((getuid()) || (getgid())) {
 		fprintf(stderr, "Error - you must be root to run '%s'\n", argv[0]);
 		return EXIT_FAILURE;
-	}
+	}*/
 
 	if (usb_ingenic_init(&ingenic_dev) < 1)
 	 	return EXIT_FAILURE;
@@ -106,7 +104,7 @@ int main(int argc, char **argv)
 	if (parse_configure(&hand, cfgpath) < 1)
 		return EXIT_FAILURE;
 
-	if (command) {		/* direct run command */
+	if (cmdpt) {		/* direct run command */
 		char *delim=";";
 		char *p;
 		p = strtok(cmdpt, delim);
@@ -128,8 +126,7 @@ int main(int argc, char **argv)
 		if (cptr == NULL)
 			continue;
 
-		if (command_handle(com_buf) == -1 )
-			break;
+		command_handle(com_buf);
 	}
 
 out:
diff --git a/usbboot/src/nand.c b/usbboot/src/nand.c
new file mode 100644
index 0000000..991aeb6
--- /dev/null
+++ b/usbboot/src/nand.c
@@ -0,0 +1,419 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <string.h>
+
+#include "nand.h"
+#include "ingenic_usb.h"
+#include "usb_boot_defines.h"
+
+extern struct hand hand;
+
+#define NAND_OP(idx, op, mode) (((mode << 12) & 0xf000) | ((idx << 4) & 0xff0) | op)
+
+#define DIV_ROUND_UP(n,d) (((n) + (d) - 1) / (d))
+
+static const char IMAGE_TYPE[][30] = {
+	"with oob and ecc",
+	"with oob and without ecc",
+	"without oob",
+};
+
+static int error_check(const char *org, const char *obj, unsigned int size)
+{
+	unsigned int i;
+	printf("Comparing %d bytes - ", size);
+	for (i = 0; i < size; i++) {
+		if (org[i] != obj[i]) {
+			unsigned int s = (i < 8) ? i : i - 8; // start_dump
+			printf("FAIL at off %d, wrote 0x%x, read 0x%x\n", i, org[i], obj[i]);
+			printf("  off %d write: %02x %02x %02x %02x %02x %02x %02x %02x"
+			       " %02x %02x %02x %02x %02x %02x %02x %02x\n", s,
+				org[s], org[s+1], org[s+2], org[s+3], org[s+4], org[s+5], org[s+6], org[s+7],
+				org[s+8], org[s+9], org[s+10], org[s+11], org[s+12], org[s+13], org[s+14], org[s+15]);
+			printf("  off %d read:  %02x %02x %02x %02x %02x %02x %02x %02x"
+			       " %02x %02x %02x %02x %02x %02x %02x %02x\n", s,
+				obj[s], obj[s+1], obj[s+2], obj[s+3], obj[s+4], obj[s+5], obj[s+6], obj[s+7],
+				obj[s+8], obj[s+9], obj[s+10], obj[s+11], obj[s+12], obj[s+13], obj[s+14], obj[s+15]);
+			return 0;
+		}
+	}
+	printf("SUCCESS\n");
+	return 1;
+}
+
+static int nand_read_pages(struct ingenic_dev *dev, unsigned int start_page, int num_pages, char *buf,
+                          int length, uint16_t op, char *ret)
+{
+        usb_send_data_address_to_ingenic(dev, start_page);
+        usb_send_data_length_to_ingenic(dev, num_pages);
+
+        usb_ingenic_nand_ops(dev, op);
+
+        usb_read_data_from_ingenic(dev, buf, length);
+
+        usb_read_data_from_ingenic(dev, ret, 8);
+
+        return 0;
+}
+int nand_markbad(struct ingenic_dev *dev, uint8_t nand_idx, uint32_t block)
+{
+    char ret[8];
+    (void)nand_idx;
+
+	if (usb_get_ingenic_cpu(dev) < 3) {
+		printf("Device unboot! Boot it first!\n");
+		return -1;
+	}
+	printf("Mark bad block : %d\n", block);
+	usb_send_data_address_to_ingenic(dev, block);
+	usb_ingenic_nand_ops(dev, NAND_MARK_BAD);
+	usb_read_data_from_ingenic(dev, ret, ARRAY_SIZE(ret));
+	printf("Mark bad block at %d\n",((ret[3] << 24) |
+					   (ret[2] << 16) |
+					   (ret[1] << 8)  |
+					   (ret[0] << 0)) / hand.nand_ppb);
+
+	return 0;
+}
+
+int nand_program_check(struct ingenic_dev *dev, uint8_t nand_idx,
+						unsigned int start_page, const char *data,
+                        uint32_t length, unsigned int mode)
+{
+	unsigned int page_num, cur_page = -1;
+	unsigned short op;
+    static char read_back_buf[MAX_TRANSFER_SIZE];
+    char ret[8];
+
+	printf("Writing NAND page %u len %u...\n", start_page, length);
+	if (length > (unsigned int)MAX_TRANSFER_SIZE) {
+		printf("Buffer size too long!\n");
+		return -ENOMEM;
+	}
+
+	if (usb_get_ingenic_cpu(dev) < 3) {
+		printf("Device unboot! Boot it first!\n");
+		return -ENODEV;
+	}
+	usb_send_data_to_ingenic(dev, data, length);
+
+    if (mode == NO_OOB)
+        page_num = DIV_ROUND_UP(length, hand.nand_ps);
+	else
+		page_num = DIV_ROUND_UP(length, hand.nand_ps + hand.nand_os);
+
+	op = NAND_OP(nand_idx, NAND_PROGRAM, mode);
+
+	usb_send_data_address_to_ingenic(dev, start_page);
+	usb_send_data_length_to_ingenic(dev, page_num);
+	usb_ingenic_nand_ops(dev, op);
+
+	usb_read_data_from_ingenic(dev, ret, 8);
+	printf("Finish! (len %d start_page %d page_num %d)\n",
+		       length, start_page, page_num);
+
+    switch(mode) {
+	case NAND_READ:
+        op = NAND_OP(nand_idx, NAND_READ, NO_OOB);
+		break;
+	case NAND_READ_OOB:
+        op = NAND_OP(nand_idx, NAND_READ_OOB, 0);
+		break;
+	case NAND_READ_RAW:
+        op = NAND_OP(nand_idx, NAND_READ_RAW, NO_OOB);
+		break;
+    }
+
+    nand_read_pages(dev, start_page, page_num, read_back_buf, length, op, ret);
+    printf("Checking %d bytes...", length);
+
+    cur_page = (ret[3] << 24) | (ret[2] << 16) |  (ret[1] << 8) |
+        (ret[0] << 0);
+
+    if (start_page < 1 &&
+        hand.nand_ps == 4096 &&
+        hand.fw_args.cpu_id == 0x4740) {
+        printf("no check! End at Page: %d\n", cur_page);
+    }
+
+    if (!error_check(data, read_back_buf, length)) {
+        // tbd: doesn't the other side skip bad blocks too? Can we just deduct 1 from cur_page?
+        // tbd: why do we only mark a block as bad if the last page in the block was written?
+        if (cur_page % hand.nand_ppb == 0)
+            nand_markbad(dev, nand_idx, (cur_page - 1) / hand.nand_ppb);
+    }
+
+    printf("End at Page: %d\n", cur_page);
+
+/*	*start_page = cur_page;*/
+	return 0;
+}
+
+int nand_erase(struct ingenic_dev *dev, uint8_t nand_idx, uint32_t start_block,
+				uint32_t num_blocks)
+{
+    uint32_t end_block;
+    uint16_t op;
+    static char ret[8];
+
+	if (start_block > (unsigned int)NAND_MAX_BLK_NUM)  {
+		printf("Start block number overflow!\n");
+		return -1;
+	}
+	if (num_blocks > (unsigned int)NAND_MAX_BLK_NUM) {
+		printf("Length block number overflow!\n");
+		return -1;
+	}
+
+	if (usb_get_ingenic_cpu(dev) < 3) {
+		printf("Device unboot! Boot it first!\n");
+		return -1;
+	}
+
+    printf("Erasing No.%d device No.%d flash (start_blk %u blk_num %u)......\n",
+           0, nand_idx, start_block, num_blocks);
+
+    usb_send_data_address_to_ingenic(dev, start_block);
+    usb_send_data_length_to_ingenic(dev, num_blocks);
+
+    op = NAND_OP(nand_idx, NAND_ERASE, 0);
+    usb_ingenic_nand_ops(dev, op);
+
+    usb_read_data_from_ingenic(dev, ret, 8);
+    printf("Finish!");
+
+	end_block = ((ret[3] << 24) | (ret[2] << 16) |
+		     (ret[1] << 8) | (ret[0] << 0)) / hand.nand_ppb;
+	printf("Return: %02x %02x %02x %02x %02x %02x %02x %02x (position %d)\n",
+	       ret[0], ret[1], ret[2], ret[3], ret[4], ret[5], ret[6], ret[7], end_block);
+	if (!hand.nand_force_erase) {
+	/* not force erase, show bad block infomation */
+		printf("There are marked bad blocks: %d\n",
+		       end_block - start_block - num_blocks );
+	} else {
+	/* force erase, no bad block infomation can show */
+		printf("Force erase, no bad block infomation!\n" );
+	}
+
+	return 0;
+}
+
+int nand_program_file(struct ingenic_dev *dev, uint8_t nand_idx,
+					uint32_t start_page, const char *filename, int mode)
+{
+	uint32_t start_block, num_blocks;
+	int flen, m, j, k;
+	unsigned int page_num, code_len, offset, transfer_size;
+	int fd, status;
+	struct stat fstat;
+    static char code_buf[MAX_TRANSFER_SIZE];
+
+	status = stat(filename, &fstat);
+
+	if (status < 0) {
+		fprintf(stderr, "Error - can't get file size from '%s': %s\n",
+			filename, strerror(errno));
+		return -1;
+	}
+	flen = fstat.st_size;
+
+	fd = open(filename, O_RDONLY);
+	if (fd < 0) {
+		fprintf(stderr, "Error - can't open file '%s': %s\n",
+			filename, strerror(errno));
+		return -1;
+	}
+
+	printf("Programing No.%d device, flen %d, start page %d...\n", 0,
+			flen, start_page);
+
+	/* printf("length %d flen %d\n", n_in.length, flen); */
+	if (mode == NO_OOB)
+		transfer_size = (hand.nand_ppb * hand.nand_ps);
+	else
+		transfer_size = (hand.nand_ppb * (hand.nand_ps + hand.nand_os));
+
+	start_block = start_page / hand.nand_ppb;
+	num_blocks = flen / (transfer_size - 1) + 1;
+
+	if (nand_erase(dev, nand_idx, start_block, num_blocks))
+		return -1;
+
+	m = flen / transfer_size;
+	j = flen % transfer_size;
+
+	printf("Size to send %d, transfer_size %d\n", flen, transfer_size);
+	printf("Image type : %s\n", IMAGE_TYPE[mode]);
+	printf("It will cause %d times buffer transfer.\n", j == 0 ? m : m + 1);
+
+	if (mode == NO_OOB)
+		page_num = transfer_size / hand.nand_ps;
+	else
+		page_num = transfer_size / (hand.nand_ps + hand.nand_os);
+
+
+	offset = 0;
+	for (k = 0; k < m; k++)	{
+		code_len = transfer_size;
+		status = read(fd, code_buf, code_len);
+		if (status < (int)code_len) {
+			fprintf(stderr, "Error - can't read file '%s': %s\n",
+				filename, strerror(errno));
+			goto close;
+		}
+
+		if (nand_program_check(dev, nand_idx, start_page, code_buf, code_len, mode) == -1)
+            goto close;
+
+/*		if (start_page - nand_in->start > hand.nand_ppb)
+			printf("Skip a old bad block !\n");*/
+
+		offset += code_len ;
+	}
+
+	if (j) {
+		code_len = j;
+		if (j % hand.nand_ps)
+			j += hand.nand_ps - (j % hand.nand_ps);
+		memset(code_buf, 0, j);		/* set all to null */
+
+		status = read(fd, code_buf, code_len);
+
+		if (status < (int)code_len) {
+			fprintf(stderr, "Error - can't read file '%s': %s\n",
+				filename, strerror(errno));
+			goto close;
+		}
+
+		if (nand_program_check(dev, nand_idx, start_page, code_buf, j, mode) == -1)
+			goto close;
+
+/*
+		if (start_page - nand_in->start > hand.nand_ppb)
+			printf("Skip a old bad block !");
+*/
+	}
+close:
+	close(fd);
+	return 0;
+}
+
+int nand_prog(struct ingenic_dev *dev, uint8_t nand_idx, uint32_t start_page,
+			const char *filename, int mode)
+{
+	if (hand.nand_plane > 1)
+		printf("ERROR");
+	else
+		nand_program_file(dev, nand_idx, start_page, filename, mode);
+
+	return 0;
+}
+
+int nand_query(struct ingenic_dev *dev, uint8_t nand_idx)
+{
+    uint16_t op;
+    char ret[8];
+
+	if (usb_get_ingenic_cpu(dev) < 3) {
+		printf("Device unboot! Boot it first!\n");
+		return -1;
+	}
+
+	printf("ID of No.%u device No.%u flash: \n", 0, nand_idx);
+
+	op = NAND_OP(nand_idx, NAND_QUERY, 0);
+
+	usb_ingenic_nand_ops(dev, op);
+	usb_read_data_from_ingenic(dev, ret, ARRAY_SIZE(ret));
+	printf("Vendor ID    :0x%x \n", (unsigned char)ret[0]);
+	printf("Product ID   :0x%x \n", (unsigned char)ret[1]);
+	printf("Chip ID      :0x%x \n", (unsigned char)ret[2]);
+	printf("Page ID      :0x%x \n", (unsigned char)ret[3]);
+	printf("Plane ID     :0x%x \n", (unsigned char)ret[4]);
+
+	usb_read_data_from_ingenic(dev, ret, ARRAY_SIZE(ret));
+	printf("Operation status: Success!\n");
+
+	return 0;
+}
+
+int nand_read(struct ingenic_dev *dev, uint8_t nand_idx, int mode,
+				uint32_t start_page, uint32_t length, uint32_t ram_addr)
+{
+	uint16_t op;
+    uint32_t page;
+    uint32_t request_length;
+    uint32_t pages_per_request;
+    char ret[8];
+    char *buf;
+    int fd;
+
+	if (start_page > NAND_MAX_PAGE_NUM) {
+		printf("Page number overflow!\n");
+		return -1;
+	}
+	if (usb_get_ingenic_cpu(dev) < 3) {
+		printf("Device unboot! Boot it first!\n");
+		return -1;
+	}
+	if (nand_idx >= 16)
+        return -1;
+
+	printf("Reading from No.%u device No.%u flash....\n", 0, nand_idx);
+
+
+	switch(mode) {
+	case NAND_READ:
+        op = NAND_OP(nand_idx, NAND_READ, NO_OOB);
+		break;
+	case NAND_READ_OOB:
+        op = NAND_OP(nand_idx, NAND_READ_OOB, 0);
+		break;
+	case NAND_READ_RAW:
+        op = NAND_OP(nand_idx, NAND_READ_RAW, NO_OOB);
+		break;
+	case NAND_READ_TO_RAM:
+        op = NAND_OP(nand_idx, NAND_READ_TO_RAM, NO_OOB);
+		printf("Reading nand to RAM: 0x%x\n", ram_addr);
+		usb_ingenic_start(dev, VR_PROGRAM_START1, ram_addr);
+		break;
+	default:
+		printf("unknow mode!\n");
+		return -1;
+	}
+
+    pages_per_request = 1;
+    request_length = hand.nand_ps * pages_per_request;
+
+    page = start_page;
+
+    buf = malloc(request_length);
+
+    fd = open("/tmp/dump.bin", O_WRONLY | O_TRUNC | O_CREAT);
+    if (fd < 0) {
+        printf("Failed to open file\n");
+        return errno;
+    }
+
+    while (length > 0) {
+        if (request_length > length)
+            request_length = length;
+
+        nand_read_pages(dev, page, pages_per_request, buf, request_length, op, ret);
+
+        write(fd, buf, request_length);
+
+        length -= request_length;
+        page += pages_per_request;
+    }
+    close(fd);
+	printf("Operation end position : %u \n",
+	       (ret[3]<<24)|(ret[2]<<16)|(ret[1]<<8)|(ret[0]<<0));
+    free(buf);
+
+	return 1;
+}
diff --git a/usbboot/src/nand.h b/usbboot/src/nand.h
new file mode 100644
index 0000000..2eed582
--- /dev/null
+++ b/usbboot/src/nand.h
@@ -0,0 +1,20 @@
+
+#ifndef __NAND_H
+#define __NAND_H
+
+#include <stdint.h>
+
+struct ingenic_dev;
+
+int nand_query(struct ingenic_dev *dev, uint8_t nand_idx);
+int nand_read(struct ingenic_dev *dev, uint8_t nand_idx, int mode,
+				uint32_t start_page, uint32_t length, uint32_t ram_addr);
+int nand_dump(struct ingenic_dev *dev, uint8_t nand_idx,
+				uint32_t start_page, uint32_t length, const char *filename);
+int nand_erase(struct ingenic_dev *dev, uint8_t nand_idx,
+				uint32_t start_block, uint32_t num_blocks);
+int nand_prog(struct ingenic_dev *dev, uint8_t nand_idx,
+				uint32_t start_page, const char *filename, int mode);
+int nand_markbad(struct ingenic_dev *dev, uint8_t nand_idx, uint32_t block);
+
+#endif
diff --git a/usbboot/src/usb_boot_defines.h b/usbboot/src/usb_boot_defines.h
index 8b11f0f..aaa23e2 100644
--- a/usbboot/src/usb_boot_defines.h
+++ b/usbboot/src/usb_boot_defines.h
@@ -134,25 +134,9 @@ struct hand {
 	struct fw_args fw_args;
 } __attribute__((packed));
 
-struct nand_in {
-	unsigned char dev;
-	unsigned char max_chip;
-	unsigned char *buf;
-	unsigned char *cs_map;
-	unsigned int start;
-	unsigned int length;
-	unsigned int option;
-
-	int (* check) (unsigned char *,unsigned char *,unsigned int);
-};
-
-struct nand_out {
-	unsigned char *status;
-};
-
 struct sdram_in {
 	unsigned char dev;
-	unsigned char *buf;
+	char *buf;
 	unsigned int start;
 	unsigned int length;
 	unsigned int option;
diff --git a/usbboot/xburst_include/usb_boot.h b/usbboot/xburst_include/usb_boot.h
index 1b6070f..7526d8e 100644
--- a/usbboot/xburst_include/usb_boot.h
+++ b/usbboot/xburst_include/usb_boot.h
@@ -22,8 +22,7 @@
 #ifndef __USB_BOOT_H__
 #define __USB_BOOT_H__
 
-#define BULK_OUT_BUF_SIZE 0x21000
-#define BULK_IN_BUF_SIZE 0x21000
+#define BULK_BUF_SIZE (128 * 4096)
 
 enum UDC_STATE
 {
@@ -43,7 +42,8 @@ enum USB_JZ4740_REQUEST	/* add for USB_BOOT */
 	VR_NOR_OPS,
 	VR_NAND_OPS,
 	VR_SDRAM_OPS,
-	VR_CONFIGRATION
+	VR_CONFIGRATION,
+    VR_MEM_OPS,
 };
 
 #endif	/* __USB_BOOT_H__ */
diff --git a/usbboot/xburst_stage2/boothandler.c b/usbboot/xburst_stage2/boothandler.c
index b166b6a..bd5a7ee 100644
--- a/usbboot/xburst_stage2/boothandler.c
+++ b/usbboot/xburst_stage2/boothandler.c
@@ -20,7 +20,7 @@
  */
 
 #include "jz4740.h"
-#include "usb.h" 
+#include "usb.h"
 #include "error.h"
 #include "usb_boot.h"
 #include "usb_boot_defines.h"
@@ -28,6 +28,22 @@
 #include "udc.h"
 #define dprintf(x) serial_puts(x)
 
+#define MEM_WRITE 0
+#define MEM_READ 1
+
+#define MEM_8BIT (0 << 1)
+#define MEM_16BIT (1 << 1)
+#define MEM_32BIT (2 << 1)
+
+#define MEM_WRITE8  (MEM_WRITE | MEM_8BIT)
+#define MEM_WRITE16 (MEM_WRITE | MEM_16BIT)
+#define MEM_WRITE32 (MEM_WRITE | MEM_32BIT)
+#define MEM_READ8   (MEM_READ  | MEM_8BIT)
+#define MEM_READ16  (MEM_READ  | MEM_16BIT)
+#define MEM_READ32  (MEM_READ  | MEM_32BIT)
+
+
+
 unsigned int (*nand_query)(u8 *);
 int (*nand_init)(int bus_width, int row_cycle, int page_size, int page_per_block,
 		 int,int,int,int);
@@ -42,8 +58,7 @@ void (*nand_enable) (unsigned int csn);
 void (*nand_disable) (unsigned int csn);
 
 struct hand Hand,*Hand_p;
-extern u32 Bulk_out_buf[BULK_OUT_BUF_SIZE];
-extern u32 Bulk_in_buf[BULK_IN_BUF_SIZE];
+extern u32 Bulk_buf[BULK_BUF_SIZE];
 extern u16 handshake_PKT[4];
 extern udc_state;
 extern void *memset(void *s, int c, size_t count);
@@ -68,8 +83,8 @@ void dump_data(unsigned int *p, int size)
 void config_hand()
 {
 	struct hand *hand_p;
-	hand_p=(struct hand *)Bulk_out_buf;
-	memcpy(&Hand, (unsigned char *)Bulk_out_buf, sizeof(struct hand));
+	hand_p=(struct hand *)Bulk_buf;
+	memcpy(&Hand, (unsigned char *)Bulk_buf, sizeof(struct hand));
 
 #if 0
 	Hand.nand_bw=hand_p->nand_bw;
@@ -104,9 +119,9 @@ int GET_CUP_INFO_Handle()
 	else
 		HW_SendPKT(0,temp2,8);
 	udc_state = IDLE;
-	return ERR_OK; 
+	return ERR_OK;
 }
-	       
+
 int SET_DATA_ADDERSS_Handle(u8 *buf)
 {
 	USB_DeviceRequest *dreq = (USB_DeviceRequest *)buf;
@@ -115,7 +130,7 @@ int SET_DATA_ADDERSS_Handle(u8 *buf)
 	serial_put_hex(start_addr);
 	return ERR_OK;
 }
-		
+
 int SET_DATA_LENGTH_Handle(u8 *buf)
 {
 	USB_DeviceRequest *dreq = (USB_DeviceRequest *)buf;
@@ -129,7 +144,7 @@ int FLUSH_CACHES_Handle()
 {
 	return ERR_OK;
 }
-    
+
 int PROGRAM_START1_Handle(u8 *buf)
 {
 	USB_DeviceRequest *dreq = (USB_DeviceRequest *)buf;
@@ -150,10 +165,58 @@ int PROGRAM_START2_Handle(u8 *buf)
 	f();
 	return ERR_OK;
 }
-	      
+
+int MEM_OPS_Handle(u8 *buf)
+{
+	USB_DeviceRequest *dreq = (USB_DeviceRequest *)buf;
+    u32 val;
+
+    switch (dreq->wValue) {
+    case MEM_WRITE8:
+        *((volatile u8*)(start_addr)) = (u8)ops_length;
+		handshake_PKT[0] = ops_length & 0xff;
+		handshake_PKT[1] = 8;
+        break;
+    case MEM_WRITE16:
+        *((volatile u16*)(start_addr)) = (u16)ops_length;
+		handshake_PKT[0] = ops_length & 0xffff;
+		handshake_PKT[1] = 16;
+        break;
+    case MEM_WRITE32:
+        *((volatile u32*)(start_addr)) = ops_length;
+		handshake_PKT[0] = ops_length;
+		handshake_PKT[1] = ops_length >> 16;
+        break;
+    case MEM_READ8:
+        val = *((volatile u8*)(start_addr));
+		handshake_PKT[0] = (u16)val;
+		handshake_PKT[1] = (u16)(val >> 16);
+        break;
+    case MEM_READ16:
+        val = *((volatile u16*)(start_addr));
+		handshake_PKT[0] = (u16)val;
+		handshake_PKT[1] = (u16)(val >> 16);
+        break;
+    case MEM_READ32:
+        val = *((volatile u32*)(start_addr));
+		handshake_PKT[0] = (u16)val;
+		handshake_PKT[1] = (u16)(val >> 16);
+        break;
+    default:
+		handshake_PKT[0] = 0xDEAD;
+		handshake_PKT[1] = 0xC0DE;
+        break;
+    }
+    handshake_PKT[2] = 0;
+    handshake_PKT[3] = 0;
+    HW_SendPKT(1,handshake_PKT,sizeof(handshake_PKT));
+    return ERR_OK;
+}
+
 int NOR_OPS_Handle(u8 *buf)
 {
 	USB_DeviceRequest *dreq = (USB_DeviceRequest *)buf;
+    udc_state = IDLE;
 	return ERR_OK;
 }
 
@@ -171,8 +234,8 @@ int NAND_OPS_Handle(u8 *buf)
 	{
 	case NAND_QUERY:
 		dprintf("\n Request : NAND_QUERY!");
-		nand_query((u8 *)Bulk_in_buf);
-		HW_SendPKT(1, Bulk_in_buf, 8);
+		nand_query((u8 *)Bulk_buf);
+		HW_SendPKT(1, Bulk_buf, 8);
 		handshake_PKT[3]=(u16)ERR_OK;
 		udc_state = BULK_IN;
 		break;
@@ -186,32 +249,32 @@ int NAND_OPS_Handle(u8 *buf)
 		handshake_PKT[0] = (u16) ret_dat;
 		handshake_PKT[1] = (u16) (ret_dat>>16);
 		HW_SendPKT(1,handshake_PKT,sizeof(handshake_PKT));
-		udc_state = IDLE;		
+		udc_state = IDLE;
 
 		break;
 	case NAND_READ_OOB:
 		dprintf("\n Request : NAND_READ_OOB!");
-		memset(Bulk_in_buf,0,ops_length*Hand.nand_ps);
-		ret_dat = nand_read_oob(Bulk_in_buf,start_addr,ops_length);
+		memset(Bulk_buf,0,ops_length*Hand.nand_ps);
+		ret_dat = nand_read_oob(Bulk_buf,start_addr,ops_length);
 		handshake_PKT[0] = (u16) ret_dat;
 		handshake_PKT[1] = (u16) (ret_dat>>16);
-		HW_SendPKT(1,(u8 *)Bulk_in_buf,ops_length*Hand.nand_ps);
-		udc_state = BULK_IN;		
+		HW_SendPKT(1,(u8 *)Bulk_buf,ops_length*Hand.nand_ps);
+		udc_state = BULK_IN;
 		break;
 	case NAND_READ_RAW:
 		dprintf("\n Request : NAND_READ_RAW!");
 		switch (option)
 		{
 		case OOB_ECC:
-			nand_read_raw(Bulk_in_buf,start_addr,ops_length,option);
-			HW_SendPKT(1,(u8 *)Bulk_in_buf,ops_length*(Hand.nand_ps + Hand.nand_os));
+			nand_read_raw(Bulk_buf,start_addr,ops_length,option);
+			HW_SendPKT(1,(u8 *)Bulk_buf,ops_length*(Hand.nand_ps + Hand.nand_os));
 			handshake_PKT[0] = (u16) ret_dat;
 			handshake_PKT[1] = (u16) (ret_dat>>16);
 			udc_state = BULK_IN;
 			break;
 		default:
-			nand_read_raw(Bulk_in_buf,start_addr,ops_length,option);
-			HW_SendPKT(1,(u8 *)Bulk_in_buf,ops_length*Hand.nand_ps);
+			nand_read_raw(Bulk_buf,start_addr,ops_length,option);
+			HW_SendPKT(1,(u8 *)Bulk_buf,ops_length*Hand.nand_ps);
 			handshake_PKT[0] = (u16) ret_dat;
 			handshake_PKT[1] = (u16) (ret_dat>>16);
 			udc_state = BULK_IN;
@@ -234,24 +297,24 @@ int NAND_OPS_Handle(u8 *buf)
 		switch (option)
 		{
 		case 	OOB_ECC:
-			ret_dat = nand_read(Bulk_in_buf,start_addr,ops_length,OOB_ECC);
+			ret_dat = nand_read(Bulk_buf,start_addr,ops_length,OOB_ECC);
 			handshake_PKT[0] = (u16) ret_dat;
 			handshake_PKT[1] = (u16) (ret_dat>>16);
-			HW_SendPKT(1,(u8 *)Bulk_in_buf,ops_length*(Hand.nand_ps + Hand.nand_os ));
+			HW_SendPKT(1,(u8 *)Bulk_buf,ops_length*(Hand.nand_ps + Hand.nand_os ));
 			udc_state = BULK_IN;
 			break;
 		case 	OOB_NO_ECC:
-			ret_dat = nand_read(Bulk_in_buf,start_addr,ops_length,OOB_NO_ECC);
+			ret_dat = nand_read(Bulk_buf,start_addr,ops_length,OOB_NO_ECC);
 			handshake_PKT[0] = (u16) ret_dat;
 			handshake_PKT[1] = (u16) (ret_dat>>16);
-			HW_SendPKT(1,(u8 *)Bulk_in_buf,ops_length*(Hand.nand_ps + Hand.nand_os));
+			HW_SendPKT(1,(u8 *)Bulk_buf,ops_length*(Hand.nand_ps + Hand.nand_os));
 			udc_state = BULK_IN;
 			break;
 		case 	NO_OOB:
-			ret_dat = nand_read(Bulk_in_buf,start_addr,ops_length,NO_OOB);
+			ret_dat = nand_read(Bulk_buf,start_addr,ops_length,NO_OOB);
 			handshake_PKT[0] = (u16) ret_dat;
 			handshake_PKT[1] = (u16) (ret_dat>>16);
-			HW_SendPKT(1,(u8 *)Bulk_in_buf,ops_length*Hand.nand_ps);
+			HW_SendPKT(1,(u8 *)Bulk_buf,ops_length*Hand.nand_ps);
 			udc_state = BULK_IN;
 			break;
 		}
@@ -260,7 +323,7 @@ int NAND_OPS_Handle(u8 *buf)
 	case NAND_PROGRAM:
 		dprintf("\n Request : NAND_PROGRAM!");
 //		dprintf("\n Option : %x",option);
-		ret_dat = nand_program((void *)Bulk_out_buf,
+		ret_dat = nand_program((void *)Bulk_buf,
 			     start_addr,ops_length,option);
 		dprintf("\n NAND_PROGRAM finish!");
 		handshake_PKT[0] = (u16) ret_dat;
@@ -294,7 +357,7 @@ int SDRAM_OPS_Handle(u8 *buf)
 	{
 	case 	SDRAM_LOAD:
 //		dprintf("\n Request : SDRAM_LOAD!");
-		ret_dat = (u32)memcpy((u8 *)start_addr,Bulk_out_buf,ops_length);
+		ret_dat = (u32)memcpy((u8 *)start_addr,Bulk_buf,ops_length);
 		handshake_PKT[0] = (u16) ret_dat;
 		handshake_PKT[1] = (u16) (ret_dat>>16);
 		HW_SendPKT(1,handshake_PKT,sizeof(handshake_PKT));
@@ -314,7 +377,7 @@ void Borad_Init()
 		//Init nand flash
 		nand_init_4740(Hand.nand_bw,Hand.nand_rc,Hand.nand_ps,Hand.nand_ppb,
 		       Hand.nand_bbpage,Hand.nand_bbpos,Hand.nand_force_erase,Hand.nand_eccpos);
-	
+
 		dprintf("\nnand_ps, nand_ppb, nand_bbpage, nand_bbpos, nand_eccpos\n");
 		serial_put_hex(Hand.nand_ps);
 		serial_put_hex(Hand.nand_ppb);
@@ -368,7 +431,6 @@ int CONFIGRATION_Handle(u8 *buf)
 		config_hand();
 		break;
 	default:;
-		
 	}
 	Borad_Init();
 	return ERR_OK;
diff --git a/usbboot/xburst_stage2/nandflash_4740.c b/usbboot/xburst_stage2/nandflash_4740.c
index a2c8d77..5bbf8ed 100644
--- a/usbboot/xburst_stage2/nandflash_4740.c
+++ b/usbboot/xburst_stage2/nandflash_4740.c
@@ -219,6 +219,7 @@ u32 nand_read_oob_4740(void *buf, u32 startpage, u32 pagenum)
 static int nand_check_block(u32 block)
 {
 	u32 pg,i;
+
 	if ( bad_block_page >= ppb ) {
 		/* do absolute bad block detect! */
 		pg = block * ppb + 0;
diff --git a/usbboot/xburst_stage2/udc.c b/usbboot/xburst_stage2/udc.c
index 8f074f9..5b4cbd4 100644
--- a/usbboot/xburst_stage2/udc.c
+++ b/usbboot/xburst_stage2/udc.c
@@ -21,11 +21,10 @@
 #include "udc.h"
 #include "usb_boot.h"
 
-#define dprintf(x...)  
+#define dprintf(x...)
 #define TXFIFOEP0 USB_FIFO_EP0
 
-u32 Bulk_in_buf[BULK_IN_BUF_SIZE];
-u32 Bulk_out_buf[BULK_OUT_BUF_SIZE];
+u32 Bulk_buf[BULK_BUF_SIZE];
 u32 Bulk_in_size, Bulk_in_finish, Bulk_out_size;
 u16 handshake_PKT[4] = {0, 0, 0, 0};
 u8 udc_state;
@@ -35,7 +34,7 @@ static u32 tx_buf[32];
 static u32 tx_size, rx_size, finished, fifo;
 static u8 ep0state, USB_Version;
 
-static u32 fifoaddr[] = 
+static u32 fifoaddr[] =
 {
 	TXFIFOEP0, TXFIFOEP0+4 ,TXFIFOEP0+8
 };
@@ -88,7 +87,7 @@ static void udcWriteFifo(u8 *ptr, int size)
 			while (q--)
 				REG8(fifo) = *c++;
 		}
-	} 
+	}
 }
 
 void HW_SendPKT(int ep, const u8 *buf, int size)
@@ -114,7 +113,7 @@ void HW_SendPKT(int ep, const u8 *buf, int size)
 		tx_size = size;
 		finished = 0;
 		memcpy((void *)tx_buf, buf, size);
-		ep0state = USB_EP0_TX;		
+		ep0state = USB_EP0_TX;
 	}
 }
 
@@ -131,7 +130,7 @@ void HW_GetPKT(int ep, const u8 *buf, int size)
 	memcpy((u8 *)rx_buf, (u8 *)((u32)rx_buf+size), rx_size);
 }
 
-static USB_DeviceDescriptor devDesc = 
+static USB_DeviceDescriptor devDesc =
 {
 	sizeof(USB_DeviceDescriptor),
 	DEVICE_DESCRIPTOR,	//1
@@ -220,7 +219,7 @@ void sendDevDescString(int size)
 		size = 26;
 	str_ret[0] = (0x0300 | size);
 	HW_SendPKT(0, (u8 *)str_ret,size);
-	
+
 }
 
 void sendDevDesc(int size)
@@ -340,7 +339,7 @@ void usbHandleStandDevReq(u8 *buf)
 			}
 		dprintf("\nSet ep0state=TX!");
 		ep0state=USB_EP0_TX;
-		
+
 		break;
 	case SET_ADDRESS:
 		dprintf("\nSET_ADDRESS!");
@@ -407,12 +406,16 @@ void usbHandleVendorReq(u8 *buf)
 		SDRAM_OPS_Handle(buf);
 		Bulk_out_size = 0;
 		break;
+    case VR_MEM_OPS:
+        MEM_OPS_Handle(buf);
+		Bulk_out_size = 0;
+        break;
 	}
 }
 
 void Handshake_PKT()
 {
-	
+
 	if (udc_state!=IDLE)
 	{
 		HW_SendPKT(1,(u8 *)handshake_PKT,sizeof(handshake_PKT));
@@ -483,7 +486,7 @@ void EP0_Handler ()
 		}
 		rx_size = 0;
 	}
-	
+
 	if (ep0state == USB_EP0_TX) {
 		fifo=fifoaddr[0];
 		if (tx_size - finished <= 64) {
@@ -513,12 +516,12 @@ void EPIN_Handler(u8 EP)
 	}
 
 	if (Bulk_in_size - Bulk_in_finish <= fifosize[EP]) {
-		udcWriteFifo((u8 *)((u32)Bulk_in_buf+Bulk_in_finish),
+		udcWriteFifo((u8 *)((u32)Bulk_buf+Bulk_in_finish),
 			     Bulk_in_size - Bulk_in_finish);
 		usb_setw(USB_REG_INCSR, USB_INCSR_INPKTRDY);
 		Bulk_in_finish = Bulk_in_size;
 	} else {
-		udcWriteFifo((u8 *)((u32)Bulk_in_buf+Bulk_in_finish),
+		udcWriteFifo((u8 *)((u32)Bulk_buf+Bulk_in_finish),
 			    fifosize[EP]);
 		usb_setw(USB_REG_INCSR, USB_INCSR_INPKTRDY);
 		Bulk_in_finish += fifosize[EP];
@@ -531,7 +534,13 @@ void EPOUT_Handler(u8 EP)
 	jz_writeb(USB_REG_INDEX, EP);
 	size = jz_readw(USB_REG_OUTCOUNT);
 	fifo = fifoaddr[EP];
-	udcReadFifo((u8 *)((u32)Bulk_out_buf+Bulk_out_size), size);
+
+	if (size + Bulk_out_size > BULK_BUF_SIZE) {
+		serial_puts("\n Bulk_out overflow");
+		return;
+	}
+
+	udcReadFifo((u8 *)((u32)Bulk_buf+Bulk_out_size), size);
 	usb_clearb(USB_REG_OUTCSR,USB_OUTCSR_OUTPKTRDY);
 	Bulk_out_size += size;
 	dprintf("\nEPOUT_handle return!");
@@ -546,13 +555,13 @@ void udc4740Proc ()
 	IntrUSB = jz_readb(USB_REG_INTRUSB);
 	IntrIn  = jz_readw(USB_REG_INTRIN);
 	IntrOut = jz_readw(USB_REG_INTROUT);
-		
+
 	if ( IntrUSB == 0 && IntrIn == 0 && IntrOut == 0)
 		return;
-		
+
 	if (IntrIn & 2) {
 		dprintf("\nUDC EP1 IN operation!");
-		EPIN_Handler(1);	     
+		EPIN_Handler(1);
 	}
 
 	if (IntrOut & 2) {
@@ -561,10 +570,10 @@ void udc4740Proc ()
 	}
 
 	if (IntrUSB & USB_INTR_RESET) {
-		dprintf("\nUDC reset intrupt!");  
+		dprintf("\nUDC reset intrupt!");
 		udc_reset();
 	}
-		
+
 /* Check for endpoint 0 interrupt */
 	if (IntrIn & USB_INTR_EP0) {
 		dprintf("\nUDC EP0 operations!");