/* Write a function to convert a 16-bit number 
 * into a packed BCD number.
 * Return packed BCD result in register r24.
 * For example if r24 = 0x32 then result r24 = 0b0010 0000
 *
 * Input:  r25:r24 a 16-bit binary number (0 to 65,535)
 * Output: r25:r24,r23:r22:r21:r20  (r25 = 0)
 *
 * Source: Beginners Introduction to the Assembly Language of ATMEL­AVR­Microprocessors
 *         http://www.ic.unicamp.br/~celio/mc404-2008/docs/beginner_avr.pdf
 */

.INCLUDE <m328pdef.inc> 

BINARYtoBCD:
; Conversion of a 16-bit binary number to BCDs
    push  r15
	in    r15, SREG
	push  r19
	push  r18
	push  r17
	push  r16

	mov   r17, r25
	mov   r16, r24

	clr   r25          ;  0
	ser   r24          ; -1
	ser   r23
	ser   r22
	ser   r21
	ser   r20

    ldi   r19,0x27    ; subtract 10,000 (0x2710) until an overflow occurs, 
	ldi	  r18,0x10
digit_4:
	inc   r24         ; yielding the most significant digit.
	sub   r16,r18
	sbc   r17,r19
	brcc  digit_4
	add   r16,r18
	adc   r17,r19

    ldi   r19,0x03   ; subtract 1,000 (0x03E8) until an overflow occurs, 
	ldi	  r18,0xE8
digit_3:
	inc   r23        ; yielding the next digit.
	sub   r16,r18
	sbc   r17,r19
	brcc  digit_3
	add   r16,r18
	adc   r17,r19

	ldi	  r18,0x64  ; subtract 100 (0x0064) until an overflow occurs, 
digit_2:
	inc   r22       ; yielding the next digit.
	sub   r16,r18
	brcc  digit_2
	add   r16,r18

	ldi	  r18,0x0A  ; subtract 10 (0x000A) until an overflow occurs, 
digit_1:
	inc   r21
	sub   r16,r18
	brcc  digit_3   ; yielding the next digit.
	add   r16,r18

	mov   r20,r16   ; remainder is the least significant digit

	pop   r16
	pop   r17
	pop   r18
	pop   r19
	out   SREG, r15
	pop   r15
	ret