;Convert binary [Tmp1:Tmp2] assumed to be <= 9999 to 4 BCD digits
;returned in [Tmp1:Tmp2]
;Input Tmp1,Tmp2 output Tmp1,Tmp2 BCD format
;Return carry=1 - succes
;else carry=0 - when ([Tmp1:Tmp2] >9999) 
CBCD16:
	ldi	xh,high(10000)
	ldi	xl,low(10000)
	rcall	cp16
	brcc	err_bcd
	ldi	xh,high(1000)
	ldi	xl,low(1000)
	clr	op1l
cbcd1000:
	rcall	cp16
	brcs	cbcd161
	inc	op1l
	rcall	subi16
	rjmp	cbcd1000
cbcd161:
	push	op1l
	clr	op1l
	clr	xh
	ldi	xl,100
cbcd100:
	rcall	cp16
	brcs	cbcd162
	inc	op1l
	rcall	subi16
	rjmp	cbcd100
cbcd162:
	push	op1l
	clr	op1l
	ldi	xl,10
cbcd10:
	cpi	tmp2,10
	brlo	cbcd1
	inc	op1l
	subi	tmp2,10
	rjmp	cbcd10
cbcd1:
	swap	op1l
	or	tmp2,op1l
	pop	tmp1
	pop	op1l
	swap	op1l
	or	tmp1,op1l
	ret
err_bcd:
	clc
	ret
cp16:
	cp	tmp2,xl
	cpc	tmp1,xh
	ret
subi16:
	sub	tmp2,xl
	sbc	tmp1,xh
	ret