static int xmodem_crc_check( int crcflag, const unsigned char *buffer, int size )
{
// crcflag=0 - do regular checksum
// crcflag=1 - do CRC checksum
if(crcflag)
{
unsigned short crc=0;
unsigned short pktcrc = (buffer[size]<<8)+buffer[size+1];
// do CRC checksum
while(size--)
crc = crc_xmodem_update(crc, *buffer++);
// check checksum against packet
if(crc == pktcrc)
return 1;
}
else
{
int i;
unsigned char cksum = 0;
// do regular checksum
for(i=0; i<size; ++i)
{
cksum += buffer[i];
}
// check checksum against packet
if(cksum == buffer[size])
return 1;
}
return 0;
}
static int sd_generic_read(struct sd_host *host,
u8 opcode, u32 arg,
void *data, size_t len, int token)
{
struct sd_command *cmd = &host->cmd;
u16 crc, calc_crc = 0xffff;
u8 *d;
size_t l;
int retval;
/* build raw command */
sd_cmd(cmd, opcode, arg);
/* select card, send command and wait for response */
retval = sd_start_command(host, cmd);
if (retval < 0)
goto out;
if (retval != 0x00) {
retval = -EIO;
goto out;
}
/* wait for read token, then read data */
retval = sd_read_data(host, data, len, token);
if (retval < 0)
goto out;
/* read trailing crc */
spi_read(host, &crc, sizeof(crc));
retval = 0;
/* FIXME, rewrite this a bit */
{
calc_crc = 0;
d = data;
l = len;
while (l-- > 0)
calc_crc = crc_xmodem_update(calc_crc, *d++);
if (calc_crc != crc)
retval = -EIO;
}
out:
/* burn extra cycles and deselect card */
sd_end_command(host);
if (retval < 0) {
DBG("read, offset=%u, len=%d\n", arg, len);
DBG("crc=%04x, calc_crc=%04x, %s\n", crc, calc_crc,
(retval < 0) ? "failed" : "ok");
}
return retval;
}
uint16_t calc_crc(unsigned char *msg,int n,uint16_t init)
{
uint16_t x = init;
while(n--)
{
x=crc_xmodem_update(x, (uint16_t)*msg++);
}
return(x);
}
uint16_t calculate_crc16 (char *input)
{
uint16_t crc;
crc = 0xFFFF;
while (*input)
{
crc = crc_xmodem_update(crc, *input);
input++;
}
return crc;
}
static int sd_write_data(struct sd_host *host, void *data, size_t len,
int token)
{
u16 crc;
u8 t, *d;
size_t l;
int retval = 0;
/* FIXME, rewrite this a bit */
{
crc = 0;
d = data;
l = len;
while (l-- > 0)
crc = crc_xmodem_update(crc, *d++);
}
/* send the write block token */
t = token;
spi_write(host, &t, sizeof(t));
/* send the data */
spi_write(host, data, len);
/* send the crc */
spi_write(host, &crc, sizeof(crc));
/* get the card data response */
retval = spi_wait_for_resp(host, 0x01, 0x11, host->write_timeout);
if (retval < 0)
goto out;
if ((retval & DR_SPI_MASK) != DR_SPI_DATA_ACCEPTED) {
DBG("data response=%02x\n", retval);
retval = -EIO;
goto out;
}
/* wait for the busy signal to clear */
retval = spi_wait_for_resp(host, 0xff, 0xff, host->write_timeout);
if (retval < 0)
goto out;
retval = 0;
out:
return retval;
}
uint16_t getCRC16Checksum(char *string){
size_t i;
uint16_t crc;
uint8_t c;
crc = 0xFFFF;
// Calculate checksum ignoring the first two $s
for (i = 0; i < strlen(string); i++)
{
c = string[i];
crc = crc_xmodem_update (crc, c);
}
return crc;
}
short CRC16_checksum (char *string)
{
int i;
int crc;
char c;
crc = 0xFFFF;
// Calculate checksum ignoring the first two $s
for (i = 2; i < strlen(string); i++)
{
c = string[i];
crc = crc_xmodem_update (crc, c);
}
return crc;
}
/*
update CRC state for blheli protocol
*/
void AP_BLHeli::blheli_crc_update(uint8_t c)
{
blheli.crc = crc_xmodem_update(blheli.crc, c);
}
