/*
 * crc16.c - calculate next CRC value from
 * current CRC and a N-bit input data word
 * CRC-16 = x^16 + x^15 + x^2 + 1
 */
int crc(int init, int data_in)
{
  int i, b, c;
  // init the CRC register
  if(init)
    c = 0xffff;
  // shift in data from MSB of low byte
  for(i = 0; i < 8; i++ ){
    b = (data_in & (1 << (7-i))) >> (7-i);
    c = ((c & 0x3ffd) << 1) + 
      (((((c & 0x8000) >> 15) + b + ((c & 0x4000) >> 14))%2) << 15) +
      (((((c & 0x8000) >> 15) + b + ((c & 0x2) >> 1))%2) << 2) +
      ((((c & 0x8000) >> 15) + b)%2);
  }
  // shift in data from MSB of high byte
  for(i = 0; i < 8; i++ ){
    b = (data_in & (1 << (15-i))) >> (15-i);
    c = ((c & 0x3ffd) << 1) + 
      (((((c & 0x8000) >> 15) + b + ((c & 0x4000) >> 14))%2) << 15) +
      (((((c & 0x8000) >> 15) + b + ((c & 0x2) >> 1))%2) << 2) +
      ((((c & 0x8000) >> 15) + b)%2);
  }
  return c;
}