forth85_09. Numberh OK

;this is forth85_09.
; task today is to get numberh working (all numbers of the form HHHH
; that are in the interpret line of words in buf 1 end up as binary on stack)
; plus all the pointers are right for the next word.
;numh seems to work OK. It stacks HHHHH ie hex vals with 4+ chars. The first 4 anyway.
; but still gives a STOP signal which will clear the tack anyway, later.
;next: save this then do 16 bit + and minus then play with stack nums.
.NOLIST
.include "tn85def.inc"
.LIST
.LISTMAC ;sometimes macro code gets in way of clarity in listing
.MACRO header
	.db high(@0), low(@0), @1, @2
.ENDMACRO
.MACRO mypop
	ld @0,-y
.ENDMACRO
.MACRO mypush
	st y+, @0
.ENDMACRO
.MACRO mypop2
	mypop @0
	mypop @1
.ENDMACRO
.MACRO mypush2
	mypush @0
	mypush @1
.ENDMACRO
.MACRO pushx
	push xl
	push xh
.ENDMACRO
.MACRO popx
	pop xh
	pop xl
.ENDMACRO
.MACRO pushz
	push zl
	push zh
.ENDMACRO
.MACRO popz
	pop zh
	pop zl
.ENDMACRO


.def FOUND = r15	;if found=1 we have a match of Ram word on dictionary
.def BOTTOM = r14	;have hit the bottom of the dict and not found a match
.def STOP = r13		;stop interpreting line of words
.def STATE = r12
.def FOUNDCOUNTER = r11	;dealWithWord clicks this if found =1. Counts successful finds in dictionary.
.def SECONDLETTER =r10	;helpful for debugging
.def vl = r22
.def vh = r23		; u,v,w,x,y,z are all pointers
.DSEG
.ORG 0x60

;consts: .DB "jksdafhsdf",8, 255, 0b01010101, -128, 0xaa
.equ BUF1LENGTH = 64

buf1: .byte BUF1LENGTH
buf2: .byte 64			;could have third buffer?
varSpace: .byte 64		;might need more than 32 variables
;.org 0x1E0
myStackStart: .byte 64

.cseg
.ORG 0x800 ;dictionary starts at 4K (2K  words) mark
;----------------------------------------------------
one_1:
.db 0,0,3, "one" ;code for one
one:
;	rcall stackme
	rcall stackme_2
.db 01, 00
ret
;----------------------------------------------
two_1:
header one_1, 3, "two"
two:
	rcall stackme_2
	.db 02,00
ret
;------------------------------------------
dup_1:
header two_1,3,"dup"
dup:
	mypop r17
	mypop r16
	mypush r16
	mypush r17
	mypush r16
	mypush r17

ret
;-------------------------------------------
drop_1:
header dup_1,4,"drop"
drop:
	mypop r17
	mypop r16			;TODO what if stack pointer goes thru floor?
ret
;----------------------------------
swapp_1:		;twp p's becasue assembler recognizes avr opcode swap
header drop_1,5, "swapp"
swapp:
	mypop2 r17,r16
	mypop2 r19,r18
	mypush2 r16,r17
	mypush2 r18,r19
ret


;-------------------------------------------------
		;shift this later

S_1:
;the EOL token that gets put into end of buf1 to stop parsing
header swapp_1,1,"S"
S:
	clr STOP
	inc STOP	;set time-to-quit flag
ret
;------------------------------------------

fetch_1:	;doesn't like label = @-1
;classic fetch. (adr -- num). Only in RAM
header S_1,1,"@"
fetch:
	pushx	;going to use x to point so better save
	mypop xh
	mypop xl
	ld r16,x+
	ld r17,x
	mypush r16
	mypush r17	; and put them on my stack
	popx		;return with x intact and RAM val on my stack
ret
;dddddddddddddddddddddddddddddddddddddddddddddddd

cfetch_1:	;doesn't like label = c@-1
;classic fetch. (adr -- num). Only in RAM. Do I want y to advance just one byte on mystack
header fetch_1,2,"c@"
cfetch:
	pushx	;going to use x to point so better save
	mypop xh
	mypop xl
	ld r16,x+
	mypush r16
	popx		;return with x intact and RAM val on my stack
ret
;dddddddddddddddddddddddddddddddddddddddddddddddd

store_1:		;classic != "store"(adr num --) . Num is now at cell adr.
header cfetch_1,1,"!"
store:
	mypop2 r17,r16		;there goes the num
	pushx
	mypop2 xh,xl		;there goes the address
	st x+,r16
	st x,r17			;num goes to cell with location=adr
	popx
ret
;ddddddddddddddddddddddddddddddddddddddddddddddddddd

cstore_1:		;classic c!= "store"(adr 8-bitnum --) . 8 bit Num is now at cell adr.
header store_1,2,"c!"
cstore:
	mypop r16		;there goes the num. Just 8 bits at this stage.
	pushx
	mypop2 xh,xl		;there goes the address
	st x+,r16
;	st x,r17			;num goes to cell with location=adr
	popx
ret
;------------------------------------

star_1:		;classic 16*16 mulitply (n n -- n*n)
header cstore_1,1,"*"
star:
	mypop2 r17,r16
	mypop2 r19,r18	;now have both numbers in r16..r19
	rcall mpy16s	; multiply them. Result in r18..r21. Overflow in r20,21
	mypush2 r18,r19
ret
;-----------------------------------------
LATEST:
slashMod_1:		;classic /MOD (n m -- n/m rem)
header star_1,4,"/mod"
slashMod:
	mypop2 r19,r18	; that's m
	mypop2 r17,r16	;that's n
	rcall div16s	;the the 16 by 16 bit divsion
	mypush2 r16,r17	;answer ie n/m
	mypush2 r14,r15	;remainder
ret






;-------------------------------------------------
stackme_2:		;stacks on my stack next 16bit num. Address of 16bit number is on SP-stack
	; Used like this stackme_2 0034. Puts 0034 on myStack and increments past number on return stack.
	pop r17
	pop r16		; they now contain eg 0x0804 which contain the 16bit num
	movw zl,r16	;z now points to cell that cobtains the number
	clc
	rol zl
	rol zh		;double word address  for z. lpm coming up



	lpm r16,z+
	lpm r17,z+	;now have 16bit number in r16,17

	st y+,r16
	st y+, r17	;mystack now contains the number

	clc
	ror zh
	ror zl	;halve the z pointer to step past the number to return at the right place

	push zl
	push zh

ret



;====================================================================================================

.ORG 0
	rjmp start
typein: .db " one two  14568 1298", 0x0d

;stackme dropx onex stackme swap drop",0x0d
start:
	ldi r16, low(RAMEND)
	out SPL, r16
	ldi r16,high(RAMEND)
	out SPH, r16

	ldi YL,low(myStackStart)
	ldi YH,high(myStackStart)

;rjmp test_interpretLine
;rjmp test_cfetch
;rjmp test_store
;rjmp test_cstore
;rjmp test_mpy16s
;rjmp test_mpy16s0
;rjmp test_star
;rjmp test_div16s
;rjmp test_slashMod
;rjmp test_Hex4ToBin2
rjmp test_interpretLine

	rjmp start

getline0:	;force a line into buf1 via flash string. Simulates GETLINE
	ldi zl, low(typein<<1)
	ldi zh, high(typein<<1)
	ldi xl, low(buf1)
	ldi xh, high(buf1)
type0:
	lpm r16,Z+
	st x+,r16
	cpi r16,0x0d		;have we got to the end of the line?
	brne type0
ret
;--------------------------------------------
;WORD gets x to point to start of word (copy in w=r24,25) with the length in len = r20
;assume word points to somewhere in buf1. Should advance thru spaces=0x20 to first real char
word: 			;maybe give it a header later
	ld r16,x+	;get char
	ld SECONDLETTER, x	;for debugging

	cpi r16,0x20	;is it a space?
	breq word		;if so get next char
;if here we're point to word start. so save this adr in w
	mov r24,xl
	mov r25,xh		;wordstart now saved in w


	clr r20			;length initially 0
nextchar:
	inc r20			;r20 = word length
	ld r16,x+		;get next char
	cpi r16,0x20
	brne nextchar
	dec r24			;adjust start of word
;if here we've found a word.Starting at w length in r20.x points to space just past word
ret
;----------------------------------------

compare: 		;given a word in buf1 and a word in the dic are they the same? The word in the dic is pointed to by Z.
				; and the word in buf1 is pointed to by w=r24,25. len = r20. Z on entry points to the link. Needs +2 to
	lpm r23,z+
	lpm r22,z+	;store next link in v=r22,23. z now points to len byte

startc:
	push r20	;save length
	lpm r16,Z+	;length of dictionary word, first entry now in r16
	cp r16,r20	;same lengths?
	brne outcom	;not = so bail out
;if here the words are the same length, what about the rest of the chars.First get x to point to word.
	mov xl,r24
	mov xh,r25	;x now point to start of buf1 word
upcom:
	lpm r16,z+
	ld r17,x+	;get one corresponding char from each word
	cp r16,r17	;same word?
	brne outcom	;bail out if chars are different
	dec r20		;count chars
	brne upcom	;still matching and not finished so keep going
;if here r20 is 0 so match must have been perfect so FOUND = 1
	clr FOUND
	inc FOUND
outcom:
	pop r20		;get old lngth of buf1 word back
ret
;-------------------------------------------
jmpNextWord:	;go to next word in the dictionary. Assume v=r22,23 contains next link word(not byte)
				; and w = r24,25 contains RAM word start with len in r20
				;exit with z pointing to next word ready for next COMPARE.
	clc
	rol r22
	rol r23		;above 3 instructions change word address into byte address by doubling
	movw r30,r22	;z now points to next word
ret
;-----------------------------------------

doLatest:	;set up so first jump in dictionary is to top=LATEST and other flags set up.
	ldi vl, low(LATEST)
	ldi vh, high(LATEST)
	clr FOUND
	clr BOTTOM		;not yet found the match, not yet at the bottom. Either will stop search.
	clr STOP		;keep parsing words til this goes to a 1
ret
;---------------------------------------------
interpretLine:		;given line of words in buf one, search for words one by one. Don't do code
					; or compile at this stage, just find and report that and go into next one.
	rcall getline0	;change later to real getline via terminal
	rcall pasteEOL
	ldi xl, low(buf1)
	ldi xh,high(buf1)	;last 3 statemnts are done onece. Now the main loop.
	clr FOUNDCOUNTER	;counts finds in line parsing.

nextWord:
	tst STOP
	brne stopLine
	rcall word
	rcall findWord			;not done yet
	rcall dealWithWord		;go and run code STATE=0, or compile (STATE =1).{ c0de, comp1le}
	rjmp nextWord
stopLine:
	ret
;-----------------------------------------------------------------
findWord:
	rcall doLatest
upjmpf:
	rcall jmpNextWord
	rcall compare
	tst FOUND
	brne stopsearchf		;if last compare got a match (FOUND=1) then stop searching
	tst vl
	brne upjmpf			;if v=0000 then we've hit the bottom of the dictionary
	tst vh
	brne upjmpf			;not found and not at bottom so keep going
;if here FOUND =0, ie no match yet and we've hit the bottom of the dictionary
	clr BOTTOM
	inc BOTTOM			;exit with FOUND=0 and BOTTOM =1
stopsearchf: nop
ret
;----------------------------
test_interpretLine:
	rcall interpretLine
til: rjmp til ;** with r24 pointing to 'S' and FOUND = r15 =1
;------------------------------
dealWithWord:			;come here when it's time to compile or run code
;Good debugging spot. Enter here with Z pointing to CFA of word found. Y points to myStack. X points to just
; past the word we are seeking (w-s). r10 is 2nd letter of w-s. w = start adr of w-s. v is a link
; to the next word in dic. Either just below the found word or 0000 if we get to the bottome with no match
;
	nop
	tst FOUND
	breq notfound
	inc FOUNDCOUNTER

	;want to hop over filler bytes,0's added to keep codes on even byte boundaries
	; so if r30 is odd at this stage inc it. odd is lsbit = 1.
	sbrs r30,0 	;skip 	next instruction if final bit lsb = 1
	rjmp downdw
;if here lsb = 1 so we're on a padding byte and have to add 1 to get to a 2 byte boundary
	inc r30
	brcc downdw
	inc r31		;add one to z before converting to bytes

downdw:
		clc
	ror zh
	ror zl		;put z back into word values


	rcall executeCode



	.MESSAGE "Word found"
	rjmp outdww
notfound:
	nop
;	.MESSAGE "Word not found"
;	clr STOP
;	inc STOP		;stop parsing line
	rcall numberh		; word not in dict so must be a number? Form = HHHH
;now have to add 3 to x so it points past this word ready not next one
	clc
	inc r26
	inc r26
	inc r26
	brcc outdww
	inc r27		;but only if overflow
	nop
outdww:
ret					;with STOP =1 in not a number
;------------------------------------------------------------------------
pasteEOL:		;when a line of text is TYPEd into buf1 it should end with CR=$0d. This gets replaced with ]}, a
	; special end of line word. When the word is invoked it casues a QUIT back to  the waiting for input stage.
	; Start at buf1 start and inspect each char for a $0D. When found replace with a "$20 S $20 "
	ldi xl, low(buf1)
	ldi xh, high(buf1)	;pnt to start of buffer
	clr r17
nxtChar:
	inc r17				;r17 is counter. Bail out when r17 > BUF1LENGTH
	cpi r17, BUF1LENGTH -4
	breq outProb
	ld r16, x+
	cpi r16, $0d
	brne nxtChar
;if here we've found a $0d in buf1 before the end, so replace with an EOL token. x points to just after it.
	ldi r16,$20
	st -x, r16		;back up. Then go forward.
;	ldi r16, ']'
	st x+, r16
	ldi r16,'S'
	st x+, r16
;	ldi r16, '}'
;	st x+, r16
	ldi r16, $20
	st x, r16
	rjmp outpel


outProb:
	nop
	.MESSAGE "Couldn't find $0d"
outpel:
ret

;-------------------------------------
executeCode:		;with Z pointing to cfa. Not sure whether to jmp or call

	ijmp
	ret
;---------------------------------------
test_fetch:		;do run thru of @
	rcall getline0	;change later to real getline via terminal
	rcall pasteEOL
	ldi xl, low(buf1)
	ldi xh,high(buf1)	;last 3 statemnts are done onece. Now the main loop.

	ldi r16,$62
	mypush r16
	ldi r16,$0
	mypush r16		;should now have adr $0062 on mystack
	rcall fetch
tf1:
	rjmp tf1
;---------------------------------
test_cfetch:		;do run thru of @
	rcall getline0	;change later to real getline via terminal
	rcall pasteEOL
	ldi xl, low(buf1)
	ldi xh,high(buf1)	;last 3 statemnts are done onece. Now the main loop.

	ldi r16,$62
	mypush r16
	ldi r16,$0
	mypush r16		;should now have adr $62 on mystack
	rcall cfetch
tcf1:
	rjmp tcf1
;----------------------------
test_store:
	rcall getline0	;change later to real getline via terminal
	rcall pasteEOL
	ldi xl, low(buf1)
	ldi xh,high(buf1)	;last 3 statemnts are done onece. Now the main loop.
	ldi r16,$62
	ldi r17,$0
	mypush2 r16,r17 	;should now have adr $62 on mystack
	ldi r16, $AB
	ldi r17, $CD
	mypush2 r16,r17		;now have $ABCD on mystack
	rcall store
ts1:
	rjmp ts1
;------------------------
test_cstore:
	rcall getline0	;change later to real getline via terminal
	rcall pasteEOL
	ldi xl, low(buf1)
	ldi xh,high(buf1)	;last 3 statemnts are done onece. Now the main loop.
	ldi r16,$62
	ldi r17,$0
	mypush2 r16,r17 	;should now have adr $62 on mystack
	ldi r16, $AB
;	ldi r17, $CD
	mypush r16	;now have $ABCD on mystack
	rcall cstore

ts11:
	rjmp ts11
;Now put arith routines here. Are from AVR200. Just using 16*16 for * but get 32bit result.


;***************************************************************************
;*
;* "mpy16s" - 16x16 Bit Signed Multiplication
;*
;* This subroutine multiplies signed the two 16-bit register variables
;* mp16sH:mp16sL and mc16sH:mc16sL.
;* The result is placed in m16s3:m16s2:m16s1:m16s0.
;* The routine is an implementation of Booth's algorithm. If all 32 bits
;* in the result are needed, avoid calling the routine with
;* -32768 ($8000) as multiplicand
;*
;* Number of words	:16 + return
;* Number of cycles	:210/226 (Min/Max) + return
;* Low registers used	:None
;* High registers used  :7 (mp16sL,mp16sH,mc16sL/m16s0,mc16sH/m16s1,
;*			    m16s2,m16s3,mcnt16s)
;*
;***************************************************************************

;***** Subroutine Register Variables

.def	mc16sL	=r16		;multiplicand low byte
.def	mc16sH	=r17		;multiplicand high byte
.def	mp16sL	=r18		;multiplier low byte
.def	mp16sH	=r19		;multiplier high byte
.def	m16s0	=r18		;result byte 0 (LSB)
.def	m16s1	=r19		;result byte 1
.def	m16s2	=r20		;result byte 2
.def	m16s3	=r21		;result byte 3 (MSB)
.def	mcnt16s	=r22		;loop counter

;***** Code
mpy16s:	clr	m16s3		;clear result byte 3
	sub	m16s2,m16s2	;clear result byte 2 and carry
	ldi	mcnt16s,16	;init loop counter
m16s_1:	brcc	m16s_2		;if carry (previous bit) set
	add	m16s2,mc16sL	;    add multiplicand Low to result byte 2
	adc	m16s3,mc16sH	;    add multiplicand High to result byte 3
m16s_2:	sbrc	mp16sL,0	;if current bit set
	sub	m16s2,mc16sL	;    sub multiplicand Low from result byte 2
	sbrc	mp16sL,0	;if current bit set
	sbc	m16s3,mc16sH	;    sub multiplicand High from result byte 3
	asr	m16s3		;shift right result and multiplier
	ror	m16s2
	ror	m16s1
	ror	m16s0
	dec	mcnt16s		;decrement counter
	brne	m16s_1		;if not done, loop more
	ret
;----------------------------------------------------------
;***** Multiply Two Signed 16-Bit Numbers (-12345*(-4321))
test_mpy16s:
	ldi	mc16sL,low(-12345)
	ldi	mc16sH,high(-12345)
	ldi	mp16sL,low(-4321)
	ldi	mp16sH,high(-4321)
	rcall	mpy16s		;result: m16s3:m16s2:m16s1:m16s0
				;=$032df219 (53,342,745)
tmpy: rjmp tmpy

test_mpy16s0:
	ldi	mc16sL,low(123)
	ldi	mc16sH,high(123)
	ldi	mp16sL,low(147)
	ldi	mp16sH,high(147)
	rcall	mpy16s		;result: m16s3:m16s2:m16s1:m16s0
tmpy0: rjmp tmpy0
;-----------------------
test_star:
	ldi r16,-$7b
	mypush r16
	ldi r16,$00
	mypush r16	;that's decimal 123 on stack
	ldi r16,$93
	mypush r16
	ldi r16,$00
	mypush r16	; and thats dec'147
	rcall star
tsr:	rjmp tsr

;--------------------------
;***************************************************************************
;*
;* "div16s" - 16/16 Bit Signed Division
;*
;* This subroutine divides signed the two 16 bit numbers
;* "dd16sH:dd16sL" (dividend) and "dv16sH:dv16sL" (divisor).
;* The result is placed in "dres16sH:dres16sL" and the remainder in
;* "drem16sH:drem16sL".
;*
;* Number of words	:39
;* Number of cycles	:247/263 (Min/Max)
;* Low registers used	:3 (d16s,drem16sL,drem16sH)
;* High registers used  :7 (dres16sL/dd16sL,dres16sH/dd16sH,dv16sL,dv16sH,
;*			    dcnt16sH)
;*
;***************************************************************************

;***** Subroutine Register Variables

.def	d16s	=r13		;sign register
.def	drem16sL=r14		;remainder low byte
.def	drem16sH=r15		;remainder high byte
.def	dres16sL=r16		;result low byte
.def	dres16sH=r17		;result high byte
.def	dd16sL	=r16		;dividend low byte
.def	dd16sH	=r17		;dividend high byte
.def	dv16sL	=r18		;divisor low byte
.def	dv16sH	=r19		;divisor high byte
.def	dcnt16s	=r20		;loop counter

;***** Code

div16s:	mov	d16s,dd16sH	;move dividend High to sign register
	eor	d16s,dv16sH	;xor divisor High with sign register
	sbrs	dd16sH,7	;if MSB in dividend set
	rjmp	d16s_1
	com	dd16sH		;    change sign of dividend
	com	dd16sL
	subi	dd16sL,low(-1)
	sbci	dd16sL,high(-1)
d16s_1:	sbrs	dv16sH,7	;if MSB in divisor set
	rjmp	d16s_2
	com	dv16sH		;    change sign of divisor
	com	dv16sL
	subi	dv16sL,low(-1)
	sbci	dv16sL,high(-1)
d16s_2:	clr	drem16sL	;clear remainder Low byte
	sub	drem16sH,drem16sH;clear remainder High byte and carry
	ldi	dcnt16s,17	;init loop counter

d16s_3:	rol	dd16sL		;shift left dividend
	rol	dd16sH
	dec	dcnt16s		;decrement counter
	brne	d16s_5		;if done
	sbrs	d16s,7		;    if MSB in sign register set
	rjmp	d16s_4
	com	dres16sH	;        change sign of result
	com	dres16sL
	subi	dres16sL,low(-1)
	sbci	dres16sH,high(-1)
d16s_4:	ret			;    return
d16s_5:	rol	drem16sL	;shift dividend into remainder
	rol	drem16sH
	sub	drem16sL,dv16sL	;remainder = remainder - divisor
	sbc	drem16sH,dv16sH	;
	brcc	d16s_6		;if result negative
	add	drem16sL,dv16sL	;    restore remainder
	adc	drem16sH,dv16sH
	clc			;    clear carry to be shifted into result
	rjmp	d16s_3		;else
d16s_6:	sec			;    set carry to be shifted into result
	rjmp	d16s_3

;-----------------------------------------------

test_div16s:
;***** Divide Two Signed 16-Bit Numbers (-22,222/10)
	ldi	dd16sL,low(-22222)
	ldi	dd16sH,high(-22222)
	ldi	dv16sL,low(10)
	ldi	dv16sH,high(10)
	rcall	div16s		;result:	$f752 (-2222)
				;remainder:	$0002 (2)

forever:rjmp	forever
;----------------------------------
test_slashMod:
	ldi r16,$12
	mypush r16
		ldi r16,$34
	mypush r16
		ldi r16,$56 ;NB this is $3412 not $1234
	mypush r16
		ldi r16,$00
	mypush r16
	rcall slashMod		;$3412 / $56 = $9b rem 0 works
tslm:	rjmp tslm

;---------------------------------------
;From http://www.avr-asm-tutorial.net/avr_en/calc/CONVERT.html#hex2bin
; Hex4ToBin2
; converts a 4-digit-hex-ascii to a 16-bit-binary
; In: Z points to first digit of a Hex-ASCII-coded number
; Out: T-flag has general result:
;   T=0: rBin1H:L has the 16-bit-binary result, Z points
;     to the first digit of the Hex-ASCII number
;   T=1: illegal character encountered, Z points to the
;     first non-hex-ASCII character
; Used registers: rBin1H:L (result), R0 (restored after
;   use), rmp
; Called subroutines: Hex2ToBin1, Hex1ToBin1

.def	rBin1H	=r17
.def	rBin1L = r16
.def rmp = r18
;
Hex4ToBin2:
	clt ; Clear error flag
	rcall Hex2ToBin1 ; convert two digits hex to Byte
	brts Hex4ToBin2a ; Error, go back
	mov rBin1H,rmp ; Byte to result MSB
	rcall Hex2ToBin1 ; next two chars
	brts Hex4ToBin2a ; Error, go back
	mov rBin1L,rmp ; Byte to result LSB
	sbiw ZL,4 ; result ok, go back to start
Hex4ToBin2a:
	ret
;
; Hex2ToBin1 converts 2-digit-hex-ASCII to 8-bit-binary
; Called By: Hex4ToBin2
;
Hex2ToBin1:
	push R0 ; Save register
	rcall Hex1ToBin1 ; Read next char
	brts Hex2ToBin1a ; Error
	swap rmp; To upper nibble
	mov R0,rmp ; interim storage
	rcall Hex1ToBin1 ; Read another char
	brts Hex2ToBin1a ; Error
	or rmp,R0 ; pack the two nibbles together
Hex2ToBin1a:
	pop R0 ; Restore R0
	ret ; and return
;
; Hex1ToBin1 reads one char and converts to binary
;
Hex1ToBin1:
	ld rmp,z+ ; read the char
	subi rmp,'0' ; ASCII to binary
	brcs Hex1ToBin1b ; Error in char
	cpi rmp,10 ; A..F
	brcs Hex1ToBin1c ; not A..F
	cpi rmp,$30 ; small letters?
	brcs Hex1ToBin1a ; No
	subi rmp,$20 ; small to capital letters
Hex1ToBin1a:
	subi rmp,7 ; A..F
	cpi rmp,10 ; A..F?
	brcs Hex1ToBin1b ; Error, is smaller than A
	cpi rmp,16 ; bigger than F?
	brcs Hex1ToBin1c ; No, digit ok
Hex1ToBin1b: ; Error
	sbiw ZL,1 ; one back
	set ; Set flag
Hex1ToBin1c:
	ret ; Return
;--------------------------------------
test_Hex4ToBin2:
	pushz
	ldi zl,$60
	clr zh			;z now points to start of buf1
	ldi r16,'0'
	st z+,r16
ldi r16,'f'
	st z+,r16
ldi r16,'2'
	st z+,r16
ldi r16,'3'
	st z+,r16
		ldi zl,$60
	clr zh			;z now points back to start of buf1
	rcall Hex4ToBin2
	popz
th4:	rjmp th4
;-------------------------------------
numberh:		;word not in dictionary. Try to convert it to hex.
	pushz		;algorithm uses z, pity
	movw zl,r24	;r4,25 = w holds start of current word
;z now points eg to '12ab'start. If t=0 then it coverts to real hex
	rcall hex4ToBin2	;try to convert
;above call needs 4 hex digits to emerge with t=0 and binary in r16,17
;want this. If t=0 stack r16,17 and carry on interpreting, else emerge with
; t=1 and zpointing to first problem char
	brtc gotHex
; if here there's a problem that z is pointing to. Bail out of interpret line
	clr STOP
	inc STOP
	rjmp outnh

gotHex: 		;sucess.Real hex in r16,17
	mypush2 r16,r17	; so push num onto mystack
outnh:
	popz	; but will it be pointing to "right"place in buf1?

	ret
; numberh not working fully, ie doesn't point to right place after action.
; also no action if not a number? TODO	better save this first.