/*
 * ul_bcd.c
 *
 *  Created on: Sep 11, 2011
 *      Author: qwer1
 */

#include "ul_bcd.h"

U8 asc_point = '.';
U8 asc_space = ' ';
U8 asc_minus = '-';



U8 bcd2bin_U8( U8 bcd ) {
    return ( (bcd & 0xf) + (bcd >> 4) * 10 );
}
U8 bin2bcd_U8( U8 bin ) {
    U8 tens = 0;
    while (bin > 9) { bin -= 10; tens++; }
    return ( (tens << 4) + bin );
}

#ifdef __AVR__
    U32 bin2bcd_U32_avr(U32 data, U8 result_bytes) __attribute__((naked)) __attribute__((noinline));
    U32 bin2bcd_U32_avr(U32 data, U8 result_bytes) {
        U32 result;
        asm volatile (
            "push __tmp_reg__ \n"
            "mov __tmp_reg__, %[result_bytes] \n" // __tmp_reg__ = number of bytes of result

            "bin2bcd_U32_00: \n"      // correct input number by left shifts
                "mov r31, %A[data] \n"
                "mov %A[data], %B[data] \n"
                "mov %B[data], %C[data] \n"
                "mov %C[data], %D[data] \n"
                "mov %D[data], r31 \n"
                "dec __tmp_reg__ \n"
                "brne bin2bcd_U32_00 \n"

            "eor r26, r26 \n"         // result=0
            "eor r27, r27 \n"
            "eor r30, r30 \n"
            "eor r31, r31 \n"
            "mov __tmp_reg__, %[result_bytes] \n"
            "lsl __tmp_reg__ \n"      // __tmp_reg__ = size in bits
            "lsl __tmp_reg__ \n"
            "lsl __tmp_reg__ \n"

            "bin2bcd_U32_01: \n"      // bits shift and correction loop
                "subi r26,-0x33 \n"   // add 0x33
                "sbrs r26, 3 \n"      // if carry to bit 3,
                "subi r26, 3 \n"      // subtract 3
                "sbrs r26, 7 \n"      // if carry to bit 7,
                "subi r26, 0x30 \n"   // subtract 0x30
                "subi r27,-0x33 \n"   // add 0x33
                "sbrs r27, 3 \n"      // if carry to bit 3,
                "subi r27, 3 \n"      // subtract 3
                "sbrs r27, 7 \n"      // if carry to bit 7,
                "subi r27, 0x30 \n"   // subtract 0x30
                "subi r30,-0x33 \n"   // add 0x33
                "sbrs r30, 3 \n"      // if carry to bit 3,
                "subi r30, 3 \n"      // subtract 3
                "sbrs r30, 7 \n"      // if carry to bit 7,
                "subi r30, 0x30 \n"   // subtract 0x30
                "subi r31,-0x33 \n"   // add 0x33
                "sbrs r31, 3 \n"      // if carry to bit 3,
                "subi r31, 3 \n"      // subtract 3
                "sbrs r31, 7 \n"      // if carry to bit 7,
                "subi r31, 0x30 \n"   // subtract 0x30
                "lsl r26 \n"          // shift result number
                "rol r27 \n"
                "rol r30 \n"
                "rol r31 \n"

                "sbrc %D[data], 7 \n" // skip if msbit of input == 0
                "ori  r26, 1 \n"      // set lsb of output
                "lsl %A[data] \n"     // shift input number
                "rol %B[data] \n"
                "rol %C[data] \n"
                "rol %D[data] \n"

                "dec __tmp_reg__ \n"
                "brne bin2bcd_U32_01 \n" // repeat for all bits

            //"mov %A[result], r26 \n"     // move to result
            //"mov %B[result], r27 \n"
            //"mov %C[result], r30 \n"
            //"mov %D[result], r31 \n"

            "mov r22, r26 \n"     // move to result
            "mov r23, r27 \n"
            "mov r24, r30 \n"
            "mov r25, r31 \n"

            "pop __tmp_reg__ \n"
            "ret \n"
            :[result]"=r"(result) // output
            :[data]"r"(data),[result_bytes]"r"(result_bytes) // input
            :"r26","r27","r30","r31" // clobbers
        );
        return result;
    }
#endif

U32 bin2bcd_U32_soft(U32 data, U8 result_bytes) {
    U32 result = 0; /*result*/
    for (U8 cnt_bytes=(4 - result_bytes); cnt_bytes; cnt_bytes--) /* adjust input bytes */
        data <<= 8;
    for (U8 cnt_bits=(result_bytes << 3); cnt_bits; cnt_bits--) { /* bits shift loop */
        /*result BCD nibbles correction*/
        result += 0x33333333;
        /*result correction loop*/
        for (U8 cnt_bytes=4; cnt_bytes; cnt_bytes--) {
            U8 corr_byte = result >> 24;
            if (!(corr_byte & 0x08)) corr_byte -= 0x03;
            if (!(corr_byte & 0x80)) corr_byte -= 0x30;
            result <<= 8; /*shift result*/
            result += corr_byte; /*set 8 bits of result*/
        }
        /*shift next bit of input to result*/
        result <<= 1;
        if (((U8)(data >> 24)) & 0x80)
            result |= 1;
        data <<= 1;
    }
    return(result);
}

#ifdef __AVR__
    U32 bcd2bin_U32_avr(U32 data, U8 result_bytes) __attribute__((naked)) __attribute__((noinline));
    U32 bcd2bin_U32_avr(U32 data, U8 result_bytes) {
        U32 result;
        asm volatile (
            "push __tmp_reg__ \n"
            "eor r26, r26 \n"           /* result = 0 */
            "eor r27, r27 \n"
            "eor r30, r30 \n"
            "eor r31, r31 \n"
            "mov __tmp_reg__, %A[result_bytes] \n"   /* __tmp_reg__ = size in bits of input parameter  */
            "lsl __tmp_reg__ \n"
            "lsl __tmp_reg__ \n"
            "lsl __tmp_reg__ \n"

            "bcd2bin_U32_00: \n"        /* bits shift and correction loop */
                "lsr r31 \n"            /* shift out buffer */
                "ror r30 \n"
                "ror r27 \n"
                "ror r26 \n"

                "sbrc %A[data], 0 \n"        /* move lowest bit to result */
                "ori  r31, 0x80 \n"

                "lsr %D[data] \n"
                "ror %C[data] \n"
                "ror %B[data] \n"
                "ror %A[data] \n"

                "sbrc %D[data], 7 \n"        /* if carry to bit 7, */
                "subi %D[data], 0x30 \n"     /* subtract 0x30 */
                "sbrc %D[data], 3 \n"        /* if carry to bit 3, */
                "subi %D[data], 3 \n"        /* subtract 3 */
                "sbrc %C[data], 7 \n"        /* if carry to bit 7, */

                "subi %C[data], 0x30 \n"     /* subtract 0x30 */
                "sbrc %C[data], 3 \n"        /* if carry to bit 3, */
                "subi %C[data], 3 \n"        /* subtract 0x30 */
                "sbrc %B[data], 7 \n"        /* if carry to bit 7, */
                "subi %B[data], 0x30 \n"     /* subtract 0x30 */
                "sbrc %B[data], 3 \n"        /* if carry to bit 3, */
                "subi %B[data], 3 \n"        /* subtract 3 */
                "sbrc %A[data], 7 \n"        /* if carry to bit 7, */
                "subi %A[data], 0x30 \n"     /* subtract 0x30 */
                "sbrc %A[data], 3 \n"        /* if carry to bit 3, */
                "subi %A[data], 3 \n"        /* subtract 3 */

                "dec __tmp_reg__ \n"    /* repeat for all bits */
                "brne bcd2bin_U32_00 \n"

//            "movw %A[result], r26 \n"          /* adjust result */
//            "movw %C[result], r30 \n"
            "mov r22, r26 \n"     // move to result
            "mov r23, r27 \n"
            "mov r24, r30 \n"
            "mov r25, r31 \n"

            "bcd2bin_U32_01: \n"
                "mov __tmp_reg__,r25 \n"
                "mov r25,r24 \n"
                "mov r24,r23 \n"
                "mov r23,r22 \n"
                "mov r22,__tmp_reg__ \n"
                "dec %[result_bytes] \n"
                "brne bcd2bin_U32_01 \n"

            "pop __tmp_reg__ \n"
            "ret \n"
            :[result]"=r"(result):[data]"r"(data),[result_bytes]"r"(result_bytes):"r0","r26","r27","r30","r31" /*input and output parameters*/
        );
        return result;
    }
#endif

U32 bcd2bin_U32_soft(U32 data, U8 input_bytes) {
    U32 result = 0; /*result*/
    for (U8 cnt_bits = (input_bytes << 3); cnt_bits; cnt_bits--) {
        /*shift next bit*/
        result >>= 1;
        if (((U8)(data)) & (U8)0x01) result |= 0x80000000;
        data >>= 1;
        /* result BCD correction */
        for (U8 cnt_bytes = 4; cnt_bytes; cnt_bytes--) {
            U8 tmp_byte = (data >> 24);
            if (tmp_byte & 0x80) tmp_byte -= 0x30;
            if (tmp_byte & 0x08) tmp_byte -= 0x03;
            data <<= 8;
            data |= tmp_byte;
        }
    }
    /*adjust result bytes*/
    for (U8 cnt_bits = (4 - input_bytes); cnt_bits; cnt_bits--)
        result >>= 8;
    return(result);
}

U8 asc2bin_U8(U8 data) {
    if ((data >= '0') && (data <= '9')) data -= '0';
    else if ((data >= 'a') && (data <= 'f')) data -= ('a' - 0xa);
    else if ((data >= 'A') && (data <= 'F')) data -= ('A' - 0xa);
    else data = 0xf0;
    return(data);
}

U8 bin2asc_U8(U8 data) {
    data &= 0xf;
    if (data < 10) data += '0';
    else data += ('A'-0x0a);
    return(data);
}

U32 asc2bin_U32(U8 *buf, S8 size) {
    U32 result;
    U8 flag_negative;
    if (size < 0) {
        flag_negative = 1;
        size = -size;
    } else
        flag_negative = 0;
    result = 0;
    while(size) {
        U8 tmp_U8 = asc2bin_U8(*buf);
        if (tmp_U8 <= 0xf) { /*skip non-HEX symbols*/
            result <<= 4;
            result |= tmp_U8;
        }
        size--;
        if (flag_negative) buf--;
        else buf++; /*go to next symbol*/
    }
    return(result);
}

void bin2asc_U32(U32 data, U8 *buf, S8 size) {
    U8 flag_negative;
    if (size < 0) {
        flag_negative = 1;
        size = -size;
    } else
        flag_negative = 0;
    while (size) {
        *buf = bin2asc_U8(data);
        data >>= 4;
        if (flag_negative) buf--;
        else buf++;
        size--;
        if (data == 0) size = 0; /*stop conversion on zero*/
    }
}

void num2asc_S32(S32 data, U8 *buf, U8 options) {
    U8 cnt_chars = (options & 7); //number of digits
    //convert negative to positive, set '-' flag
    if (data < 0) {data = -data; options |= 1;}
    else options &= ~1;
    //if necessary, convert to BCD
    if (options & 0x80) data = bin2bcd_U32(data, (cnt_chars >> 1) + 1);
    cnt_chars++;
    //calculate point position
    U8 point_pos = cnt_chars - ((options >> 4) & 7);
    if (point_pos) point_pos++;
    options |= 2;
    while (cnt_chars) { //digits conversion loop
        if ((options & 8) && (data == 0) && (cnt_chars < point_pos)) {
            if (options & 1) *buf = asc_minus;
            else if (options & 2) *buf = '0';
            else *buf = asc_space;
            options &= ~1;
        } else {
        	//show point
            if ((cnt_chars == point_pos) && (options & 0x70)) {
                *buf-- = asc_point;
                cnt_chars--;
            }
            *buf = bin2asc_U8(data); //show ASCII digit
        }
        cnt_chars--;
        buf--;
        data >>= 4;
        options &= ~2;
    }
    if (options & 1) *buf = asc_minus;
}
