#include "crc7.h"

#define CRC7_POLYNOMIAL 0x89 /* x^7 + x^3 + 1 */

static unsigned char crc7_syndrome_table[256];

/*
 * Generate a table of CRC-7 syndromes for x^7 * each possible input byte
 */
void
gen_crc7_syndrome_table (void)
{
  int i, j, syndrome;
  for (i = 0;  i < 256;  ++i)
    {
      syndrome = ((i & 0x80) != 0)? i ^ CRC7_POLYNOMIAL : i;
      for (j = 0;  j < 7;  ++j)
        {
          if (((syndrome <<= 1) & 0x80) != 0)
            {
              syndrome ^= CRC7_POLYNOMIAL;
            }
        }
      crc7_syndrome_table[i] = (unsigned char) syndrome;
    }
}

unsigned char
calc_crc7(unsigned long* resp)
{
	unsigned char crc7_accum = 0;
	unsigned char byte;
	int i, j;
	
	for (i = 0;i < 4;i++)
	{
		for (j = 0;j < 4;j++)
		{
			if ((i == 3) && (j == 3))
				break;
			byte = resp[i] >> (24 - 8 * j);
			crc7_accum = crc7_syndrome_table[(crc7_accum << 1) ^ byte];
		}
	}
	
	return crc7_accum;
}