;****************************************************************************************
;	Mega 128 Scope Controller Program
;	(c) Martin Cibulski
;
;	Monitor/Debugging routines
;****************************************************************************************


;****************************************************************************************
;	Labels

.EQU	LABEL_LEN	= 10			;characters (+ ptr + type + nullbyte)

.EQU	L_TYPE_BYTE	= 0b00000000			;1 byte
.EQU	L_TYPE_WORD	= 0b00000001			;2 byte
.EQU	L_TYPE_LONG	= 0b00000010			;4 byte
.EQU	L_TYPE_FLOAT	= 0b00000011			;8 byte
.EQU	L_TYPE_CMD	= 0b00000100			;small subroutine
.EQU	L_TYPE_XCMD	= 0b00000101			;large subroutine
.EQU	L_TYPES	= 0b00000111			;mask for compares

.EQU	L_LOC_REG	= 0b10000000			;cpu/io register (offset 0x20)
.EQU	L_LOC_RAM	= 0b01000000			;RAM
.EQU	L_LOC_EEP	= 0b00100000			;EEPROM
.EQU	L_LOC_PGM	= 0b00010000			;Flash ROM
.EQU	L_LOC_SEP	= 0b00001000			;external serial EEPROM
.EQU	L_LOCATIONS	= 0b11111000			;mask for compares


.EQU	L_BREG	= L_TYPE_BYTE |L_LOC_REG		;cpu byte register name (r0 - r31)
.EQU	L_WREG	= L_TYPE_WORD |L_LOC_REG		;cpu word register name (x,y,z)
.EQU	L_BRAM	= L_TYPE_BYTE |L_LOC_RAM		;RAM address (variable)
.EQU	L_WRAM	= L_TYPE_WORD |L_LOC_RAM		;RAM address (variable)
.EQU	L_LRAM	= L_TYPE_LONG |L_LOC_RAM		;RAM address (variable)
.EQU	L_FRAM	= L_TYPE_FLOAT|L_LOC_RAM		;RAM address (variable)
.EQU	L_BEEP	= L_TYPE_BYTE |L_LOC_EEP		;EEPROM address (variable)
.EQU	L_WEEP	= L_TYPE_WORD |L_LOC_EEP		;EEPROM address (variable)
.EQU	L_LEEP	= L_TYPE_LONG |L_LOC_EEP		;EEPROM address (variable)
.EQU	L_FEEP	= L_TYPE_FLOAT|L_LOC_EEP		;EEPROM address (variable)
.EQU	L_CMD	= L_TYPE_CMD  |L_LOC_PGM		;subroutine adress
.EQU	L_XCMD	= L_TYPE_XCMD |L_LOC_PGM		;subroutine adress
.EQU	L_END	= 0			;end of table

.EQU	IO_BASE	= 0x20


;****************************************************************************************
;standard functions

monitor_label_tab:
	.dw	twi_test
	.db	L_CMD	,"TWITEST    "
	.dw	mon_help
	.db	L_CMD	,"?          "
	.dw	mon_help
	.db	L_CMD	,"HELP       "
	.dw	mon_list_labels
	.db	L_CMD	,"LABELS     "
	.dw	monitor_da
	.db	L_CMD	,"DA         "
	.dw	monitor_db
	.db	L_CMD	,"DB         "
	.dw	monitor_dw
	.db	L_CMD	,"DW         "
	.dw	monitor_dl
	.db	L_CMD	,"DL         "
	.dw	monitor_dq
	.db	L_CMD	,"DQ         "
	.dw	monitor_df
	.db	L_XCMD	,"DF         "
	.dw	monitor_dbe
	.db	L_CMD	,"DBE        "
	.dw	monitor_dwe
	.db	L_CMD	,"DWE        "
	.dw	monitor_dle
	.db	L_CMD	,"DLE        "
	.dw	monitor_dqe
	.db	L_CMD	,"DQE        "
	.dw	monitor_dfe
	.db	L_XCMD	,"DFE        "
	.dw	monitor_dbs
	.db	L_CMD	,"DBS        "
	.dw	monitor_dws
	.db	L_CMD	,"DWS        "
	.dw	monitor_dls
	.db	L_CMD	,"DLS        "
	.dw	monitor_dqs
	.db	L_CMD	,"DQS        "
	.dw	monitor_dfs
	.db	L_XCMD	,"DFS        "
	.dw	monitor_mb
	.db	L_CMD	,"MB         "
	.dw	monitor_mw
	.db	L_CMD	,"MW         "
	.dw	monitor_ml
	.db	L_CMD	,"ML         "
	.dw	monitor_mf
	.db	L_XCMD	,"MF         "
	.dw	monitor_mq
	.db	L_CMD	,"MQ         "
	.dw	monitor_mbe
	.db	L_CMD	,"MBE        "
	.dw	monitor_mwe
	.db	L_CMD	,"MWE        "
	.dw	monitor_mle
	.db	L_CMD	,"MLE        "
	.dw	monitor_mfe
	.db	L_XCMD	,"MFE        "
	.dw	monitor_mbs
	.db	L_CMD	,"MBS        "
	.dw	monitor_mws
	.db	L_CMD	,"MWS        "
	.dw	monitor_mls
	.db	L_CMD	,"MLS        "
	.dw	monitor_mfs
	.db	L_XCMD	,"MFS        "
	.dw	monitor_ports
	.db	L_CMD	,"PORTS      "
	.dw	0x0000
	.db	L_CMD	,"RESET      "

;****************************************************************************************
;special functions

	.dw	mon_eep_load_profile
	.db	L_CMD	,"LOADPROF   "
	.dw	mon_eep_save_profile
	.db	L_CMD	,"SAVEPROF   "
	.dw	main_op_mode
	.db	L_BRAM	,"MAINOPMODE "

;****************************************************************************************
;standard registers

	.dw	GIMSK + IO_BASE
	.db	L_BRAM	,"GIMSK      "
	.dw	GIFR + IO_BASE
	.db	L_BRAM	,"GIFR       "
	.dw	TIMSK + IO_BASE
	.db	L_BRAM	,"TIMSK      "
	.dw	TIFR + IO_BASE
	.db	L_BRAM	,"TIFR       "
	.dw	MCUCR + IO_BASE
	.db	L_BRAM	,"MCUCR      "
	.dw	PORTA + IO_BASE
	.db	L_BRAM	,"PORTA      "
	.dw	DDRA + IO_BASE
	.db	L_BRAM	,"DDRA       "
	.dw	PINA + IO_BASE
	.db	L_BRAM	,"PINA       "
	.dw	PORTB + IO_BASE
	.db	L_BRAM	,"PORTB      "
	.dw	DDRB + IO_BASE
	.db	L_BRAM	,"DDRB       "
	.dw	PINB + IO_BASE
	.db	L_BRAM	,"PINB       "
	.dw	PORTC + IO_BASE
	.db	L_BRAM	,"PORTC      "
	.dw	DDRC + IO_BASE
	.db	L_BRAM	,"DDRC       "
	.dw	PINC + IO_BASE
	.db	L_BRAM	,"PINC       "
	.dw	PORTD + IO_BASE
	.db	L_BRAM	,"PORTD      "
	.dw	DDRD + IO_BASE
	.db	L_BRAM	,"DDRD       "
	.dw	PIND + IO_BASE
	.db	L_BRAM	,"PIND       "

	.dw	0
	.db	L_END	, 0

      
;****************************************************************************************
;	find label
;	xh:xl	: pointer to label name
;	r16 	: type of label
;
;	xh:xl	:adress
;	t-flag	:1=found
;************************************************************

mon_master_label_tab:
	.dw	lx200_label_tab
	.dw	monitor_label_tab
	.dw	xyz_label_tab
	.dw	hbx_label_tab
	.dw	lcd_label_tab
	.dw	flt_label_tab
	.dw	mco_label_tab
	.dw	tgo_label_tab
	.dw	cor_label_tab
	.dw	alg_label_tab
	.dw	mot_label_tab
	.dw	a39_label_tab
	.dw	util_label_tab
	.dw	0x0000

find_label:
	push	yl
	push	yh
	push	zl
	push	zh

	ldi	zl	,low(mon_master_label_tab<<1)
	ldi	zh	,high(mon_master_label_tab<<1)
	clt

find_label_loop:
	lpm	yl	,Z+
	lpm	yh	,Z+
	cpi	yl	,0
	cpc	yl	,yh
	breq	find_label_end

	lsl	yl
	rol	yh
	rcall	find_label_sub
	brtc	find_label_loop

find_label_end:
	pop	zh
	pop	zl
	pop	yh
	pop	yl
	ret

;****************************************************************************************
;	Test

.DSEG
monitor_inptr:
	.byte	1

.CSEG

monitor_test:
	ldi	r24	,low(uart_ibuf)		;store character as uart_ibuf [uart_iput]
	ldi	r25	,high(uart_ibuf)
	lds	r16	,uart_iput
	clr	r17
	lds	r18	,uart_iget
	ldi	zl	,low((monitor_test_command<<1))
	ldi	zh	,high((monitor_test_command<<1))
monitor_test_loop:
	cp	r16	,r18		;check if buffer full
	breq	monitor_test_full

	lpm	r0	,Z+
	movw	xh:xl	,r25:r24
	add	xl	,r16
	adc	xh	,r17
	st	X	,r0

	inc	r16			;uart_iput ++ (post increment)
	andi	r16	,UART_ISIZE-1
	sts	uart_iput	,r16

	tst	r0
	brne	monitor_test_loop

monitor_test_full:
	rjmp	monitor

;****************************************************************************************
;	Check for Commands

monitor:
	lds	r16	,monitor_inptr
	tst	r16
	brpl	monitor_no_prompt

	;ldi	zl	,low(monitor_msg_prompt<<1)
	;ldi	zh	,high(monitor_msg_prompt<<1)
	;call	puts_rom_func
	ldi	r16	,0
	sts	monitor_inptr	,r16

monitor_no_prompt:
	call	gets
	brtc	monitor_exit

	rcall	next_word
	brtc	monitor_error

	ldi	r16	,L_XCMD
	rcall	find_label
	brtc	monitor_error

	mov	zl	,xl
	mov	zh	,xh
	icall

	brts	monitor_init

monitor_error:
	ldi	zl	,low(msg_monitor_error<<1)
	ldi	zh	,high(msg_monitor_error<<1)
	call	puts_rom_func

	rjmp	monitor_init

monitor_init:
	ldi	r16	,low(uart_cmd)
	sts	cmd_ptr	,r16
	ldi	r16	,high(uart_cmd)
	sts	cmd_ptr+1	,r16
	ldi	r16	,-1
	sts	monitor_inptr	,r16
monitor_exit:
	ret

monitor_msg_prompt:
	.db	62, 0


;****************************************************************************************
;
mon_help:
	PUTS_ROM	msg_monitor_help
	set
	ret

;****************************************************************************************
;
mon_list_labels:
	rcall	next_int_value
	brtc	mon_llb_exit

	ldi	zl	,low(mon_master_label_tab<<1)
	ldi	zh	,high(mon_master_label_tab<<1)
	clt

mon_llb_01:
	lpm	r16	,Z+
	lpm	r17	,Z+
	cpi	r16	,0
	cpc	r16	,r17
	breq	mon_llb_exit

	dec	AKKU_8
	brne	mon_llb_01

	movw	zh:zl	,r17:r16
	lsl	zl
	rol	zh
	set

mon_llb_02:
	lpm	r16	,Z+		;label adress
	lpm	r17	,Z+
	cpi	r16	,0
	cpc	r16	,r17
	breq	mon_llb_exit

	lpm	r16	,Z+		;label type
	ldi	r17	,11		;11 characters label name
mon_llb_03:
	lpm	r16	,Z+
	call	putc_func
	dec	r17
	brne	mon_llb_03

	ldi	r16	,13
	call	putc_func
	rjmp	mon_llb_02

mon_llb_exit:
	ret


;****************************************************************************************
;
mon_eep_save_profile:
	rcall	next_int_value
	brtc	mon_esp_exit

	mov	r16	,AKKU_8
	dec	r16
	call	eep_save_profile
mon_esp_exit:
	ret


;****************************************************************************************
;
mon_eep_load_profile:
	rcall	next_int_value
	brtc	mon_elp_exit

	mov	r16	,AKKU_8
	dec	r16
	call	eep_load_profile
mon_elp_exit:
	ret


;************************************************************
;	Monitor Command DA
;	Display Adress
;
;	DA <label>
;************************************************************

monitor_da:
	ldi	r16	,0xFF
	rcall	next_adress
	brtc	monitor_da_exit

	putc	'0'
	putc	'x'
	mov	r16	,xh
	rcall	printf_byte_hex
	mov	r16	,xl
	rcall	printf_byte_hex
	putc	13

monitor_da_exit:
	ret


;************************************************************
;	Monitor Command DBS
;	Display serial EEPROM Byte
;
;	DBS <adress|label>
;************************************************************

monitor_dbs:
	ldi	r16	,L_BEEP
	rcall	next_adress
	brtc	monitor_dbs_exit

	movw	r25:r24	,xh:xl
	call	twi_read
	push	r16
	putc	'0'
	putc	'x'
	pop	r16
	push	r16
	rcall	printf_byte_hex
	putc	' '
	putc	'b'
	pop	r16
	rcall	printf_byte_binary
	putc	13

monitor_dbs_exit:
	ret


;************************************************************
;	Monitor Command DWS
;	Display serial EEPROM Word
;
;	DWS <adress|label>
;************************************************************

monitor_dws:
	ldi	r16	,L_WEEP	;search eeprom word adress
	rcall	next_adress
	brtc	monitor_dws_exit

	putc	'0'
	putc	'x'

	movw	r25:r24	,xh:xl
	call	twi_read
	mov	r17	,r16

	adiw	r25:r24	,1
	call	twi_read
	mov	r18	,r16

	mov	r16	,r17
	rcall	printf_byte_hex
	mov	r16	,r18
	rcall	printf_byte_hex
	putc	13

monitor_dws_exit:
	ret


;************************************************************
;	Monitor Command DLS
;	Display serial EEPROM Long
;
;	DLS <adress|label>
;************************************************************

monitor_dls:
	ldi	r16	,L_LEEP		;search eeprom long adress
	rcall	next_adress
	brtc	monitor_dls_exit

	putc	'0'
	putc	'x'

	movw	r25:r24	,xh:xl
	call	twi_read
	mov	r17	,r16

	adiw	r25:r24	,1
	call	twi_read
	mov	r18	,r16

	adiw	r25:r24	,1
	call	twi_read
	mov	r19	,r16

	adiw	r25:r24	,1
	call	twi_read
	mov	r20	,r16

	mov	r16	,r17
	rcall	printf_byte_hex
	mov	r16	,r18
	rcall	printf_byte_hex
	mov	r16	,r19
	rcall	printf_byte_hex
	mov	r16	,r20
	rcall	printf_byte_hex
	putc	13

monitor_dls_exit:
	ret


;************************************************************
;	Monitor Command DQS
;	Display serial EEPROM Float as Hex (Quadword)
;
;	DLS <adress|label>
;************************************************************

monitor_dqs:
	ldi	r16	,L_FEEP		;search eeprom long adress
	rcall	next_adress
	brtc	monitor_dqs_exit

	putc	'0'
	putc	'x'

	movw	r25:r24	,xh:xl
	call	twi_read
	rcall	printf_byte_hex

	adiw	r25:r24	,1
	call	twi_read
	rcall	printf_byte_hex

	adiw	r25:r24	,1
	call	twi_read
	rcall	printf_byte_hex

	adiw	r25:r24	,1
	call	twi_read
	rcall	printf_byte_hex

	adiw	r25:r24	,1
	call	twi_read
	rcall	printf_byte_hex

	adiw	r25:r24	,1
	call	twi_read
	rcall	printf_byte_hex

	adiw	r25:r24	,1
	call	twi_read
	rcall	printf_byte_hex

	adiw	r25:r24	,1
	call	twi_read
	rcall	printf_byte_hex

	putc	13

monitor_dqs_exit:
	ret


;************************************************************
;	Monitor Command DFS
;	Display serial EEPROM Float
;
;	DFS <adress|label>
;************************************************************

monitor_dfs:
	ldi	r16	,L_FEEP	;search eeprom float adress
	rcall	next_adress
	brtc	monitor_dfs_exit

	movw	r25:r24	,xh:xl
	call	twi_read
	mov	AKKU_S	,r16

	adiw	r25:r24	,1
	call	twi_read
	mov	AKKU_2	,r16

	adiw	r25:r24	,1
	call	twi_read
	mov	AKKU_3	,r16

	adiw	r25:r24	,1
	call	twi_read
	mov	AKKU_4	,r16

	adiw	r25:r24	,1
	call	twi_read
	mov	AKKU_5	,r16

	adiw	r25:r24	,1
	call	twi_read
	mov	AKKU_6	,r16

	adiw	r25:r24	,1
	call	twi_read
	mov	AKKU_7	,r16

	adiw	r25:r24	,1
	call	twi_read
	mov	AKKU_8	,r16

	call	_flt_load
	call	flt_print
	putc	13

monitor_dfs_exit:
	ret


;************************************************************
;	Monitor Command DBE
;	Display EEPROM Byte
;
;	DBE <adress|label>
;************************************************************

monitor_dbe:
	ldi	r16	,L_BEEP
	rcall	next_adress
	brtc	monitor_dbe_exit

	movw	zh:zl	,xh:xl
	call	eep_read
	push	r16
	putc	'0'
	putc	'x'
	pop	r16
	push	r16
	rcall	printf_byte_hex
	putc	' '
	putc	'b'
	pop	r16
	rcall	printf_byte_binary
	putc	13

monitor_dbe_exit:
	ret


;************************************************************
;	Monitor Command DWE
;	Display EEPROM Word
;
;	DWE <adress|label>
;************************************************************

monitor_dwe:
	ldi	r16	,L_WEEP	;search eeprom word adress
	rcall	next_adress
	brtc	monitor_dwe_exit

	putc	'0'
	putc	'x'

	movw	zh:zl	,xh:xl
	call	eep_read
	mov	r17	,r16

	adiw	zh:zl	,1
	call	eep_read
	mov	r18	,r16

	mov	r16	,r17
	rcall	printf_byte_hex
	mov	r16	,r18
	rcall	printf_byte_hex
	putc	13

monitor_dwe_exit:
	ret


;************************************************************
;	Monitor Command DLE
;	Display EEPROM Long
;
;	DLE <adress|label>
;************************************************************

monitor_dle:
	ldi	r16	,L_LEEP		;search eeprom long adress
	rcall	next_adress
	brtc	monitor_dle_exit

	putc	'0'
	putc	'x'

	movw	zh:zl	,xh:xl
	call	eep_read
	mov	r17	,r16

	adiw	zh:zl	,1
	call	eep_read
	mov	r18	,r16

	adiw	zh:zl	,1
	call	eep_read
	mov	r19	,r16

	adiw	zh:zl	,1
	call	eep_read
	mov	r20	,r16

	mov	r16	,r17
	rcall	printf_byte_hex
	mov	r16	,r18
	rcall	printf_byte_hex
	mov	r16	,r19
	rcall	printf_byte_hex
	mov	r16	,r20
	rcall	printf_byte_hex
	putc	13

monitor_dle_exit:
	ret


;************************************************************
;	Monitor Command DQE
;	Display EEPROM Float as Hex (Quadword)
;
;	DQE <adress|label>
;************************************************************

monitor_dqe:
	ldi	r16	,L_FEEP		;search eeprom long adress
	rcall	next_adress
	brtc	monitor_dqe_exit

	putc	'0'
	putc	'x'

	movw	zh:zl	,xh:xl
	call	eep_read
	rcall	printf_byte_hex

	adiw	zh:zl	,1
	call	eep_read
	rcall	printf_byte_hex

	adiw	zh:zl	,1
	call	eep_read
	rcall	printf_byte_hex

	adiw	zh:zl	,1
	call	eep_read
	rcall	printf_byte_hex

	adiw	zh:zl	,1
	call	eep_read
	rcall	printf_byte_hex

	adiw	zh:zl	,1
	call	eep_read
	rcall	printf_byte_hex

	adiw	zh:zl	,1
	call	eep_read
	rcall	printf_byte_hex

	adiw	zh:zl	,1
	call	eep_read
	rcall	printf_byte_hex

	putc	13

monitor_dqe_exit:
	ret


;************************************************************
;	Monitor Command DFE
;	Display EEPROM Float
;
;	DFE <adress|label>
;************************************************************

monitor_dfe:
	ldi	r16	,L_FEEP	;search eeprom float adress
	rcall	next_adress
	brtc	monitor_dfe_exit

	movw	zh:zl	,xh:xl
	call	eep_read
	mov	AKKU_S	,r16

	adiw	zh:zl	,1
	call	eep_read
	mov	AKKU_2	,r16

	adiw	zh:zl	,1
	call	eep_read
	mov	AKKU_3	,r16

	adiw	zh:zl	,1
	call	eep_read
	mov	AKKU_4	,r16

	adiw	zh:zl	,1
	call	eep_read
	mov	AKKU_5	,r16

	adiw	zh:zl	,1
	call	eep_read
	mov	AKKU_6	,r16

	adiw	zh:zl	,1
	call	eep_read
	mov	AKKU_7	,r16

	adiw	zh:zl	,1
	call	eep_read
	mov	AKKU_8	,r16

	call	_flt_load
	call	flt_print
	putc	13

monitor_dfe_exit:
	ret


;************************************************************
;	Monitor Command DB
;	Display Byte
;
;	DB <adress|label>
;************************************************************

monitor_db:
	ldi	r16	,L_BRAM
	rcall	next_adress
	brtc	monitor_db_exit

	ld	r16	,X
	push	r16
	putc	'0'
	putc	'x'
	pop	r16
	push	r16
	rcall	printf_byte_hex
	putc	' '
	putc	'b'
	pop	r16
	rcall	printf_byte_binary
	putc	13

monitor_db_exit:
	ret


;************************************************************
;	Monitor Command DW
;	Display Word
;
;	DW <adress|label>
;************************************************************

monitor_dw:
	ldi	r16	,L_WREG | L_WRAM	;search reg/ram word adress
	rcall	next_adress
	brtc	monitor_dw_exit

	putc	'0'
	putc	'x'
	cli
	ld	r16	,X+
	ld	r17	,X
	sei
	rcall	printf_byte_hex
	mov	r16	,r17
	rcall	printf_byte_hex
	putc	13

monitor_dw_exit:
	ret


;************************************************************
;	Monitor Command DL
;	Display Long
;
;	DL <adress|label>
;************************************************************

monitor_dl:
	ldi	r16	,L_LRAM		;search ram long adress
	rcall	next_adress
	brtc	monitor_dl_exit

	putc	'0'
	putc	'x'
	cli
	ld	r16	,X+
	ld	r17	,X+
	ld	r18	,X+
	ld	r19	,X
	sei

	rcall	printf_byte_hex
	mov	r16	,r17
	rcall	printf_byte_hex
	mov	r16	,r18
	rcall	printf_byte_hex
	mov	r16	,r19
	rcall	printf_byte_hex
	putc	13

monitor_dl_exit:
	ret


;************************************************************
;	Monitor Command DQ
;	Display Float as Hex (Quadword)
;
;	DQ <adress|label>
;************************************************************

monitor_dq:
	ldi	r16	,L_FRAM		;search ram float adress
	rcall	next_adress
	brtc	monitor_dq_exit

	putc	'0'
	putc	'x'
	cli
	ld	r16	,X+
	ld	r17	,X+
	ld	r18	,X+
	ld	r19	,X+
	ld	r20	,X+
	ld	r21	,X+
	ld	r22	,X+
	ld	r23	,X
	sei

	rcall	printf_byte_hex
	mov	r16	,r17
	rcall	printf_byte_hex
	mov	r16	,r18
	rcall	printf_byte_hex
	mov	r16	,r19
	rcall	printf_byte_hex
	mov	r16	,r20
	rcall	printf_byte_hex
	mov	r16	,r21
	rcall	printf_byte_hex
	mov	r16	,r22
	rcall	printf_byte_hex
	mov	r16	,r23
	rcall	printf_byte_hex
	putc	13

monitor_dq_exit:
	ret


;************************************************************
;	Monitor Command DF
;	Display Float
;
;	DF <adress|label>
;************************************************************

monitor_df:
	ldi	r16	,L_FRAM	;search ram float adress
	rcall	next_adress
	brtc	monitor_df_exit

monitor_df_print:
	call	flt_load_x
	call	flt_print
	putc	13

monitor_df_exit:
	ret


;************************************************************
;	Monitor Command MBE
;	Modify Byte
;
;	MBE <adress|label> <byte>
;************************************************************

monitor_mbe:
	ldi	r16	,L_BEEP		;search reg/io/ram byte adress
	rcall	next_adress
	brtc	monitor_mbe_exit

	push	xl
	push	xh
	rcall	next_int_value			;value
	pop	zh
	pop	zl

	brtc	monitor_mbe_exit

	mov	r16	,AKKU_8
	call	eep_write

monitor_mbe_exit:
	ret


;************************************************************
;	Monitor Command MWE
;	Modify Word
;
;	MWE <adress|label> <word>
;************************************************************

monitor_mwe:
	ldi	r16	,L_WEEP		;search ram word adress
	rcall	next_adress
	brtc	monitor_mwe_exit

	push	xl
	push	xh
	rcall	next_int_value			;value
	pop	zh
	pop	zl

	brtc	monitor_mwe_exit

	mov	r16	,AKKU_7
	call	eep_write

	adiw	zh:zl	,1

	mov	r16	,AKKU_8
	call	eep_write

monitor_mwe_exit:
	ret


;************************************************************
;	Monitor Command MLE
;	Modify Long
;
;	MWE <adress|label> <long>
;************************************************************

monitor_mle:
	ldi	r16	,L_LEEP		;search ram word adress
	rcall	next_adress
	brtc	monitor_mle_exit

	push	xl
	push	xh
	rcall	next_int_value			;value
	pop	zh
	pop	zl

	brtc	monitor_mle_exit

	mov	r16	,AKKU_5
	call	eep_write

	adiw	zh:zl	,1

	mov	r16	,AKKU_6
	call	eep_write

	adiw	zh:zl	,1

	mov	r16	,AKKU_7
	call	eep_write

	adiw	zh:zl	,1

	mov	r16	,AKKU_8
	call	eep_write

monitor_mle_exit:
	ret


;************************************************************
;	Monitor Command MFE
;	Modify Float
;
;	MBE <adress|label> <float>
;************************************************************

monitor_mfe:
	.EQU	mon_mfe_adr	=  1
	.EQU	mon_mfe_val	=  3
	.EQU	mon_mfe_lspace	= 11
	LOCAL	mon_mfe_lspace

	ldi	r16	,L_FEEP	;search ram float adress
	rcall	next_adress
	brtc	monitor_mfe_exit

	std	Y+mon_mfe_adr	,xh
	std	Y+mon_mfe_adr+1	,xl

	rcall	next_flt_value
	brtc	monitor_mfe_exit

	movw	xh:xl	,yh:yl
	adiw	xh:xl	,mon_mfe_val
	call	flt_store_x

	movw	xh:xl	,yh:yl
	adiw	xh:xl	,mon_mfe_val
	ldd	zh	,Y+mon_mfe_adr
	ldd	zl	,Y+mon_mfe_adr+1

	ldi	r17	,8

monitor_mfe_loop:
	ld	r16	,X+
	call	eep_write
	adiw	zh:zl	,1
	dec	r17
	brne	monitor_mfe_loop

monitor_mfe_exit:
	ENDLOCAL	mon_mfe_lspace
	ret


;************************************************************
;	Monitor Command MBS
;	Modify Byte
;
;	MBS <adress|label> <byte>
;************************************************************

monitor_mbs:
	ldi	r16	,L_BEEP		;search reg/io/ram byte adress
	rcall	next_adress
	brtc	monitor_mbs_exit

	movw	r25:r24	,xh:xl

	rcall	next_int_value			;value
	brtc	monitor_mbs_exit

	mov	r16	,AKKU_8
	call	twi_write

monitor_mbs_exit:
	ret


;************************************************************
;	Monitor Command MWS
;	Modify Word
;
;	MWS <adress|label> <word>
;************************************************************

monitor_mws:
	ldi	r16	,L_WEEP		;search ram word adress
	rcall	next_adress
	brtc	monitor_mws_exit

	movw	r25:r24	,xh:xl

	rcall	next_int_value			;value
	brtc	monitor_mws_exit

	mov	xl	,r0
	mov	xh	,r1

	mov	r16	,AKKU_7
	call	twi_write
	brtc	monitor_mws_exit

	adiw	r25:r24	,1

monitor_mws_01:
	mov	r16	,AKKU_8
	call	twi_write
	brtc	monitor_mws_01

monitor_mws_exit:
	ret


;************************************************************
;	Monitor Command MLS
;	Modify Long
;
;	MWS <adress|label> <long>
;************************************************************

monitor_mls:
	ldi	r16	,L_LEEP		;search ram word adress
	rcall	next_adress
	brtc	monitor_mls_exit

	movw	r25:r24	,xh:xl

	rcall	next_int_value			;value
	brtc	monitor_mls_exit

	mov	xl	,r0
	mov	xh	,r1

	mov	r16	,AKKU_5
	call	twi_write
	brtc	monitor_mls_exit

	adiw	r25:r24	,1

monitor_mls_01:
	mov	r16	,AKKU_6
	call	twi_write
	brtc	monitor_mls_01

	adiw	r25:r24	,1

monitor_mls_02:
	mov	r16	,AKKU_7
	call	twi_write
	brtc	monitor_mls_02

	adiw	r25:r24	,1

monitor_mls_03:
	mov	r16	,AKKU_8
	call	twi_write
	brtc	monitor_mls_03

monitor_mls_exit:
	ret


;************************************************************
;	Monitor Command MFS
;	Modify Float
;
;	MBS <adress|label> <float>
;************************************************************

monitor_mfs:
	.EQU	mon_mfs_adr	=  1
	.EQU	mon_mfs_val	=  3
	.EQU	mon_mfs_lspace	= 11
	LOCAL	mon_mfs_lspace

	ldi	r16	,L_FEEP	;search ram float adress
	rcall	next_adress
	brtc	monitor_mfs_exit

	std	Y+mon_mfs_adr	,xh
	std	Y+mon_mfs_adr+1	,xl

	rcall	next_flt_value
	brtc	monitor_mfs_exit

	movw	xh:xl	,yh:yl
	adiw	xh:xl	,mon_mfs_val
	call	flt_store_x

	movw	xh:xl	,yh:yl
	adiw	xh:xl	,mon_mfs_val
	ldd	r25	,Y+mon_mfs_adr
	ldd	r24	,Y+mon_mfs_adr+1

	ld	r16	,X+
	call	twi_write			;write first byte
	brtc	monitor_mfs_exit

	adiw	r25:r24	,1
	ldi	r17	,7

monitor_mfs_loop:
	ld	r16	,X+

monitor_mfs_01:
	call	twi_write
	brtc	monitor_mfs_01

	adiw	r25:r24	,1
	dec	r17
	brne	monitor_mfs_loop

monitor_mfs_exit:
	ENDLOCAL	mon_mfs_lspace
	ret


;************************************************************
;	Monitor Command MB
;	Modify Byte
;
;	MB <adress|label> <byte>
;************************************************************

monitor_mb:
	ldi	r16	,L_BRAM		;search reg/io/ram byte adress
	rcall	next_adress
	brtc	monitor_mb_exit

	mov	r0	,xl
	mov	r1	,xh

	rcall	next_int_value			;value
	brtc	monitor_mb_exit

	mov	xl	,r0
	mov	xh	,r1

	st	X	,AKKU_8		;write cpu register

monitor_mb_exit:
	ret


;************************************************************
;	Monitor Command MW
;	Modify Word
;
;	MW <adress|label> <word>
;************************************************************

monitor_mw:
	ldi	r16	,L_WRAM		;search ram word adress
	rcall	next_adress
	brtc	monitor_mw_exit

	mov	r0	,xl
	mov	r1	,xh

	rcall	next_int_value			;value
	brtc	monitor_mw_exit

	mov	xl	,r0
	mov	xh	,r1

	cli
	st	X+	,AKKU_7
	st	X+	,AKKU_8
	sei

monitor_mw_exit:
	ret


;************************************************************
;	Monitor Command ML
;	Modify Long
;
;	MW <adress|label> <long>
;************************************************************

monitor_ml:
	ldi	r16	,L_LRAM		;search ram word adress
	rcall	next_adress
	brtc	monitor_ml_exit

	mov	r0	,xl
	mov	r1	,xh

	rcall	next_int_value			;value
	brtc	monitor_mw_exit

	mov	xl	,r0
	mov	xh	,r1

	cli
	st	X+	,AKKU_5
	st	X+	,AKKU_6
	st	X+	,AKKU_7
	st	X+	,AKKU_8
	sei

monitor_ml_exit:
	ret


;************************************************************
;	Monitor Command MQ
;	Modify Quadword
;
;	MQ <adress|label> <quadword>
;************************************************************

monitor_mq:
	ldi	r16	,L_FRAM		;search ram word adress
	rcall	next_adress
	brtc	monitor_mq_exit

	mov	r0	,xl
	mov	r1	,xh

	rcall	next_int_value			;value
	brtc	monitor_mw_exit

	mov	xl	,r0
	mov	xh	,r1

	cli
	st	X+	,AKKU_S
	st	X+	,AKKU_2
	st	X+	,AKKU_3
	st	X+	,AKKU_4
	st	X+	,AKKU_5
	st	X+	,AKKU_6
	st	X+	,AKKU_7
	st	X+	,AKKU_8
	sei

monitor_mq_exit:
	ret


;************************************************************
;	Monitor Command MF
;	Modify Float
;
;	MB <adress|label> <float>
;************************************************************

monitor_mf:
	ldi	r16	,L_FRAM	;search ram float adress
	rcall	next_adress
	brtc	monitor_mf_exit

	push	xl
	push	xh

	rcall	next_flt_value

	pop	xh
	pop	xl
	brtc	monitor_mf_exit

	call	flt_store_x

monitor_mf_exit:
	ret


;************************************************************
;	Print Byte as Hex-String
;	R16	: Byte
;************************************************************
	
printf_byte_hex:
	push	r16
	push	zl
	push	zh
	push	r0

	ldi	zl	,low(hex_digits<<1)
	ldi	zh	,high(hex_digits<<1)
	push	r16
	swap	r16
	andi	r16	,0x0F
	add	zl	,r16
	clr	r16
	adc	zh	,r16
	lpm
	mov	r16	,r0
	call	putc_func

	ldi	zl	,low(hex_digits<<1)
	ldi	zh	,high(hex_digits<<1)
	pop	r16
	andi	r16	,0x0F
	add	zl	,r16
	clr	r16
	adc	zh	,r16
	lpm
	mov	r16	,r0
	call	putc_func

	pop	r0
	pop	zh
	pop	zl
	pop	r16
	ret

hex_digits:
	.db	48,49,50,51,52,53,54,55
	.db	56,57,65,66,67,68,69,70

;************************************************************
;	print one byte as Binary
;	r16 : byte to print
;************************************************************

printf_byte_binary:
	push	r16
	push	r0
	push	r17

	mov	r0	,r16
	ldi	r17	,8

print_byte_binary_loop:
	rol	r0
	ldi	r16	,48		;'0'
	brcc	print_byte_binary_null
	ldi	r16	,49		;'1'

print_byte_binary_null:
	call	putc_func
	dec	r17
	brne	print_byte_binary_loop

	pop	r17
	pop	r0
	pop	r16
	ret


;************************************************************
next_adress:
	rcall	next_word			;label or address
	brtc	next_adress_exit

	cpi	r16	,0xFF
	brne	next_adress_one_type

	ldi	r16	,L_TYPE_BYTE|L_LOCATIONS	;search for byte label
	rcall	find_label
	brts	next_adress_exit

	ldi	r16	,L_TYPE_WORD|L_LOCATIONS	;search for word label
	rcall	find_label
	brts	next_adress_exit

	ldi	r16	,L_TYPE_LONG|L_LOCATIONS	;search for long label
	rcall	find_label
	brts	next_adress_exit

	ldi	r16	,L_TYPE_FLOAT|L_LOCATIONS	;search for float label

next_adress_one_type:
	rcall	find_label
	brts	next_adress_exit

	rcall	monitor_atoi_x
	mov	xl	,AKKU_8
	mov	xh	,AKKU_7

next_adress_exit:
	ret


;************************************************************
next_int_value:
	rcall	next_word
	brtc	next_value_exit

	rjmp	monitor_atoi_x

next_flt_value:
	rcall	next_word
	brtc	next_value_exit

	jmp	flt_atof_x

next_value_exit:
	ret


;************************************************************
;	print all ports
;************************************************************

monitor_ports:
	puts_rom	monitor_ports_header
	putc	'A'
	putc	':'
	putc	' '
	in	r16	,PORTA
	rcall	printf_byte_binary
	putc	' '
	in	r16	,PINA
	rcall	printf_byte_binary
	putc	' '
	in	r16	,DDRA
	rcall	printf_byte_binary
	putc	13

	putc	'B'
	putc	':'
	putc	' '
	in	r16	,PORTB
	rcall	printf_byte_binary
	putc	' '
	in	r16	,PINB
	rcall	printf_byte_binary
	putc	' '
	in	r16	,DDRB
	rcall	printf_byte_binary
	putc	13

	putc	'C'
	putc	':'
	putc	' '
	in	r16	,PORTC
	rcall	printf_byte_binary
	putc	' '
	in	r16	,PINC
	rcall	printf_byte_binary
	putc	' '
	in	r16	,DDRC
	rcall	printf_byte_binary
	putc	13

	putc	'D'
	putc	':'
	putc	' '
	in	r16	,PORTD
	rcall	printf_byte_binary
	putc	' '
	in	r16	,PIND
	rcall	printf_byte_binary
	putc	' '
	in	r16	,DDRD
	rcall	printf_byte_binary
	putc	13

	putc	'E'
	putc	':'
	putc	' '
	in	r16	,PORTE
	rcall	printf_byte_binary
	putc	' '
	in	r16	,PINE
	rcall	printf_byte_binary
	putc	' '
	in	r16	,DDRE
	rcall	printf_byte_binary
	putc	13

	;putc	'F'
	;putc	':'
	;putc	' '
	;lds	r16	,PORTF
	;rcall	printf_byte_binary
	;putc	' '
	;lds	r16	,PINF
	;rcall	printf_byte_binary
	;putc	' '
	;lds	r16	,DDRF
	;rcall	printf_byte_binary
	;putc	13

	;putc	'G'
	;putc	':'
	;putc	' '
	;lds	r16	,PORTG
	;rcall	printf_byte_binary
	;putc	' '
	;lds	r16	,PING
	;rcall	printf_byte_binary
	;putc	' '
	;lds	r16	,DDRG
	;rcall	printf_byte_binary
	;putc	13

	ret

monitor_ports_header:
	.db	"   PORT     PIN      DDR"
	.db	13, 0


;************************************************************
;	get next word from command line
;
;	arguments:
;
;	returns:
;	xh:xl	: pointer to null terminated string
;	t-flag	: 1=OK,found
;	
;	globals:
;	cmd_ptr	: command line pointer
;************************************************************

next_word:
	push	r16
	push	yl
	push	yh

	clt				;FALSE

	lds	xl	,cmd_ptr		;load pointer
	lds	xh	,cmd_ptr+1

next_expr_loop1:
	ld	r16	,X		;null character ?
	tst	r16
	breq	next_expr_exit

	cpi	r16	,32		;space
	brne	next_expr_found

	adiw	xh:xl	,1		;next character
	rjmp	next_expr_loop1

next_expr_found:
	set				;TRUE

	mov	yl	,xl
	mov	yh	,xh

next_expr_loop2:
	ld	r16	,Y		;load character
	tst	r16
	brne	next_expr_not_null

	sts	cmd_ptr	,yl		;null byte
	sts	cmd_ptr+1	,yh
	rjmp	next_expr_exit

next_expr_not_null:
	cpi	r16	,32		;space
	breq	next_expr_space

	adiw	yh:yl	,1
	rjmp	next_expr_loop2

next_expr_space:
	ldi	r16	,0
	st	Y+	,r16
	sts	cmd_ptr	,yl
	sts	cmd_ptr+1	,yh

next_expr_exit:
	pop	yh
	pop	yl
	pop	r16
	ret


;*****************************************************************************************
;Convert Integer to String
;
;r17:r16	Number
;r18	Digits (max 5)
;xh:xl	String
;*****************************************************************************************

itoa_digits:	.db	"0123456789ABCDEF"
itoa_dec_numbers:	.dw	1,10,100,1000,10000
itoa_hex_numbers:	.dw	0x0001,0x0010,0x0100,0x1000

itoa:
	push	r16
	push	r17
	push	r18
	push	r19
	push	xl
	push	xh
	push	zl
	push	zh

	dec	r18	
	lsl	r18
	clt
itoa_loop:
	ldi	zl	,low(itoa_dec_numbers<<1)
	ldi	zh	,high(itoa_dec_numbers<<1)
	add	zl	,r18
	clr	r0
	adc	zh	,r0

	ldi	r19	,0
	lpm	r0	,Z+
	lpm	r1	,Z+

itoa_sub:
	inc	r19
	sub	r16	,r0
	sbc	r17	,r1
	brcc	itoa_sub

	dec	r19
	add	r16	,r0
	adc	r17	,r1

	tst	r19			;digit > 0 ?
	brne	itoa_01			;yes, print it

	brtc	itoa_02			;no digit printed before ?

itoa_01:
	set
	ldi	zl	,low(itoa_digits<<1)
	ldi	zh	,high(itoa_digits<<1)
	add	zl	,r19
	clr	r0
	adc	zh	,r0
	lpm	r19	,Z
	st	X+	,r19

itoa_02:
	subi	r18	,2
	brpl	itoa_loop

	brts	itoa_03

	st	X+	,r19

itoa_03:
	pop	zh
	pop	zl
	pop	xh
	pop	xl
	pop	r19
	pop	r18
	pop	r17
	pop	r16
	ret


;************************************************************
;	Convert ASCII-String to Binary Number
;	X Register points to String
;************************************************************

monitor_atoi_x:
	push	r16
	push	r17
	push	r18
	push	xl
	push	xh

	clr	AKKU_S
	clr	AKKU_2
	clr	AKKU_3
	clr	AKKU_4
	clr	AKKU_5
	clr	AKKU_6
	clr	AKKU_7
	clr	AKKU_8
	clt

	ldi	r17	,0
	ld	r16	,X
	cpi	r16	,'-'
	brne	akku_atoi_no_minus

	ldi	r17	,1
	adiw	xh:xl	,1

akku_atoi_no_minus:
	ld	r16	,X
	cpi	r16	,'0'
	brne	akku_atoi_dezimal

	adiw	xh:xl	,1
	ld	r16	,X+
	cpi	r16	,'B'
	breq	akku_atoi_binary

	cpi	r16	,'X'
	breq	akku_atoi_hex


	sbiw	xh:xl	,1

akku_atoi_octal:
	set
	ld	r16	,X+
	subi	r16	,'0'
	brmi	akku_atoi_exit		;< '0'

	cpi	r16	,8
	brge	akku_atoi_exit		;> '7'

	ldi	r18	,3

akku_atoi_octal_shift:
	lsl	AKKU_S
	rol	AKKU_2
	rol	AKKU_3
	rol	AKKU_4
	rol	AKKU_5
	rol	AKKU_6
	rol	AKKU_7
	rol	AKKU_8
	dec	r18
	brne	akku_atoi_octal_shift
	
	or	AKKU_8	,r16
	rjmp	akku_atoi_octal
akku_atoi_dezimal:
	ld	r16	,X+
	subi	r16	,'0'
	brmi	akku_atoi_exit		;< '0'

	cpi	r16	,10
	brge	akku_atoi_exit		;> '9'

	set
	rcall	akku_mal10
	add	AKKU_8	,r16
	clr	r16
	adc	AKKU_7	,r16
	adc	AKKU_6	,r16
	adc	AKKU_5	,r16
	adc	AKKU_4	,r16
	adc	AKKU_3	,r16
	adc	AKKU_2	,r16
	adc	AKKU_S	,r16
	rjmp	akku_atoi_dezimal

akku_atoi_binary:
	ld	r16	,X+
	subi	r16	,'0'
	brmi	akku_atoi_exit		;< '0'

	cpi	r16	,2
	brge	akku_atoi_exit		;> '1'

	set
	lsl	AKKU_8
	rol	AKKU_7
	rol	AKKU_6
	rol	AKKU_5
	rol	AKKU_4
	rol	AKKU_3
	rol	AKKU_2
	rol	AKKU_S
	or	AKKU_8	,r16
	rjmp	akku_atoi_binary

akku_atoi_hex:
	ld	r16	,X+

	cpi	r16	,'0'		;< '0'
	brmi	akku_atoi_exit

	cpi	r16	,'F'+1		;> 'F'
	brge	akku_atoi_exit

	cpi	r16	,'9'+1		;> '9'
	brge	akku_atoi_hex_not_09

	subi	r16	,'0'
	rjmp	akku_atoi_hex_calc

akku_atoi_exit:
	tst	r17
	breq	akku_atoi_positive

	com	AKKU_8
	com	AKKU_7
	com	AKKU_6
	com	AKKU_5
	com	AKKU_4
	com	AKKU_3
	com	AKKU_2
	com	AKKU_S
	ldi	r16	,0xFF
	sub	AKKU_8	,r16
	sbc	AKKU_7	,r16
	sbc	AKKU_6	,r16
	sbc	AKKU_5	,r16
	sbc	AKKU_4	,r16
	sbc	AKKU_3	,r16
	sbc	AKKU_2	,r16
	sbc	AKKU_S	,r16

akku_atoi_positive:
	pop	xh
	pop	xl
	pop	r18
	pop	r17
	pop	r16
	ret

akku_atoi_hex_not_09:		
	cpi	r16	,65		;< 'A'
	brmi	akku_atoi_exit
	
	subi	r16	,65 - 10

akku_atoi_hex_calc:
	set

	ldi	r18	,4
akku_atoi_hex_shift:
	lsl	AKKU_8
	rol	AKKU_7
	rol	AKKU_6
	rol	AKKU_5
	rol	AKKU_4
	rol	AKKU_3
	rol	AKKU_2
	rol	AKKU_S
	dec	r18
	brne	akku_atoi_hex_shift

	or	AKKU_8	,r16
	rjmp	akku_atoi_hex

	
	

;************************************************************
;	Multiply Akku by 10
;************************************************************

akku_mal10:
	push	r0
	push	AKKU_S
	push	AKKU_2
	push	AKKU_3
	push	AKKU_4
	push	AKKU_5
	push	AKKU_6
	push	AKKU_7
	mov	r0	,AKKU_8

	lsl	AKKU_8
	rol	AKKU_7
	rol	AKKU_6
	rol	AKKU_5
	rol	AKKU_4
	rol	AKKU_3
	rol	AKKU_2
	rol	AKKU_S

	lsl	AKKU_8
	rol	AKKU_7
	rol	AKKU_6
	rol	AKKU_5
	rol	AKKU_4
	rol	AKKU_3
	rol	AKKU_2
	rol	AKKU_S

	add	AKKU_8	,r0
	pop	r0
	adc	AKKU_7	,r0
	pop	r0
	adc	AKKU_6	,r0
	pop	r0
	adc	AKKU_5	,r0
	pop	r0
	adc	AKKU_4	,r0
	pop	r0
	adc	AKKU_3	,r0
	pop	r0
	adc	AKKU_2	,r0
	pop	r0
	adc	AKKU_S	,r0

	lsl	AKKU_8
	rol	AKKU_7
	rol	AKKU_6
	rol	AKKU_5
	rol	AKKU_4
	rol	AKKU_3
	rol	AKKU_2
	rol	AKKU_S

	pop	r0
	ret
	
.MACRO	a_mul_10
	call	akku_mal10
.ENDMACRO


;************************************************************
;	find label subroutine
;	xh:xl	: pointer to label name
;	r16 	: type and location of label
;
;	xh:xl	:adress
;	t-flag	:1=found
;************************************************************

find_label_sub:
	push	r0
	push	r1
	push	r2
	push	r17
	push	r18
	push	zl
	push	zh

	mov	r18	,r16
	mov	r19	,r16
	andi	r18	,L_TYPES

	mov	r1	,xl		;save string pointer
	mov	r2	,xh

	sbiw	yh:yl	,LABEL_LEN + 4	
	clt				;set status NOT OK

fls_loop:
	adiw	yh:yl	,LABEL_LEN + 4		;next entry

	mov	zl	,yl		;load label type
	mov	zh	,yh
	adiw	zh:zl	,2
	lpm	r17	,Z

	tst	r17			;end of table (0)
	breq	find_label_exit

	andi	r17	,L_LOCATIONS
	and	r17	,r16		;storage (RAM,ROM...) OK ?
	breq	fls_loop			;no, look at next entry

	lpm	r17	,Z
	andi	r17	,L_TYPES

	cpi	r18	,L_TYPE_CMD		;looking for a command label ?
	brcs	fls_no_command			;no

	cp	r18	,r17		;XCMD allowed ?
	brcs	fls_loop			;no

	rjmp	fls_type_ok
		
fls_no_command:
	cp	r17	,r18		;data type OK ?
	brne	fls_loop			;no, look at next entry

fls_type_ok:
	mov	xl	,r1		;set string pointer
	mov	xh	,r2

fls_strcmp_loop:
	adiw	zh:zl	,1		;next char of label name
	lpm
	ldi	r17	,' '
	cp	r0	,r17
	breq	fls_space_found

	ld	r17	,X+
	cp	r17	,r0
	brne	fls_loop			;wrong char, check next entry

	tst	r17			;check all chars until null char
	brne	fls_strcmp_loop

fls_space_found:
	ld	r17	,X+		;next char from command line
	tst	r17			;must be null byte
	brne	fls_loop			;wrong char, check next entry
	
	set				;set status OK

	mov	zl	,yl		;load pointer into xh:xl
	mov	zh	,yh

	lpm	xl	,Z+
	lpm	xh	,Z+
	lpm	r16	,Z

;	lpm
;	mov	xl	,r0
;	adiw	zh:zl	,1
;	lpm
;	mov	xh	,r0
;	adiw	zh:zl	,1
;	lpm
;	mov	r16	,r17		;return label type from table

	rjmp	find_label_ok

find_label_exit:
	mov	xl	,r1		;pointer to label name
	mov	xh	,r2

find_label_ok:
	pop	zh
	pop	zl
	pop	r18
	pop	r17
	pop	r2
	pop	r1
	pop	r0	
	ret

;************************************************************