/*******************************************************************************
 *  bin2bcd16.c - 16 bit binary to BCD conversion.
 *
 *  Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/ 
 *  ALL RIGHTS RESERVED 
 * 
*/

#include <inttypes.h>
#if defined(__MSP430__)
#include <msp430.h>
#endif
#if !(defined(__GNUC__)  &&  defined(__MSP430__))
#include <stdio.h>
#endif
#include "emeter-toolkit.h"

void bin2bcd16(uint8_t bcd[3], uint16_t bin)
{
#if defined(__GNUC__)  &&  defined(__MSP430__)
    int i;
    uint16_t decimal_0;
    uint16_t decimal_1;

    decimal_0 =
    decimal_1 = 0;
    i = 16;
    __asm__ __volatile__(
        "1: \n"
        " rla.w     %[bin] \n"
        " dadd.w    %[decimal_0],%[decimal_0] \n"
        " dadd.w    %[decimal_1],%[decimal_1] \n"
        " dec.w     %[i] \n"
        " jnz       1b \n"
        " mov.b     %[decimal_1],0(%[bcd]) \n"
        " mov.b     %[decimal_0],2(%[bcd]) \n"
        " swpb      %[decimal_0] \n"
        " mov.b     %[decimal_0],1(%[bcd]) \n"
        : [bcd] "+r"(bcd), [decimal_0] "+r"(decimal_0), [decimal_1] "+r"(decimal_1)
        : [bin] "r"(bin), [i] "r"(i));
#else
    int i;
    char buf[6 + 1];
    
    sprintf (buf, "%06d", bin);
    for (i = 0;  i < 3;  i++)
        bcd[i] = ((buf[2*i] & 0x0F) << 4) | (buf[2*i + 1] & 0x0F);
#endif
}
