int spi_link_send(void *buf_out, size_t count, void *buf_in, uint8_t* crc_valid) { int ret; struct spi_ioc_transfer tr = { .tx_buf = (unsigned long)buf_out, .rx_buf = (unsigned long)buf_in, .len = count, .delay_usecs = spi_link.delay, .speed_hz = spi_link.speed, .bits_per_word = spi_link.bits, }; ((uint8_t*)buf_out)[count-1] = crc_calc_block_crc8(buf_out, count-1); ret = ioctl(spi_link.fd, SPI_IOC_MESSAGE(1), &tr); spi_link.msg_cnt++; uint8_t computed_crc = crc_calc_block_crc8(buf_in, count-1); if (computed_crc == ((uint8_t*)buf_in)[count-1]) *crc_valid = 1; else { *crc_valid = 0; spi_link.crc_err_cnt++; } return ret; } #define POLYNOMIAL 0x31 #define WIDTH (8 * sizeof(uint8_t)) #define TOPBIT (1 << (WIDTH - 1)) uint8_t crc_calc_block_crc8(const uint8_t buf[], uint32_t len) { uint8_t _remainder = 0; for (uint32_t byte = 0; byte < len; ++byte) { _remainder ^= (buf[byte] << (WIDTH - 8)); for (uint8_t bit = 8; bit > 0; --bit) { if (_remainder & TOPBIT) _remainder = (_remainder << 1) ^ POLYNOMIAL; else _remainder = (_remainder << 1); } } return (_remainder); }
int main(int argc, char *argv[]) { uint32_t us_delay; if(argc > 1) { us_delay = atoi(argv[1]); } else { us_delay = 1953; } printf("Delay: %dus\n", us_delay); if (spi_link_init()) { TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); return -1; } uint8_t skip_buf_check = 0; uint8_t skip_crc_check = 0; uint32_t buf_check_errors = 0; while (1) { struct AutopilotMessageCRCFrame crc_msg_out; struct AutopilotMessageCRCFrame msg_out_prev; struct AutopilotMessageCRCFrame crc_msg_in; uint8_t crc_valid; /* backup message for later comparison */ memcpy(&msg_out_prev, &crc_msg_out, sizeof(struct AutopilotMessageCRCFrame)); /* fill message with data */ fill_msg(&crc_msg_out); /* send it over spi */ spi_link_send(&crc_msg_out, sizeof(struct AutopilotMessageCRCFrame), &crc_msg_in, &crc_valid); /* check that received message is identical to the one previously sent */ if (!skip_buf_check && spi_link.msg_cnt > 1) { if (memcmp(&crc_msg_in.payload, &msg_out_prev.payload, sizeof(struct OVERO_LINK_MSG_DOWN))) { printf("Compare failed: (received != expected): \n"); print_up_msg(&crc_msg_in); print_down_msg(&msg_out_prev); buf_check_errors++; } } /* report crc error */ if (!skip_crc_check & !crc_valid) { printf("CRC checksum failed: received %04X != computed %04X\n", crc_msg_in.crc, crc_calc_block_crc8((uint8_t*)&crc_msg_in.payload, sizeof(struct OVERO_LINK_MSG_DOWN))); } /* report message count */ if (!(spi_link.msg_cnt % 1000)) printf("msg %d, buf err %d, CRC errors: %d\n", spi_link.msg_cnt, buf_check_errors, spi_link.crc_err_cnt); /* give it some rest */ if(us_delay > 0) usleep(us_delay); } return 0; }