bin2bcd & bcd2bin, 8-6-4 digits, double dabble algorithm

uint16_t convtab[4]={10,100,1000,10000};

void tobcd(volatile uint8_t *p, uint_fast16_t val){

  uint_fast8_t i,res;

  volatile uint8_t *bp;

  register uint8_t sreg;

  sreg = SREG;

  asm("cli");

  // first clear old values

  // keep signs and double informations

  bp = p;

  for(i=0; i < (DISP_BUF_MAX/2); i++)

      *bp++ &= 0xf0;

  i= ((DISP_BUF_MAX/2) - 1);    // Number of digits minus 1

  do {

    i--;

    for(res=0; val >= convtab[i]; val -= convtab[i]){

      res++;

    }

    *p |= res;

    p++;

  } while(i);

  *p |= val;

  clear_leading_zero();

  SREG = sreg;

}