#include <stdio.h>

#define RFID_AS_CRC_TABLE_SIZE    256
#define RFID_AS_MSB_16            (1<<15)
#define RFID_AS_MSB_8             (1<<7)
#define RFID_AS_BYTE_LEN           8
#define RFID_AS_CRC5_LEN           5
#define RFID_AS_CRC16_LEN          16
#define RFID_AS_CRC5_POLYNOMIAL   (0x9)
#define RFID_AS_CRC16_POLYNOMIAL  (0x1021)
#define RFID_AS_CRC5_POLYNOMIAL_BMSK ((1<<RFID_AS_CRC5_LEN)-1)
#define RFID_AS_CRC5_SEED (0x9)
#define RFID_AS_CRC16_SEED (0xFFFF)
#define RFID_AS_CRC5_RESIDUAL 0x0
#define RFID_AS_CRC16_RESIDUAL (0xFFFF-0x1D0F)

typedef struct
{
  unsigned int                                                  TRext:1;
  unsigned int                                                  M:2;
  unsigned int                                                  DR:1;
  unsigned int                                                  Command:4; // end of byte 0
  unsigned int                                                  Q_high:3;
  unsigned int                                                  Target:1;
  unsigned int                                                  Session:2;
  unsigned int                                                  Sel:2; // end of byte 1
  unsigned int                                                  reserved_0:2;
  unsigned int                                                  CRC:5;
  unsigned int                                                  Q_low:1; // end of byte 2
  unsigned int                                                  reserved_1:8;
} query_cmd_s;

unsigned char rfid_as_crc5_table[RFID_AS_CRC_TABLE_SIZE];
unsigned short rfid_as_crc16_table[RFID_AS_CRC_TABLE_SIZE];

void rfid_crc5_init()
{
  unsigned char bit_pos, remainder;
  unsigned short byte_val;
  
  for(byte_val = 0; byte_val < RFID_AS_CRC_TABLE_SIZE; byte_val++)
  {
    remainder = byte_val;

    for(bit_pos = 0; bit_pos < RFID_AS_BYTE_LEN; bit_pos++)
    {
      if(remainder & RFID_AS_MSB_8)
      {
        remainder = (remainder << 1) ^ (RFID_AS_CRC5_POLYNOMIAL << (RFID_AS_BYTE_LEN - RFID_AS_CRC5_LEN));
      }
      else
      {
        remainder = remainder << 1;
      }
    }

    rfid_as_crc5_table[byte_val] = remainder >> (RFID_AS_BYTE_LEN - RFID_AS_CRC5_LEN);

#if 0    
    printf("0x%02x, ", rfid_as_crc5_table[byte_val]);
    if((byte_val + 1)%8 == 0)
    printf("\n");
#endif
  }

  return;
}

void rfid_crc16_init()
{
  unsigned char  bit_pos;
  unsigned short byte_val, remainder;

  
  for(byte_val = 0; byte_val < RFID_AS_CRC_TABLE_SIZE; byte_val++)
  {
    remainder = byte_val << (RFID_AS_CRC16_LEN-RFID_AS_BYTE_LEN);

    for(bit_pos = 0; bit_pos < RFID_AS_BYTE_LEN; bit_pos++)
    {
      if(remainder & RFID_AS_MSB_16)
      {
        remainder = (remainder << 1) ^ RFID_AS_CRC16_POLYNOMIAL;
      }
      else
      {
        remainder = remainder << 1;
      }
    }

    rfid_as_crc16_table[byte_val] = remainder;

#if 0
    printf("0x%04x, ", rfid_as_crc16_table[byte_val]);
    if((byte_val + 1)%8 == 0)
    printf("\n");
#endif
  }

  return;
}

unsigned char rfid_crc5_cal
(
  unsigned char *buf_ptr,
  unsigned short bit_len
)
{
  unsigned char crc5, last_byte;
 
  for(crc5 = RFID_AS_CRC5_SEED; bit_len >= RFID_AS_BYTE_LEN; bit_len -= RFID_AS_BYTE_LEN, buf_ptr++)
  {
    crc5 = rfid_as_crc5_table[(crc5 << (RFID_AS_BYTE_LEN-RFID_AS_CRC5_LEN)) ^ *buf_ptr];
  }
 
  if(bit_len !=0)
  {
    last_byte = *buf_ptr;
 
    while(bit_len-- != 0)
    {
      if(((crc5 << (RFID_AS_BYTE_LEN - RFID_AS_CRC5_LEN)) ^ last_byte) & RFID_AS_MSB_8)
      {
        crc5 = ((crc5 << 1) ^ RFID_AS_CRC5_POLYNOMIAL) & RFID_AS_CRC5_POLYNOMIAL_BMSK;
      }
      else
      {
        crc5 = (crc5 << 1) & RFID_AS_CRC5_POLYNOMIAL_BMSK;
      }
 
      last_byte <<= 1;
    }
  }
 
  return crc5;
}

unsigned short rfid_crc16_cal
(
  unsigned char *buf_ptr,
  unsigned short bit_len
)
{
  unsigned short crc16, last_byte;
 
  for(crc16 = RFID_AS_CRC16_SEED; bit_len >= RFID_AS_BYTE_LEN; bit_len -= RFID_AS_BYTE_LEN, buf_ptr++)
  {
    crc16 = rfid_as_crc16_table[(crc16 >> (RFID_AS_CRC16_LEN - RFID_AS_BYTE_LEN)) ^ *buf_ptr] ^ (crc16 << RFID_AS_BYTE_LEN);
  }
 
  if(bit_len !=0)
  {
    last_byte = (*buf_ptr) << (RFID_AS_CRC16_LEN - RFID_AS_BYTE_LEN);
 
    while(bit_len-- != 0)
    {
      if((crc16 ^ last_byte) & RFID_AS_MSB_16)
      {
        crc16 = (crc16 << 1) ^ RFID_AS_CRC16_POLYNOMIAL;
      }
      else
      {
        crc16 = crc16 << 1;
      }
 
      last_byte <<= 1;
    }
  }
 
  return ~crc16;
}

void main()
{
    rfid_crc5_init();
    rfid_crc16_init();
    
    unsigned char data[] = {0x80,0xb6,0xa4,0x41,0xec,0xB2,0xDD,0xD9,0x01,0x40, 0xC4, 0xF3};
    
    unsigned short crc = rfid_crc16_cal(data, 41);
    
    printf("crc 0x%x \n", crc);
}