X-Git-Url: http://cloudbase.mooo.com/gitweb/avrcpm.git/blobdiff_plain/3c3744f11ae4402e060b58d5002894749105e954..18be946b6d6633ccda8b9ab2d5894b23e815a63d:/avrcpm/avr/z80.asm?ds=sidebyside

diff --git a/avrcpm/avr/z80.asm b/avrcpm/avr/z80.asm
old mode 100755
new mode 100644
index 9911e8c..64f0786
--- a/avrcpm/avr/z80.asm
+++ b/avrcpm/avr/z80.asm
@@ -15,18 +15,26 @@
 ;
 ;    You should have received a copy of the GNU General Public License
 ;    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+;
+;    $Id$
+;
 
-
+;.nolist
 #if defined atmega8
 	.include "m8def.inc"
-
-#elif defined atmega48
-	.include "m48def.inc"
+#elif defined atmega168
+	.include "m168def.inc"
+#elif defined atmega328P
+	.include "m328Pdef.inc"
 #else                               /* default */
 	.include "m88def.inc"
 	;FUSE_H=0xDF
 	;FUSE_L=0xF7
 #endif
+.list
+.listmac
+
+
 
 #ifndef F_CPU
 	#define F_CPU  20000000        /* system clock in Hz; defaults to 20MHz */
@@ -35,26 +43,31 @@
 	#define BAUD   38400           /* console baud rate */
 #endif
  
-; 
-.equ UBRR_VAL   = ((F_CPU+BAUD*8)/(BAUD*16)-1)  ; clever rounding
+#define PARTID 0x52		/* Partition table id */
+				/* http://www.win.tue.nl/~aeb/partitions/partition_types-1.html */
+
+#define UBRR_VAL  ((F_CPU+BAUD*8)/(BAUD*16)-1)  /* clever rounding */
+
+#define	RXBUFSIZE 64		/* USART recieve buffer size. Must be power of 2 */
+#define	TXBUFSIZE 64		/* USART transmit buffer size. Must be power of 2 */
 
-#define REFR_RATE   64000       /* dram refresh rate. most drams need 1/15.6µs */
+#define DRAM_WAITSTATES 1	/* Number of additional clock cycles for dram read access */
+#define REFR_RATE   64000       /* dram refresh rate in cycles/s. */
+				/* Most drams need 1/15.6µs. */
 #define REFR_PRE    8           /* timer prescale factor */
 #define REFR_CS     0x02        /* timer clock select for 1/8  */
 #define REFR_CNT    F_CPU / REFR_RATE / REFR_PRE
 
 
-#if defined __ATmega8__
-	.equ refr_vect = OC2addr
-#else
-	.equ refr_vect = OC2Aaddr
-#endif
 
+#define DRAM_WORD_ACCESS 0	/* experimental */
+
+#define EM_Z80	0		/* we don't have any z80 instructions yet */
 
 .equ MMC_DEBUG   = 0
 .equ INS_DEBUG   = 0
-.equ MEMTEST     = 0
-.equ BOOTWAIT    = 0
+.equ MEMTEST     = 1
+.equ BOOTWAIT    = 1
 .equ PORT_DEBUG  = 0
 .equ DISK_DEBUG  = 0
 .equ MEMFILL_CB  = 1
@@ -73,26 +86,47 @@
 .equ ram_a6 = 6
 .equ ram_a7 = 7
 
+.equ P_OE  = PORTD
+.equ P_AH  = PORTD
+.equ P_A8  = PORTD
+.equ P_MMC_CS = PORTD
+	             ; ram_a[7..5]
+.equ RAM_AH_MASK = (1<<ram_a8)|(1<<ram_a7)|(1<<ram_a6)|(1<<ram_a5)
+.equ PD_OUTPUT_MASK = (1<<mmc_cs) | (1<<ram_oe) | RAM_AH_MASK
+
 
 ;Port B
-.equ ram_a4	=	0
-.equ ram_a3	=	1
-.equ ram_a2	=	2
-.equ ram_a1	=	3
-.equ mmc_mosi=	3
-.equ ram_a0	=	4
-.equ mmc_miso=	4
-.equ ram_ras=	5
-.equ mmc_sck=	5
+.equ ram_a4 =	0
+.equ ram_a3 =	1
+.equ ram_a2 =	2
+.equ ram_a1 =	3
+.equ mmc_mosi =	3
+.equ ram_a0 =	4
+.equ mmc_miso =	4
+.equ ram_ras =	5
+.equ mmc_sck =	5
+
+
+.equ P_RAS = PORTB
+.equ P_AL  = PORTB
+			; ram_a[4..0]
+.equ RAM_AL_MASK = (1<<ram_a4)|(1<<ram_a3)|(1<<ram_a2)|(1<<ram_a1)|(1<<ram_a0)
+.equ PB_OUTPUT_MASK = (1<<ram_ras) | RAM_AL_MASK
 
 ;Port C
-.equ ram_d1	=	0
-.equ ram_w	=	1
-.equ ram_d2	=	2
-.equ ram_d4 =	3
-.equ ram_d3	=	4
+.equ ram_d0 =	0
+.equ ram_d1 =	1
+.equ ram_d2 =	2
+.equ ram_d3 =	3
+.equ ram_w  =	4
 .equ ram_cas=	5
 
+.equ P_DQ  = PORTC
+.equ P_W   = PORTC
+.equ P_CAS = PORTC
+.equ RAM_DQ_MASK = (1<<ram_d3)|(1<<ram_d2)|(1<<ram_d1)|(1<<ram_d0)
+.equ PC_OUTPUT_MASK = (1<<ram_cas)|(1<<ram_w)
+
 
 ;Flag bits in z_flags
 .equ ZFL_S	=	7
@@ -103,148 +137,240 @@
 .equ ZFL_C	=	0
 
 ;Register definitions
-.def z_a      = r2
-.def z_b      = r3
-.def z_c      = r4
-.def z_d      = r5
-.def z_e      = r6
-.def z_l      = r7
-.def z_h      = r8
-.def z_spl    = r9
-.def z_sph    = r10
-
-.def dsk_trk  = r11
-.def dsk_sec  = r12
-.def dsk_dmah = r13
-.def dsk_dmal = r14
-
-.def parityb  = r15
-
-.def temp     = R16 	;The temp register
-.def temp2    = R17 	;Second temp register
-.def trace    = r18
-.def opl      = r19
-.def oph      = r20
-.def adrl     = r21
-.def adrh     = r22
-.def insdecl  = r23
-.def z_pcl    = r24
-.def z_pch    = r25
-.def insdech  = r26
-.def z_flags  = r27
-
-
-;SRAM
-.dseg
-;Sector buffer for 512 byte reads/writes from/to SD-card
-
-sectbuff:
-    .byte   512
+
+.def	_tmp	= r0	;  0
+.def	_0	= r1
+
+.def	z_c	= r4
+.def	z_b	= r5
+.def	z_e	= r6
+.def	z_d	= r7
+.def	z_l	= r8
+.def	z_h	= r9
+.def	z_a	= r10
+
+.def	insdecl	= r12	;
+.def	insdech	= r13	;
+.def	z_spl	= r14
+.def	z_sph	= r15	;
+.def	temp	= r16 	;
+.def	temp2	= r17 	;
+.def	temp3	= r18
+.def	temp4	= r19
+.def	z_flags	= r20	;
+			;
+.def	opl	= r22	;
+.def	oph	= r23	;
+.def	z_pcl	= r24	;
+.def	z_pch	= r25	;
+; xl 		;r26
+; xh		;r27
+; yl		;r28
+; yh		;r29
+; zl		;r30	;
+; zh		;r31	;
+
+
+
+#if defined __ATmega8__
+.equ	flags	= TWBR
+.equ	P_PUD	= SFIOR
+#else
+.equ	flags	= GPIOR0
+.equ	P_PUD	= MCUCR
+#endif
+
+; Flags:
+	.equ	hostact	= 7		;host active flag
+	.equ	hostwrt	= 6		;host written flag
+	.equ	rsflag	= 5		;read sector flag
+	.equ	readop	= 4		;1 if read operation
+	.equ	trace	= 0
+
+; This is the base z80 port address for clock access
+#define	TIMERPORT 0x40
+#define TIMER_CTL   TIMERPORT
+#define TIMER_MSECS TIMERPORT+1
+#define TIMER_SECS  TIMER_MSECS+2
+
+#define starttimercmd	1
+#define quitTimerCmd	2
+#define printTimerCmd	15
+#define uptimeCmd	16
+
+#if defined __ATmega8__
+.equ	RXTXDR0	= UDR
+.equ	UCSR0A	= UCSRA
+.equ	 UDRE0	= UDRE
+.equ	UCSR0B	= UCSRB
+.equ	 RXCIE0	= RXCIE
+.equ	 UDRIE0	= UDRIE
+.equ	 RXEN0	= RXEN
+.equ	 TXEN0	= TXEN
+.equ	UCSR0C	= UCSRC
+.equ	 UCSZ00	= UCSZ0
+.equ	 UCSZ01	= UCSZ1
+.equ	UBRR0H	= UBRRH
+.equ	UBRR0L	= UBRRL
+.equ	OCR2A	= OCR2
+.equ	OC2Aaddr= OC2addr
+.equ	TCCR2A	= TCCR2
+.equ	TCCR2B	= TCCR2
+.equ	TIMSK1	= TIMSK
+.equ	TIMSK2	= TIMSK
+.equ	OCIE2A	= OCIE2
+.equ	TIFR1	= TIFR
+.equ	ICIE1	= TICIE1
+#else
+.equ	RXTXDR0	= UDR0
+#endif
+
+
+;----------------------------------------
+; 
+.macro	outm8
+.if	@0 > 0x3f
+	sts	@0,@1
+.else
+	out	@0,@1
+.endif
+.endm
+
+;----------------------------------------
+; 
+.macro	inm8
+.if	@1 > 0x3f
+	lds	@0,@1
+.else
+	in	@0,@1
+.endif
+.endm
+
+
+
+;----------------------------------------
+; add wait states
+;	dram_wait  number_of_cycles
+
+.macro dram_wait
+.if @0 > 1
+	rjmp	PC+1
+	dram_wait @0 - 2
+.elif @0 > 0
+	nop
+	dram_wait @0 - 1
+.endif
+.endm
 
 
 .cseg
 .org 0
 	rjmp start		; reset vector
-.org refr_vect
-	rjmp refrint	; tim2cmpa
-
+.org OC2Aaddr
+	rjmp refrint		; tim2cmpa
+.org OC1Aaddr   		; Timer/Counter1 Compare Match A
+    	rjmp sysclockint	; 1ms system timer
+.org URXCaddr   
+	rjmp rxint		; USART receive int.
+.org UDREaddr
+	rjmp txint		; USART transmit int.
+	
 .org INT_VECTORS_SIZE
+
 start:
 	ldi temp,low(RAMEND)	; top of memory
 	out SPL,temp		; init stack pointer
 	ldi temp,high(RAMEND)	; top of memory
 	out SPH,temp		; init stack pointer
 
+	clr	_0
+
 ; - Kill wdt
 	wdr
-#if defined __ATmega8__
-	ldi temp,0
-	out MCUCSR,temp
-	
-	ldi temp,(1<<WDCE) | (1<<WDE)
-	out WDTCSR,temp
-	ldi temp,(1<<WDCE)
-	out WDTCSR,temp
-#else
-	ldi temp,0
-	out MCUSR,temp
+	out MCUSR,_0
 
 	ldi temp,(1<<WDCE) | (1<<WDE)
-	sts WDTCSR,temp
+	outm8	WDTCSR,temp
 	ldi temp,(1<<WDCE)
-	sts WDTCSR,temp
-#endif
+	outm8	WDTCSR,temp
 
-; - Setup Ports
-	ldi temp,$3F
-	out DDRB,temp
-	ldi temp,$FE
-	out DDRD,temp
-	ldi temp,$22
-	out DDRC,temp
+; - Clear RAM
 
-	sbi PORTC,ram_w
-	sbi PORTC,ram_cas
-	sbi PORTB,ram_ras
-	sbi PORTD,ram_oe
-	sbi PORTD,mmc_cs
+	ldi	zl,low(SRAM_START)
+	ldi	zh,high(SRAM_START)
+	ldi	temp2,high(ramtop)
+clr_l:
+	st	z+,_0
+	cpi	zl,low(ramtop)
+	cpc	zh,temp2
+	brne	clr_l
+
+; - Setup Ports
+	ldi 	temp,(1<<PUD)		;disable pullups
+	outm8	P_PUD,temp
+	ldi	temp,0xFF
+	out 	PORTD,temp		;all pins high
+	out 	PORTB,temp
+	out 	PORTC,temp
+	out 	DDRD,temp		; all outputs
+	out	DDRB,temp
+	out	DDRC,temp
+
+	outm8	TIMSK1,_0
+	outm8	TIMSK2,_0
+	outm8	TCCR2A,_0
+	outm8	TCCR2B,_0
 
 
 ; - Init serial port
-#if defined __ATmega8__
-	ldi temp, (1<<TXEN) | (1<<RXEN)
-	out UCSRB,temp
-	ldi temp, (1<<URSEL) | (1<<UCSZ1) | (1<<UCSZ0)
-	out UCSRC,temp
-	ldi temp, HIGH(UBRR_VAL)
-	out UBRRH,temp
-	ldi temp, LOW(UBRR_VAL)
-	out UBRRL,temp
-#else
-	ldi temp, (1<<TXEN0) | (1<<RXEN0)
-	sts UCSR0B,temp
+
+	ldi temp, (1<<TXEN0) | (1<<RXEN0) | (1<<RXCIE0)
+	outm8 UCSR0B,temp
+.ifdef URSEL
+	ldi temp, (1<<URSEL) | (1<<UCSZ01) | (1<<UCSZ00)
+.else
 	ldi temp, (1<<UCSZ01) | (1<<UCSZ00)
-	sts UCSR0C,temp
+.endif
+	outm8 UCSR0C,temp
 	ldi temp, HIGH(UBRR_VAL)
-	sts UBRR0H,temp
+	outm8 UBRR0H,temp
 	ldi temp, LOW(UBRR_VAL)
-	sts UBRR0L,temp
-#endif
+	outm8 UBRR0L,temp
+
+; Init clock/timer system
+
+; Init timer 1 as 1 ms system clock tick.
+
+	ldi	temp,high(F_CPU/1000)
+	outm8	OCR1AH,temp
+	ldi	temp,low(F_CPU/1000)
+	outm8	OCR1AL,temp
+	ldi	temp,(1<<WGM12) | (1<<CS10)	;CTC, clk/1
+	outm8	TCCR1B,temp
+	inm8	temp,TIMSK1
+	ori	temp,(1<<OCIE1A)
+	outm8	TIMSK1,temp
 
 ;Init timer2. Refresh-call should happen every (8ms/512)=312 cycles.
 
-#ifdef __ATmega8__
-	ldi temp,REFR_CNT
-	out OCR2,temp
-	ldi temp,(1<<WGM21) | REFR_CS	;CTC, clk/REFR_PRE
-	out TCCR2,temp
-	ldi temp, (1<<OCIE2)
-	out TIMSK,temp
-#else
-	ldi temp,REFR_CNT ;=312 cycles
-	sts OCR2A,temp
-	ldi temp, (1<<WGM21)
-	sts TCCR2A,temp
-	ldi temp, REFR_CS		;clk/REFR_PRE
-	sts TCCR2B,temp
-	ldi temp,(1<<OCIE2A)
-	sts TIMSK2,temp
-#endif
+	ldi	temp,REFR_CNT*2			; 2 cycles per int
+	outm8	OCR2A,temp
+	inm8	temp,TCCR2A
+	ori	temp,(1<<WGM21)			;CTC mode
+	outm8	TCCR2A,temp
+	inm8	temp,TCCR2B
+	ori	temp,REFR_CS			;clk/REFR_PRE
+	outm8	TCCR2B,temp
+	inm8	temp,TIMSK2
+	ori	temp, (1<<OCIE2A)
+	outm8	TIMSK2,temp
+
 	sei
 
 
 .if BOOTWAIT
-	push temp
-	ldi temp,0
-bootwait1:
-	push temp
-	ldi temp,0
-bootwait2:
-	dec temp
-	brne bootwait2
-	pop temp
-	dec temp
-	brne bootwait1
+	ldi temp,10
+	rcall delay_ms
 
 .endif
 
@@ -252,102 +378,199 @@ bootwait2:
 	.db "CPM on an AVR, v1.0",13,0,0
 
 
-	rcall printstr
-	.db "Initing mmc...",13,0
-	rcall mmcInit
-
-
 .if MEMTEST
 	rcall printstr
-	.db "Testing RAM...",13,0
+	.db "Testing RAM: fill...",0,0
 
 ;Fill RAM
-	ldi adrl,0
-	ldi adrh,0
+	ldi xl,0
+	ldi xh,0
 ramtestw:
-	mov temp,adrh
-	eor temp,adrl
+	mov temp,xh
+	eor temp,xl
 	rcall memwritebyte
-	ldi temp,1
-	ldi temp2,0
-	add adrl,temp
-	adc adrh,temp2
+	adiw xl,1
 	brcc ramtestw
+	rcall printstr
+	.db "wait...",0
+
+	ldi	temp2,8
+ramtestwl:
+	ldi	temp,255
+	rcall	delay_ms
+	dec	temp2
+	brne	ramtestwl
+
+	rcall printstr
+	.db "reread...",13,0,0
 
 ;re-read RAM
-	ldi adrl,0
-	ldi adrh,0
+	ldi xl,0
+	ldi xh,0
 ramtestr:
-	rcall memreadbyte
-	mov temp2,adrh
-	eor temp2,adrl
+	rcall memReadByte
+	mov temp2,xh
+	eor temp2,xl
 	cp temp,temp2
 	breq ramtestrok
 	rcall printhex
 	ldi temp,'<'
 	rcall uartPutc
-	mov temp,adrh
-	eor temp,adrl
+	mov temp,xh
+	eor temp,xl
 	rcall printhex
 	ldi temp,'@'
 	rcall uartPutc
-	mov temp,adrh
+	mov temp,xh
 	rcall printhex
-	mov temp,adrl
+	mov temp,xl
 	rcall printhex
 	ldi temp,13
 	rcall uartPutc
 ramtestrok:
-	ldi temp,1
-	ldi temp2,0
-	add adrl,temp
-	adc adrh,temp2
+	adiw xl,1
 	brcc ramtestr
 
 .endif
 
 .if MEMFILL_CB
 	;Fill ram with cbs, which (for now) will trigger an invalid opcode error.
-	ldi adrl,0
-	ldi adrh,0
+	ldi xl,0
+	ldi xh,0
 ramfillw:
 	ldi temp,0xcb
 	rcall memwritebyte
-	ldi temp,1
-	ldi temp2,0
-	add adrl,temp
-	adc adrh,temp2
+	adiw xl,1
 	brcc ramfillw
 .endif
 
 
+	rcall printstr
+	.db "Initing mmc...",13,0
+	rcall mmcInit
 
-;Load initial sector from MMC (512 bytes)
-	ldi adrh,0
-	ldi adrl,0
-	rcall mmcReadSect
+;----------------------------------------------------------------------------
+
+; Partition table offsets:
+#define PART_TYPE   4
+#define PART_START  8
+#define PART_SIZE  12
+
+;Load first sector from MMC (boot sector)
+
+boot_again:
+	ldi	yh,0
+	ldi	yl,0
+	ldi	xh,0
+	ldi	xl,0
+	rcall	mmcReadSect
+
+;Test, if it has a valid MBR
+
+	lds	temp,hostbuf+510	;MBR signature (0xAA55)  at and of sector?
+	lds	temp2,hostbuf+511
+	ldi	temp4,0xAA
+	cpi	temp,0x55		
+	cpc	temp2,temp4
+	breq	boot_part
+
+;No MBR, no partition table ...
+	ldi	opl,1
+	sts	ndisks,opl
+	rjmp	boot_ipl
+		
+;Search Partition Table for CP/M partitions
+boot_part:
+	ldi	zl,low(hostbuf+510-64)
+	ldi	zh,high(hostbuf+510-64)
+	ldi	yl,low(hostdstart)
+	ldi	yh,high(hostdstart)
+	ldi	opl,0
+	ldi	oph,high(hostbuf+510)
+boot_ploop:
+	ldd	temp,z+PART_TYPE
+	cpi	temp,PARTID
+	brne	boot_nextp
+	
+	ldd	temp,z+PART_START
+	st	y+,temp
+	ldd	temp2,z+PART_START+1
+	st	y+,temp2
+	ldd	temp3,z+PART_START+2
+	st	y+,temp3
+	ldd	temp4,z+PART_START+3
+	st	y+,temp4
+	
+	rcall	printstr
+	.db	"CP/M partition at: ",0
+	rcall	print_ultoa
+	rcall	printstr
+	.db	", size: ",0,0
+	ldd	temp,z+PART_SIZE
+	ldd	temp2,z+PART_SIZE+1
+	ldd	temp3,z+PART_SIZE+2
+	ldd	temp4,z+PART_SIZE+3
+	lsr	temp4
+	ror	temp3
+	ror	temp2
+	ror	temp
+	rcall	print_ultoa
+	rcall	printstr
+	.db	"KB.",13,0,0
+	
+	inc	opl
+	cpi	temp2,MAXDISKS
+	breq	boot_pend	
+boot_nextp:
+	adiw	zl,16
+	cpi	zl,low(hostbuf+510)
+	cpc	zh,oph
+	brsh	boot_pend
+	rjmp	boot_ploop
+boot_pend:
+	sts	ndisks,opl
+
+; Read first sector of first CP/M partition
+
+	lds	xl,hostdstart
+	lds	xh,hostdstart+1
+	lds	yl,hostdstart+2
+	lds	yh,hostdstart+3
+	rcall	mmcReadSect
+
+boot_ipl:
+	lds	opl,ndisks
+	tst	opl
+	brne	boot_ipl2
+	rcall	printstr
+	.db	"No bootable CP/M disk found! Please change MMC/SD-Card",13,0
+	ldi	temp2,18
+boot_wl:
+	ldi	temp,255
+	rcall	delay_ms
+	dec	temp2
+	brne	boot_wl
+	rjmp	boot_again
+	
+
+boot_ipl2:
+
+;First sector of disk or first CP/M partition is in hostbuf.
 
 ;Save to Z80 RAM (only 128 bytes because that's retro)
-	ldi zl,low(sectbuff)
-	ldi zh,high(sectbuff)
-	ldi adrh,0x20
-	ldi adrl,0x00
+	ldi zl,low(hostbuf)
+	ldi zh,high(hostbuf)
+	ldi xh,0x20
+	ldi xl,0x00
 iplwriteloop:
 	ld temp,z+
-	push zh
-	push zl
 	rcall memWriteByte
-	pop zl
-	pop zh
-	ldi temp,1
-	ldi temp2,0
-	add adrl,temp
-	adc adrh,temp2
-	cpi zl,low(sectbuff+128)
+	adiw xl,1
+	cpi zl,low(hostbuf+128)
 	brne iplwriteloop
-	cpi zh,high(sectbuff+128)
+	cpi zh,high(hostbuf+128)
 	brne iplwriteloop
-
+	rcall	dsk_boot		;init (de)blocking buffer
 
 
 ;Init z80
@@ -356,17 +579,17 @@ iplwriteloop:
 	ldi temp,0x20
 	mov z_pch,temp
 
-	ldi trace,0
+	cbi	flags,trace
 	rcall printstr
 	.db 13,"Ok, CPU is live!",13,0,0
 
 main:
-	ldi trace,0
+	cbi	flags,trace
 	cpi z_pch,1
 	brlo notraceon
 	cpi z_pch,$dc
 	brsh notraceon
-	ldi trace,1
+	sbi	flags,trace
 notraceon:
 
 
@@ -388,21 +611,19 @@ noprintpc:
 .endif
 
 	; *** Stage 1: Fetch next opcode
-	mov adrl,z_pcl
-	mov adrh,z_pch
+	movw xl,z_pcl
 	rcall memReadByte
-	adiw z_pcl,1
 
 
 .if INS_DEBUG
-	cpi trace,0
-	breq notrace1
+	sbis	flags,trace
+	rjmp	notrace1
 	rcall printstr
 	.db "PC=",0
 	push temp
-	mov temp,adrh
+	mov temp,z_pch
 	rcall printhex
-	mov temp,adrl
+	mov temp,z_pcl
 	rcall printhex
 	pop temp
 	rcall printstr
@@ -410,53 +631,43 @@ noprintpc:
 	rcall printhex
 notrace1:
 .endif
+	adiw z_pcl,1
 
 	; *** Stage 2: Decode it using the ins_table.
-	ldi temp2,0
-	ldi zl,low(inst_table*2)
 	ldi zh,high(inst_table*2)
+	mov zl,temp
 	add zl,temp
-	adc zh,temp2
-	add zl,temp
-	adc zh,temp2
+	adc zh,_0
 	lpm insdecl,Z+
 	lpm insdech,Z
 
 .if INS_DEBUG
-	cpi trace,0
-	breq notrace2
+	sbis	flags,trace
+	rjmp	notrace2
 	rcall printstr
-	.db ", decoded=",0
+	.db ", decoded=",0,0
 	mov temp,insdech
 	rcall printhex
 	mov temp,insdecl
 	rcall printhex
 	rcall printstr
-	.db ".",13,0
+	.db ".",13,0,0
 notrace2:
 .endif
 
 	; *** Stage 3: Fetch operand. Use the fetch jumptable for this.
 	mov temp,insdecl
 	andi temp,0x1F
-	cpi temp,0
 	breq nofetch
-	ldi temp2,0
-	lsl temp
-	ldi zl,low(fetchjumps*2)
-	ldi zh,high(fetchjumps*2)
+	ldi zl,low(fetchjumps)
+	ldi zh,high(fetchjumps)
 	add zl,temp
-	adc zh,temp2
-
-	lpm temp,Z+
-	lpm temp2,Z
-	mov zl,temp
-	mov zh,temp2
+	adc zh,_0
 	icall
 
 .if INS_DEBUG
-	cpi trace,0
-	breq notrace3
+	sbis	flags,trace
+	rjmp	notrace3
 	rcall printstr
 	.db "pre: oph:l=",0
 	mov temp,oph
@@ -464,7 +675,7 @@ notrace2:
 	mov temp,opl
 	rcall printhex
 	rcall printstr
-	.db " -- ",0
+	.db " -- ",0,0
 notrace3:
 .endif
 
@@ -472,25 +683,20 @@ nofetch:
 	; *** Stage 4: Execute operation :) Use the op jumptable for this.
 	mov temp,insdech
 	andi temp,0xFC
-	lsr temp
-	cpi temp,0
 	breq nooper
-	ldi zl,low(opjumps*2)
-	ldi zh,high(opjumps*2)
-	ldi temp2,0
+	lsr temp
+	lsr temp
+	ldi zl,low(opjumps)
+	ldi zh,high(opjumps)
 	add zl,temp
-	adc zh,temp2
-	lpm temp,Z+
-	lpm temp2,Z
-	mov zl,temp
-	mov zh,temp2
+	adc zh,_0
 	icall
 
 .if INS_DEBUG
-	cpi trace,0
-	breq notrace4
+	sbis	flags,trace
+	rjmp	notrace4
 	rcall printstr
-	.db ",post:oph:l=",0
+	.db ",post:oph:l=",0,0
 	mov temp,oph
 	rcall printhex
 	mov temp,opl
@@ -502,26 +708,21 @@ nooper:
 	; *** Stage 5: Store operand. Use the store jumptable for this.
 	swap insdecl
 	swap insdech
-	mov temp,insdecl
+	movw temp,insdecl
 	andi temp,0x0E
-	andi insdech,0x30
-	or temp,insdech
-	cpi temp,0
+	andi temp2,0x30
+	or temp,temp2
 	breq nostore
-	ldi zl,low(storejumps*2)
-	ldi zh,high(storejumps*2)
-	ldi temp2,0
+	lsr temp
+	ldi zl,low(storejumps)
+	ldi zh,high(storejumps)
 	add zl,temp
-	adc zh,temp2
-	lpm temp,Z+
-	lpm temp2,Z
-	mov zl,temp
-	mov zh,temp2
+	adc zh,_0
 	icall
 
 .if INS_DEBUG
-	cpi trace,0
-	breq notrace5
+	sbis	flags,trace
+	rjmp	notrace5
 	rcall printstr
 	.db ", stored.",0
 notrace5:
@@ -530,8 +731,8 @@ notrace5:
 nostore:
 
 .if INS_DEBUG
-	cpi trace,0
-	breq notrace6
+	sbis	flags,trace
+	rjmp	notrace6
 	rcall printstr
 	.db 13,0
 notrace6:
@@ -545,22 +746,88 @@ notrace6:
 
 ;The hw is modelled to make writing a CPM BIOS easier.
 ;Ports:
-;0 - Con status. Returns 0xFF if the UART has a byte, 0 otherwise.
-;1 - Console input, aka UDR.
-;2 - Console output
-;16 - Track select
-;18 - Sector select
-;20 - Write addr l
-;21 - Write addr h
-;22 - Trigger - write 1 to read, 2 to write a sector using the above info.
-;	This will automatically move track, sector and dma addr to the next sector.
-
+;0 	- Con status. Returns 0xFF if the UART has a byte, 0 otherwise.
+;1 	- Console input, aka UDR.
+;2 	- Console output
+;15	- Disk select
+;16,17 	- Track select
+;18 	- Sector select
+;20 	- Write addr l
+;21 	- Write addr h
+;22 	- Trigger - write  to read, to write a sector using the above info;
+;			, write to allocated/dirctory/unallocated
+
+	.equ	READ_FUNC  = 7
+	.equ	WRITE_FUNC = 6
+	.equ	BOOT_FUNC  = 5
+	.equ	HOME_FUNC  = 4
+
+
+
+;*****************************************************
+;*         CP/M to host disk constants               *
+;*****************************************************
+	.equ	MAXDISKS  = 4		;Max number of Disks (partitions)
+	.equ	blksize = 1024		;CP/M allocation size
+	.equ	hostsize = 512		;host disk sector size
+;	.equ	hostspt = 20		;host disk sectors/trk
+	.equ	hostblk = hostsize/128	;CP/M sects/host buff
+;	.equ	CPMSPT = hostblk*hostspt;CP/M sectors/track
+	.equ	CPMSPT = 26		;
+	.equ	SECMSK = hostblk-1	;sector mask
+	.equ	SECSHF = log2(hostblk)	;sector shift
+
+;*****************************************************
+;*        BDOS constants on entry to write           *
+;*****************************************************
+	.equ	WRALL = 0		;write to allocated
+	.equ	WRDIR = 1		;write to directory
+	.equ	WRUAL = 2		;write to unallocated
+	.equ	WRTMSK= 3		;write type mask
+
+
+	.dseg
+ndisks:		.byte	1	;Number of CP/M disks
+
+seekdsk:	.byte	1	;seek disk number
+seektrk:	.byte	2	;seek track number
+seeksec:	.byte	1	;seek sector number
+
+hostdsk:	.byte	1	;host disk number
+hostlba:	.byte	3	;host sector number (relative to partition start)
+hostdstart:	.byte	4*MAXDISKS ; host disks start sector (partition)
+
+unacnt:		.byte	1	;unalloc rec cnt
+unadsk:		.byte	1	;last unalloc disk
+unatrk:		.byte	2	;last unalloc track
+unasec:		.byte	1	;last unalloc sector
+
+erflag:		.byte	1	;error reporting
+wrtype:		.byte	1	;write operation type
+dmaadr:		.byte	2	;last dma address
+hostbuf:	.byte	hostsize;host buffer (from/to SD-card)
+
+
+	.cseg
+	
 ;Called with port in temp2. Should return value in temp.
 portRead:
 	cpi temp2,0
 	breq conStatus
 	cpi temp2,1
 	breq conInp
+
+	cpi	temp2,15
+	breq	dskDiskCheck
+
+	cpi	temp2,TIMER_MSECS
+	brlo	pr_noclock
+	cpi	temp2,TIMER_MSECS+6
+	brsh	pr_noclock
+	rjmp	clockget
+
+pr_noclock:
+	ldi	temp,0xFF
 	ret
 
 ;Called with port in temp2 and value in temp.
@@ -569,33 +836,43 @@ portWrite:
 	breq dbgOut
 	cpi temp2,2
 	breq conOut
+
+	cpi temp2,15
+	breq dskDiskSel
 	cpi temp2,16
-	breq dskTrackSel
+	breq dskTrackSel_l
+	cpi temp2,17
+	breq dskTrackSel_h
 	cpi temp2,18
 	breq dskSecSel
 	cpi temp2,20
 	breq dskDmaL
 	cpi temp2,21
 	breq dskDmaH
+
 	cpi temp2,22
 	breq dskDoIt
+	
+	cpi	temp2,TIMERPORT
+	brlo	pw_noclock
+	cpi	temp2,TIMER_MSECS+6
+	brsh	pw_noclock
+	rjmp	clockput
+
+pw_noclock:
 	ret
 
 
 conStatus:
-	ldi temp,0
-#if defined __ATmega8__
-    sbic  UCSRA,RXC
-#else
-	lds temp2,UCSR0A
-	sbrc temp2,RXC0
-#endif
+
+	lds temp,rxcount
+	tst temp
+	breq PC+2
 	 ldi temp,0xff
 	ret
 
 conInp:
-	rcall uartGetc
-	ret
+	rjmp uartGetc
 
 dbgOut:
 	rcall printstr
@@ -606,205 +883,439 @@ dbgOut:
 	ret
 
 conOut:
-	rcall uartputc
+	rjmp uartputc
+
+
+dskDiskCheck:
+	lds	temp,seekdsk
+	lds	temp2,ndisks	;check if selected disk # is less then # of disks
+	cp	temp,temp2
+	ldi	temp,0
+	brlt	PC+2
+	ldi	temp,0xff		;error return
 	ret
 
-dskTrackSel:
-	mov dsk_trk,temp
+dskDiskSel:
+	sts seekdsk,temp
+	ret
+
+dskTrackSel_l:
+	sts seektrk,temp
+	sts seektrk+1,_0
+	ret
+
+dskTrackSel_h:
+	sts seektrk+1,temp
 	ret
 
 dskSecSel:
-	mov dsk_sec,temp
+	sts seeksec,temp
 	ret
 
 dskDmal:
-	mov dsk_dmal,temp
+	sts dmaadr,temp
 	ret
 
 dskDmah:
-	mov dsk_dmah,temp
+	sts dmaadr+1,temp
 	ret
 
 dskDoIt:
+	;See what has to be done.
+	sbrc	temp,READ_FUNC
+	 rjmp	dsk_read
+	sbrc	temp,WRITE_FUNC
+	 rjmp	dsk_write
+	sbrc	temp,HOME_FUNC
+	 rjmp	dsk_home
+	sbrc	temp,BOOT_FUNC
+	 rjmp	dsk_boot
+
+	rcall	printstr
+	.db "DISK I/O: Invalid Function code: ",0
+	rcall	printhex
+	rjmp	haltinv
+
+dsk_boot:
+	cbi	flags,hostact		;host buffer inactive
+	sts	unacnt,_0		;clear unalloc count
+	ret
+
+dsk_home:
+	sbis	flags,hostwrt		;check for pending write
+	cbi	flags,hostact		;clear host active flag
+	ret
+
+
+dsk_read:
+
+.if 0
+	rcall	timer_quit
+	rcall printstr
+	.db " In",0
+.endif
+
 .if DISK_DEBUG
 	push temp
 	rcall printstr
+	.db 13,0
+	rcall printstr
 	.db "Disk read: track ",0
-	mov temp,dsk_trk
+	lds temp,seektrk+1
+	rcall printhex
+	lds temp,seektrk
 	rcall printhex
 	rcall printstr
-	.db " sector ",0
-	mov temp,dsk_sec
+	.db " sector ",0,0
+	lds temp,seeksec
 	rcall printhex
 	rcall printstr
-	.db " dma-addr ",0
-	mov temp,dsk_dmah
+	.db " dma-addr ",0,0
+	lds temp,dmaadr+1
 	rcall printhex
-	mov temp,dsk_dmal
+	lds temp,dmaadr
 	rcall printhex
-	rcall printstr
-	.db ".",13,0
+;	rcall printstr
+;	.db ".",13,0,0
 	pop temp
 .endif
+	sts	unacnt,_0
+	sbi	flags,readop		;read operation
+	sbi	flags,rsflag		;must read data
+	ldi	temp,WRUAL		;write type
+	sts	wrtype,temp		;treat as unalloc
+	rjmp	dsk_rwoper		;to perform the read
 
-	;First, convert track/sector to an LBA address (in 128byte blocks)
-	push temp
-	mov adrl,dsk_sec
-	ldi adrh,0
-	mov temp2,dsk_trk
-dskXlateLoop:
-	cpi temp2,0
-	breq dskXlateLoopEnd
-	ldi temp,26
-	add adrl,temp
-	ldi temp,0
-	adc adrh,temp
-	dec temp2
-	rjmp dskXlateLoop
-dskXlateLoopEnd:
-	pop temp
 
-	;Now, see what has to be done.
-	cpi temp,1
-	breq dskDoItRead
-	cpi temp,2
-	breq dskDoItWrite
-
-dskDoItRead:
-	push adrl
-	;Convert from 128-byte LBA blocks to 512-byte LBA blocks
-	lsr adrh
-	ror adrl
-	lsr adrh
-	ror adrl
-	;Read 512-byte sector
-	rcall mmcReadSect
-	pop adrl
-
-	;Now, move the correct portion of the sector from AVR ram to Z80 ram
-	ldi zl,low(sectbuff)
-	ldi zh,high(sectbuff)
-	ldi temp,128
-	ldi temp2,0
-	sbrc adrl,0
-	 add zl,temp
-	sbrc adrl,0
-	 adc zh,temp2
-	sbrc adrl,1
-	 inc zh
-
-	mov adrh,dsk_dmah
-	mov adrl,dsk_dmal
-
-	ldi temp2,128
-dskDoItReadMemLoop:
-	push temp2
-	ld temp,z+
-	push zh
-	push zl
-	rcall memWriteByte
-	pop zl
-	pop zh
-	ldi temp,1
-	ldi temp2,0
-	add adrl,temp
-	adc adrh,temp2
-	pop temp2
-	dec temp2
-	brne dskDoItReadMemLoop
-	ret
+dsk_write:
+	;write the selected CP/M sector
 
-dskDoItWrite:
-;The write routines is a bit naive: it'll read the 512-byte sector the 128byte CPM-sector
-;resides in into memory, will overwrite the needed 128 byte with the Z80s memory buffer
-;and will then write it back to disk. In theory, this would mean that every 512 bytes
-;written will take 4 write cycles, while theoretically the writes could be deferred so we
-;would only have to do one write cycle.
+	andi	temp,WRTMSK
+	sts	wrtype,temp		;save write type
+	cbi	flags,readop		;not a read operation
 
 .if DISK_DEBUG
 	push temp
 	rcall printstr
-	.db "Disk write: track ",0
-	mov temp,dsk_trk
+	.db 13,0
+	rcall printstr
+	.db "Disk write: track ",0,0
+	lds temp,seektrk+1
+	rcall printhex
+	lds temp,seektrk
 	rcall printhex
 	rcall printstr
-	.db " sector ",0
-	mov temp,dsk_sec
+	.db " sector ",0,0
+	lds temp,seeksec
 	rcall printhex
 	rcall printstr
-	.db " dma-addr ",0
-	mov temp,dsk_dmah
+	.db " dma-addr ",0,0
+	lds temp,dmaadr+1
 	rcall printhex
-	mov temp,dsk_dmal
+	lds temp,dmaadr
 	rcall printhex
 	rcall printstr
-	.db ".",13,0
+	.db " wrtype ",0,0
+	lds temp,wrtype
+	rcall printhex
+;	rcall printstr
+;	.db ".",13,0,0
 	pop temp
 .endif
 
+	cpi	temp,WRUAL		;write unallocated?
+	brne	dsk_chkuna		;check for unalloc
+
+;	write to unallocated, set parameters
+	ldi	temp,blksize/128	;next unalloc recs
+	sts	unacnt,temp
+	lds	temp,seekdsk		;disk to seek
+	sts	unadsk,temp		;unadsk = sekdsk
+	lds	temp,seektrk
+	sts	unatrk,temp		;unatrk = sectrk
+	lds	temp,seektrk+1
+	sts	unatrk+1,temp		;unatrk = sectrk
+	lds	temp,seeksec
+	sts	unasec,temp		;unasec = seksec
+;
+dsk_chkuna:
+	;check for write to unallocated sector
+	lds	temp,unacnt		;any unalloc remain?
+	tst	temp
+	breq	dsk_alloc		;skip if not
+
+;	more unallocated records remain
+	dec	temp			;unacnt = unacnt-1
+	sts	unacnt,temp
+	lds	temp,seekdsk		;same disk?
+	lds	temp2,unadsk
+	cp	temp,temp2		;seekdsk = unadsk?
+	brne	dsk_alloc		;skip if not
+
+;	disks are the same
+	lds	temp,unatrk
+	lds	temp2,unatrk+1
+	lds	temp3,seektrk
+	lds	temp4,seektrk+1
+	cp	temp,temp3		;seektrk = unatrk?
+	cpc	temp2,temp4
+	brne	dsk_alloc		;skip if not
+
+;	tracks are the same
+	lds	temp,seeksec		;same sector?
+	lds	temp2,unasec
+	cp	temp,temp2		;seeksec = unasec?
+	brne	dsk_alloc		;skip if not
+
+;	match, move to next sector for future ref
+	inc	temp2			;unasec = unasec+1
+	sts	unasec,temp2
+	cpi	temp2,CPMSPT		;end of track? (count CP/M sectors)
+	brlo	dsk_noovf		;skip if no overflow
+
+;	overflow to next track
+	sts	unasec,_0		;unasec = 0
+	lds	temp,unatrk
+	lds	temp2,unatrk+1
+	subi	temp, low(-1)		;unatrk = unatrk+1
+	sbci	temp2,high(-1)
+	sts	unatrk,temp
+	sts	unatrk+1,temp2
+;
+dsk_noovf:
+	cbi	flags,rsflag		;rsflag = 0
+	rjmp	dsk_rwoper		;to perform the write
+;
+dsk_alloc:
+	;not an unallocated record, requires pre-read
+	sts	unacnt,_0		;unacnt = 0
+	sbi	flags,rsflag		;rsflag = 1
+
+;*****************************************************
+;*	Common code for READ and WRITE follows       *
+;*****************************************************
+
+dsk_rwoper:
+	;enter here to perform the read/write
+	sts	erflag,_0	;no errors (yet)
+
+	;Convert track/sector to an LBA address (in 128byte blocks)
+
+	lds	xl,seeksec		;
+	ldi	xh,0			;
+	ldi	yl,0			;
+	lds	temp3,seektrk		;
+	lds	temp4,seektrk+1		;
+	ldi	temp,CPMSPT		;
+	mul	temp3,temp		;
+	add	xl,r0			;
+	adc	xh,r1			;
+	mul	temp4,temp		;
+	add	xh,r0			;xh:xl := sec + trk * SectorsPerTrack
+	adc	yl,r1			;
+	clr	_0
+
+	mov	temp,xl
+	andi	temp,SECMSK		;mask buffer number
+	push	temp			;save for later
+
+	;Convert from CP/M LBA blocks to host LBA blocks
+	ldi temp,SECSHF
+dsk_sh1:
+	lsr	xh
+	ror	xl
+	ror	yl
+	dec	temp
+	brne	dsk_sh1
+					;yl:xh:xl = host block to seek
+;	active host sector?
+	sbis	flags,hostact		;host active?
+	 rjmp	dsk_filhst		;fill host if not
+
+;	host buffer active, same as seek buffer?
+	lds	temp,seekdsk
+	lds	temp2,hostdsk		;same disk?
+	cp	temp,temp2		;seekdsk = hostdsk?
+	brne	dsk_nomatch
+
+;	same disk, same block?
+	lds	temp,hostlba
+	lds	temp2,hostlba+1
+	lds	temp3,hostlba+2
+	cp	xl,temp
+	cpc	xh,temp2
+	cpc	yl,temp3
+	breq	dsk_match
+;
+dsk_nomatch:
+	;proper disk, but not correct sector
+	sbic	flags,hostwrt		;host written?
+	 rcall	dsk_writehost		;clear host buff
+
+dsk_filhst:
+	;may have to fill the host buffer
+	lds	temp,seekdsk
+	sts	hostdsk,temp
+	sts	hostlba,xl
+	sts	hostlba+1,xh
+	sts	hostlba+2,yl
+
+	sbic	flags,rsflag		;need to read?
+	 rcall	dsk_readhost		;yes, if 1
+	cbi	flags,hostwrt		;no pending write
+
+dsk_match:
+	sbi	flags,hostact		;host buffer active now
+
+	;copy data to or from buffer
+	ldi	zl,low(hostbuf)
+	ldi	zh,high(hostbuf)
+	ldi	temp,128
+	pop	temp2			;get buffer number (which part of hostbuf)
+	mul	temp2,temp
+	add	zl,r0			;offset in hostbuf
+	adc	zh,r1
+.if DISK_DEBUG
+	push	r0
+	push	r1
+	clr	_0
+	rcall printstr
+	.db "; host buf adr: ",0,0
+	pop	temp
+	rcall printhex
+	pop	temp
+	rcall printhex
+.endif
+	clr	_0
+
+	lds	xl,dmaadr
+	lds	xh,dmaadr+1
+	ldi	temp3,128			;length of move
+	sbic	flags,readop		;which way?
+	 rjmp	dsk_rmove		;skip if read
+
+;	mark write operation
+	sbi	flags,hostwrt		;hostwrt = 1
+dsk_wmove:
+	rcall	memReadByte
+	st	z+,temp
+	adiw	xl,1
+	dec	temp3
+	brne dsk_wmove
+	rjmp	dsk_rwmfin
+	
+dsk_rmove:
+	ld	temp,z+
+	rcall	memWriteByte
+	adiw	xl,1
+	dec	temp3
+	brne	dsk_rmove
+dsk_rwmfin:
+;	data has been moved to/from host buffer
+	lds	temp,wrtype	;write type
+	cpi	temp,WRDIR	;to directory?
+	breq	dsk_wdir
+	lds	temp,erflag
+.if 0
+	rcall	timer_quit
+	rcall printstr
+	.db " Out",0
+.endif
+	ret			;no further processing
+dsk_wdir:
+;	clear host buffer for directory write
+	lds	temp,erflag
+	tst	temp		;errors?
+	breq	dsk_wdir1
+	ret			;skip if so
+dsk_wdir1:
+	rcall	dsk_writehost	;clear host buff
+	cbi	flags,hostwrt	;buffer written
+	lds	temp,erflag
+	ret
 
-	push adrl
-	push adrh
-	;Convert from 128-byte LBA blocks to 512-byte LBA blocks
-	lsr adrh
-	ror adrl
-	lsr adrh
-	ror adrl
-	;Read 512-byte sector
-	rcall mmcReadSect
-	pop adrh
-	pop adrl
-
-	push adrl
-	push adrh
-
-;Copy the data from the Z80 DMA buffer in external memory to the right place in the
-;sector buffer.
-	;Now, move the correct portion of the sector from AVR ram to Z80 ram
-	ldi zl,low(sectbuff)
-	ldi zh,high(sectbuff)
-	ldi temp,128
-	ldi temp2,0
-	sbrc adrl,0
-	 add zl,temp
-	sbrc adrl,0
-	 adc zh,temp2
-	sbrc adrl,1
-	 inc zh
-	mov adrh,dsk_dmah
-	mov adrl,dsk_dmal
-	ldi temp2,128
-dskDoItWriteMemLoop:
-	push temp2
 
-	push zh
-	push zl
-	rcall memReadByte
-	pop zl
-	pop zh
-	st z+,temp
-	ldi temp,1
-	ldi temp2,0
-	add adrl,temp
-	adc adrh,temp2
+;*****************************************************
+;*	WRITEhost performs the physical write to     *
+;*	the host disk, READhost reads the physical   *
+;*	disk.					     *
+;*****************************************************
 
-	pop temp2
-	dec temp2
-	brne dskDoItWriteMemLoop
+dsk_writehost:
+	;hostdsk = host disk #, hostlba = host block #.
+	;Write "hostsize" bytes from hostbuf and return 
+	;error flag in erflag.
+	;Return erflag non-zero if error
+	
+	push	yh
+	push	yl
+	push	xh
+	push	xl
+	ldi	zl,low(hostdstart)
+	ldi	zh,high(hostdstart)
+	lds	temp,hostdsk
+	lsl	temp
+	lsl	temp
+	add	zl,temp
+	adc	zh,_0
+	
+	ldd	xl,z+0
+	ldd	xh,z+1
+	ldd	yl,z+2
+	ldd	yh,z+3
+	lds	temp,hostlba
+	add	xl,temp
+	lds	temp,hostlba+1
+	adc	xh,temp
+	lds	temp,hostlba+2
+	adc	zl,temp
+	adc	zh,_0
+	rcall	mmcWriteSect
+	pop	xl
+	pop	xh
+	pop	yl
+	pop	yh
+	sts	erflag,_0
+	ret
 
-	pop adrh
-	pop adrl
+dsk_readhost:
+	;hostdsk = host disk #, hostlba = host block #.
+	;Read "hostsiz" bytes into hostbuf and return 
+	;error flag in erflag.
+
+	push	yh
+	push	yl
+	push	xh
+	push	xl
+	ldi	zl,low(hostdstart)
+	ldi	zh,high(hostdstart)
+	lds	temp,hostdsk
+	lsl	temp
+	lsl	temp
+	add	zl,temp
+	adc	zh,_0
+
+	ldd	xl,z+0
+	ldd	xh,z+1
+	ldd	yl,z+2
+	ldd	yh,z+3
+	lds	temp,hostlba
+	add	xl,temp
+	lds	temp,hostlba+1
+	adc	xh,temp
+	lds	temp,hostlba+2
+	adc	zl,temp
+	adc	zh,_0
+	rcall	mmcReadSect
+	pop	xl
+	pop	xh
+	pop	yl
+	pop	yh
+	sts	erflag,_0
+	ret
 
-	;Convert from 128-byte LBA blocks to 512-byte LBA blocks
-	lsr adrh
-	ror adrl
-	lsr adrh
-	ror adrl
-	;Write the sector back.
-	rcall mmcWriteSect
 
-	;All done :)
-	ret
+;***************************************************************************
 
 ; ----------------- MMC/SD routines ------------------
 
@@ -813,8 +1324,6 @@ mmcByteNoSend:
 mmcByte:
 
 .if MMC_DEBUG
-	push zl
-	push zh
 	rcall printstr
 	.db "MMC: <--",0
 	rcall printhex
@@ -828,15 +1337,11 @@ mmcWrByteW:
 	in temp,SPDR
 
 .if MMC_DEBUG
-	push temp
 	rcall printstr
 	.db ", -->",0
 	rcall printhex
 	rcall printstr
 	.db ".",13,0
-	pop temp
-	pop zh
-	pop zl
 .endif
 	ret
 
@@ -869,30 +1374,31 @@ mmcInit:
 	out SPCR,temp
 	
 	;Init start: send 80 clocks with cs disabled
-	sbi PORTD,mmc_cs
+	sbi P_MMC_CS,mmc_cs
 
-	ldi temp2,20
+;	ldi temp2,20
+	ldi temp2,10     ; exactly 80 clocks
 mmcInitLoop:
 	mov temp,temp2
 	rcall mmcByte
 	dec temp2
 	brne mmcInitLoop
 
-	cbi PORTD,mmc_cs
+	cbi P_MMC_CS,mmc_cs
 	rcall mmcByteNoSend
 	rcall mmcByteNoSend
 	rcall mmcByteNoSend
 	rcall mmcByteNoSend
 	rcall mmcByteNoSend
 	rcall mmcByteNoSend
-	sbi PORTD,mmc_cs
+	sbi P_MMC_CS,mmc_cs
 	rcall mmcByteNoSend
 	rcall mmcByteNoSend
 	rcall mmcByteNoSend
 	rcall mmcByteNoSend
 
 	;Send init command
-	cbi PORTD,mmc_cs
+	cbi P_MMC_CS,mmc_cs
 	ldi temp,0xff	;dummy
 	rcall mmcByte
 	ldi temp,0xff	;dummy
@@ -912,19 +1418,19 @@ mmcInitLoop:
 	ldi temp,0xff	;return byte
 	rcall mmcByte
 
-	ldi temp2,0
-	rcall mmcWaitResp
+	ldi temp2,0 			;Error Code 0
+	rcall mmcWaitResp  		;Test on CMD0 is OK
 
-	sbi PORTD,mmc_cs
+	sbi P_MMC_CS,mmc_cs		;disable /CS
 	rcall mmcByteNoSend
 
 
 ;Read OCR till card is ready
-	ldi temp2,150
+	ldi temp2,20			;repeat counter
 mmcInitOcrLoop:	
 	push temp2
 
-	cbi PORTD,mmc_cs
+	cbi P_MMC_CS,mmc_cs		;enable /CS
 	ldi temp,0xff	;dummy
 	rcall mmcByte
 	ldi temp,0x41	;cmd
@@ -937,54 +1443,59 @@ mmcInitOcrLoop:
 	rcall mmcByte
 	ldi temp,0	;pyl
 	rcall mmcByte
-	ldi temp,0x95	;crc
+;	ldi temp,0x95			;crc
+	ldi temp,0x01			;crc
 	rcall mmcByte
 	rcall mmcByteNoSend
 
 	ldi temp2,1
-	rcall mmcWaitResp
+	rcall mmcWaitResp		;wait until mmc-card send a byte <> 0xFF
+							;the first answer must be 0x01 (Idle-Mode)
 	cpi temp,0
-	breq mmcInitOcrLoopDone
+	breq mmcInitOcrLoopDone ;second answer is 0x00 (Idle-Mode leave) CMD1 is OK
 
-	sbi PORTD,mmc_cs
-	rcall mmcByteNoSend
+	sbi P_MMC_CS,mmc_cs		;disable /CS
+
+;	rcall mmcByteNoSend     ;unnecessary
+
+	ldi	temp,10
+	rcall	delay_ms
 	
 	pop temp2
 	dec temp2
 	cpi temp2,0
-	brne mmcInitOcrLoop
+	brne mmcInitOcrLoop		;repeat 
 
-	ldi temp,4
+	ldi temp2,4  
 	rjmp mmcWaitErr
 
 mmcInitOcrLoopDone:
 	pop temp2
-	sbi PORTD,mmc_cs
+	sbi P_MMC_CS,mmc_cs  		;disable /CS
 	rcall mmcByteNoSend
 
-	ldi temp,0
-	out SPCR,temp
+	out SPCR,_0
 	ret
 
 
-;Call this with adrh:adrl = sector number
-;16bit lba address means a max reach of 32M.
+;Call this with yh:yl:xh:xl = sector number
+;
 mmcReadSect:
 	ldi temp,0x50
 	out SPCR,temp
 
-	cbi PORTD,mmc_cs
+	cbi P_MMC_CS,mmc_cs
 	rcall mmcByteNoSend
 	ldi temp,0x51	;cmd (read sector)
 	rcall mmcByte
-	ldi temp,0
-	lsl adrl
-	rol adrh
-	rol temp
+	lsl	xl			;convert to byte address (*512)
+	rol	xh
+	rol 	yl
+	mov	temp,yl
 	rcall mmcByte
-	mov temp,adrh ;pxl
+	mov temp,xh ;pxl
 	rcall mmcByte
-	mov temp,adrl ;pyh
+	mov temp,xl ;pyh
 	rcall mmcByte
 	ldi temp,0  ;pyl
 	rcall mmcByte
@@ -1002,47 +1513,46 @@ mmcReadSect:
 	rcall mmcWaitResp
 
 	;Read sector to AVR RAM
-	ldi zl,low(sectbuff)
-	ldi zh,high(sectbuff)
+	ldi zl,low(hostbuf)
+	ldi zh,high(hostbuf)
 mmcreadloop:
 	rcall mmcByteNoSend
 	st z+,temp
-	cpi zl,low(sectbuff+512)
+	cpi zl,low(hostbuf+512)
 	brne mmcreadloop
-	cpi zh,high(sectbuff+512)
+	cpi zh,high(hostbuf+512)
 	brne mmcreadloop
 
 	;CRC
 	rcall mmcByteNoSend
 	rcall mmcByteNoSend
 
-	sbi PORTD,mmc_cs
+	sbi P_MMC_CS,mmc_cs
 	rcall mmcByteNoSend
 
-	ldi temp,0
-	out SPCR,temp
+	out SPCR,_0
 	ret
 
 
-;Call this with adrh:adrl = sector number
-;16bit lba address means a max reach of 32M.
+;Call this with yh:yl:xh:xl = sector number
+;
 mmcWriteSect:
 	ldi temp,0x50
 	out SPCR,temp
 
-	cbi PORTD,mmc_cs
+	cbi P_MMC_CS,mmc_cs
 	rcall mmcByteNoSend
 
 	ldi temp,0x58	;cmd (write sector)
 	rcall mmcByte
-	ldi temp,0
-	lsl adrl
-	rol adrh
-	rol temp
+	lsl	xl			;convert to byte address (*512)
+	rol	xh
+	rol 	yl
+	mov	temp,yl
 	rcall mmcByte
-	mov temp,adrh ;pxl
+	mov temp,xh ;pxl
 	rcall mmcByte
-	mov temp,adrl ;pyh
+	mov temp,xl ;pyh
 	rcall mmcByte
 	ldi temp,0  ;pyl
 	rcall mmcByte
@@ -1060,14 +1570,14 @@ mmcWriteSect:
 	rcall mmcByte
 
 	;Write sector from AVR RAM
-	ldi zl,low(sectbuff)
-	ldi zh,high(sectbuff)
+	ldi zl,low(hostbuf)
+	ldi zh,high(hostbuf)
 mmcwriteloop:
 	ld temp,z+
 	rcall mmcByte
-	cpi zl,low(sectbuff+512)
+	cpi zl,low(hostbuf+512)
 	brne mmcwriteloop
-	cpi zh,high(sectbuff+512)
+	cpi zh,high(hostbuf+512)
 	brne mmcwriteloop
 
 	;CRC
@@ -1083,239 +1593,591 @@ mmcwaitwritten:
 	cpi temp,0xff
 	brne mmcwaitwritten
 
-	sbi PORTD,mmc_cs
+	sbi P_MMC_CS,mmc_cs
 	rcall mmcByteNoSend
 
-	ldi temp,0
-	out SPCR,temp
+	out SPCR,_0
 	ret
 
 
 ;Set up wdt to time out after 1 sec.
 resetAVR:
 	cli
-#if defined __ATmega8__
 	ldi temp,(1<<WDCE)
-	out WDTCSR,temp
+	outm8 WDTCSR,temp
 	ldi temp,(1<<WDCE) | (1<<WDE) | (110<<WDP0)
-	out WDTCSR,temp
-#else
-	ldi temp,(1<<WDCE)
-	sts WDTCSR,temp
-	ldi temp,(1<<WDCE) | (1<<WDE) | (110<<WDP0)
-	sts WDTCSR,temp
-#endif
+	outm8 WDTCSR,temp
 resetwait:
 	rjmp resetwait
 
-
 ; ------------------ DRAM routines -------------
 
-;Sends the address in zh:zl to the ram
-dram_setaddr:
-	push temp
-	in temp,PORTD
-	andi temp,0x17
-	out PORTD,temp
-	in temp,PORTB
-	andi temp,0xE0
-	out PORTB,temp
-	sbrc zl,0
-	 sbi PORTB,ram_a0
-	sbrc zl,1
-	 sbi PORTB,ram_a1
-	sbrc zl,2
-	 sbi PORTB,ram_a2
-	sbrc zl,3
-	 sbi PORTB,ram_a3
-	sbrc zl,4
-	 sbi PORTB,ram_a4
-	sbrc zl,5
-	 sbi PORTD,ram_a5
-	sbrc zl,6
-	 sbi PORTD,ram_a6
-	sbrc zl,7
-	 sbi PORTD,ram_a7
-	sbrc zh,0
-	 sbi PORTD,ram_a8
-	pop temp
-	ret
-
-dram_getnibble:
-	andi temp,0xf0
-	sbic pinc,ram_d1
-	 ori temp,0x1
-	sbic pinc,ram_d2
-	 ori temp,0x2
-	sbic pinc,ram_d3
-	 ori temp,0x4
-	sbic pinc,ram_d4
-	 ori temp,0x8
-	ret
+; DRAM_SETADDR val, low_and_mask, low_or_mask, high_and_mask, high_or_mask
+.macro DRAM_SETADDR
+	mov temp,@0
+.if low(@1) != 0xff
+	andi temp,@1
+.endif
+.if  low(@2) != 0
+	ori temp, @2
+.endif
+	out P_AL,temp
+	
+	mov temp,@0
+.if low(@3) != 0xff
+	andi temp,@3
+.endif
+	ori temp, @4 | (1<<mmc_cs)
+	out P_AH,temp
+.endm
 
-dram_sendnibble:
-	push temp2
-	in temp2,PORTC
-	andi temp2,0xE2
-
-	sbrc temp,0
-	 ori temp2,(1<<ram_d1)
-	sbrc temp,1
-	 ori temp2,(1<<ram_d2)
-	sbrc temp,2
-	 ori temp2,(1<<ram_d3)
-	sbrc temp,3
-	 ori temp2,(1<<ram_d4)
+;Loads the byte on address xh:xl into temp.
+;must not alter xh:xl
 
-	out PORTC,temp2
-	pop temp2
+dram_read:
+	cli
+	DRAM_SETADDR xh, ~0,(1<<ram_ras), ~(1<<ram_a8), (1<<ram_oe)
+	cbi P_RAS,ram_ras
+	DRAM_SETADDR xl, ~(1<<ram_ras),0, ~((1<<ram_oe)), (1<<ram_a8)
+	cbi P_CAS,ram_cas
+	cbi P_A8,ram_a8
+	dram_wait DRAM_WAITSTATES	;
+	in  temp,P_DQ-2		; PIN
+	sbi P_CAS,ram_cas
+
+	cbi P_CAS,ram_cas
+	andi temp,0x0f
+	swap temp
+	dram_wait DRAM_WAITSTATES	;
+	in  temp2,P_DQ-2	; PIN
+	andi temp2,0x0f
+	or  temp,temp2
+
+	sbi P_OE,ram_oe
+	sbi P_CAS,ram_cas
+	sbi P_RAS,ram_ras
+	sei
 	ret
 
+#if DRAM_WORD_ACCESS
+dram_read_w:
+	cpi xl,255
+	brne dram_read_w1
+	
+	rcall dram_read
+	push temp
+	adiw xl,1
+	rcall dram_read
+	mov temp2,temp
+	pop temp
+	ret	
 
-;Loads the byte on address adrh:adrl into temp.
-dram_read:
+dram_read_w1:
 	cli
-	mov zl,adrh
-	ldi zh,0
-	mov temp2,adrl
-	lsl temp2
-	rol zl
-	rol zh
-	;z=addr[15-7]
-	rcall dram_setaddr
-	cbi PORTB,ram_ras
-
-	ldi zh,0
-	mov zl,adrl
-	andi zl,0x7F
-	rcall dram_setaddr
-	nop
-	cbi PORTC,ram_cas
+	DRAM_SETADDR xh, ~0,(1<<ram_ras), ~(1<<ram_a8),(1<<ram_oe)
+	cbi P_RAS,ram_ras
+	DRAM_SETADDR xl, ~(1<<ram_ras),0, ~((1<<ram_oe)), (1<<ram_a8)
+	cbi P_CAS,ram_cas
+	cbi P_A8,ram_a8
 	nop
+	in  temp,P_DQ-2		; PIN
+	sbi P_CAS,ram_cas
+	cbi P_CAS,ram_cas
+	andi temp,0x0f
+	swap temp
 	nop
-	cbi PORTD,ram_oe
+	in  temp2,P_DQ-2	; PIN
+	sbi P_CAS,ram_cas
+	andi temp2,0x0f
+	or  temp,temp2
+	
+;	push temp
+	mov _wl,temp
+	inc xl
+	DRAM_SETADDR xl, ~(1<<ram_ras),0, ~((1<<ram_oe)), (1<<ram_a8)
+	cbi P_CAS,ram_cas
+	cbi P_A8,ram_a8
 	nop
-	rcall dram_getnibble	
-	sbi PORTD,ram_oe
+	in  temp,P_DQ-2		; PIN
+	sbi P_CAS,ram_cas
+	cbi P_CAS,ram_cas
+	andi temp,0x0f
 	swap temp
+	nop
+	in  temp2,P_DQ-2	; PIN
+	sbi P_CAS,ram_cas
+	andi temp2,0x0f
+	or  temp2,temp
+;	pop temp
+	mov temp,_wl
+
+	sbi P_OE,ram_oe
+	sbi P_RAS,ram_ras
+	sei
+	ret
+#endif
+
+;Writes the byte in temp to  xh:xl
+;must not alter xh:xl
+
+dram_write:
+	cli
+	ldi temp2,RAM_DQ_MASK | (1<<ram_w) | (1<<ram_cas)
+	out DDRC,temp2
+
+	mov  temp2,temp
+	andi temp,RAM_DQ_MASK & ~(1<<ram_w)
+	ori temp,(1<<ram_cas)
+	out PORTC,temp
+	DRAM_SETADDR xh, ~0,(1<<ram_ras), ~(1<<ram_a8),(1<<ram_oe)
+	cbi P_RAS,ram_ras
+	DRAM_SETADDR xl, ~(1<<ram_ras),0, ~((1<<ram_a8)),(1<<ram_oe)
+	cbi PORTC,ram_cas
 	sbi PORTC,ram_cas
 
-	ldi zh,0
-	mov zl,adrl
-	ori zl,0x80
-	rcall dram_setaddr
-	nop
+	sbi PORTD,ram_a8
+	swap temp2
+
+	andi temp2,RAM_DQ_MASK & ~(1<<ram_w)
+	ori temp2,(1<<ram_cas)
+	out PORTC,temp2
+
 	cbi PORTC,ram_cas
-	nop
-	cbi PORTD,ram_oe
-	nop
-	nop
-	rcall dram_getnibble	
+	sbi P_RAS,ram_ras
 
-	sbi PORTD,ram_oe
-	sbi PORTC,ram_cas
-	sbi PORTB,ram_ras
+	ldi temp,~RAM_DQ_MASK | (1<<ram_w) | (1<<ram_cas)
+	out DDRC,temp
+	out PORTC,temp
 	sei
 	ret
 
-;Writes the byte in temp to  adrh:adrl
-dram_write:
+#if DRAM_WORD_ACCESS
+dram_write_w:
+	cpi xl,255
+	brne dram_write_w1
+	
+	push temp2
+	rcall dram_write
+	pop temp
+	adiw xl,1
+	rcall dram_write
+	ret	
+
+dram_write_w1:
 	cli
+	push temp2
+	ldi temp2,RAM_DQ_MASK | (1<<ram_w) | (1<<ram_cas)
+	out DDRC,temp2
 
-	in temp2,ddrc
-	ori temp2,0x1d
-	out ddrc,temp2
+	mov  temp2,temp
+	andi temp,RAM_DQ_MASK & ~(1<<ram_w)
+	ori temp,(1<<ram_cas)
+	out PORTC,temp
 
-	rcall dram_sendnibble
+	DRAM_SETADDR xh, ~0,(1<<ram_ras), ~(1<<ram_a8),(1<<ram_oe)
+	cbi P_RAS,ram_ras
+	DRAM_SETADDR xl, ~(1<<ram_ras),0, ~((1<<ram_a8)),(1<<ram_oe)
+	cbi PORTC,ram_cas
+	sbi PORTC,ram_cas
 
-	mov zl,adrh
-	ldi zh,0
-	mov temp2,adrl
-	lsl temp2
-	rol zl
-	rol zh
-	;z=addr[15-7]
-	rcall dram_setaddr
-	nop
-	nop
-	cbi PORTB,ram_ras
+	sbi PORTD,ram_a8
+	swap temp2
+
+	andi temp2,RAM_DQ_MASK & ~(1<<ram_w)
+	ori temp2,(1<<ram_cas)
+	out PORTC,temp2
 
-	ldi zh,0
-	mov zl,adrl
-	ori zl,0x80
-	rcall dram_setaddr
-	nop
-	nop
 	cbi PORTC,ram_cas
-	nop
-	nop
-	cbi PORTC,ram_w
-	nop
-	nop
-	nop
-	sbi PORTC,ram_w
 	sbi PORTC,ram_cas
 
+	pop temp
+	inc xl
+	mov  temp2,temp
+	andi temp,RAM_DQ_MASK & ~(1<<ram_w)
+	ori temp,(1<<ram_cas)
+	out PORTC,temp
 
-	ldi zh,0
-	mov zl,adrl
-	andi zl,0x7F
-	rcall dram_setaddr
-	swap temp
-	rcall dram_sendnibble
+	DRAM_SETADDR xl, ~(1<<ram_ras),0, ~((1<<ram_a8)),(1<<ram_oe)
 	cbi PORTC,ram_cas
-	nop
-	nop
-	cbi PORTC,ram_w
-	nop
-	nop
-	sbi PORTC,ram_w
-	nop
-	nop
 	sbi PORTC,ram_cas
-	sbi PORTB,ram_ras
 
-	in temp,ddrc
-	andi temp,0xE2
-	out ddrc,temp
-	in temp,PORTC
-	andi temp,0xE2
+	sbi PORTD,ram_a8
+	swap temp2
+
+	andi temp2,RAM_DQ_MASK & ~(1<<ram_w)
+	ori temp2,(1<<ram_cas)
+	out PORTC,temp2
+	cbi PORTC,ram_cas
+
+	sbi P_RAS,ram_ras
+
+	ldi temp,~RAM_DQ_MASK | (1<<ram_w) | (1<<ram_cas)
+	out DDRC,temp
 	out PORTC,temp
 	sei
 	ret
+#endif
 
+; ****************************************************************************
+
+; refresh interupt; exec 2 cbr cycles
 refrint:
-	nop
-;	nop
-;	nop
-	cbi PORTC,ram_cas
-;	nop
-;	nop
-;	nop
-;	nop
-	cbi PORTB,ram_ras
-	nop
-;	nop
-;	nop
-;	nop
-	sbi PORTC,ram_cas
-;	nop
-;	nop
-;	nop
-;	nop
-	sbi PORTB,ram_ras
-;	nop
-;	nop
-;	nop
-;	nop
-;	nop
+				;4      CAS  RAS  
+	cbi P_CAS,ram_cas	;2       1|   1|  
+				;        1|   1|  
+	cbi P_RAS,ram_ras	;2      |0    1|  
+				;       |0    1|  
+	nop			;1      |0   |0   
+;	nop			;1      |0   |0   
+	sbi P_RAS,ram_ras	;2      |0   |0   
+				;       |0   |0   
+	dram_wait DRAM_WAITSTATES-1	;
+;	nop			;1      |0   |0   
+	cbi P_RAS,ram_ras	;2      |0    1|  
+				;       |0    1|  
+	sbi P_CAS,ram_cas	;2      |0   |0   
+				;       |0   |0   
+	sbi P_RAS,ram_ras	;2       1|  |0   
+				;        1|   1|  
+	reti			;4  --> 21 cycles
+
+
+; ------------- system timer 1ms ---------------
+    .dseg
+
+delay_timer:
+	.byte	1
+timer_base:
+timer_ms:
+	.byte	2
+timer_s:
+	.byte	4
+; don't change order here, clock put/get depends on it.
+cntms_out:		; register for ms
+	.byte	2
+utime_io:		; register for uptime. 
+	.byte	4	
+cnt_1ms:
+	.byte	2
+uptime:
+	.byte	4
+timer_top:
+	.equ timer_size = timer_top - timer_base
+	
+	.equ clkofs = cnt_1ms-cntms_out
+	.equ timerofs = cnt_1ms-timer_ms
+ 
+    .cseg	
+sysclockint:
+	push    zl
+	in      zl,SREG
+	push    zl
+	push	zh
+	
+	lds	zl,delay_timer
+	subi	zl,1
+	brcs	syscl1
+	sts	delay_timer,zl
+syscl1:	
+	lds     zl,cnt_1ms
+	lds     zh,cnt_1ms+1
+	adiw	z,1
+	
+	sts	cnt_1ms,zl
+	sts	cnt_1ms+1,zh
+	cpi	zl,low(1000)
+	ldi	zl,high(1000)		;doesn't change flags
+	cpc	zh,zl
+	brlo	syscl_end
+	
+	sts	cnt_1ms,_0
+	sts	cnt_1ms+1,_0
+
+	lds	zl,uptime+0
+	inc	zl
+	sts	uptime+0,zl
+	brne	syscl_end
+	lds	zl,uptime+1
+	inc	zl
+	sts	uptime+1,zl
+	brne	syscl_end
+	lds	zl,uptime+2
+	inc	zl
+	sts	uptime+2,zl
+	brne	syscl_end
+	lds	zl,uptime+3
+	inc	zl
+	sts	uptime+3,zl
+	
+syscl_end:
+	pop	zh
+	pop     zl
+	out     SREG,zl
+	pop     zl
 	reti
+
+; wait for temp ms
+
+delay_ms:
+	sts	delay_timer,temp
+dly_loop:
+	lds	temp,delay_timer
+	cpi	temp,0
+	brne	dly_loop
+	ret
+
+; 
+
+clockget:
+	ldi	temp,0xFF
+	subi	temp2,TIMER_MSECS
+	brcs	clkget_end		;Port number in range?
+	ldi	zl,low(cntms_out)
+	ldi	zh,high(cntms_out)
+	breq	clkget_copy		;lowest byte requestet, latch clock
+	cpi	temp2,6
+	brsh	clkget_end		;Port number to high?
+	
+	add	zl,temp2
+	brcc	PC+2
+	 inc	zh
+	ld	temp,z
+clkget_end:
+	ret
+	
+	
+		
+clkget_copy:
+	ldi	temp2,6
+	cli
+clkget_l:
+	ldd	temp,z+clkofs
+	st	z+,temp
+	dec	temp2
+	brne	clkget_l
+	sei
+	lds	temp,cntms_out
+					;req. byte in temp
+	ret
+
+clockput:
+	subi	temp2,TIMERPORT
+	brcs	clkput_end		;Port number in range?
+	brne	clkput_1
 	
+	; clock control
+
+	cpi	temp,starttimercmd
+	breq	timer_start
+	cpi	temp,quitTimerCmd
+	breq	timer_quit
+	cpi	temp,printTimerCmd
+	breq	timer_print
+	cpi	temp,uptimeCmd
+	brne	cp_ex
+	rjmp	uptime_print
+cp_ex:
+	ret	
+	
+timer_quit:
+	rcall	timer_print
+	rjmp	timer_start
+
+clkput_1:
+	dec	temp2
+	ldi	zl,low(cntms_out)
+	ldi	zh,high(cntms_out)
+	breq	clkput_copy		;lowest byte requestet, latch clock
+	cpi	temp2,6
+	brsh	clkput_end		;Port number to high?
+	
+	add	zl,temp2
+	 brcc	PC+2
+	inc	zh
+	st	z,temp
+clkput_end:
+	ret
+		
+clkput_copy:
+	st	z,temp
+	adiw	z,5
+	ldi	temp2,6
+	cli
+clkput_l:
+	ldd	temp,z+clkofs
+	st	z+,temp
+	dec	temp2
+	brne	clkput_l
+	sei
+	ret
+
+; start/reset timer
+;
+timer_start:
+	ldi	zl,low(timer_ms)
+	ldi	zh,high(timer_ms)
+	ldi	temp2,6
+	cli
+ts_loop:
+	ldd	temp,z+timerofs
+	st	z+,temp
+	dec	temp2
+	brne	ts_loop
+	sei
+	ret
+
+
+; print timer
+;
+	
+timer_print:
+	push	yh
+	push	yl
+	ldi	zl,low(timer_ms)
+	ldi	zh,high(timer_ms)
+
+; put ms on stack (16 bit)
+
+	cli
+	ldd	yl,z+timerofs
+	ld	temp2,z+
+	sub	yl,temp2
+	ldd	yh,z+timerofs
+	ld	temp2,z+
+	sbc	yh,temp2
+	brsh	tp_s
+	
+	subi	yl,low(-1000)
+	sbci	yh,high(-1000)
+	sec	
+tp_s:
+	push	yh
+	push	yl
+
+	ldd	temp,z+timerofs
+	ld	yl,z+
+	sbc	temp,yl
+
+	ldd	temp2,z+timerofs
+	ld	yh,z+
+	sbc	temp2,yh
+
+	ldd	temp3,z+timerofs
+	ld	yl,z+
+	sbc	temp3,yl
+
+	sei
+	ldd	temp4,z+timerofs
+	ld	yh,z+
+	sbc	temp4,yh
+	
+	rcall printstr
+	.db 13,"Timer running. Elapsed: ",0
+	rcall	print_ultoa
+
+	rcall printstr
+	.db ".",0
+	pop	temp
+	pop	temp2
+	ldi	temp3,0
+	ldi	temp4,0
+	rcall	print_ultoa
+	rcall printstr
+	.db "s.",0,0
+
+	pop	yl
+	pop	yh
+	ret
+	
+uptime_print:
+
+	ldi	zl,low(cnt_1ms)
+	ldi	zh,high(cnt_1ms)
+	
+	cli
+	ld	temp,z+
+	push	temp
+	ld	temp,z+
+	push	temp
+	
+	ld	temp,z+
+	ld	temp2,z+
+	ld	temp3,z+
+	sei
+	ld	temp4,z+
+	
+	rcall printstr
+	.db 13,"Uptime: ",0
+	
+	rcall	print_ultoa
+	rcall printstr
+	.db ",",0
+
+	ldi	temp3,0
+	ldi	temp4,0
+	pop	temp2
+	pop	temp
+	rcall print_ultoa
+	rcall printstr
+	.db "s.",0,0
 
+	ret
 
 
+	
 ; --------------- Debugging stuff ---------------
 
+;Print a unsigned lonng value to the uart
+; temp4:temp3:temp2:temp = value
+
+print_ultoa:
+	push	yh
+	push	yl
+	push	z_flags
+				
+	clr	yl		;yl = stack level
+
+ultoa1:	ldi	z_flags, 32	;yh = temp4:temp % 10
+	clr	yh		;temp4:temp /= 10
+ultoa2:	lsl	temp	
+	rol	temp2	
+	rol	temp3	
+	rol	temp4	
+	rol	yh	
+	cpi	yh,10	
+	brcs	ultoa3	
+	subi	yh,10	
+	inc	temp
+ultoa3:	dec	z_flags	
+	brne	ultoa2
+	cpi	yh, 10	;yh is a numeral digit '0'-'9'
+	subi	yh, -'0'
+	push	yh		;Stack it
+	inc	yl	
+	cp	temp,_0		;Repeat until temp4:temp gets zero
+	cpc	temp2,_0
+	cpc	temp3,_0
+	cpc	temp4,_0
+	brne	ultoa1	
+	
+	ldi	temp, '0'
+ultoa5:	cpi	yl,3		; at least 3 digits (ms)
+	brge	ultoa6
+	push	temp	
+	inc	yl
+	rjmp	ultoa5
+
+ultoa6:	pop	temp		;Flush stacked digits
+	rcall	uartputc
+	dec	yl	
+	brne	ultoa6	
+
+	pop	z_flags
+	pop	yl
+	pop	yh
+	ret
+
+
 ;Prints the lower nibble of temp in hex to the uart
 printhexn:
 	push temp
@@ -1340,15 +2202,21 @@ printhex:
 	rcall printhexn
 	ret
 
-;Prints the zero-terminated string following the call statement. WARNING: Destroys temp.
+;Prints the zero-terminated string following the call statement. 
+
 printstr:
-	pop zh
-	pop zl
-	push temp
+	push	zh
+	push	zl
+	push	yh
+	push	yl
+	push	temp
+	in	r29,sph
+	in	r28,spl
+	ldd	zl,y+7
+	ldd	zh,y+6
 
 	lsl zl
 	rol zh
-
 printstr_loop:
 	lpm temp,z+
 	cpi temp,0
@@ -1365,49 +2233,181 @@ printstr_end:
 	lsr zh
 	ror zl
 
-	pop temp
-	push zl
-	push zh
+	std	y+7,zl
+	std	y+6,zh
+	pop	temp
+	pop	yl
+	pop	yh
+	pop	zl
+	pop	zh
 	ret
 	
-
 ; --------------- AVR HW <-> Z80 periph stuff ------------------
 
 .equ memReadByte	=	dram_read
 .equ memWriteByte	=	dram_write
+#if DRAM_WORD_ACCESS
+.equ memReadWord	=	dram_read_w
+.equ memWriteWord	=	dram_write_w
+#endif
+
+; --------------------------------------------------------------
+
+	.dseg
+	
+#define RXBUFMASK  RXBUFSIZE-1
+#define TXBUFMASK  TXBUFSIZE-1
+
+rxcount:
+	.byte	1
+rxidx_w:
+	.byte	1
+rxidx_r:
+	.byte	1
+txcount:
+	.byte	1
+txidx_w:
+	.byte	1
+txidx_r:
+	.byte	1
+rxfifo:
+	.byte	RXBUFSIZE
+txfifo:
+	.byte	TXBUFSIZE
+
+ramtop:		
+	.cseg
+
+; Save received character in a circular buffer. Do nothing if buffer overflows.
+
+rxint:
+	push	temp
+	in	temp,sreg
+	push	temp
+	push	zh
+	push	zl
+	inm8	temp,RXTXDR0
+	lds	zh,rxcount		;if rxcount < RXBUFSIZE
+	cpi	zh,RXBUFSIZE		;   (room for at least 1 char?)
+	brsh	rxi_ov			; 
+	inc	zh			;
+	sts	rxcount,zh		;   rxcount++
+
+	ldi	zl,low(rxfifo)		;  
+	lds	zh,rxidx_w		;
+	add	zl,zh			;
+	inc	zh			;
+	andi	zh,RXBUFMASK		;
+	sts	rxidx_w,zh		;   rxidx_w = ++rxidx_w % RXBUFSIZE
+	ldi	zh,high(rxfifo)		;
+	brcc	PC+2			;
+	inc	zh			;
+	st	z,temp			;   rxfifo[rxidx_w] = char
+rxi_ov:					;endif
+	pop	zl
+	pop	zh
+	pop	temp
+	out	sreg,temp
+	pop	temp
+	reti
 
 
+;Fetches a char from the buffer to temp. If none available, waits till one is.
 
-;Fetches a char from the uart to temp. If none available, waits till one is.
 uartgetc:
-#if defined __ATmega8__
-	sbis UCSRA,RXC
-	 rjmp uartgetc
-    in  temp,UDR
-#else
-	lds temp,UCSR0A
-	sbrs temp,RXC0
-	 rjmp uartgetc
-	lds temp,UDR0
-#endif
+	lds	temp,rxcount		; Number of characters in buffer
+	tst	temp
+	breq	uartgetc
+	
+	push	zh
+	push	zl
+	ldi	zl,low(rxfifo)
+	ldi	zh,high(rxfifo)
+	lds	temp,rxidx_r
+	add	zl,temp
+	brcc	PC+2
+	inc	zh
+	inc	temp
+	andi	temp,RXBUFMASK
+	sts	rxidx_r,temp
+	cli
+	lds	temp,rxcount
+	dec	temp
+	sts	rxcount,temp
+	sei
+	ld	temp,z		;don't forget to get the char
+	pop	zl
+	pop	zh
 	ret
 
+txint:	
+	push	temp
+	in	temp,sreg
+	push	temp
+	lds	temp,txcount		;if txcount != 0
+	tst	temp			;
+	breq	txi_e			; 
+
+	dec	temp			;
+	sts	txcount,temp		;   --txcount
+	push	zh			;
+	push	zl			;
+	ldi	zl,low(txfifo)		;  
+	ldi	zh,high(txfifo)		;
+	lds	temp,txidx_r		;
+	add	zl,temp			;
+	brcc	PC+2			;
+	inc	zh			;
+	inc	temp			;
+	andi	temp,TXBUFMASK		;
+	sts	txidx_r,temp		;
+	ld	temp,z
+	outm8 RXTXDR0,temp
+	pop	zl
+	pop	zh
+txi_e:					;endif
+	lds	temp,txcount
+	tst	temp
+	brne	txi_x
+	ldi temp, (1<<TXEN0) | (1<<RXEN0) | (1<<RXCIE0)
+	outm8 UCSR0B,temp
+txi_x:
+	pop	temp
+	out	sreg,temp
+	pop	temp
+	reti
+
+
 ;Sends a char from temp to the uart. 
 uartputc:
-#if defined __ATmega8__
-uartputc_l:
-	sbis UCSRA,UDRE
-	 rjmp uartputc_l
-    out UDR,temp
-#else
-	push temp
-uartputc_l:
-	lds temp,UCSR0A
-	sbrs temp,UDRE0
-	 rjmp uartputc_l
-	pop temp
-	sts UDR0,temp
-#endif
+	push	zh
+	push	zl
+	push	temp
+putc_l:
+	lds	temp,txcount		;do {
+	cpi	temp,TXBUFSIZE		;
+	brsh	putc_l			;} while (txcount >= TXBUFSIZE)
+
+	ldi	zl,low(txfifo)		;  
+	ldi	zh,high(txfifo)		;
+	lds	temp,txidx_w		;
+	add	zl,temp			;
+	brcc	PC+2			;
+	inc	zh			;
+	inc	temp			;
+	andi	temp,TXBUFMASK		;
+	sts	txidx_w,temp		;   txidx_w = ++txidx_w % TXBUFSIZE
+	pop	temp			;
+	st	z,temp			;   txfifo[txidx_w] = char
+	cli
+	lds	zl,txcount
+	inc	zl
+	sts	txcount,zl
+	ldi	zl, (1<<UDRIE0) | (1<<TXEN0) | (1<<RXEN0) | (1<<RXCIE0)
+	outm8	UCSR0B,zl
+	sei
+	pop	zl
+	pop	zh
 	ret
 
 ; ------------ Fetch phase stuff -----------------
@@ -1436,26 +2436,26 @@ uartputc_l:
 
 ;Jump table for fetch routines. Make sure to keep this in sync with the .equs!
 fetchjumps:
-.dw do_fetch_nop
-.dw do_fetch_a
-.dw do_fetch_b
-.dw do_fetch_c
-.dw do_fetch_d
-.dw do_fetch_e
-.dw do_fetch_h
-.dw do_fetch_l
-.dw do_fetch_af
-.dw do_fetch_bc
-.dw do_fetch_de
-.dw do_fetch_hl
-.dw do_fetch_sp
-.dw do_fetch_mbc
-.dw do_fetch_mde
-.dw do_fetch_mhl
-.dw do_fetch_msp
-.dw do_fetch_dir8
-.dw do_fetch_dir16
-.dw do_fetch_rst
+	rjmp do_fetch_nop
+	rjmp do_fetch_a
+	rjmp do_fetch_b
+	rjmp do_fetch_c
+	rjmp do_fetch_d
+	rjmp do_fetch_e
+	rjmp do_fetch_h
+	rjmp do_fetch_l
+	rjmp do_fetch_af
+	rjmp do_fetch_bc
+	rjmp do_fetch_de
+	rjmp do_fetch_hl
+	rjmp do_fetch_sp
+	rjmp do_fetch_mbc
+	rjmp do_fetch_mde
+	rjmp do_fetch_mhl
+	rjmp do_fetch_msp
+	rjmp do_fetch_dir8
+	rjmp do_fetch_dir16
+	rjmp do_fetch_rst
 
 do_fetch_nop:
 	ret
@@ -1491,93 +2491,82 @@ do_fetch_l:
 do_fetch_af:
 	mov opl,z_flags
 	mov oph,z_a
-	rcall do_op_calcparity
-	andi opl,~(1<<ZFL_P)
-	sbrs temp2,0
-	 ori opl,(1<<ZFL_P)
 	ret
 
 do_fetch_bc:
-	mov opl,z_c
-	mov oph,z_b
+	movw opl,z_c
 	ret
 
 do_fetch_de:
-	mov opl,z_e
-	mov oph,z_d
+	movw opl,z_e
 	ret
 
 do_fetch_hl:
-	mov opl,z_l
-	mov oph,z_h
+	movw opl,z_l
 	ret
 
 do_fetch_sp:
-	mov opl,z_spl
-	mov oph,z_sph
+	movw opl,z_spl
 	ret
 
 do_fetch_mbc:
-	mov adrh,z_b
-	mov adrl,z_c
+	movw xl,z_c
 	rcall memReadByte
 	mov opl,temp
 	ret
 
 do_fetch_mde:
-	mov adrh,z_d
-	mov adrl,z_e
+	movw xl,z_e
 	rcall memReadByte
 	mov opl,temp
 	ret
 
 do_fetch_mhl:
-	mov adrh,z_h
-	mov adrl,z_l
+	movw xl,z_l
 	rcall memReadByte
 	mov opl,temp
 	ret
 
 do_fetch_msp:
-	mov adrh,z_sph
-	mov adrl,z_spl
+	movw xl,z_spl
+#if DRAM_WORD_ACCESS
+	rcall memReadWord
+	movw opl,temp
+#else
 	rcall memReadByte
 	mov opl,temp
-
-	mov adrh,z_sph
-	mov adrl,z_spl
-	ldi temp,1
-	ldi temp2,0
-	add adrl,temp
-	adc adrh,temp2
+	adiw xl,1
 	rcall memReadByte
 	mov oph,temp
+#endif	
 	ret
 
 do_fetch_dir8:
-	mov adrl,z_pcl
-	mov adrh,z_pch
+	movw xl,z_pcl
 	rcall memReadByte
 	adiw z_pcl,1
 	mov opl,temp
 	ret
 
 do_fetch_dir16:
-	mov adrl,z_pcl
-	mov adrh,z_pch
+	movw xl,z_pcl
+#if DRAM_WORD_ACCESS
+	rcall memReadWord
+	movw opl,temp
+#else
 	rcall memReadByte
 	mov opl,temp
-	adiw z_pcl,1
-	mov adrl,z_pcl
-	mov adrh,z_pch
+	adiw xl,1
 	rcall memReadByte
-	adiw z_pcl,1
 	mov oph,temp
+#endif	
+	adiw z_pcl,2
 	ret
 
 do_fetch_rst:
-	mov adrl,z_pcl
-	mov adrh,z_pch
+	movw xl,z_pcl
+	subi xl,1
+	sbci xh,0
 	rcall memReadByte
 	andi temp,0x38
 	ldi oph,0
@@ -1612,27 +2601,27 @@ do_fetch_rst:
 
 ;Jump table for store routines. Make sure to keep this in sync with the .equs!
 storejumps:
-.dw do_store_nop
-.dw do_store_a
-.dw do_store_b
-.dw do_store_c
-.dw do_store_d
-.dw do_store_e
-.dw do_store_h
-.dw do_store_l
-.dw do_store_af
-.dw do_store_bc
-.dw do_store_de
-.dw do_store_hl
-.dw do_store_sp
-.dw do_store_pc
-.dw do_store_mbc
-.dw do_store_mde
-.dw do_store_mhl
-.dw do_store_msp
-.dw do_store_ret
-.dw do_store_call
-.dw do_store_am
+	rjmp do_store_nop
+	rjmp do_store_a
+	rjmp do_store_b
+	rjmp do_store_c
+	rjmp do_store_d
+	rjmp do_store_e
+	rjmp do_store_h
+	rjmp do_store_l
+	rjmp do_store_af
+	rjmp do_store_bc
+	rjmp do_store_de
+	rjmp do_store_hl
+	rjmp do_store_sp
+	rjmp do_store_pc
+	rjmp do_store_mbc
+	rjmp do_store_mde
+	rjmp do_store_mhl
+	rjmp do_store_msp
+	rjmp do_store_ret
+	rjmp do_store_call
+	rjmp do_store_am
 
 
 do_store_nop:
@@ -1669,10 +2658,6 @@ do_store_l:
 do_store_af:
 	mov z_a,oph
 	mov z_flags,opl
-	ldi temp,0
-	mov parityb,temp
-	sbrs z_flags,ZFL_P
-	 inc parityb
 	ret
 
 do_store_bc:
@@ -1691,72 +2676,61 @@ do_store_hl:
 	ret
 
 do_store_mbc:
-	mov adrh,z_b
-	mov adrl,z_c
+	movw xl,z_c
 	mov temp,opl
 	rcall memWriteByte
 	ret
 
 do_store_mde:
-	mov adrh,z_d
-	mov adrl,z_e
+	movw xl,z_e
 	mov temp,opl
 	rcall memWriteByte
 	ret
 
 do_store_mhl:
-	mov adrh,z_h
-	mov adrl,z_l
+	movw xl,z_l
 	mov temp,opl
 	rcall memWriteByte
 	ret
 
 do_store_msp:
-	mov adrh,z_sph
-	mov adrl,z_spl
+	movw xl,z_spl
+#if DRAM_WORD_ACCESS
+	movw temp,opl
+	rcall memWriteWord
+#else
 	mov temp,opl
 	rcall memWriteByte
-
-	mov adrh,z_sph
-	mov adrl,z_spl
-	ldi temp,1
-	ldi temp2,0
-	add adrl,temp
-	adc adrh,temp2
+	adiw xl,1
 	mov temp,oph
 	rcall memWriteByte
-
+#endif
 	ret
 
 do_store_sp:
-	mov z_sph,oph
-	mov z_spl,opl
+	movw z_spl,opl
 	ret
 
 do_store_pc:
-	mov z_pch,oph
-	mov z_pcl,opl
+	movw z_pcl,opl
 	ret
 
 do_store_ret:
 	rcall do_op_pop16
-	mov z_pcl,opl
-	mov z_pch,oph
+	movw z_pcl,opl
 	ret
 
 do_store_call:
 	push opl
 	push oph
-	mov opl,z_pcl
-	mov oph,z_pch
+	movw opl,z_pcl
 	rcall do_op_push16
 	pop z_pch
 	pop z_pcl
 	ret
 
 do_store_am:
-	mov adrh,oph
-	mov adrl,opl
+	movw xl,opl
 	mov temp,z_a
 	rcall memWriteByte
 	ret
@@ -1805,48 +2779,54 @@ do_store_am:
 .equ OP_DI		= (37<<10)
 .equ OP_EI		= (38<<10)
 .equ OP_INV		= (39<<10)
+.equ OP_CPFA	= (40<<10)
+.equ OP_INCA	= (41<<10)
+.equ OP_DECA	= (42<<10)
 
 opjumps:
-.dw do_op_nop
-.dw do_op_inc
-.dw do_op_dec
-.dw do_op_inc16
-.dw do_op_dec16
-.dw do_op_rlc
-.dw do_op_rrc
-.dw do_op_rr
-.dw do_op_rl
-.dw do_op_adda
-.dw do_op_adca
-.dw do_op_subfa
-.dw do_op_sbcfa
-.dw do_op_anda
-.dw do_op_ora
-.dw do_op_xora
-.dw do_op_addhl
-.dw do_op_sthl
-.dw do_op_rmem16
-.dw do_op_rmem8
-.dw do_op_da
-.dw do_op_scf
-.dw do_op_cpl
-.dw do_op_ccf
-.dw do_op_pop16
-.dw do_op_push16
-.dw do_op_ifnz
-.dw do_op_ifz
-.dw do_op_ifnc
-.dw do_op_ifc
-.dw do_op_ifpo
-.dw do_op_ifpe
-.dw do_op_ifp
-.dw do_op_ifm
-.dw do_op_outa
-.dw do_op_in
-.dw do_op_exhl
-.dw do_op_di
-.dw do_op_ei
-.dw do_op_inv
+	rjmp do_op_nop
+	rjmp do_op_inc
+	rjmp do_op_dec
+	rjmp do_op_inc16
+	rjmp do_op_dec16
+	rjmp do_op_rlc
+	rjmp do_op_rrc
+	rjmp do_op_rr
+	rjmp do_op_rl
+	rjmp do_op_adda
+	rjmp do_op_adca
+	rjmp do_op_subfa
+	rjmp do_op_sbcfa
+	rjmp do_op_anda
+	rjmp do_op_ora
+	rjmp do_op_xora
+	rjmp do_op_addhl
+	rjmp do_op_sthl
+	rjmp do_op_rmem16
+	rjmp do_op_rmem8
+	rjmp do_op_da
+	rjmp do_op_scf
+	rjmp do_op_cpl
+	rjmp do_op_ccf
+	rjmp do_op_pop16
+	rjmp do_op_push16
+	rjmp do_op_ifnz
+	rjmp do_op_ifz
+	rjmp do_op_ifnc
+	rjmp do_op_ifc
+	rjmp do_op_ifpo
+	rjmp do_op_ifpe
+	rjmp do_op_ifp
+	rjmp do_op_ifm
+	rjmp do_op_outa
+	rjmp do_op_in
+	rjmp do_op_exhl
+	rjmp do_op_di
+	rjmp do_op_ei
+	rjmp do_op_inv
+	rjmp do_op_cpfa
+	rjmp do_op_inca
+	rjmp do_op_deca
 
 
 ;How the flags are supposed to work:
@@ -1883,7 +2863,7 @@ opjumps:
 ;|ADD HL,ss |--?-0*|Add                  |HL=HL+ss              |
 ;|ADD IX,pp |--?-0*|Add                  |IX=IX+pp              |
 ;|ADD IY,rr |--?-0*|Add                  |IY=IY+rr              |
-;|AND s     |***P00|Logical AND          |A=A&s                 |
+;|AND s     |**1P00|Logical AND          |A=A&s                 |
 ;|BIT b,m   |?*1?0-|Test Bit             |m&{2^b}               |
 ;|CALL cc,nn|------|Conditional Call     |If cc CALL            |
 ;|CALL nn   |------|Unconditional Call   |-[SP]=PC,PC=nn        |
@@ -1933,7 +2913,7 @@ opjumps:
 ;|LDIR      |--000-|Load, Inc., Repeat   |LDI till BC=0         |
 ;|NEG       |***V1*|Negate               |A=-A                  |
 ;|NOP       |------|No Operation         |                      |
-;|OR s      |***P00|Logical inclusive OR |A=Avs                 |
+;|OR s      |**0P00|Logical inclusive OR |A=Avs                 |
 ;|OTDR      |?1??1-|Output, Dec., Repeat |OUTD till B=0         |
 ;|OTIR      |?1??1-|Output, Inc., Repeat |OUTI till B=0         |
 ;|OUT [C],r |------|Output               |[C]=r                 |
@@ -1953,10 +2933,6 @@ opjumps:
 ;|RLA       |--0-0*|Rotate Left Acc.     |A={CY,A}<-            |
 ;|RLC m     |**0P0*|Rotate Left Circular |m=m<-                 |
 ;|RLCA      |--0-0*|Rotate Left Circular |A=A<-                 |
-;----------------------------------------------------------------
-;----------------------------------------------------------------
-;|Mnemonic  |SZHPNC|Description          |Notes                 |
-;|----------+------+---------------------+----------------------|
 ;|RLD       |**0P0-|Rotate Left 4 bits   |{A,[HL]}={A,[HL]}<- ##|
 ;|RR m      |**0P0*|Rotate Right         |m=->{CY,m}            |
 ;|RRA       |--0-0*|Rotate Right Acc.    |A=->{CY,A}            |
@@ -1972,7 +2948,7 @@ opjumps:
 ;|SRA m     |**0P0*|Shift Right Arith.   |m=m/2                 |
 ;|SRL m     |**0P0*|Shift Right Logical  |m=->{0,m,CY}          |
 ;|SUB s     |***V1*|Subtract             |A=A-s                 |
-;|XOR s     |***P00|Logical Exclusive OR |A=Axs                 |
+;|XOR s     |**0P00|Logical Exclusive OR |A=Axs                 |
 ;|----------+------+--------------------------------------------|
 ;| F        |-*01? |Flag unaffected/affected/reset/set/unknown  |
 ;| S        |S     |Sign flag (Bit 7)                           |
@@ -2036,14 +3012,84 @@ opjumps:
 ;----------------------------------------------------------------
 
 
-;ToDo: Parity at more instructions...
+.equ AVR_T = SREG_T
+.equ AVR_H = SREG_H
+.equ AVR_S = SREG_S
+.equ AVR_V = SREG_V
+.equ AVR_N = SREG_N
+.equ AVR_Z = SREG_Z
+.equ AVR_C = SREG_C
+
+;------------------------------------------------;
+; Move single bit between two registers
+;
+;	bmov	dstreg,dstbit,srcreg.srcbit
+
+.macro	bmov
+	bst	@2,@3
+	bld	@0,@1
+.endm
+
+
+;------------------------------------------------;
+; Load table value from flash indexed by source reg.
+;
+;	ldpmx	dstreg,tablebase,indexreg
+;
+; (6 words, 8 cycles)
+
+.macro	ldpmx
+	ldi	zh,high(@1*2)	; table must be page aligned
+	mov	zl,@2		       
+	lpm	@0,z	
+.endm
+.macro	do_z80_flags_HP
+#if EM_Z80
+	bmov	z_flags, ZFL_P, temp, AVR_V
+	bmov	z_flags, ZFL_H, temp, AVR_H
+#endif
+.endm
+
+.macro	do_z80_flags_set_N
+#if EM_Z80
+	ori	z_flags, (1<<ZFL_N)       ; Negation auf 1
+#endif
+.endm
+
+.macro	do_z80_flags_set_HN
+#if EM_Z80
+	ori 	z_flags,(1<<ZFL_N)|(1<<ZFL_H)
+#endif
+.endm
+
+.macro	do_z80_flags_clear_N
+#if EM_Z80
+	andi	z_flags,~(1<<ZFL_N)
+#endif
+.endm
+
+.macro	do_z80_flags_op_rotate
+	; must not change avr carry flag!
+#if EM_Z80
+	andi   z_flags, ~( (1<<ZFL_H) | (1<<ZFL_N) | (1<<ZFL_C) )
+#else
+	andi   z_flags, ~( (1<<ZFL_C) )
+#endif
+.endm
+
+.macro	do_z80_flags_op_and
+#if EM_Z80
+	ori	z_flags,(1<<ZFL_H)
+#else
+	ori	z_flags,(1<<ZFL_H)
+#endif
+.endm
+
+.macro	do_z80_flags_op_or
+#if EM_Z80
+#endif
+.endm
 
-.equ AVR_H = 5
-.equ AVR_S = 4
-.equ AVR_V = 3
-.equ AVR_N = 2
-.equ AVR_Z = 1
-.equ AVR_C = 0
 
 do_op_nop:
 	ret
@@ -2054,47 +3100,67 @@ do_op_nop:
 ;|INC r     |***V0-|Increment            |r=r+1                 |
 ;|INC [HL]  |***V0-|Increment            |[HL]=[HL]+1           |
 ;|INC [xx+d]|***V0-|Increment            |[xx+d]=[xx+d]+1       |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|INC r     |**-P0-|Increment            |r=r+1                 |
+;|INC [HL]  |**-P0-|Increment            |[HL]=[HL]+1           |
 ;
-; OK
+; 
 do_op_inc:
-	andi  z_flags, (1<<ZFL_C)       ; bis auf Carry alles auf 0
-	ldi   temp, 1
-	add   opl, temp
-	in    temp, sreg
-	mov   parityb, opl
-	bst   temp, AVR_Z               ; Zero
-	bld   z_flags, ZFL_Z
-	sbrc  opl, 7                    ; Sign
-	ori   z_flags, (1<<ZFL_S)       
-	bst   temp, AVR_H               ; Half Sign
-	bld   z_flags, ZFL_H
-  bst   temp, AVR_C               ; Overflow
-  bld   z_flags, ZFL_P
+	inc	opl
+#if EM_Z80
+	in	temp, sreg
+#endif
+	andi	z_flags,(1<<ZFL_H)|(1<<ZFL_C)	; preserve C-, and H-flag
+	ldpmx	temp2, sz53p_tab, opl
+	or	z_flags,temp2		;
+	do_z80_flags_HP
+	ret
+
+do_op_inca:
+	inc	z_a
+#if EM_Z80
+	in	temp, sreg
+#endif
+	andi	z_flags,(1<<ZFL_H)|(1<<ZFL_C)	; preserve C-, and H-flag
+	ldpmx	temp2, sz53p_tab, z_a
+	or	z_flags,temp2		;
+	do_z80_flags_HP
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
-;|DEC s     |***V1-|Decrement            |s=s-1                 |
+;|DEC r     |***V1-|Decrement            |s=s-1                 |
+;|INC [HL]  |***V0-|Increment            |[HL]=[HL]+1           |
+;|INC [xx+d]|***V0-|Increment            |[xx+d]=[xx+d]+1       |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|DEC r     |**-P -|Increment            |r=r+1                 |
+;|DEC [HL]  |**-P -|Increment            |[HL]=[HL]+1           |
+;
 ;
-; OK
 do_op_dec:
-	andi  z_flags, (1<<ZFL_C)       ; bis auf Carry alles auf 0
-	ori   z_flags, (1<<ZFL_N)       ; Negation auf 1
-	ldi   temp, 1
-	sub   opl, temp
+	dec	opl
+#if EM_Z80
 	in    temp, sreg
-	mov   parityb, opl
-	bst   temp, AVR_Z               ; Zero
-	bld   z_flags, ZFL_Z
-	bst   temp, AVR_S               ; Sign
-	bld   z_flags, ZFL_S
-	bst   temp, AVR_H               ; Half Sign
-	bld   z_flags, ZFL_H
-  bst   temp, AVR_C               ; Underflow
-  bld   z_flags, ZFL_P
+#endif
+	andi	z_flags,(1<<ZFL_H)|(1<<ZFL_C)	; preserve C-, and H-flag
+	ldpmx	temp2, sz53p_tab, opl
+	or	z_flags,temp2		;
+	do_z80_flags_HP
+	do_z80_flags_set_N
 	ret
 
+do_op_deca:
+	dec	z_a
+#if EM_Z80
+	in    temp, sreg
+#endif
+	andi	z_flags,(1<<ZFL_H)|(1<<ZFL_C)	; preserve C-, and H-flag
+	ldpmx	temp2, sz53p_tab, z_a
+	or	z_flags,temp2		;
+	do_z80_flags_HP
+	do_z80_flags_set_N
+	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
@@ -2102,12 +3168,10 @@ do_op_dec:
 ;|INC xx    |------|Increment            |xx=xx+1               |
 ;|INC ss    |------|Increment            |ss=ss+1               |
 ;
-; OK
+; 
 do_op_inc16:
-	ldi   temp, 1
-	ldi   temp2, 0
-	add   opl, temp
-	adc   oph, temp2
+	subi	opl,low(-1)
+	sbci	oph,high(-1)
 	ret
 
 ;----------------------------------------------------------------
@@ -2116,24 +3180,24 @@ do_op_inc16:
 ;|DEC xx    |------|Decrement            |xx=xx-1               |
 ;|DEC ss    |------|Decrement            |ss=ss-1               |
 ;
-; OK
+; 
 do_op_dec16:
-	ldi   temp, 1
-	ldi   temp2, 0
-	sub   opl, temp
-	sbc   oph, temp2
+	subi   opl, 1
+	sbci   oph, 0
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
 ;|RLCA      |--0-0*|Rotate Left Circular |A=A<-                 |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|RLCA      |---- *|Rotate Left Circular |A=A<-                 |
+;
 ;
-; OK
 do_op_rlc:
 	;Rotate Left Cyclical. All bits move 1 to the 
 	;left, the msb becomes c and lsb.
-	andi   z_flags, ~( (1<<ZFL_H) | (1<<ZFL_N) | (1<<ZFL_C) )
+	do_z80_flags_op_rotate
 	lsl    opl
 	brcc   do_op_rlc_noc
 	ori    opl, 1
@@ -2145,12 +3209,14 @@ do_op_rlc_noc:
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
 ;|RRCA      |--0-0*|Rotate Right Circular|A=->A                 |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|RRCA      |---- *|Rotate Right Circular|A=->A                 |
+;
 ;
-; OK
 do_op_rrc: 
 	;Rotate Right Cyclical. All bits move 1 to the 
 	;right, the lsb becomes c and msb.
-	andi   z_flags, ~( (1<<ZFL_H) | (1<<ZFL_N) | (1<<ZFL_C) )
+	do_z80_flags_op_rotate
 	lsr    opl
 	brcc   do_op_rrc_noc
 	ori    opl, 0x80
@@ -2162,257 +3228,239 @@ do_op_rrc_noc:
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
 ;|RRA       |--0-0*|Rotate Right Acc.    |A=->{CY,A}            |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|RRA       |---- *|Rotate Right Acc.    |A=->{CY,A}            |
 ;
-; OK
+; 
 do_op_rr: 
 	;Rotate Right. All bits move 1 to the right, the lsb 
 	;becomes c, c becomes msb.
-	clc
+	clc				; get z80 carry to avr carry
 	sbrc    z_flags,ZFL_C
 	sec
+	do_z80_flags_op_rotate		; (clear ZFL_C, doesn't change AVR_C)
+	bmov	z_flags,ZFL_C, opl,0	; Bit 0 --> CY
 	ror     opl
-	in      temp,sreg
-	andi    z_flags,~( (1<<ZFL_H) | (1<<ZFL_N) | (1<<ZFL_C) )
-	bst     temp,AVR_C
-	bld     z_flags,ZFL_C
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|RLA       |--0-0*|Rotate Left Acc.     |A={CY,A}<-            |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|RLA       |---- *|Rotate Left Acc.     |A={CY,A}<-            |
 ;
-; Not yet checked
+; 
 do_op_rl:
 	;Rotate Left. All bits move 1 to the left, the msb 
 	;becomes c, c becomes lsb.
 	clc
 	sbrc z_flags,ZFL_C
 	 sec
+	do_z80_flags_op_rotate		; (clear ZFL_C, doesn't change AVR_C)
+	bmov	z_flags,ZFL_C, opl,7	; Bit 7 --> CY
 	rol opl
-	in temp,sreg
-	andi z_flags,0b11101100
-	bst temp,AVR_C
-	bld z_flags,ZFL_C
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|ADD A,s   |***V0*|Add                  |A=A+s                 |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|ADD A,s   |***P *|Add                  |A=A+s                 |
+;
 ;
-; Not yet checked
 do_op_adda:
-	ldi z_flags,0
-	add opl,z_a
+	add z_a,opl
 	in temp,sreg
-	bst temp,AVR_Z
-	bld z_flags,ZFL_Z
-	bst temp,AVR_S
-	cpi opl,$80
-	brne adda_no_s
-	ori z_flags,(1<<ZFL_S)
-adda_no_s:
-	bst temp,AVR_H
-	bld z_flags,ZFL_H
-	bst temp,AVR_V
-	bld z_flags,ZFL_P
-	bst temp,AVR_C
-	bld z_flags,ZFL_C
+	ldpmx	z_flags,sz53p_tab,z_a		;S,Z,P flag
+	bmov	z_flags,ZFL_C, temp,AVR_C
+	do_z80_flags_HP
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|ADC A,s   |***V0*|Add with Carry       |A=A+s+CY              |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|ADC A,s   |***P *|Add with Carry       |A=A+s+CY              |
+;
 ;
-; Not yet checked
 do_op_adca:
 	clc
 	sbrc z_flags,ZFL_C
 	 sec
-	adc opl,z_a
+	adc z_a,opl
 	in temp,sreg
-	ldi z_flags,0
-	bst temp,AVR_Z
-	bld z_flags,ZFL_Z
-	sbrc opl,7
-	 ori z_flags,(1<<ZFL_S)
-	bst temp,AVR_H
-	bld z_flags,ZFL_H
-	bst temp,AVR_V
-	bld z_flags,ZFL_P
-	bst temp,AVR_C
-	bld z_flags,ZFL_C
-	andi z_flags,~(1<<ZFL_N)
+	ldpmx	z_flags,sz53p_tab,z_a		;S,Z,P
+	bmov	z_flags,ZFL_C, temp,AVR_C
+	do_z80_flags_HP
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|SUB s     |***V1*|Subtract             |A=A-s                 |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|SUB s     |***P *|Subtract             |A=A-s                 |
+
 ;
-; Not yet checked
 do_op_subfa:
+	sub z_a,opl
+	in temp,sreg
+	ldpmx	z_flags,sz53p_tab,z_a		;S,Z,P
+	bmov	z_flags,ZFL_C, temp,AVR_C
+	do_z80_flags_HP
+	do_z80_flags_set_N
+	ret
+
+;----------------------------------------------------------------
+;|Mnemonic  |SZHPNC|Description          |Notes                 |
+;----------------------------------------------------------------
+;|CP s      |***V1*|Compare              |A-s                   |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|CP s      |***P *|Compare              |A-s                   |
+
+;
+do_op_cpfa:
 	mov temp,z_a
 	sub temp,opl
 	mov opl,temp
 	in temp,sreg
-	bst temp,AVR_Z
-	bld z_flags,ZFL_Z
-	bst temp,AVR_S
-	bld z_flags,ZFL_S
-	bst temp,AVR_H
-	bld z_flags,ZFL_H
-	bst temp,AVR_V
-	bld z_flags,ZFL_P
-	bst temp,AVR_C
-	bld z_flags,ZFL_C
-	ori z_flags,(1<<ZFL_N)
+	ldpmx	z_flags,sz53p_tab,opl		;S,Z,P
+	bmov	z_flags,ZFL_C, temp,AVR_C
+	do_z80_flags_HP
+	do_z80_flags_set_N
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|SBC A,s   |***V1*|Subtract with Carry  |A=A-s-CY              |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|SBC A,s   |***P *|Subtract with Carry  |A=A-s-CY              |
+;
 ;
-; Not yet checked
 do_op_sbcfa:
-	mov temp,z_a
 	clc
 	sbrc z_flags,ZFL_C
 	 sec
-	sbc temp,opl
-	mov opl,temp
-	in temp,sreg
-	bst temp,AVR_S
-	bld z_flags,ZFL_S
-	bst temp,AVR_H
-	bld z_flags,ZFL_H
-	bst temp,AVR_V
-	bld z_flags,ZFL_P
-	bst temp,AVR_C
-	bld z_flags,ZFL_C
-	cpi opl,0	;AVR doesn't set Z?
+	sbc z_a,opl
 	in temp,sreg
-	bst temp,AVR_Z
-	bld z_flags,ZFL_Z
-	ori z_flags,(1<<ZFL_N)
+	ldpmx	z_flags,sz53p_tab,z_a		;S,Z,P
+	bmov	z_flags,ZFL_C, temp,AVR_C
+	do_z80_flags_HP
+	do_z80_flags_set_N
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|AND s     |**1P00|Logical AND          |A=A&s                 |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|AND s     |**-P 0|Logical AND          |A=A&s                 |
 ;
-; Not yet checked
+; TODO H-Flag
 do_op_anda:
-	ldi z_flags,0
-	and opl,z_a
-	in temp,sreg
-	bst temp,AVR_Z
-	bld z_flags,ZFL_Z
-	bst temp,AVR_S
-	bld z_flags,ZFL_S
-	bst temp,AVR_H
-	bld z_flags,ZFL_H
-	mov temp,opl
+	and z_a,opl				;
+	ldpmx	z_flags,sz53p_tab,z_a		;S,Z,P,N,C
+	do_z80_flags_op_and
 	ret
 
+
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|OR s      |**0P00|Logical inclusive OR |A=Avs                 |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|OR s      |**-P00|Logical inclusive OR |A=Avs                 |
 ;
-; Not yet checked
+; TODO: H-Flag
 do_op_ora:
-	ldi z_flags,0
-	or opl,z_a
-	in temp,sreg
-	bst temp,AVR_Z
-	bld z_flags,ZFL_Z
-	bst temp,AVR_S
-	bld z_flags,ZFL_S
-	bst temp,AVR_H
-	bld z_flags,ZFL_H
-	mov temp,opl
+	or z_a,opl
+	ldpmx	z_flags,sz53p_tab,z_a		;S,Z,H,P,N,C
+	do_z80_flags_op_or
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|XOR s     |**0P00|Logical Exclusive OR |A=Axs                 |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|XOR s     |**-P 0|Logical Exclusive OR |A=Axs                 |
 ;
-; Not yet checked
+; TODO: H-Flag
 do_op_xora:
-	ldi z_flags,0
-	eor opl,z_a
-	in temp,sreg
-	bst temp,AVR_Z
-	bld z_flags,ZFL_Z
-	bst temp,AVR_S
-	bld z_flags,ZFL_S
-	bst temp,AVR_H
-	bld z_flags,ZFL_H
-	mov temp,opl
+	eor z_a,opl
+	ldpmx	z_flags,sz53p_tab,z_a		;S,Z,H,P,N,C
+	do_z80_flags_op_or
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|ADD HL,ss |--?-0*|Add                  |HL=HL+ss              |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|ADD HL,ss |---- *|Add                  |HL=HL+ss              |
+;
 ;
-; Not yet checked
 do_op_addhl:
 	add opl,z_l
 	adc oph,z_h
 	in temp,sreg
-	bst temp,AVR_C
-	bld z_flags,ZFL_C
-	andi z_flags,~(1<<ZFL_N)
+	bmov	z_flags,ZFL_H, temp,AVR_H
+	bmov	z_flags,ZFL_C, temp,AVR_C
+	do_z80_flags_clear_N
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|LD dst,src|------|Load                 |dst=src               |
 ;
-; Not yet checked
-do_op_sthl: ;store hl to mem loc in opl
-	;ToDo: check flags
-	mov adrl,opl
-	mov adrh,oph
+;
+do_op_sthl: ;store hl to mem loc in opl:h
+	movw xl,opl
+#if DRAM_WORD_ACCESS
+	movw temp,z_l
+	rcall memWriteWord
+#else
 	mov temp,z_l
 	rcall memWriteByte
-
-	ldi temp,1
-	ldi temp2,0
-	add opl,temp
-	adc oph,temp2
-
-	mov adrl,opl
-	mov adrh,oph
+	adiw xl,1
 	mov temp,z_h
 	rcall memWriteByte
-
+#endif
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|LD dst,src|------|Load                 |dst=src               |
 ;
-; Not yet checked
+; 
 do_op_rmem16:
-	mov adrl,opl
-	mov adrh,oph
+	movw xl,opl
+#if DRAM_WORD_ACCESS
+	rcall memReadWord
+	movw opl,temp
+#else
 	rcall memReadByte
 	mov opl,temp
-	ldi temp,1
-	add adrl,temp
-	ldi temp,0
-	adc adrh,temp
+	adiw xl,1
 	rcall memReadByte
 	mov oph,temp
+#endif	
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|LD dst,src|------|Load                 |dst=src               |
+;
 ;
-; Not yet checked
 do_op_rmem8:
-	mov adrl,opl
-	mov adrh,oph
+	movw xl,opl
 	rcall memReadByte
 	mov opl,temp
 	ret
@@ -2420,30 +3468,157 @@ do_op_rmem8:
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|DAA       |***P-*|Decimal Adjust Acc.  |                      |
+;|----------|SZHP C|---------- 8080 ----------------------------|
 ;
 ; Not yet checked
+
+; Description (http://www.z80.info/z80syntx.htm#DAA):
+;  This instruction conditionally adjusts the accumulator for BCD addition
+;  and subtraction operations. For addition (ADD, ADC, INC) or subtraction
+;  (SUB, SBC, DEC, NEC), the following table indicates the operation performed:
+;
+; -------------------------------------------------------------------------------
+; |          | C Flag  | HEX value in | H Flag | HEX value in | Number  | C flag|
+; | Operation| Before  | upper digit  | Before | lower digit  | added   | After |
+; |          | DAA     | (bit 7-4)    | DAA    | (bit 3-0)    | to byte | DAA   |
+; |-----------------------------------------------------------------------------|
+; |          |    0    |     0-9      |   0    |     0-9      |   00    |   0   |
+; |   ADD    |    0    |     0-8      |   0    |     A-F      |   06    |   0   |
+; |          |    0    |     0-9      |   1    |     0-3      |   06    |   0   |
+; |   ADC    |    0    |     A-F      |   0    |     0-9      |   60    |   1   |
+; |          |    0    |     9-F      |   0    |     A-F      |   66    |   1   |
+; |   INC    |    0    |     A-F      |   1    |     0-3      |   66    |   1   |
+; |          |    1    |     0-2      |   0    |     0-9      |   60    |   1   |
+; |          |    1    |     0-2      |   0    |     A-F      |   66    |   1   |
+; |          |    1    |     0-3      |   1    |     0-3      |   66    |   1   |
+; |-----------------------------------------------------------------------------|
+; |   SUB    |    0    |     0-9      |   0    |     0-9      |   00    |   0   |
+; |   SBC    |    0    |     0-8      |   1    |     6-F      |   FA    |   0   |
+; |   DEC    |    1    |     7-F      |   0    |     0-9      |   A0    |   1   |
+; |   NEG    |    1    |     6-F      |   1    |     6-F      |   9A    |   1   |
+; |-----------------------------------------------------------------------------|
+;
+; Flags:
+;     C:   See instruction.
+;     N:   Unaffected.
+;     P/V: Set if Acc. is even parity after operation, reset otherwise.
+;     H:   See instruction.
+;     Z:   Set if Acc. is Zero after operation, reset otherwise.
+;     S:   Set if most significant bit of Acc. is 1 after operation, reset otherwise.
+
+
+
+#if 1
 do_op_da:
-	;DAA -> todo
-	rcall do_op_inv
-	mov temp,opl
+	ldi 	oph,0				; what to add
+	sbrc	z_flags,ZFL_H			; if H-Flag
+	rjmp	op_da_06
+	mov	temp,opl
+	andi	temp,0x0f			; ... or lower digit > 9
+	cpi	temp,0x0a
+	brlo	op_da_06n
+op_da_06:				
+	ori	oph,0x06
+op_da_06n:				
+	sbrc	z_flags,(1<<ZFL_C)
+	rjmp	op_da_60
+	cpi	opl,0xa0
+	brlo	op_da_60n
+op_da_60:				
+	ori	oph,0x60
+op_da_60n:				
+	cpi	opl,0x9a
+	brlo	op_da_99n
+	ori	z_flags,(1<<ZFL_C); set C
+op_da_99n:
+	sbrs	z_flags,ZFL_N			; if sub-op
+	rjmp	op_da_add			; then
+	sub	opl,oph
+	rjmp	op_da_ex
+op_da_add:					; else add-op
+	cpi	opl,0x91
+	brlo	op_da_60n2
+	mov	temp,opl
+	andi	temp,0x0f
+	cpi	temp,0x0a
+	brlo	op_da_60n2
+	ori	oph,0x60
+op_da_60n2:
+	add	opl,oph
+op_da_ex:
+	in	temp,SREG	
+	sbrc	temp,AVR_H
+	ori	z_flags,(1<<ZFL_C)
+	andi	z_flags,(1<<ZFL_N)|(1<<ZFL_C)	; preserve C,N
+	ldpmx	temp2, sz53p_tab, opl		; get S,Z,P
+	or	z_flags,temp2
+	bmov	z_flags,ZFL_H, temp,AVR_H	; H  (?)
 	ret
+#else
 
+do_op_da:
+	sbrc	z_flags,ZFL_N			; if add-op	
+	rjmp	do_op_da_sub			; then
+	ldi		temp2,0			;
+	mov		temp,opl		;
+	andi	temp,0x0f			;
+	cpi		temp,0x0a		;	if lower digit > 9
+	brlo	do_op_da_h			;
+	ori		temp2,0x06		;		add 6 to lower digit
+do_op_da_h:					;
+	sbrc	z_flags,ZFL_H			;   ... or H-Flag
+	ori		temp2,0x06		;
+	add		opl,temp2		;
+
+	ldi		temp2,0			;
+	mov		temp,opl		;
+	andi	temp,0xf0			;
+	cpi		temp,0xa0		;
+	brlo	do_op_da_c			;
+	ori		temp2,0x60		;
+do_op_da_c:					; else sub-op
+	sbrc	z_flags,ZFL_C			;
+	ori		temp2,0x60		;
+	andi	z_flags, ~( (1<<ZFL_S) | (1<<ZFL_Z) | (1<<ZFL_H) )
+	add		opl,temp2		;
+	in		temp,SREG		;
+	bst		temp,AVR_Z		;Z-Flag
+	bld		z_flags,ZFL_Z		;
+	bst		temp,AVR_N		;S-Flag
+	bst		z_flags,ZFL_S		;
+	sbrc	temp2,5				;C-Flag, set if 0x06 added
+	ori		z_flags,(1<<ZFL_C)	;
+						;H-Flag?
+	ret
+	
+do_op_da_sub:					;TODO:
+	rcall do_op_inv
+	ret
+#endif
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|SCF       |--0-01|Set Carry Flag       |CY=1                  |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;
 ;
-; Not yet checked
 do_op_scf:
-	ori z_flags,(1<<ZFL_C)
+	andi	z_flags,~((1<<ZFL_H)|(1<<ZFL_N))
+	ori 	z_flags,(1<<ZFL_C)
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|CCF       |--?-0*|Complement Carry Flag|CY=~CY                |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|SCF       |---- 1|Set Carry Flag       |CY=1                  |
 ;
-; Not yet checked
+;TODO: H-Flag
 do_op_ccf:
+	do_z80_flags_clear_N
 	ldi temp,(1<<ZFL_C)
 	eor z_flags,temp
 	ret
@@ -2451,38 +3626,39 @@ do_op_ccf:
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|CPL       |--1-1-|Complement           |A=~A                  |
+;|----------|SZHP C|---------- 8080 ----------------------------|
+;|CPL       |---- -|Complement           |A=~A                  |
+;
 ;
-; Not yet checked
 do_op_cpl:
-	com opl
-	ori z_flags,(1<<ZFL_N)|(1<<ZFL_H)
+	com z_a
+	do_z80_flags_set_HN
 	ret
 
+
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|PUSH xx   |------|Push                 |-[SP]=xx              |
+;|PUSH qq   |------|Push                 |-[SP]=qq              |
+;
 ;
-; Not yet checked
 do_op_push16:
-	ldi temp,1
-	ldi temp2,0
-	sub z_spl,temp
-	sbc z_sph,temp2
-
-	mov adrl,z_spl
-	mov adrh,z_sph
-	mov temp,oph
-	rcall memWriteByte
-
-	ldi temp,1
-	ldi temp2,0
-	sub z_spl,temp
-	sbc z_sph,temp2
-
-	mov adrl,z_spl
-	mov adrh,z_sph
+	movw xl,z_spl
+	subi xl,2
+	sbci xh,0
+	movw z_spl,xl
+#if DRAM_WORD_ACCESS	
+	movw temp,opl
+	rcall memWriteWord
+#else
 	mov temp,opl
 	rcall memWriteByte
+	adiw xl,1
+	mov temp,oph
+	rcall memWriteByte
+#endif
 
 .if STACK_DBG
 	rcall printstr
@@ -2506,28 +3682,27 @@ do_op_push16:
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|POP xx    |------|Pop                  |xx=[SP]+              |
+;|POP qq    |------|Pop                  |qq=[SP]+              |
+;
 ;
-; Not yet checked
 do_op_pop16:
-	mov adrl,z_spl
-	mov adrh,z_sph
+	movw xl,z_spl
+#if DRAM_WORD_ACCESS
+	rcall memReadWord
+	movw opl,temp
+#else
 	rcall memReadByte
 	mov opl,temp
-
-	ldi temp,1
-	ldi temp2,0
-	add z_spl,temp
-	adc z_sph,temp2
-
-	mov adrl,z_spl
-	mov adrh,z_sph
+	adiw xl,1
 	rcall memReadByte
 	mov oph,temp
+#endif	
 
-	ldi temp,1
-	ldi temp2,0
+	ldi temp,2
 	add z_spl,temp
-	adc z_sph,temp2
+	adc z_sph,_0
+
 
 .if STACK_DBG
 	rcall printstr
@@ -2550,8 +3725,10 @@ do_op_pop16:
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|EX [SP],HL|------|Exchange             |[SP]<->HL             |
+;|EX DE,HL  |------|Exchange             |DE<->HL               |
 ;
-; Not yet checked
+; 
 do_op_exhl:
 	mov temp,z_h
 	mov z_h,oph
@@ -2565,7 +3742,7 @@ do_op_exhl:
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
 ;
-; Not yet checked
+; TODO: Implement IFF1, IFF2
 do_op_di:
 	ret
 
@@ -2573,113 +3750,136 @@ do_op_di:
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
 ;
-; Not yet checked
+; TODO: Implement IFF1, IFF2
 do_op_ei:
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|CALL cc,nn|------|Conditional Call     |If cc CALL            |
+;|JP cc,nn  |------|Conditional Jump     |If cc JP              |
+;|RET cc    |------|Conditional Return   |If cc RET             |
+;
 ;
-; Not yet checked
 do_op_ifnz:
 	sbrs z_flags, ZFL_Z
 	ret
-	ldi insdech, 0
-	ldi insdecl, 0
+	clr insdech
+	clr insdecl
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|CALL cc,nn|------|Conditional Call     |If cc CALL            |
+;|JP cc,nn  |------|Conditional Jump     |If cc JP              |
+;|RET cc    |------|Conditional Return   |If cc RET             |
+;
 ;
-; Not yet checked
 do_op_ifz:
 	sbrc z_flags, ZFL_Z
 	ret
-	ldi insdech, 0
-	ldi insdecl, 0
+	clr insdech
+	clr insdecl
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|CALL cc,nn|------|Conditional Call     |If cc CALL            |
+;|JP cc,nn  |------|Conditional Jump     |If cc JP              |
+;|RET cc    |------|Conditional Return   |If cc RET             |
+;
 ;
-; Not yet checked
 do_op_ifnc:
 	sbrs z_flags, ZFL_C
-  ret
-	ldi insdech, 0
-	ldi insdecl, 0
+	ret
+	clr insdech
+	clr insdecl
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|CALL cc,nn|------|Conditional Call     |If cc CALL            |
+;|JP cc,nn  |------|Conditional Jump     |If cc JP              |
+;|RET cc    |------|Conditional Return   |If cc RET             |
+;
 ;
-; Not yet checked
 do_op_ifc:
 	sbrc z_flags, ZFL_C
 	ret
-	ldi insdech, 0
-	ldi insdecl, 0
+	clr insdech
+	clr insdecl
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|CALL cc,nn|------|Conditional Call     |If cc CALL            |
+;|JP cc,nn  |------|Conditional Jump     |If cc JP              |
+;|RET cc    |------|Conditional Return   |If cc RET             |
+;
 ;
-; Not yet checked
 do_op_ifpo:
-	rcall do_op_calcparity
-	sbrs temp2, 0
+	sbrs z_flags, ZFL_P
 	ret
-	ldi insdech, 0
-	ldi insdecl, 0
+	clr insdech
+	clr insdecl
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|CALL cc,nn|------|Conditional Call     |If cc CALL            |
+;|JP cc,nn  |------|Conditional Jump     |If cc JP              |
+;|RET cc    |------|Conditional Return   |If cc RET             |
+;
 ;
-; Not yet checked
 do_op_ifpe:
-	rcall do_op_calcparity	
-	sbrc temp2, 0
+	sbrc z_flags, ZFL_P
 	ret
-	ldi insdech, 0
-	ldi insdecl, 0
+	clr insdech
+	clr insdecl
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|CALL cc,nn|------|Conditional Call     |If cc CALL            |
+;|JP cc,nn  |------|Conditional Jump     |If cc JP              |
+;|RET cc    |------|Conditional Return   |If cc RET             |
+;
 ;
-; Not yet checked
 do_op_ifp: ;sign positive, aka s=0
 	sbrs z_flags, ZFL_S
 	 ret
-	ldi insdech,0
-	ldi insdecl,0
+	clr insdech
+	clr insdecl
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|CALL cc,nn|------|Conditional Call     |If cc CALL            |
+;|JP cc,nn  |------|Conditional Jump     |If cc JP              |
+;|RET cc    |------|Conditional Return   |If cc RET             |
+;
 ;
-; Not yet checked
 do_op_ifm: ;sign negative, aka s=1
 	sbrc z_flags, ZFL_S
 	 ret
-	ldi insdech, 0
-	ldi insdecl, 0
+	clr insdech
+	clr insdecl
 	ret
 
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|OUT [n],A |------|Output               |[n]=A                 |
+;
 ;
-; Not yet checked
 ;Interface with peripherials goes here :)
 do_op_outa: ; out (opl),a
 .if PORT_DEBUG
@@ -2702,8 +3902,9 @@ do_op_outa: ; out (opl),a
 ;----------------------------------------------------------------
 ;|Mnemonic  |SZHPNC|Description          |Notes                 |
 ;----------------------------------------------------------------
+;|IN A,[n]  |------|Input                |A=[n]                 |
+;
 ;
-; Not yet checked
 do_op_in:	; in a,(opl)
 .if PORT_DEBUG
 	rcall printstr
@@ -2725,27 +3926,6 @@ do_op_in:	; in a,(opl)
 .endif
 	ret
 
-;----------------------------------------------------------------
-do_op_calcparity:
-	ldi temp2,1
-	sbrc parityb,0
-	 inc temp2
-	sbrc parityb,1
-	 inc temp2
-	sbrc parityb,2
-	 inc temp2
-	sbrc parityb,3
-	 inc temp2
-	sbrc parityb,4
-	 inc temp2
-	sbrc parityb,5
-	 inc temp2
-	sbrc parityb,6
-	 inc temp2
-	sbrc parityb,7
-	 inc temp2
-	andi temp2,1
-	ret
 
 ;----------------------------------------------------------------
 do_op_inv:
@@ -2760,6 +3940,46 @@ do_op_inv:
 haltinv:
 	rjmp haltinv
 	 
+;----------------------------------------------------------------
+; Lookup table, stolen from z80ex, Z80 emulation library.
+; http://z80ex.sourceforge.net/
+
+; The S, Z, 5 and 3 bits and the parity of the lookup value 
+.org (PC+255) & 0xff00
+sz53p_tab:
+	.db 0x44,0x00,0x00,0x04,0x00,0x04,0x04,0x00
+	.db 0x08,0x0c,0x0c,0x08,0x0c,0x08,0x08,0x0c
+	.db 0x00,0x04,0x04,0x00,0x04,0x00,0x00,0x04
+	.db 0x0c,0x08,0x08,0x0c,0x08,0x0c,0x0c,0x08
+	.db 0x20,0x24,0x24,0x20,0x24,0x20,0x20,0x24
+	.db 0x2c,0x28,0x28,0x2c,0x28,0x2c,0x2c,0x28
+	.db 0x24,0x20,0x20,0x24,0x20,0x24,0x24,0x20
+	.db 0x28,0x2c,0x2c,0x28,0x2c,0x28,0x28,0x2c
+	.db 0x00,0x04,0x04,0x00,0x04,0x00,0x00,0x04
+	.db 0x0c,0x08,0x08,0x0c,0x08,0x0c,0x0c,0x08
+	.db 0x04,0x00,0x00,0x04,0x00,0x04,0x04,0x00
+	.db 0x08,0x0c,0x0c,0x08,0x0c,0x08,0x08,0x0c
+	.db 0x24,0x20,0x20,0x24,0x20,0x24,0x24,0x20
+	.db 0x28,0x2c,0x2c,0x28,0x2c,0x28,0x28,0x2c
+	.db 0x20,0x24,0x24,0x20,0x24,0x20,0x20,0x24
+	.db 0x2c,0x28,0x28,0x2c,0x28,0x2c,0x2c,0x28
+	.db 0x80,0x84,0x84,0x80,0x84,0x80,0x80,0x84
+	.db 0x8c,0x88,0x88,0x8c,0x88,0x8c,0x8c,0x88
+	.db 0x84,0x80,0x80,0x84,0x80,0x84,0x84,0x80
+	.db 0x88,0x8c,0x8c,0x88,0x8c,0x88,0x88,0x8c
+	.db 0xa4,0xa0,0xa0,0xa4,0xa0,0xa4,0xa4,0xa0
+	.db 0xa8,0xac,0xac,0xa8,0xac,0xa8,0xa8,0xac
+	.db 0xa0,0xa4,0xa4,0xa0,0xa4,0xa0,0xa0,0xa4
+	.db 0xac,0xa8,0xa8,0xac,0xa8,0xac,0xac,0xa8
+	.db 0x84,0x80,0x80,0x84,0x80,0x84,0x84,0x80
+	.db 0x88,0x8c,0x8c,0x88,0x8c,0x88,0x88,0x8c
+	.db 0x80,0x84,0x84,0x80,0x84,0x80,0x80,0x84
+	.db 0x8c,0x88,0x88,0x8c,0x88,0x8c,0x8c,0x88
+	.db 0xa0,0xa4,0xa4,0xa0,0xa4,0xa0,0xa0,0xa4
+	.db 0xac,0xa8,0xa8,0xac,0xa8,0xac,0xac,0xa8
+	.db 0xa4,0xa0,0xa0,0xa4,0xa0,0xa4,0xa4,0xa0
+	.db 0xa8,0xac,0xac,0xa8,0xac,0xa8,0xa8,0xac
+	
 
 ; ----------------------- Opcode decoding -------------------------
 
@@ -2768,28 +3988,28 @@ haltinv:
 ; The table is made of 256 words. These 16-bit words consist of 
 ; the fetch operation (bit 0-4), the processing operation (bit 10-16) and the store 
 ; operation (bit 5-9).
-
+.org (PC+255) & 0xff00
 inst_table:
-.dw (FETCH_NOP  | OP_NOP   | STORE_NOP)	 ; 00		    NOP
-.dw (FETCH_DIR16| OP_NOP   | STORE_BC )	 ; 01 nn nn	LD BC,nn
-.dw (FETCH_A    | OP_NOP   | STORE_MBC ) ; 02		    LD (BC),A
-.dw (FETCH_BC   | OP_INC16 | STORE_BC )	 ; 03		    INC BC
-.dw (FETCH_B    | OP_INC   | STORE_B )	 ; 04       INC B
-.dw (FETCH_B    | OP_DEC   | STORE_B )	 ; 05       DEC B
-.dw (FETCH_DIR8	| OP_NOP	| STORE_B  )	 ; 06 nn    LD B,n
-.dw (FETCH_A    | OP_RLC	| STORE_A  )	 ; 07       RLCA
-.dw (FETCH_NOP	| OP_INV	| STORE_NOP)	 ; 08       EX AF,AF'	(Z80)
-.dw (FETCH_BC   | OP_ADDHL	| STORE_HL ) ; 09       ADD HL,BC
-.dw (FETCH_MBC	| OP_NOP	| STORE_A  )	 ; 0A       LD A,(BC)
-.dw (FETCH_BC   | OP_DEC16	| STORE_BC ) ; 0B       DEC BC
-.dw (FETCH_C    | OP_INC	| STORE_C  )	 ; 0C       INC C
-.dw (FETCH_C    | OP_DEC	| STORE_C  )	 ; 0D       DEC C
-.dw (FETCH_DIR8 | OP_NOP	| STORE_C  )	 ; 0E nn    LD C,n
-.dw (FETCH_A    | OP_RRC	| STORE_A  )	 ; 0F       RRCA
-.dw (FETCH_NOP  | OP_INV	| STORE_NOP)	 ; 10 oo    DJNZ o		(Z80)
+.dw (FETCH_NOP  | OP_NOP	| STORE_NOP)	 ; 00		NOP
+.dw (FETCH_DIR16| OP_NOP	| STORE_BC )	 ; 01 nn nn	LD BC,nn
+.dw (FETCH_A    | OP_NOP	| STORE_MBC)	 ; 02		LD (BC),A
+.dw (FETCH_BC   | OP_INC16	| STORE_BC )	 ; 03		INC BC
+.dw (FETCH_B    | OP_INC	| STORE_B  )	 ; 04       	INC B
+.dw (FETCH_B    | OP_DEC	| STORE_B  )	 ; 05       	DEC B
+.dw (FETCH_DIR8	| OP_NOP	| STORE_B  )	 ; 06 nn    	LD B,n
+.dw (FETCH_A    | OP_RLC	| STORE_A  )	 ; 07       	RLCA
+.dw (FETCH_NOP	| OP_INV	| STORE_NOP)	 ; 08       	EX AF,AF'	(Z80)
+.dw (FETCH_BC   | OP_ADDHL	| STORE_HL )	 ; 09       	ADD HL,BC
+.dw (FETCH_MBC	| OP_NOP	| STORE_A  )	 ; 0A       	LD A,(BC)
+.dw (FETCH_BC   | OP_DEC16	| STORE_BC )	 ; 0B       	DEC BC
+.dw (FETCH_C    | OP_INC	| STORE_C  )	 ; 0C       	INC C
+.dw (FETCH_C    | OP_DEC	| STORE_C  )	 ; 0D       	DEC C
+.dw (FETCH_DIR8 | OP_NOP	| STORE_C  )	 ; 0E nn    	LD C,n
+.dw (FETCH_A    | OP_RRC	| STORE_A  )	 ; 0F       	RRCA
+.dw (FETCH_NOP  | OP_INV	| STORE_NOP)	 ; 10 oo    	DJNZ o		(Z80)
 .dw (FETCH_DIR16| OP_NOP	| STORE_DE )	 ; 11 nn nn	LD DE,nn
 .dw (FETCH_A    | OP_NOP	| STORE_MDE)	 ; 12		LD (DE),A
-.dw (FETCH_DE	| OP_INC16  | STORE_DE )	 ; 13		INC DE
+.dw (FETCH_DE	| OP_INC16	| STORE_DE )	 ; 13		INC DE
 .dw (FETCH_D	| OP_INC	| STORE_D  )	 ; 14		INC D
 .dw (FETCH_D	| OP_DEC	| STORE_D  )	 ; 15		DEC D
 .dw (FETCH_DIR8	| OP_NOP	| STORE_D  )	 ; 16 nn	LD D,n
@@ -2817,7 +4037,7 @@ inst_table:
 .dw (FETCH_L	| OP_INC	| STORE_L  )	 ; 2C		INC L
 .dw (FETCH_L	| OP_DEC	| STORE_L  )	 ; 2D		DEC L
 .dw (FETCH_DIR8	| OP_NOP	| STORE_L  )	 ; 2E nn	LD L,n
-.dw (FETCH_A    | OP_CPL	| STORE_A  )	 ; 2F		CPL
+.dw (FETCH_NOP  | OP_CPL	| STORE_NOP)	 ; 2F		CPL
 .dw (FETCH_NOP	| OP_INV	| STORE_NOP)	 ; 30 oo	JR NC,o		(Z80)
 .dw (FETCH_DIR16| OP_NOP	| STORE_SP )	 ; 31 nn nn	LD SP,nn
 .dw (FETCH_DIR16| OP_NOP	| STORE_AM )	 ; 32 nn nn	LD (nn),A
@@ -2830,8 +4050,8 @@ inst_table:
 .dw (FETCH_SP	| OP_ADDHL	| STORE_HL )	 ; 39		ADD HL,SP
 .dw (FETCH_DIR16| OP_RMEM8	| STORE_A  )	 ; 3A nn nn	LD A,(nn)
 .dw (FETCH_SP	| OP_DEC16	| STORE_SP )	 ; 3B		DEC SP
-.dw (FETCH_A    | OP_INC	| STORE_A  )	 ; 3C		INC A
-.dw (FETCH_A    | OP_DEC	| STORE_A  )	 ; 3D		DEC A
+.dw (FETCH_NOP  | OP_INCA	| STORE_NOP)	 ; 3C		INC A
+.dw (FETCH_NOP  | OP_DECA	| STORE_NOP)	 ; 3D		DEC A
 .dw (FETCH_DIR8	| OP_NOP	| STORE_A  )	 ; 3E nn	LD A,n
 .dw (FETCH_NOP	| OP_CCF	| STORE_NOP)	 ; 3F		CCF (Complement Carry Flag, gvd)
 .dw (FETCH_B	| OP_NOP	| STORE_B  )	 ; 40		LD B,r
@@ -2898,77 +4118,77 @@ inst_table:
 .dw (FETCH_L	| OP_NOP	| STORE_A  )	 ; 7D		LD A,r
 .dw (FETCH_MHL	| OP_NOP	| STORE_A  )	 ; 7E		LD A,r
 .dw (FETCH_A    | OP_NOP	| STORE_A  )	 ; 7F		LD A,r
-.dw (FETCH_B	| OP_ADDA	| STORE_A  )	 ; 80		ADD A,r
-.dw (FETCH_C	| OP_ADDA	| STORE_A  )	 ; 81		ADD A,r
-.dw (FETCH_D	| OP_ADDA	| STORE_A  )	 ; 82		ADD A,r
-.dw (FETCH_E	| OP_ADDA	| STORE_A  )	 ; 83		ADD A,r
-.dw (FETCH_H	| OP_ADDA	| STORE_A  )	 ; 84		ADD A,r
-.dw (FETCH_L	| OP_ADDA	| STORE_A  )	 ; 85		ADD A,r
-.dw (FETCH_MHL	| OP_ADDA	| STORE_A  )	 ; 86		ADD A,r
-.dw (FETCH_A    | OP_ADDA	| STORE_A  )	 ; 87		ADD A,r
-.dw (FETCH_B	| OP_ADCA	| STORE_A  )	 ; 88		ADC A,r
-.dw (FETCH_C	| OP_ADCA	| STORE_A  )	 ; 89		ADC A,r
-.dw (FETCH_D	| OP_ADCA	| STORE_A  )	 ; 8A		ADC A,r
-.dw (FETCH_E	| OP_ADCA	| STORE_A  )	 ; 8B		ADC A,r
-.dw (FETCH_H	| OP_ADCA	| STORE_A  )	 ; 8C		ADC A,r
-.dw (FETCH_L	| OP_ADCA	| STORE_A  )	 ; 8D		ADC A,r
-.dw (FETCH_MHL	| OP_ADCA	| STORE_A  )	 ; 8E		ADC A,r
-.dw (FETCH_A    | OP_ADCA	| STORE_A  )	 ; 8F		ADC A,r
-.dw (FETCH_B	| OP_SUBFA	| STORE_A  )	 ; 90		SUB A,r
-.dw (FETCH_C	| OP_SUBFA	| STORE_A  )	 ; 91		SUB A,r
-.dw (FETCH_D	| OP_SUBFA	| STORE_A  )	 ; 92		SUB A,r
-.dw (FETCH_E	| OP_SUBFA	| STORE_A  )	 ; 93		SUB A,r
-.dw (FETCH_H	| OP_SUBFA	| STORE_A  )	 ; 94		SUB A,r
-.dw (FETCH_L	| OP_SUBFA	| STORE_A  )	 ; 95		SUB A,r
-.dw (FETCH_MHL	| OP_SUBFA	| STORE_A  )	 ; 96		SUB A,r
-.dw (FETCH_A    | OP_SUBFA	| STORE_A  )	 ; 97		SUB A,r
-.dw (FETCH_B	| OP_SBCFA	| STORE_A  )	 ; 98		SBC A,r
-.dw (FETCH_C	| OP_SBCFA	| STORE_A  )	 ; 99		SBC A,r
-.dw (FETCH_D	| OP_SBCFA	| STORE_A  )	 ; 9A		SBC A,r
-.dw (FETCH_E	| OP_SBCFA	| STORE_A  )	 ; 9B		SBC A,r
-.dw (FETCH_H	| OP_SBCFA	| STORE_A  )	 ; 9C		SBC A,r
-.dw (FETCH_L	| OP_SBCFA	| STORE_A  )	 ; 9D		SBC A,r
-.dw (FETCH_MHL	| OP_SBCFA	| STORE_A  )	 ; 9E		SBC A,r
-.dw (FETCH_A    | OP_SBCFA	| STORE_A  )	 ; 9F		SBC A,r
-.dw (FETCH_B	| OP_ANDA	| STORE_A  )	 ; A0		AND A,r
-.dw (FETCH_C	| OP_ANDA	| STORE_A  )	 ; A1		AND A,r
-.dw (FETCH_D	| OP_ANDA	| STORE_A  )	 ; A2		AND A,r
-.dw (FETCH_E	| OP_ANDA	| STORE_A  )	 ; A3		AND A,r
-.dw (FETCH_H	| OP_ANDA	| STORE_A  )	 ; A4		AND A,r
-.dw (FETCH_L	| OP_ANDA	| STORE_A  )	 ; A5		AND A,r
-.dw (FETCH_MHL	| OP_ANDA	| STORE_A  )	 ; A6		AND A,r
-.dw (FETCH_A    | OP_ANDA	| STORE_A  )	 ; A7		AND A,r
-.dw (FETCH_B	| OP_XORA	| STORE_A  )	 ; A8		XOR A,r
-.dw (FETCH_C	| OP_XORA	| STORE_A  )	 ; A9		XOR A,r
-.dw (FETCH_D	| OP_XORA	| STORE_A  )	 ; AA		XOR A,r
-.dw (FETCH_E	| OP_XORA	| STORE_A  )	 ; AB		XOR A,r
-.dw (FETCH_H	| OP_XORA	| STORE_A  )	 ; AC		XOR A,r
-.dw (FETCH_L	| OP_XORA	| STORE_A  )	 ; AD		XOR A,r
-.dw (FETCH_MHL	| OP_XORA	| STORE_A  )	 ; AE		XOR A,r
-.dw (FETCH_A    | OP_XORA	| STORE_A  )	 ; AF		XOR A,r
-.dw (FETCH_B	| OP_ORA	| STORE_A  )	 ; B0		OR A,r
-.dw (FETCH_C	| OP_ORA	| STORE_A  )	 ; B1		OR A,r
-.dw (FETCH_D	| OP_ORA	| STORE_A  )	 ; B2		OR A,r
-.dw (FETCH_E	| OP_ORA	| STORE_A  )	 ; B3		OR A,r
-.dw (FETCH_H	| OP_ORA	| STORE_A  )	 ; B4		OR A,r
-.dw (FETCH_L	| OP_ORA	| STORE_A  )	 ; B5		OR A,r
-.dw (FETCH_MHL	| OP_ORA	| STORE_A  )	 ; B6		OR A,r
-.dw (FETCH_A    | OP_ORA	| STORE_A  )	 ; B7		OR A,r
-.dw (FETCH_B	| OP_SUBFA	| STORE_NOP)	 ; B8		CP A,r
-.dw (FETCH_C	| OP_SUBFA	| STORE_NOP)	 ; B9		CP A,r
-.dw (FETCH_D	| OP_SUBFA	| STORE_NOP)	 ; BA		CP A,r
-.dw (FETCH_E	| OP_SUBFA	| STORE_NOP)	 ; BB		CP A,r
-.dw (FETCH_H	| OP_SUBFA	| STORE_NOP)	 ; BC		CP A,r
-.dw (FETCH_L	| OP_SUBFA	| STORE_NOP)	 ; BD		CP A,r
-.dw (FETCH_MHL	| OP_SUBFA	| STORE_NOP)	 ; BE		CP A,r
-.dw (FETCH_A    | OP_SUBFA	| STORE_NOP)	 ; BF	 	CP A,r
+.dw (FETCH_B	| OP_ADDA	| STORE_NOP)	 ; 80		ADD A,r
+.dw (FETCH_C	| OP_ADDA	| STORE_NOP)	 ; 81		ADD A,r
+.dw (FETCH_D	| OP_ADDA	| STORE_NOP)	 ; 82		ADD A,r
+.dw (FETCH_E	| OP_ADDA	| STORE_NOP)	 ; 83		ADD A,r
+.dw (FETCH_H	| OP_ADDA	| STORE_NOP)	 ; 84		ADD A,r
+.dw (FETCH_L	| OP_ADDA	| STORE_NOP)	 ; 85		ADD A,r
+.dw (FETCH_MHL	| OP_ADDA	| STORE_NOP)	 ; 86		ADD A,r
+.dw (FETCH_A    | OP_ADDA	| STORE_NOP)	 ; 87		ADD A,r
+.dw (FETCH_B	| OP_ADCA	| STORE_NOP)	 ; 88		ADC A,r
+.dw (FETCH_C	| OP_ADCA	| STORE_NOP)	 ; 89		ADC A,r
+.dw (FETCH_D	| OP_ADCA	| STORE_NOP)	 ; 8A		ADC A,r
+.dw (FETCH_E	| OP_ADCA	| STORE_NOP)	 ; 8B		ADC A,r
+.dw (FETCH_H	| OP_ADCA	| STORE_NOP)	 ; 8C		ADC A,r
+.dw (FETCH_L	| OP_ADCA	| STORE_NOP)	 ; 8D		ADC A,r
+.dw (FETCH_MHL	| OP_ADCA	| STORE_NOP)	 ; 8E		ADC A,r
+.dw (FETCH_A    | OP_ADCA	| STORE_NOP)	 ; 8F		ADC A,r
+.dw (FETCH_B	| OP_SUBFA	| STORE_NOP)	 ; 90		SUB A,r
+.dw (FETCH_C	| OP_SUBFA	| STORE_NOP)	 ; 91		SUB A,r
+.dw (FETCH_D	| OP_SUBFA	| STORE_NOP)	 ; 92		SUB A,r
+.dw (FETCH_E	| OP_SUBFA	| STORE_NOP)	 ; 93		SUB A,r
+.dw (FETCH_H	| OP_SUBFA	| STORE_NOP)	 ; 94		SUB A,r
+.dw (FETCH_L	| OP_SUBFA	| STORE_NOP)	 ; 95		SUB A,r
+.dw (FETCH_MHL	| OP_SUBFA	| STORE_NOP)	 ; 96		SUB A,r
+.dw (FETCH_A    | OP_SUBFA	| STORE_NOP)	 ; 97		SUB A,r
+.dw (FETCH_B	| OP_SBCFA	| STORE_NOP)	 ; 98		SBC A,r
+.dw (FETCH_C	| OP_SBCFA	| STORE_NOP)	 ; 99		SBC A,r
+.dw (FETCH_D	| OP_SBCFA	| STORE_NOP)	 ; 9A		SBC A,r
+.dw (FETCH_E	| OP_SBCFA	| STORE_NOP)	 ; 9B		SBC A,r
+.dw (FETCH_H	| OP_SBCFA	| STORE_NOP)	 ; 9C		SBC A,r
+.dw (FETCH_L	| OP_SBCFA	| STORE_NOP)	 ; 9D		SBC A,r
+.dw (FETCH_MHL	| OP_SBCFA	| STORE_NOP)	 ; 9E		SBC A,r
+.dw (FETCH_A    | OP_SBCFA	| STORE_NOP)	 ; 9F		SBC A,r
+.dw (FETCH_B	| OP_ANDA	| STORE_NOP)	 ; A0		AND A,r
+.dw (FETCH_C	| OP_ANDA	| STORE_NOP)	 ; A1		AND A,r
+.dw (FETCH_D	| OP_ANDA	| STORE_NOP)	 ; A2		AND A,r
+.dw (FETCH_E	| OP_ANDA	| STORE_NOP)	 ; A3		AND A,r
+.dw (FETCH_H	| OP_ANDA	| STORE_NOP)	 ; A4		AND A,r
+.dw (FETCH_L	| OP_ANDA	| STORE_NOP)	 ; A5		AND A,r
+.dw (FETCH_MHL	| OP_ANDA	| STORE_NOP)	 ; A6		AND A,r
+.dw (FETCH_A    | OP_ANDA	| STORE_NOP)	 ; A7		AND A,r
+.dw (FETCH_B	| OP_XORA	| STORE_NOP)	 ; A8		XOR A,r
+.dw (FETCH_C	| OP_XORA	| STORE_NOP)	 ; A9		XOR A,r
+.dw (FETCH_D	| OP_XORA	| STORE_NOP)	 ; AA		XOR A,r
+.dw (FETCH_E	| OP_XORA	| STORE_NOP)	 ; AB		XOR A,r
+.dw (FETCH_H	| OP_XORA	| STORE_NOP)	 ; AC		XOR A,r
+.dw (FETCH_L	| OP_XORA	| STORE_NOP)	 ; AD		XOR A,r
+.dw (FETCH_MHL	| OP_XORA	| STORE_NOP)	 ; AE		XOR A,r
+.dw (FETCH_A    | OP_XORA	| STORE_NOP)	 ; AF		XOR A,r
+.dw (FETCH_B	| OP_ORA	| STORE_NOP)	 ; B0		OR A,r
+.dw (FETCH_C	| OP_ORA	| STORE_NOP)	 ; B1		OR A,r
+.dw (FETCH_D	| OP_ORA	| STORE_NOP)	 ; B2		OR A,r
+.dw (FETCH_E	| OP_ORA	| STORE_NOP)	 ; B3		OR A,r
+.dw (FETCH_H	| OP_ORA	| STORE_NOP)	 ; B4		OR A,r
+.dw (FETCH_L	| OP_ORA	| STORE_NOP)	 ; B5		OR A,r
+.dw (FETCH_MHL	| OP_ORA	| STORE_NOP)	 ; B6		OR A,r
+.dw (FETCH_A    | OP_ORA	| STORE_NOP)	 ; B7		OR A,r
+.dw (FETCH_B	| OP_CPFA	| STORE_NOP)	 ; B8		CP A,r
+.dw (FETCH_C	| OP_CPFA	| STORE_NOP)	 ; B9		CP A,r
+.dw (FETCH_D	| OP_CPFA	| STORE_NOP)	 ; BA		CP A,r
+.dw (FETCH_E	| OP_CPFA	| STORE_NOP)	 ; BB		CP A,r
+.dw (FETCH_H	| OP_CPFA	| STORE_NOP)	 ; BC		CP A,r
+.dw (FETCH_L	| OP_CPFA	| STORE_NOP)	 ; BD		CP A,r
+.dw (FETCH_MHL	| OP_CPFA	| STORE_NOP)	 ; BE		CP A,r
+.dw (FETCH_A    | OP_CPFA	| STORE_NOP)	 ; BF	 	CP A,r
 .dw (FETCH_NOP  | OP_IFNZ	| STORE_RET)	 ; C0		RET NZ
 .dw (FETCH_NOP  | OP_POP16	| STORE_BC )	 ; C1		POP BC
 .dw (FETCH_DIR16| OP_IFNZ	| STORE_PC )	 ; C2 nn nn	JP NZ,nn
 .dw (FETCH_DIR16| OP_NOP	| STORE_PC )	 ; C3 nn nn	JP nn
 .dw (FETCH_DIR16| OP_IFNZ	| STORE_CALL)	 ; C4 nn nn	CALL NZ,nn
 .dw (FETCH_BC	| OP_PUSH16	| STORE_NOP)	 ; C5		PUSH BC
-.dw (FETCH_DIR8	| OP_ADDA	| STORE_A  )	 ; C6 nn	ADD A,n
+.dw (FETCH_DIR8	| OP_ADDA	| STORE_NOP)	 ; C6 nn	ADD A,n
 .dw (FETCH_RST	| OP_NOP	| STORE_CALL)	 ; C7		RST 0
 .dw (FETCH_NOP	| OP_IFZ	| STORE_RET)	 ; C8		RET Z
 .dw (FETCH_NOP	| OP_NOP	| STORE_RET)	 ; C9		RET
@@ -2976,7 +4196,7 @@ inst_table:
 .dw (FETCH_NOP	| OP_INV	| STORE_NOP)	 ; CB 		(Z80 specific)
 .dw (FETCH_DIR16| OP_IFZ	| STORE_CALL)	 ; CC nn nn	CALL Z,nn
 .dw (FETCH_DIR16| OP_NOP	| STORE_CALL)	 ; CD nn nn	CALL nn
-.dw (FETCH_DIR8	| OP_ADCA	| STORE_A  )	 ; CE nn	ADC A,n
+.dw (FETCH_DIR8	| OP_ADCA	| STORE_NOP)	 ; CE nn	ADC A,n
 .dw (FETCH_RST	| OP_NOP	| STORE_CALL)	 ; CF		RST 8H
 .dw (FETCH_NOP	| OP_IFNC	| STORE_RET)	 ; D0		RET NC
 .dw (FETCH_NOP  | OP_POP16	| STORE_DE )	 ; D1		POP DE
@@ -2984,15 +4204,15 @@ inst_table:
 .dw (FETCH_DIR8	| OP_OUTA	| STORE_NOP)	 ; D3 nn	OUT (n),A
 .dw (FETCH_DIR16| OP_IFNC	| STORE_CALL)	 ; D4 nn nn	CALL NC,nn
 .dw (FETCH_DE	| OP_PUSH16	| STORE_NOP)	 ; D5		PUSH DE
-.dw (FETCH_DIR8	| OP_SUBFA	| STORE_A  )	 ; D6 nn	SUB n
+.dw (FETCH_DIR8	| OP_SUBFA	| STORE_NOP)	 ; D6 nn	SUB n
 .dw (FETCH_RST	| OP_NOP	| STORE_CALL)	 ; D7		RST 10H
 .dw (FETCH_NOP	| OP_IFC	| STORE_RET)	 ; D8		RET C
-.dw (FETCH_NOP	| OP_INV	| STORE_NOP)	 ; D9		EXX			(Z80)
+.dw (FETCH_NOP	| OP_INV	| STORE_NOP)	 ; D9		EXX		(Z80)
 .dw (FETCH_DIR16| OP_IFC	| STORE_PC )	 ; DA nn nn	JP C,nn
 .dw (FETCH_DIR8	| OP_IN 	| STORE_A  )	 ; DB nn	IN A,(n)
 .dw (FETCH_DIR16| OP_IFC	| STORE_CALL)	 ; DC nn nn	CALL C,nn
 .dw (FETCH_NOP	| OP_INV	| STORE_NOP)	 ; DD 		(Z80)
-.dw (FETCH_DIR8	| OP_SBCFA	| STORE_A  )	 ; DE nn	SBC A,n
+.dw (FETCH_DIR8	| OP_SBCFA	| STORE_NOP)	 ; DE nn	SBC A,n
 .dw (FETCH_RST	| OP_NOP	| STORE_CALL)	 ; DF		RST 18H
 .dw (FETCH_NOP	| OP_IFPO	| STORE_RET)	 ; E0		RET PO
 .dw (FETCH_NOP	| OP_POP16	| STORE_HL )	 ; E1		POP HL
@@ -3000,7 +4220,7 @@ inst_table:
 .dw (FETCH_MSP	| OP_EXHL	| STORE_MSP)	 ; E3		EX (SP),HL
 .dw (FETCH_DIR16| OP_IFPO	| STORE_CALL)	 ; E4 nn nn	CALL PO,nn
 .dw (FETCH_HL	| OP_PUSH16	| STORE_NOP)	 ; E5		PUSH HL
-.dw (FETCH_DIR8	| OP_ANDA	| STORE_A  )	 ; E6 nn	AND n
+.dw (FETCH_DIR8	| OP_ANDA	| STORE_NOP)	 ; E6 nn	AND n
 .dw (FETCH_RST	| OP_NOP	| STORE_CALL)	 ; E7		RST 20H
 .dw (FETCH_NOP	| OP_IFPE	| STORE_RET)	 ; E8		RET PE
 .dw (FETCH_HL	| OP_NOP	| STORE_PC )	 ; E9		JP (HL)
@@ -3008,7 +4228,7 @@ inst_table:
 .dw (FETCH_DE	| OP_EXHL	| STORE_DE )	 ; EB		EX DE,HL
 .dw (FETCH_DIR16| OP_IFPE	| STORE_CALL)	 ; EC nn nn	CALL PE,nn
 .dw (FETCH_NOP	| OP_INV	| STORE_NOP)	 ; ED		(Z80 specific)
-.dw (FETCH_DIR8	| OP_XORA	| STORE_A  )	 ; EE nn	XOR n
+.dw (FETCH_DIR8	| OP_XORA	| STORE_NOP)	 ; EE nn	XOR n
 .dw (FETCH_RST	| OP_NOP	| STORE_CALL)	 ; EF		RST 28H
 .dw (FETCH_NOP	| OP_IFP	| STORE_RET)	 ; F0		RET P
 .dw (FETCH_NOP	| OP_POP16	| STORE_AF )	 ; F1		POP AF
@@ -3016,7 +4236,7 @@ inst_table:
 .dw (FETCH_NOP	| OP_DI		| STORE_NOP)	 ; F3		DI
 .dw (FETCH_DIR16| OP_IFP	| STORE_CALL)	 ; F4 nn nn	CALL P,nn
 .dw (FETCH_AF	| OP_PUSH16	| STORE_NOP)	 ; F5		PUSH AF
-.dw (FETCH_DIR8	| OP_ORA	| STORE_A  )	 ; F6 nn	OR n
+.dw (FETCH_DIR8	| OP_ORA	| STORE_NOP)	 ; F6 nn	OR n
 .dw (FETCH_RST	| OP_NOP	| STORE_CALL)	 ; F7		RST 30H
 .dw (FETCH_NOP	| OP_IFM	| STORE_RET)	 ; F8		RET M
 .dw (FETCH_HL	| OP_NOP	| STORE_SP )	 ; F9		LD SP,HL
@@ -3024,5 +4244,7 @@ inst_table:
 .dw (FETCH_NOP	| OP_EI 	| STORE_NOP)	 ; FB		EI
 .dw (FETCH_DIR16| OP_IFM	| STORE_CALL)	 ; FC nn nn	CALL M,nn
 .dw (FETCH_NOP	| OP_INV	| STORE_NOP)	 ; FD 		(Z80 specific)
-.dw (FETCH_DIR8	| OP_SUBFA	| STORE_NOP)	 ; FE nn	CP n
+.dw (FETCH_DIR8	| OP_CPFA	| STORE_NOP)	 ; FE nn	CP n
 .dw (FETCH_RST	| OP_NOP	| STORE_CALL)	 ; FF		RST 38H
+
+; vim:set ts=8 noet nowrap