;-------------------------------------------------------------------------
;				Command Interpreter ATMega128 !!!
;-------------------------------------------------------------------------
;input: Y: input buffer
;output: addrh, addrl: 16 bit hex value
;	 T = 1 if question mark found
exec_command:

	ldi	a0, 1			;M128 only !
	out	RAMPZ, a0		;point to upper 64kB

	ldi	zl, low( command_table * 2 - 3 )
	ldi	zh, high( command_table * 2 - 3 )
	sbrs	flags, f_enable
	ldi	zl, low( command_enable * 2 - 3 )
	sbrs	flags, f_enable
	ldi	zh, high( command_enable * 2 - 3 )
	mov	a1, yl			;save command start
_coi1:	adiw	zl, 3
	andi	zl, 0xFE		;adjust to next word
	mov	yl, a1
	clt
	rjmp	_coi3

_coi2:	ld	a0, y
	inc	yl
	eor	a0, r0
	andi	a0, 0x5F		;ignore case
	breq	_coi3
	set				;not matching

_coi3:
	elpm	r0, z+			;M128 only !

;	lpm	r0, z+
	tst	r0
	brne	_coi2
	brts	_coi1			;test next command

	clr	addrl
	clr	addrh
_coi4:	ld	a0, y
	inc	yl
	tst	a0
	breq	_coi6

	cpi	a0, '?'
	brne	_coi5
	set				;question mark found
_coi5:	rcall	ascii_hex		;read hex value after command
	rjmp	_coi4

_coi6:
	ldi	a0, 0			;M128 only !
	out	RAMPZ, a0		;restore default lower 64kB range

	lsr	zh			;byte to word address
	ror	zl

	sbr	zh, 1<<7		;M128 only !

	ijmp
;-------------------------------------------------------------------------