#include <stdio.h>
#include <stdint.h>
#include <string.h>

void uint16_to_bcd(uint16_t input, uint8_t* bcd) {
    uint8_t i, j;
    bcd[0] = 0; bcd[1] = 0; bcd[2] = 0;
    for (i = 0; i < 16; i++) {
        // Коррекция.
        for (j = 0; j < 2; j++) {
            bcd[j] = ((bcd[j] & 0x0F) >= 0x05) ? (bcd[j] + (uint8_t) 0x03) : bcd[j];
            bcd[j] = ((bcd[j] & 0xF0) >= 0x50) ? (bcd[j] + (uint8_t) 0x30) : bcd[j];
        }
        // Сдвиг.
        bcd[2] = (bcd[2] << 1) | (bcd[1] >> 7);
        bcd[1] = (bcd[1] << 1) | (bcd[0] >> 7);
        bcd[0] = (bcd[0] << 1) | (input >> 15);
        input <<= 1;
    }
}

int test_bcd() {
    uint16_t i;
    uint8_t bcd[3];
    char str1[10];
    char str2[10];

    for (i = 0; i < 0xFFFF; i++) {
        uint16_to_bcd(i, bcd);
        sprintf(str1, "%02X%02X%02X", bcd[2], bcd[1], bcd[0]);
        sprintf(str2, "%06d", i);
        if (strcmp(str1, str2))
            return -1;
    }
    return 0;
}

int main() {
    uint8_t bcd[3];
    uint16_to_bcd(12345, bcd);
    printf("example 1: %02X %02X %02X\n", bcd[2], bcd[1], bcd[0]);

    uint16_to_bcd(54679, bcd);
    printf("example 2: %02X %02X %02X\n", bcd[2], bcd[1], bcd[0]);

    if (test_bcd())
        printf("BCD test: FAILED");
    else
        printf("BCD test: OK");
}