Add memory commands (cmp, cp, md, mm, mw, nm)

Add kind of scheduler for background tasks

16 files changed, 1487 insertions, 81 deletions

diff --git a/avr/Tupfile b/avr/Tupfile
index d90f225..449b2a1 100644
--- a/avr/Tupfile
+++ b/avr/Tupfile
@@ -3,10 +3,10 @@ include_rules
 PROG		= stamp-bootc
 SRC		= bootc.c
 SRC		+= cli.c cli_readline.c command.c command_tbl.c
-SRC		+= cmd_help.c cmd_echo.c
+SRC		+= cmd_help.c cmd_echo.c cmd_mem.c
 SRC		+= env.c xmalloc.c
 SRC		+= timer.c con-utils.c serial.c
-SRC		+= z80-if.c
+SRC		+= background.c z180-serv.c z80-if.c
 
 #SRC_Z		= ../z180/hdrom.c
 
diff --git a/avr/background.c b/avr/background.c
new file mode 100644
index 0000000..37e4b02
--- /dev/null
+++ b/avr/background.c
@@ -0,0 +1,31 @@
+#include "common.h"
+#include "background.h"
+
+
+#define BG_FUNC_MAX 5
+
+static bg_func func_tab[BG_FUNC_MAX];
+static int_fast8_t fcount;
+
+int bg_register(bg_func f)
+{
+	if (fcount < BG_FUNC_MAX) { 
+		func_tab[fcount++] = f;
+		return 1;
+	}
+	return 0;
+}
+
+
+void bg_shed(void)
+{
+	static int_fast8_t current;
+	
+	if (func_tab[current]) {
+		func_tab[current](0);
+	}
+	if (++current >= fcount)
+		current = 0;
+}
+
+
diff --git a/avr/background.h b/avr/background.h
new file mode 100644
index 0000000..a624dbb
--- /dev/null
+++ b/avr/background.h
@@ -0,0 +1,10 @@
+#ifndef BACKGROUND_H
+#define BACKGROUND_H
+
+typedef int (*bg_func)(int);
+
+int bg_register(bg_func f);
+void bg_shed(void);
+
+#endif /* BACKGROUND_H */
+
diff --git a/avr/cli.c b/avr/cli.c
index 44cf5b8..4b77499 100644
--- a/avr/cli.c
+++ b/avr/cli.c
@@ -296,15 +296,10 @@ int run_command_list(const char *cmd, int len)
 
 /****************************************************************************/
 
-#define DYN_BUFFER 1
 
 void cli_loop(void)
 {
-#if DYN_BUFFER
 	char *lastcommand = NULL;
-#else
-	static char lastcommand[CONFIG_SYS_CBSIZE];
-#endif
 	int len;
 	int flag;
 	int rc = 1;
@@ -314,15 +309,11 @@ void cli_loop(void)
 
 		flag = 0;	/* assume no special flags for now */
 		if (len > 0) {
-#if DYN_BUFFER
 			lastcommand = (char *) xrealloc(lastcommand, len+1);
 			if (lastcommand != NULL) {
 				strncpy(lastcommand, console_buffer, len+1);
 				lastcommand[len] = '\0';
 			}
-#else
-			strcpy(lastcommand, console_buffer);
-#endif
 		} else if (len == 0)
 			flag |= CMD_FLAG_REPEAT;
 
@@ -333,12 +324,8 @@ void cli_loop(void)
 
 		if (rc <= 0) {
 			/* invalid command or not repeatable, forget it */
-#if DYN_BUFFER
 			free(lastcommand);
 			lastcommand = NULL;
-#else
-			lastcommand[0] = 0;
-#endif
 		}
 	}
 }
diff --git a/avr/cmd_mem.c b/avr/cmd_mem.c
new file mode 100644
index 0000000..df30196
--- /dev/null
+++ b/avr/cmd_mem.c
@@ -0,0 +1,878 @@
+/*
+ * (C) Copyright 2000
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+/*
+ * Memory Functions
+ *
+ * Copied from FADS ROM, Dan Malek (dmalek@jlc.net)
+ */
+
+#include "common.h"
+#include <stdlib.h>
+#include <ctype.h>
+
+#include "config.h"
+#include "debug.h"
+#include "command.h"
+#include "cli_readline.h"
+#include "con-utils.h"
+#include "z80-if.h"
+
+/* 
+ * TODO: printf() --> printf_P() 
+ */
+
+#ifndef CONFIG_SYS_MEMTEST_SCRATCH
+#define CONFIG_SYS_MEMTEST_SCRATCH 0
+#endif
+
+static int mod_mem(cmd_tbl_t *, int, int, int, char * const []);
+
+/* Display values from last command.
+ * Memory modify remembered values are different from display memory.
+ */
+static uint32_t	dp_last_addr;
+static uint32_t	dp_last_length = 0x100;
+static uint32_t	mm_last_addr;
+
+static	uint32_t	base_address = 0;
+
+/*--------------------------------------------------------------------------*/
+
+static void print_blanks(uint_fast8_t count)
+{
+	while(count--)
+		putchar(' ');
+}
+
+int z180_dump_mem(uint32_t startaddr, uint32_t len, const char *title)
+{
+	uint8_t buf[16];
+	uint8_t llen = 16;
+	uint8_t pre = startaddr % 16;
+	uint32_t addr = startaddr & ~0x0f;
+	len += pre;
+	uint8_t i;
+	
+	if (title && *title)
+		printf_P(PSTR("%s\n"),title);
+		
+	while (len) {
+		if (len < 16)
+			llen = len;
+
+		z80_request_bus();
+		for (i = pre; i < llen; i++)
+			buf[i] = z80_read(addr + i);
+		z80_release_bus();
+
+		printf_P(PSTR("%.5lx:"), addr);
+#if 0
+		print_blanks(3 * pre);
+
+		/* Print hex values */
+		for (i = pre; i < llen; i++)
+			printf_P(PSTR(" %.2x"), buf[i]);
+#else
+		for (i = 0; i < llen; i++) {
+			if ((i % 8) == 0)
+				putchar(' ');
+			if (i < pre)
+				printf_P(PSTR(".. "));
+			else
+				printf_P(PSTR("%.2x "), buf[i]);
+		}
+#endif
+		/* fill line with whitespace for nice ASCII print */
+#if 1
+		print_blanks(3 * (16u - i) + (16u-i)/8 + 1 + pre);
+#else
+
+#endif
+		/* Print data in ASCII characters */
+		for (i = pre; i < llen; i++)
+			printf_P(PSTR("%c"), isprint(buf[i]) ? buf[i] : '.');
+		putchar('\n');
+
+		pre = 0;
+		addr += 16;
+		len -= llen;
+
+		if (ctrlc())
+			return -1;
+	}
+	return 0;
+}
+
+
+/*--------------------------------------------------------------------------*/
+
+/* Memory Display
+ *
+ * Syntax:
+ *	md {addr} {len}
+ */
+int do_mem_md(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	uint32_t addr, length;
+	
+	(void) cmdtp;
+
+#if 0	
+	printf_P(PSTR("flag: %d, argc: %d"), flag, argc);
+	for (int i = 0; i < argc; i++) {
+		printf_P(PSTR(", argv[%d]: %s"), i, argv[i] ? argv[i] : "<NULL>");
+	}
+	putchar('\n');
+#endif
+
+	/* We use the last specified parameters, unless new ones are
+	 * entered.
+	 */
+	addr = dp_last_addr;
+	length = dp_last_length;
+
+	if (argc < 2)
+		return CMD_RET_USAGE;
+
+	if ((flag & CMD_FLAG_REPEAT) == 0) {
+		/* Address is specified since argc > 1 */
+		addr = strtoul(argv[1], NULL, 16);
+		addr += base_address;
+
+		/* If another parameter, it is the length to display. */
+		if (argc > 2)
+			length = strtoul(argv[2], NULL, 16);
+	}
+
+	/* Print the lines. */
+	z180_dump_mem(addr, length, NULL);
+
+	dp_last_addr = addr + length;
+	dp_last_length = length;
+	return 0;
+}
+
+int do_mem_mm(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	return mod_mem (cmdtp, 1, flag, argc, argv);
+}
+int do_mem_nm(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	return mod_mem (cmdtp, 0, flag, argc, argv);
+}
+
+int do_mem_mw(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	uint8_t writeval;
+	uint32_t addr, count;
+
+	(void) cmdtp;
+	(void) flag;
+
+	if ((argc < 3) || (argc > 4))
+		return CMD_RET_USAGE;
+
+	/* Address is specified since argc > 1
+	*/
+	addr = strtoul(argv[1], NULL, 16);
+	addr += base_address;
+
+	/* Get the value to write.
+	*/
+	writeval = (uint8_t) strtoul(argv[2], NULL, 16);
+
+	/* Count ? */
+	if (argc == 4) {
+		count = strtoul(argv[3], NULL, 16);
+	} else {
+		count = 1;
+	}
+
+	z80_request_bus();
+	while (count-- > 0) {
+		z80_write(addr, writeval);
+		++addr; 
+	}
+	z80_release_bus();
+
+	return 0;
+}
+
+#ifdef CONFIG_MX_CYCLIC
+int do_mem_mdc ( cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	int i;
+	uint32_t count;
+
+	if (argc < 4)
+		return CMD_RET_USAGE;
+
+	count = strtoul(argv[3], NULL, 10);
+
+	for (;;) {
+		do_mem_md (NULL, 0, 3, argv);
+
+		/* delay for <count> ms... */
+/* TODO: use timer */
+		for (i=0; i<count; i++)
+			udelay (1000);
+
+		/* check for ctrl-c to abort... */
+		if (ctrlc()) {
+			my_puts("Abort\n");
+			return 0;
+		}
+	}
+
+	return 0;
+}
+
+int do_mem_mwc ( cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	int i;
+	uint32_t count;
+
+	if (argc < 4)
+		return CMD_RET_USAGE;
+
+	count = strtoul(argv[3], NULL, 10);
+
+	for (;;) {
+		do_mem_mw (NULL, 0, 3, argv);
+
+		/* delay for <count> ms... */
+/* TODO: use timer */
+		for (i=0; i<count; i++)
+			udelay (1000);
+
+		/* check for ctrl-c to abort... */
+		if (ctrlc()) {
+			my_puts("Abort\n");
+			return 0;
+		}
+	}
+
+	return 0;
+}
+#endif /* CONFIG_MX_CYCLIC */
+
+int do_mem_cmp(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	uint32_t addr1, addr2, count, ngood;
+	int rcode = 0;
+	uint8_t byte1, byte2;
+
+	(void) cmdtp;
+	(void) flag;
+
+	if (argc != 4)
+		return CMD_RET_USAGE;
+
+
+	addr1 = strtoul(argv[1], NULL, 16);
+	addr1 += base_address;
+	addr2 = strtoul(argv[2], NULL, 16);
+	addr2 += base_address;
+	count = strtoul(argv[3], NULL, 16);
+
+	for (ngood = 0; ngood < count; ++ngood) {
+		z80_request_bus();
+		byte1 = z80_read(addr1);
+		byte2 = z80_read(addr2);
+		z80_release_bus();
+		if (byte1 != byte2) {
+			printf(	"byte at 0x%05lx (%#02x) != "
+				"byte at 0x%05lx (%#02x)\n",
+				addr1, byte1, addr2, byte2);
+			rcode = 1;
+			break;
+		}
+		addr1++;
+		addr2++;
+
+		/* check for ctrl-c to abort... */
+		if (ctrlc()) {
+			my_puts("Abort\n");
+			return 0;
+		}
+	}
+
+	printf("Total of %ld byte(s) (0x%lx) were the same\n", ngood, ngood);
+	return rcode;
+}
+
+int do_mem_cp(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	uint32_t src, dest, count;
+	int_fast8_t step;
+
+	(void) cmdtp;
+	(void) flag;
+
+	if (argc != 4)
+		return CMD_RET_USAGE;
+
+	src = strtoul(argv[1], NULL, 16);
+	src += base_address;
+	dest = strtoul(argv[2], NULL, 16);
+	dest += base_address;
+	count = strtoul(argv[3], NULL, 16);
+
+	if (count == 0) {
+		my_puts ("Zero length ???\n");
+		return 1;
+	}
+	
+	if (dest > src) {
+		src += count - 1;
+		dest += count - 1;
+		step = -1;
+	} else
+		step = 1;
+
+	while (count-- > 0) {
+		uint8_t data;
+		z80_request_bus();
+		data = z80_read(src);
+		z80_write(dest, data);
+		z80_release_bus();
+		src += step;
+		dest += step;
+
+		/* check for ctrl-c to abort... */
+		if (ctrlc()) {
+			my_puts("Abort\n");
+			return 0;
+		}
+	}
+	return 0;
+}
+
+int do_mem_base(cmd_tbl_t *cmdtp, int flag, int argc,
+		       char * const argv[])
+{
+	(void) cmdtp;
+	(void) flag;
+
+	if (argc > 1) {
+		/* Set new base address. */
+		base_address = strtoul(argv[1], NULL, 16);
+	}
+	/* Print the current base address. */
+	printf("Base Address: 0x%05lx\n", base_address);
+	return 0;
+}
+
+int do_mem_loop(cmd_tbl_t *cmdtp, int flag, int argc,
+		       char * const argv[])
+{
+	uint32_t addr, length;
+
+	(void) cmdtp;
+	(void) flag;
+
+	if (argc < 3)
+		return CMD_RET_USAGE;
+
+	/* Address is always specified. */
+	addr = strtoul(argv[1], NULL, 16);
+
+	/* Length is the number of bytes. */
+	length = strtoul(argv[2], NULL, 16);
+
+
+	/* We want to optimize the loops to run as fast as possible.
+	 * If we have only one object, just run infinite loops.
+	 */
+	if (length == 1) {
+		z80_request_bus();
+		for (;;)
+			z80_read(addr);
+		z80_release_bus();
+	}
+
+	z80_request_bus();
+	for (;;) {
+		uint32_t i = length;
+		uint32_t p = addr;
+		while (i-- > 0)
+			z80_read(p++);
+	}
+	z80_release_bus();
+
+	return 0;
+}
+
+#ifdef CONFIG_LOOPW
+int do_mem_loopw (cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	uint32_t addr, length;
+	uint8_t	data;
+
+	(void) cmdtp;
+	(void) flag;
+
+	if (argc < 4)
+		return CMD_RET_USAGE;
+
+	/* Address is always specified. */
+	addr = strtoul(argv[1], NULL, 16);
+
+	/* Length is the number of bytes. */
+	length = strtoul(argv[2], NULL, 16);
+
+	data = strtoul(argv[3], NULL, 16);
+
+	/* We want to optimize the loops to run as fast as possible.
+	 * If we have only one object, just run infinite loops.
+	 */
+	if (length == 1) {
+		z80_request_bus();
+		for (;;)
+			z80_write(addr, data);
+	}
+
+	for (;;) {
+		uint32_t i = length;
+		uint32_t p = addr;
+		while (i-- > 0)
+			z80_write(p++, data);
+	}
+}
+#endif /* CONFIG_LOOPW */
+
+#ifdef CONFIG_CMD_MEMTEST
+static uint32_t mem_test_alt(vu_long *buf, uint32_t start_addr, uint32_t end_addr,
+			  vu_long *dummy)
+{
+	vu_long *addr;
+	uint32_t errs = 0;
+	uint32_t val, readback;
+	int j;
+	vu_long offset;
+	vu_long test_offset;
+	vu_long pattern;
+	vu_long temp;
+	vu_long anti_pattern;
+	vu_long num_words;
+	static const FLASH uint32_t bitpattern[] = {
+		0x00000001,	/* single bit */
+		0x00000003,	/* two adjacent bits */
+		0x00000007,	/* three adjacent bits */
+		0x0000000F,	/* four adjacent bits */
+		0x00000005,	/* two non-adjacent bits */
+		0x00000015,	/* three non-adjacent bits */
+		0x00000055,	/* four non-adjacent bits */
+		0xaaaaaaaa,	/* alternating 1/0 */
+	};
+
+	num_words = (end_addr - start_addr) / sizeof(vu_long);
+
+	/*
+	 * Data line test: write a pattern to the first
+	 * location, write the 1's complement to a 'parking'
+	 * address (changes the state of the data bus so a
+	 * floating bus doesn't give a false OK), and then
+	 * read the value back. Note that we read it back
+	 * into a variable because the next time we read it,
+	 * it might be right (been there, tough to explain to
+	 * the quality guys why it prints a failure when the
+	 * "is" and "should be" are obviously the same in the
+	 * error message).
+	 *
+	 * Rather than exhaustively testing, we test some
+	 * patterns by shifting '1' bits through a field of
+	 * '0's and '0' bits through a field of '1's (i.e.
+	 * pattern and ~pattern).
+	 */
+	addr = buf;
+	for (j = 0; j < sizeof(bitpattern) / sizeof(bitpattern[0]); j++) {
+		val = bitpattern[j];
+		for (; val != 0; val <<= 1) {
+			*addr = val;
+			*dummy  = ~val; /* clear the test data off the bus */
+			readback = *addr;
+			if (readback != val) {
+				printf("FAILURE (data line): "
+					"expected %05lx, actual %05lx\n",
+						val, readback);
+				errs++;
+				if (ctrlc())
+					return -1;
+			}
+			*addr  = ~val;
+			*dummy  = val;
+			readback = *addr;
+			if (readback != ~val) {
+				printf("FAILURE (data line): "
+					"Is %05lx, should be %05lx\n",
+						readback, ~val);
+				errs++;
+				if (ctrlc())
+					return -1;
+			}
+		}
+	}
+
+	/*
+	 * Based on code whose Original Author and Copyright
+	 * information follows: Copyright (c) 1998 by Michael
+	 * Barr. This software is placed into the public
+	 * domain and may be used for any purpose. However,
+	 * this notice must not be changed or removed and no
+	 * warranty is either expressed or implied by its
+	 * publication or distribution.
+	 */
+
+	/*
+	* Address line test
+
+	 * Description: Test the address bus wiring in a
+	 *              memory region by performing a walking
+	 *              1's test on the relevant bits of the
+	 *              address and checking for aliasing.
+	 *              This test will find single-bit
+	 *              address failures such as stuck-high,
+	 *              stuck-low, and shorted pins. The base
+	 *              address and size of the region are
+	 *              selected by the caller.
+
+	 * Notes:	For best results, the selected base
+	 *              address should have enough LSB 0's to
+	 *              guarantee single address bit changes.
+	 *              For example, to test a 64-Kbyte
+	 *              region, select a base address on a
+	 *              64-Kbyte boundary. Also, select the
+	 *              region size as a power-of-two if at
+	 *              all possible.
+	 *
+	 * Returns:     0 if the test succeeds, 1 if the test fails.
+	 */
+	pattern = (vu_long) 0xaaaaaaaa;
+	anti_pattern = (vu_long) 0x55555555;
+
+	debug("%s:%d: length = 0x%.5lx\n", __func__, __LINE__, num_words);
+	/*
+	 * Write the default pattern at each of the
+	 * power-of-two offsets.
+	 */
+	for (offset = 1; offset < num_words; offset <<= 1)
+		addr[offset] = pattern;
+
+	/*
+	 * Check for address bits stuck high.
+	 */
+	test_offset = 0;
+	addr[test_offset] = anti_pattern;
+
+	for (offset = 1; offset < num_words; offset <<= 1) {
+		temp = addr[offset];
+		if (temp != pattern) {
+			printf("\nFAILURE: Address bit stuck high @ 0x%.5lx:"
+				" expected 0x%.5lx, actual 0x%.5lx\n",
+				start_addr + offset*sizeof(vu_long),
+				pattern, temp);
+			errs++;
+			if (ctrlc())
+				return -1;
+		}
+	}
+	addr[test_offset] = pattern;
+
+	/*
+	 * Check for addr bits stuck low or shorted.
+	 */
+	for (test_offset = 1; test_offset < num_words; test_offset <<= 1) {
+		addr[test_offset] = anti_pattern;
+
+		for (offset = 1; offset < num_words; offset <<= 1) {
+			temp = addr[offset];
+			if ((temp != pattern) && (offset != test_offset)) {
+				printf("\nFAILURE: Address bit stuck low or"
+					" shorted @ 0x%.5lx: expected 0x%.5lx,"
+					" actual 0x%.5lx\n",
+					start_addr + offset*sizeof(vu_long),
+					pattern, temp);
+				errs++;
+				if (ctrlc())
+					return -1;
+			}
+		}
+		addr[test_offset] = pattern;
+	}
+
+	/*
+	 * Description: Test the integrity of a physical
+	 *		memory device by performing an
+	 *		increment/decrement test over the
+	 *		entire region. In the process every
+	 *		storage bit in the device is tested
+	 *		as a zero and a one. The base address
+	 *		and the size of the region are
+	 *		selected by the caller.
+	 *
+	 * Returns:     0 if the test succeeds, 1 if the test fails.
+	 */
+	num_words++;
+
+	/*
+	 * Fill memory with a known pattern.
+	 */
+	for (pattern = 1, offset = 0; offset < num_words; pattern++, offset++) {
+		addr[offset] = pattern;
+	}
+
+	/*
+	 * Check each location and invert it for the second pass.
+	 */
+	for (pattern = 1, offset = 0; offset < num_words; pattern++, offset++) {
+		temp = addr[offset];
+		if (temp != pattern) {
+			printf("\nFAILURE (read/write) @ 0x%.5lx:"
+				" expected 0x%.5lx, actual 0x%.5lx)\n",
+				start_addr + offset*sizeof(vu_long),
+				pattern, temp);
+			errs++;
+			if (ctrlc())
+				return -1;
+		}
+
+		anti_pattern = ~pattern;
+		addr[offset] = anti_pattern;
+	}
+
+	/*
+	 * Check each location for the inverted pattern and zero it.
+	 */
+	for (pattern = 1, offset = 0; offset < num_words; pattern++, offset++) {
+		WATCHDOG_RESET();
+		anti_pattern = ~pattern;
+		temp = addr[offset];
+		if (temp != anti_pattern) {
+			printf("\nFAILURE (read/write): @ 0x%.5lx:"
+				" expected 0x%.5lx, actual 0x%.5lx)\n",
+				start_addr + offset*sizeof(vu_long),
+				anti_pattern, temp);
+			errs++;
+			if (ctrlc())
+				return -1;
+		}
+		addr[offset] = 0;
+	}
+
+	return 0;
+}
+
+static uint32_t mem_test_quick(vu_long *buf, uint32_t start_addr, uint32_t end_addr,
+			    vu_long pattern, int iteration)
+{
+	vu_long *end;
+	vu_long *addr;
+	uint32_t errs = 0;
+	uint32_t incr, length;
+	uint32_t val, readback;
+
+	/* Alternate the pattern */
+	incr = 1;
+	if (iteration & 1) {
+		incr = -incr;
+		/*
+		 * Flip the pattern each time to make lots of zeros and
+		 * then, the next time, lots of ones.  We decrement
+		 * the "negative" patterns and increment the "positive"
+		 * patterns to preserve this feature.
+		 */
+		if (pattern & 0x80000000)
+			pattern = -pattern;	/* complement & increment */
+		else
+			pattern = ~pattern;
+	}
+	length = (end_addr - start_addr) / sizeof(uint32_t);
+	end = buf + length;
+	printf("\rPattern %08lX  Writing..."
+		"%12s"
+		"\b\b\b\b\b\b\b\b\b\b",
+		pattern, "");
+
+	for (addr = buf, val = pattern; addr < end; addr++) {
+		*addr = val;
+		val += incr;
+	}
+
+	my_puts("Reading...");
+
+	for (addr = buf, val = pattern; addr < end; addr++) {
+		readback = *addr;
+		if (readback != val) {
+			uint32_t offset = addr - buf;
+
+			printf("\nMem error @ 0x%08X: "
+				"found %08lX, expected %08lX\n",
+				(unsigned int)(uintptr_t)(start_addr + offset*sizeof(vu_long)),
+				readback, val);
+			errs++;
+			if (ctrlc())
+				return -1;
+		}
+		val += incr;
+	}
+
+	return 0;
+}
+
+/*
+ * Perform a memory test. A more complete alternative test can be
+ * configured using CONFIG_SYS_ALT_MEMTEST. The complete test loops until
+ * interrupted by ctrl-c or by a failure of one of the sub-tests.
+ */
+int do_mem_mtest(cmd_tbl_t *cmdtp, int flag, int argc,
+			char * const argv[])
+{
+	uint32_t start, end;
+	vu_long *buf, *dummy;
+	int iteration_limit;
+	int ret;
+	uint32_t errs = 0;	/* number of errors, or -1 if interrupted */
+	uint32_t pattern;
+	int iteration;
+#if defined(CONFIG_SYS_ALT_MEMTEST)
+	const int alt_test = 1;
+#else
+	const int alt_test = 0;
+#endif
+
+	if (argc > 1)
+		start = strtoul(argv[1], NULL, 16);
+	else
+		start = CONFIG_SYS_MEMTEST_START;
+
+	if (argc > 2)
+		end = strtoul(argv[2], NULL, 16);
+	else
+		end = CONFIG_SYS_MEMTEST_END;
+
+	if (argc > 3)
+		pattern = (uint32_t)strtoul(argv[3], NULL, 16);
+	else
+		pattern = 0;
+
+	if (argc > 4)
+		iteration_limit = (uint32_t)strtoul(argv[4], NULL, 16);
+	else
+		iteration_limit = 0;
+
+	printf("Testing %08x ... %08x:\n", (unsigned int)start, (unsigned int)end);
+	debug("%s:%d: start %#05lx end %#05lx\n", __func__, __LINE__,
+	      start, end);
+
+/* TODO: */
+//	buf = map_sysmem(start, end - start);
+//	dummy = map_sysmem(CONFIG_SYS_MEMTEST_SCRATCH, sizeof(vu_long));
+	for (iteration = 0;
+			!iteration_limit || iteration < iteration_limit;
+			iteration++) {
+		if (ctrlc()) {
+			errs = -1UL;
+			break;
+		}
+
+		printf("Iteration: %6d\r", iteration + 1);
+		debug("\n");
+		if (alt_test) {
+			errs = mem_test_alt(buf, start, end, dummy);
+		} else {
+			errs = mem_test_quick(buf, start, end, pattern,
+					      iteration);
+		}
+		if (errs == -1UL)
+			break;
+	}
+
+	if (errs == -1UL) {
+		/* Memory test was aborted - write a newline to finish off */
+		putc('\n');
+		ret = 1;
+	} else {
+		printf("Tested %d iteration(s) with %lu errors.\n",
+			iteration, errs);
+		ret = errs != 0;
+	}
+
+	return ret;	/* not reached */
+}
+#endif	/* CONFIG_CMD_MEMTEST */
+
+/* Modify memory.
+ *
+ * Syntax:
+ *	mm {addr}
+ *	nm {addr}
+ */
+static int
+mod_mem(cmd_tbl_t *cmdtp, int incrflag, int flag, int argc, char * const argv[])
+{
+	uint32_t addr;
+	uint8_t data;
+	int nbytes;
+
+	(void) cmdtp;
+
+	if (argc != 2)
+		return CMD_RET_USAGE;
+
+	/* We use the last specified parameters, unless new ones are
+	 * entered.
+	 */
+	addr = mm_last_addr;
+
+	if ((flag & CMD_FLAG_REPEAT) == 0) {
+		/* New command specified.  
+		 */
+
+		/* Address is specified since argc > 1
+		*/
+		addr = strtoul(argv[1], NULL, 16);
+		addr += base_address;
+	}
+
+	/* Print the address, followed by value.  Then accept input for
+	 * the next value.  A non-converted value exits.
+	 */
+	do {
+		z80_request_bus();
+		data = z80_read(addr);
+		printf("%05lx: %02x", addr, data);
+		z80_release_bus();
+
+		nbytes = cli_readline(PSTR(" ? "));
+		if (nbytes == 0 || (nbytes == 1 && console_buffer[0] == '-')) {
+			/* <CR> pressed as only input, don't modify current
+			 * location and move to next. "-" pressed will go back.
+			 */
+			if (incrflag)
+				addr += nbytes ? -1 : 1;
+			nbytes = 1;
+		}
+		else {
+			char *endp;
+			data = strtoul(console_buffer, &endp, 16);
+			nbytes = endp - console_buffer;
+			if (nbytes) {
+				z80_request_bus();
+				z80_write(addr, data);
+				z80_release_bus();
+				if (incrflag)
+					addr++;
+			}
+		}
+	} while (nbytes);
+
+	mm_last_addr = addr;
+	return 0;
+}
+
diff --git a/avr/cmd_mem.h b/avr/cmd_mem.h
new file mode 100644
index 0000000..9803fd2
--- /dev/null
+++ b/avr/cmd_mem.h
@@ -0,0 +1,30 @@
+#ifndef CMD_MEM_H
+#define CMD_MEM_H
+
+//#include "common.h"
+
+#include "command.h"
+#include "cmd_mem.h"
+
+
+extern int do_mem_md(cmd_tbl_t *, int, int, char * const []);
+extern int do_mem_mm(cmd_tbl_t *, int, int, char * const []);
+extern int do_mem_nm(cmd_tbl_t *, int, int, char * const []);
+extern int do_mem_mw(cmd_tbl_t *, int, int, char * const []);
+extern int do_mem_cp(cmd_tbl_t *, int, int, char * const []);
+extern int do_mem_cmp(cmd_tbl_t *, int, int, char * const []);
+extern int do_mem_base(cmd_tbl_t *, int, int, char * const []);
+extern int do_mem_loop(cmd_tbl_t *, int, int, char * const []);
+#ifdef CONFIG_LOOPW
+extern int do_mem_loopw(cmd_tbl_t *, int, int, char * const []);
+#endif
+#ifdef CONFIG_CMD_MEMTEST
+extern int do_mem_mtest(cmd_tbl_t *, int, int, char * const []);
+#endif
+#ifdef CONFIG_MX_CYCLIC
+extern int do_mem_mdc(cmd_tbl_t *, int, int, char * const []);
+extern int do_mem_mwc(cmd_tbl_t *, int, int, char * const []);
+#endif /* CONFIG_MX_CYCLIC */
+
+#endif /* CMD_MEM_H */
+
diff --git a/avr/command.c b/avr/command.c
index ad39006..9af1c32 100644
--- a/avr/command.c
+++ b/avr/command.c
@@ -67,7 +67,7 @@ int cmd_tbl_item_count(void)
  * for long help messages
  */
 
-int _do_help (cmd_tbl_t *cmd_start, int cmd_items, cmd_tbl_t * cmdtp, 
+int _do_help(cmd_tbl_t *cmd_start, int cmd_items, cmd_tbl_t * cmdtp,
 		int flag, int argc, char * const argv[])
 {
 	uint_fast8_t i;
@@ -177,6 +177,7 @@ int cmd_usage(const FLASH cmd_tbl_t *cmdtp)
 //	printf("Usage:\n%s ", cmdtp->name);
 	my_puts_P(PSTR("Usage:\n"));
 	my_puts_P(cmdtp->name);
+	putchar(' ');
 
 	if (!cmdtp->help) {
 		my_puts_P(PSTR(" - No additional help available.\n"));
diff --git a/avr/command_tbl.c b/avr/command_tbl.c
index c0579bc..78f7a24 100644
--- a/avr/command_tbl.c
+++ b/avr/command_tbl.c
@@ -2,7 +2,7 @@
 #include "common.h"
 
 #include "command.h"
-
+#include "cmd_mem.h"
 
 
 extern int do_help(cmd_tbl_t *, int, int, char * const []);
@@ -29,7 +29,8 @@ CMD_TBL_ITEM_COMPLETE(
 CMD_TBL_ITEM_COMPLETE(
 	printenv, CONFIG_SYS_MAXARGS,	1,	do_env_print,
 	"print environment variables",
-	"\n    - print [all] values of all environment variables\n"
+	"\n"
+	"    - print [all] values of all environment variables\n"
 	"printenv name ...\n"
 	"    - print value of environment variable 'name'",
 	var_complete
@@ -37,12 +38,86 @@ CMD_TBL_ITEM_COMPLETE(
 CMD_TBL_ITEM_COMPLETE(
 	setenv, CONFIG_SYS_MAXARGS,	0,	do_env_set,
 	"set environment variables",
-	"[-f] name value ...\n"
-	"    - [forcibly] set environment variable 'name' to 'value ...'\n"
-	"setenv [-f] name\n"
-	"    - [forcibly] delete environment variable 'name'",
+	"name value ...\n"
+	"    - set environment variable 'name' to 'value ...'\n"
+	"setenv name\n"
+	"    - delete environment variable 'name'",
 	var_complete
 ),
+
+CMD_TBL_ITEM(
+	md,	3,	1,	do_mem_md,
+	"memory display",
+	"address [# of objects]"
+),
+CMD_TBL_ITEM(
+	mm,	2,	1,	do_mem_mm,
+	"memory modify (auto-incrementing address)",
+	"address"
+),
+CMD_TBL_ITEM(
+	nm,	2,	1,	do_mem_nm,
+	"memory modify (constant address)",
+	"address"
+),
+CMD_TBL_ITEM(
+	mw,	4,	1,	do_mem_mw,
+	"memory write (fill)",
+	"address value [count]"
+),
+CMD_TBL_ITEM(
+	cp,	4,	1,	do_mem_cp,
+	"memory copy",
+	"source target count"
+),
+CMD_TBL_ITEM(
+	cmp,	4,	1,	do_mem_cmp,
+	"memory compare",
+	"addr1 addr2 count"
+),
+CMD_TBL_ITEM(
+	base,	2,	1,	do_mem_base,
+	"print or set address offset",
+	"\n"
+	"    - print address offset for memory commands\n"
+	"base offset\n"
+	"    - set address offset for memory commands to 'offset'"
+),
+CMD_TBL_ITEM(
+	loop,	3,	1,	do_mem_loop,
+	"infinite loop on address range",
+	"address number_of_bytes"
+),
+#ifdef CONFIG_LOOPW
+CMD_TBL_ITEM(
+	loopw,	4,	1,	do_mem_loopw,
+	"infinite write loop on address range",
+	"address number_of_bytes data_to_write"
+),
+#endif /* CONFIG_LOOPW */
+
+#ifdef CONFIG_CMD_MEMTEST
+CMD_TBL_ITEM(
+	mtest,	5,	1,	do_mem_mtest,
+	"simple RAM read/write test",
+	"[start [end [pattern [iterations]]]]"
+),
+#endif	/* CONFIG_CMD_MEMTEST */
+
+#ifdef CONFIG_MX_CYCLIC
+CMD_TBL_ITEM(
+	mdc,	4,	1,	do_mem_mdc,
+	"memory display cyclic",
+	"address count delay(ms)"
+),
+CMD_TBL_ITEM(
+	mwc,	4,	1,	do_mem_mwc,
+	"memory write cyclic",
+	"address value delay(ms)"
+),
+#endif /* CONFIG_MX_CYCLIC */
+
+
 CMD_TBL_ITEM(
 	help,	CONFIG_SYS_MAXARGS,	1,	do_help,
 	"print command description/usage",
@@ -52,7 +127,7 @@ CMD_TBL_ITEM(
 	"       - print detailed usage of 'command'"
 ),
 
-/* This does not use the U_BOOT_CMD macro as ? can't be used in symbol names */
+/* This does not use the CMD_TBL_ITEM macro as ? can't be used in symbol names */
 	{FSTR("?"),   CONFIG_SYS_MAXARGS, 1, do_help,
 	 FSTR("alias for 'help'"),
 #ifdef  CONFIG_SYS_LONGHELP
@@ -65,12 +140,3 @@ CMD_TBL_ITEM(
 /* Mark end of table */
 { 0 },
 };
-
-
-
-
-
-
-
-
-
diff --git a/avr/common.h b/avr/common.h
index ace0fcb..a92f62c 100644
--- a/avr/common.h
+++ b/avr/common.h
@@ -11,7 +11,7 @@
 
 #else
 // TODO: stm32
-#endif
+#endif /* __AVR__ */
 
 #include <stdio.h>
 
@@ -32,26 +32,30 @@
 #define Stat GPIOR0
 #else
 extern volatile uint_least8_t Stat;
-#endif
+#endif /* __AVR__ */
 
 #define S_10MS_TO	(1<<0)
-
-
-#include <util/delay.h>
+#define S_Z180_RUNNING	(2<<0)
 
 static inline 
 void my_puts(const char *s)
 {
 	fputs(s, stdout);
-//	_delay_ms(10);
 }
 
+#ifdef __AVR__
 static inline 
 void my_puts_P(const char *s)
 {
 	fputs_P(s, stdout);
-//	_delay_ms(10);
 }
 
+#else
+static inline
+void my_puts_P(const char *s)
+{
+	fputs(s, stdout);
+}
+#endif /* __AVR__ */
 #endif /* COMMON_H */
 
diff --git a/avr/con-utils.c b/avr/con-utils.c
index 430ba98..02657b9 100644
--- a/avr/con-utils.c
+++ b/avr/con-utils.c
@@ -1,13 +1,14 @@
 
 #include <string.h>
-#include <stdio.h>
+#include "common.h"
 
 #include "serial.h"
+#include "background.h"
 #include "con-utils.h"
 
-
 uint_fast8_t tstc(void)
 {
+	bg_shed();
 	return serial_tstc();
 }
 
@@ -15,16 +16,19 @@ int my_getchar(void)
 {
 	int c;
 	
-	while((c = serial_getc()) < 0)
-		;
+	do {
+		bg_shed();
+		c = serial_getc();
+	} while (c < 0);
+
 	return c;
 }
 
 
 /* test if ctrl-c was pressed */
 
-static uint_fast8_t ctrlc_disabled = 0;	/* see disable_ctrl() */
-static uint_fast8_t ctrlc_was_pressed = 0;
+static uint_fast8_t ctrlc_disabled;	/* see disable_ctrl() */
+static uint_fast8_t ctrlc_was_pressed;
 
 uint_fast8_t ctrlc(void)
 {
diff --git a/avr/config.h b/avr/config.h
index 9e6d4d7..2c6210b 100644
--- a/avr/config.h
+++ b/avr/config.h
@@ -8,6 +8,10 @@
 #define CONFIG_BOOTDELAY	4
 //#define CONFIG_ZERO_BOOTDELAY_CHECK	1
 
+//#define CONFIG_LOOPW
+//#define CONFIG_CMD_MEMTEST
+//#define CONFIG_MX_CYCLIC
+
 #define CONFIG_SYS_CBSIZE	250
 #define CONFIG_SYS_ENV_NAMELEN	16
 #define CONFIG_SYS_MAXARGS	8
diff --git a/avr/env.c b/avr/env.c
index 97bd2e3..3488afc 100644
--- a/avr/env.c
+++ b/avr/env.c
@@ -114,9 +114,9 @@ typedef struct environment_s {
 
 
 typedef struct env_item_s {
-#define EF_N_EEP		(1<<7)
-#define EF_V_EEP		(1<<6)
-#define EF_DIRTY		(1<<0)
+#define EF_N_EEP	(1<<7)		/* Variable name is in EEPROM */
+#define EF_V_EEP	(1<<6)		/* Variable value is in EEPROM */
+#define EF_DIRTY	(1<<0)		/* Variable is new or value changed */
 	uint8_t flags;
 	union {
 		uint16_t eep;
@@ -307,7 +307,7 @@ static env_item_t *envlist_search(const char *name)
 				sizeof(env_item_t), comp_env_key_item);
 }
 			
-
+#if 0
 env_item_t *envlist_insert(const char *key, env_item_t *e)
 {
 	const size_t size = sizeof(env_item_t);
@@ -322,7 +322,7 @@ env_item_t *envlist_insert(const char *key, env_item_t *e)
 	} else
 		return NULL;
 }	
-
+#endif
 
 env_item_t *envlist_enter(env_item_t *e)
 {
@@ -406,14 +406,11 @@ static int envlist_delete(const char *name)
 			free(ep->name.ram); 
 
 		entrycount--;
-printf_P(PSTR("*** envlist_delete memmove: 0x%.4x, 0x%.4x, %d\n"), 
-			(unsigned) ep, (unsigned) ep + size, 
-			(env_list - ep)*size + entrycount*size);
 		memmove(ep, ep + 1, (env_list - ep)*size + entrycount*size);
 		
 		rc = 1;
 	}
-#if 1
+#if 0
 	dump_ram((uint8_t *) &env_list[0], entrycount * sizeof(env_item_t),
 			"=== env_list:");
 	dump_heap();
@@ -427,7 +424,6 @@ printf_P(PSTR("*** envlist_delete memmove: 0x%.4x, 0x%.4x, %d\n"),
 	return rc;
 }
 			
-
 char *getenv(const char *name)
 {
 	env_item_t *ep;
@@ -438,8 +434,7 @@ char *getenv(const char *name)
 	ep = envlist_get(name, ENV_GET_VAL);
 	if (ep != NULL)
 		ret = ep->val.ram;
-		
-#if 1
+#if 0
 	dump_ram((uint8_t *) &env_list[0], entrycount * sizeof(env_item_t),
 			"=== env_list:");
 	dump_heap();
@@ -449,23 +444,20 @@ char *getenv(const char *name)
 		env_item_print(&env_list[i]);
 	}
 #endif
-
 	return ret;
 }
 
+/* TODO: implement saveenv() */
 int saveenv(void)
 {
 	int	rc = 0;
 
-
 //	rc = env_export(&env_new);
 	if (rc)
 		return rc;
 
-
 //	rc = eeprom_bus_write(CONFIG_SYS_DEF_EEPROM_ADDR,
 //			      off, (uchar *)&env_new, CONFIG_ENV_SIZE);
-
 	return rc;
 }
 
@@ -481,9 +473,9 @@ int set_default_env(void)
 	unsigned int len = CONFIG_ENV_SIZE - offsetof(env_t, data);
 	unsigned int i, src = 0;
 	char c = 0xff, c0 = c;
-	
+#if 0
 printf_P(PSTR("\n\n** set_default_env()\n"));
-	
+#endif
 	while (len)  {
 	
 		memcpy_P(buf, default_env+src, sizeof(buf));
@@ -497,11 +489,12 @@ printf_P(PSTR("\n\n** set_default_env()\n"));
 		}
 		
 		eeprom_update_block(buf, (char *) eep, i);
-/**/		printf_P(PSTR("eeprom_update_block: eep: 0x%.4x, i:%d\n"), 
+#if 0
+		printf_P(PSTR("eeprom_update_block: eep: 0x%.4x, i:%d\n"), 
 			(unsigned int) eep, i);
-/**/		dump_ram((uint8_t *) buf, i, "=== buf:");
-/**/		dump_eep((const uint8_t *) eep, i);
-
+		dump_ram((uint8_t *) buf, i, "=== buf:");
+		dump_eep((const uint8_t *) eep, i);
+#endif
 		if (c == 0 && c0 == 0)
 			len = 0;
 		if (len > sizeof(buf))
@@ -509,13 +502,9 @@ printf_P(PSTR("\n\n** set_default_env()\n"));
 		src += sizeof(buf);
 		eep += sizeof(buf);
 	}
-	
-printf_P(PSTR("** crc adr: 0x%.4x, crc: 0x%.4x\n"),
-	(unsigned int) (uint16_t *) CONFIG_ENV_OFFSET + offsetof(env_t,crc), crc);
-	
-	eeprom_update_word((uint16_t *) CONFIG_ENV_OFFSET + offsetof(env_t,crc), crc);
-	
-/**/	dump_eep(0, 128);
+#if 0
+	dump_eep(0, 128);
+#endif
 	return 0;
 }
 
@@ -635,14 +624,24 @@ int do_env_print(cmd_tbl_t *cmdtp, int flag, int argc,
 }
 
 
-/*
+/**
+ * Set or delete environment variable
+ *
  * Set a new environment variable,
  * or replace or delete an existing one.
+ *
+ * @param flag		(not used)
+ * @param argc
+ * @param argv[1]	Environment variable to set or delete
+ *			if no more args
+ * @param argv[2] ...	Value to set it to
+ *
+ * @return 0 if ok, 1 on error
  */
 static int _do_env_set(int flag, int argc, char * const argv[])
 {
-	int   i, len;
-	char  *name, *value, *s;
+	int i, len;
+	char *name, *value, *s;
 	env_item_t e, *ep;
 
 	(void) flag;
@@ -704,6 +703,13 @@ static int _do_env_set(int flag, int argc, char * const argv[])
 	return 0;
 }
 
+/**
+ * Set an environment variable
+ *
+ * @param varname	Environment variable to set
+ * @param varvalue	Value to set it to
+ * @return 0 if ok, 1 on error
+ */
 int setenv(const char *varname, const char *varvalue)
 {
 	const char * const argv[3] = { NULL, varname, varvalue };
@@ -748,6 +754,13 @@ int setenv_hex(const char *varname, unsigned long value)
 	return setenv(varname, str);
 }
 
+/**
+ * Get an environment variable as a hex value
+ *
+ * @param varname	Environment variable to get
+ * @param default_val	Return this, if variable does not exist
+ * @return hex value of variable or default_val
+ */
 unsigned long getenv_hex(const char *varname, unsigned long default_val)
 {
 	const char *s;
diff --git a/avr/z180-serv.c b/avr/z180-serv.c
new file mode 100644
index 0000000..d28cb98
--- /dev/null
+++ b/avr/z180-serv.c
@@ -0,0 +1,354 @@
+/*
+ */
+
+#include "common.h"
+//#include <avr/power.h>
+//#include <avr/pgmspace.h>
+//#include <util/atomic.h>
+//#include <avr/sleep.h>
+//#include <string.h>
+
+
+#include "debug.h"
+#include "serial.h"
+#include "z80-if.h"
+
+
+/* ugly hack to get Z180 loadfile into flash memory */
+#define const const FLASH
+#include "../z180/hdrom.h"
+#undef const
+
+
+/*--------------------------------------------------------------------------*/
+
+uint32_t z80_sram_cmp(uint32_t addr, uint32_t length, uint8_t wval, int inc)
+{
+	uint8_t rval;
+	int_fast8_t errors = 0;
+	
+	DBG_P(1, "SRAM: Check 0x%.5lx byte... ", length);
+	while (length--) {
+		if ((rval = z80_read(addr)) != wval) {
+			if (errors == 0) { 
+				DBG_P(1, "\nSRAM: Address   W    R\n" \
+				         "      ------------------\n");
+			}
+			errors++;
+			if (errors > 20) {
+				DBG_P(1, "      ...\n");
+				break;
+			}
+			DBG_P(1, "      0x%.5lx  0x%.2x 0x%.2x\n", addr, wval, rval);
+		}
+		addr++;
+		wval += inc;
+	}
+	DBG_P(1, "Done.\n");
+
+	return addr;
+}
+
+void z80_sram_fill(uint32_t addr, uint32_t length, uint8_t startval, int inc)
+{
+	printf("SRAM: Write 0x%.5lx byte... ", length);
+	while (length--) {
+		z80_write(addr, startval);
+		++addr; 
+		startval += inc;
+	}
+	printf("Done.\n");
+}
+
+
+#if 0
+void z80_sram_fill_string(uint32_t addr, int length, const char *text)
+{
+	char c;
+	const char *p = text;
+
+	while (length--) {
+		z80_write(addr++, c = *p++);
+		if (c == 0)
+			p = text;
+	}
+}
+
+
+uint32_t z80_sram_cmp_string(uint32_t addr, int length, const char *text)
+{
+	char c;
+	const char *p = text;
+
+	while (length--) {
+		c = *p++;
+		if (z80_read(addr) != c)
+			break;
+		++addr;
+		if (c == 0)
+			p = text;
+	}
+	return addr;
+}
+
+const char * const qbfox = "Zhe quick brown fox jumps over the lazy dog!";
+const char * const qbcat = "Zhe quick brown fox jumps over the lazy cat!";
+
+#endif
+
+uint8_t z80_get_byte(uint32_t adr)
+{
+	uint8_t data;
+	
+	z80_request_bus();
+	data = z80_read(adr),
+	z80_release_bus();
+	
+	return data;
+}
+
+
+/*--------------------------------------------------------------------------*/
+
+struct msg_item {
+	uint8_t fct;
+	uint8_t sub_min, sub_max;
+	void (*func)(uint8_t, int, uint8_t *);
+};
+
+uint32_t msg_to_addr(uint8_t *msg)
+{
+	union {
+		uint32_t as32;
+		uint8_t as8[4];
+	} addr;
+
+	addr.as8[0] = msg[0];
+	addr.as8[1] = msg[1];
+	addr.as8[2] = msg[2];
+	addr.as8[3] = 0;
+
+	return addr.as32;
+}
+
+void do_msg_ini_msgfifo(uint8_t subf, int len, uint8_t * msg)
+{
+	(void)subf; (void)len;
+
+	z80_init_msg_fifo(msg_to_addr(msg));
+}
+
+
+void do_msg_ini_memfifo(uint8_t subf, int len, uint8_t * msg)
+{
+	(void)len;
+
+	z80_memfifo_init(subf - 1, msg_to_addr(msg));
+}
+
+
+void do_msg_char_out(uint8_t subf, int len, uint8_t * msg)
+{
+	(void)subf;
+
+	while (len--)
+		putchar(*msg++);
+}
+
+
+const FLASH struct msg_item z80_messages[] =
+{
+	{ 0,			/* fct nr. */
+	  0, 0,			/* sub fct nr. from, to */
+	  do_msg_ini_msgfifo},
+	{ 0,
+	  1, 2,
+	  do_msg_ini_memfifo},
+	{ 1,
+	  1, 1,
+	  do_msg_char_out},
+	{ 0xff,				/* end mark */
+	  0, 0,
+	  0},
+
+};
+
+
+
+
+void do_message(int len, uint8_t *msg)
+{
+	uint8_t fct, sub_fct;
+	int_fast8_t i = 0;
+
+	if (len >= 2) {
+		fct = *msg++;
+		sub_fct = *msg++;
+		len -= 2;
+
+		while (fct != z80_messages[i].fct)
+			++i;
+
+		if (z80_messages[i].fct == 0xff) {
+			DBG_P(1, "do_message: Unknown function: %i, %i\n",
+					fct, sub_fct);
+			return; /* TODO: unknown message # */
+		}
+
+		while (fct == z80_messages[i].fct) {
+			if (sub_fct >= z80_messages[i].sub_min && sub_fct <= z80_messages[i].sub_max )
+				break;
+			++i;
+		}
+
+		if (z80_messages[i].fct != fct) {
+			DBG_P(1, "do_message: Unknown sub function: %i, %i\n",
+					fct, sub_fct);
+			return; /* TODO: unknown message sub# */
+		}
+
+		(z80_messages[i].func)(sub_fct, len, msg);
+
+
+	} else {
+		/* TODO: error */
+		DBG_P(1, "do_message: to few arguments (%i); this shouldn't happen!\n", len);
+	}
+}
+
+
+
+#define CTRBUF_LEN 256
+
+void check_msg_fifo(void)
+{
+	int ch;
+	static int_fast8_t state;
+	static int msglen,idx;
+	static uint8_t buffer[CTRBUF_LEN];
+
+	while (state != 3 && (ch = z80_msg_fifo_getc()) >= 0) {
+		switch (state) {
+		case 0:		/* wait for start of message */
+			if (ch == 0x81)	{
+				msglen = 0;
+				idx = 0;
+				state = 1;
+			}
+			break;
+		case 1:		/* get msg len */
+			if (ch > 0 && ch <= CTRBUF_LEN) {
+				msglen = ch;
+				state = 2;
+			} else
+				state = 0;
+			break;
+		case 2: 	/* get message */
+			buffer[idx++] = ch;
+			if (idx == msglen)
+				state = 3;
+			break;
+		}
+	}
+
+	if (state == 3) {
+		do_message(msglen, buffer);
+		state = 0;
+	}
+}
+
+
+/*--------------------------------------------------------------------------*/
+
+void dump_mem(const FLASH uint8_t *addr, uint32_t len)
+{
+	DBG_P(1, "hdrom dump:");
+	while (len) {
+		DBG_P(1, "\n    %.5x:", addr);
+		for (unsigned i = 0; i<16; i++)
+			DBG_P(1, " %.2x", *addr++);
+		len -= len > 16 ? 16 : len;
+	}
+	DBG_P(1, "\n");
+}
+
+/*--------------------------------------------------------------------------*/
+
+void z80_load_mem(void)
+{
+	unsigned sec = 0;
+	uint32_t sec_base = hdrom_start;
+
+	DBG_P(1, "Loading z80 memory... \n");
+
+	while (sec < hdrom_sections) {
+		DBG_P(2, "  From: 0x%.5lX to: 0x%.5lX    (%5li bytes)\n",
+				hdrom_address[sec],
+				hdrom_address[sec]+hdrom_length_of_sections[sec] - 1,
+				hdrom_length_of_sections[sec]);
+
+		z80_write_block((const FLASH unsigned char *) &hdrom[sec_base],  /* src */
+		                hdrom_address[sec],                  /* dest */
+				hdrom_length_of_sections[sec]);      /* len */
+		sec_base+=hdrom_length_of_sections[sec];
+		sec++;
+	}
+}
+
+/*--------------------------------------------------------------------------*/
+
+
+const FLASH uint8_t iniprog[] = {
+	0xAF,			// xor     a
+	0xED, 0x39, 0x36,	// out0    (rcr),a         ;disable  DRAM refresh
+	0x3E, 0x30,		// ld      a,030h
+	0xED, 0x39, 0x32	//out0    (dcntl),a       ;0 mem, max i/0 wait states
+};
+
+const FLASH uint8_t sertest[] = {
+	0xAF,              //  	xor	a
+	0xED, 0x39, 0x36,  //  	out0	(rcr),a		;disable  DRAM refresh
+	0x3E, 0x30,        //  	ld	a,030h
+	0xED, 0x39, 0x32,  //  	out0	(dcntl),a	;0 mem, max i/0 wait states
+	0x3E, 0x80,        //  	ld	a,M_MPBT		;no MP, PS=10, DR=16, SS=0
+	0xED, 0x39, 0x03,  //  	out0	(cntlb1),a
+	0x3E, 0x64,        //  	ld	a,M_RE + M_TE + M_MOD2	;
+	0xED, 0x39, 0x01,  //  	out0	(cntla1),a
+	0x3E, 0x00,        //  	ld	a,0
+	0xED, 0x39, 0x05,  //  	out0	(stat1),a	;Enable rx interrupts
+	0xED, 0x38, 0x05,  //l0:in0	a,(stat1)
+	0xE6, 0x80,        //  	and	80h
+	0x28, 0xF9,        //  	jr	z,l0
+	0xED, 0x00, 0x09,  //  	in0	b,(rdr1)
+	0xED, 0x38, 0x05,  //l1:in0	a,(stat1)
+	0xE6, 0x02,        //  	and	02h
+	0x28, 0xF9,        //  	jr	z,l1
+	0xED, 0x01, 0x07,  //  	out0	(tdr1),b
+	0x18, 0xEA,        //  	jr	l0
+};
+
+const FLASH uint8_t test1[] = {
+	0xAF,              //	xor	a
+	0xED, 0x39, 0x36,  //	out0	(rcr),a		;disable  DRAM refresh
+	0x3E, 0x30,        //	ld	a,030h
+	0xED, 0x39, 0x32,  //	out0	(dcntl),a	;0 mem, max i/0 wait states
+	0x21, 0x1E, 0x00,  // 	ld	hl,dmclrt	;load DMA registers
+	0x06, 0x08,        //	ld	b,dmct_e-dmclrt
+	0x0E, 0x20,        //	ld	c,sar0l
+	0xED, 0x93,        //	otimr	
+	0x3E, 0xC3,        //	ld	a,0c3h		;dst +1, src +1, burst
+	0xED, 0x39, 0x31,  //	out0	(dmode),a	;
+	0x3E, 0x62,        //	ld	a,062h		;enable dma0, 
+	0xED, 0x39, 0x30,  //cl_1:	out0	(dstat),a	;copy 64k
+	0x18, 0xFB,        //	jr	cl_1		;
+	0x00, 0x00,        //dmclrt:	dw	0		;src (inc) 
+	0x00,              //	db 	0		;src
+	0x00, 0x00,        //	dw	0		;dst (inc),
+	0x00,              //	db	0		;dst
+	0x00, 0x00,        //	dw 	0		;count (64k)
+};
+
+
+
+//		check_msg_fifo();
+
diff --git a/avr/z180-stamp-avr.c b/avr/z180-stamp-avr.c
index b5a3fb8..e6edb33 100644
--- a/avr/z180-stamp-avr.c
+++ b/avr/z180-stamp-avr.c
@@ -497,8 +497,10 @@ int main(void)
 	DBG_P(1, "done.\n");
 
 	DBG_P(1, "Get bus... ");
+/* Now done via S_Z180_RUNNING
 	z80_busreq(LOW);
 	z80_reset(HIGH);
+*/
 	z80_request_bus();
 	DBG_P(1, "got it!\n");
 	
@@ -519,9 +521,11 @@ int main(void)
 //	z80_dump_mem(0, 0x200);
 
 
+/* Now done via S_Z180_RUNNING
 	z80_reset(LOW);
-	DBG_P(1, "Bus released!\n");
+*/
 	z80_release_bus();
+	DBG_P(1, "Bus released!\n");
 	z80_reset(HIGH);
 	DBG_P(1, "Reset released!\n");
 	
diff --git a/avr/z80-if.c b/avr/z80-if.c
index 207aed1..9ee9cb0 100644
--- a/avr/z80-if.c
+++ b/avr/z80-if.c
@@ -2,7 +2,7 @@
  *
  * Pin assignments
  *
- * | Z180-Sig 	|   AVR-Port	| Dir	|   Special Function	|
+ * | Z180-Sig	|   AVR-Port	| Dir	|   Special Function	|
  * +------------+---------------+-------+-----------------------+
  * |	A0	| PA 	0	|  O	|			|
  * |	A1	| PA 	1	|  O	|			|
@@ -137,14 +137,20 @@ struct bits {
 //#define Z80_I_HALT	SBIT(P_HALT, )
 
 
+#if 0
 void z80_busreq(level_t level)
 {
 	Z80_O_BUSREQ = level;
 }
+#endif
 
 void z80_reset(level_t level)
 {
 	Z80_O_RST = level;
+	if (level)
+		Stat |= S_Z180_RUNNING;
+	else
+		Stat &= ~S_Z180_RUNNING;
 }
 
 
@@ -153,6 +159,7 @@ void z80_reset_pulse(void)
 	Z80_O_RST = 0;
 	_delay_us(10);
 	Z80_O_RST = 1;
+	Stat |= S_Z180_RUNNING;
 }
 
 #if 0
@@ -241,6 +248,8 @@ void z80_setup_bus(void)
 
 	z80_setup_addrbus_tristate();
 	z80_setup_dbus_in();
+
+	Stat &= ~S_Z180_RUNNING;
 }
 
 /*--------------------------------------------------------------------------*/
@@ -248,6 +257,10 @@ void z80_setup_bus(void)
 void z80_request_bus(void)
 {
 	Z80_O_BUSREQ = 0;
+
+	if (!(Stat & S_Z180_RUNNING))
+		Z80_O_RST = 1;
+
 	while(Z80_I_BUSACK == 1);
 	z80_setup_addrbus_active();
 }
@@ -256,6 +269,10 @@ void z80_release_bus(void)
 {
 	z80_setup_dbus_in();
 	z80_setup_addrbus_tristate();
+
+	if (!(Stat & S_Z180_RUNNING))
+		Z80_O_RST = 0;
+
 	Z80_O_BUSREQ = 1;
 	//while(Z80_I_BUSACK == 0);
 }
@@ -456,6 +473,9 @@ void z80_memfifo_putc(fifo_t f, uint8_t val)
 }
 
 /*--------------------------------------------------------------------------*/
+/*
+	TODO: Rewrite msg_fifo routines for AVR
+*/
 
 static struct {
 	uint32_t base;
diff --git a/avr/z80-if.h b/avr/z80-if.h
index 7d08df4..ecb6712 100644
--- a/avr/z80-if.h
+++ b/avr/z80-if.h
@@ -9,8 +9,8 @@ void z80_release_bus(void);
 void z80_memset(uint32_t addr, uint8_t data, uint32_t length);
 void z80_reset(level_t level);
 void z80_reset_pulse(void);
-void z80_busreq(level_t level);
-void z80_write_block(const __flash uint8_t *src, uint32_t dest, uint32_t length);
+//void z80_busreq(level_t level);
+void z80_write_block(const FLASH uint8_t *src, uint32_t dest, uint32_t length);
 int z80_stat_halt(void);