diff --git a/src/host_stream/can_bus.c b/src/host_stream/can_bus.c index 9a8586c..916a71f 100644 --- a/src/host_stream/can_bus.c +++ b/src/host_stream/can_bus.c @@ -364,6 +364,19 @@ static void *can_rx_thread(void *arg) } pfd.events = POLLPRI | POLLERR; + /* Drain any frames left in MCP2515 from before init (e.g. after a crash + * restart where CANINTF was not cleared). Doing this BEFORE the initial + * GPIO read ensures INT is HIGH when we start polling, so the first real + * frame produces a clean falling edge that poll() can detect. */ + { + canid_t drain_id; uint8_t drain_data[8]; uint8_t drain_len; + pthread_mutex_lock(&s_spi_mtx); + while (mcp2515_readMessage(s_dev, &drain_id, drain_data, &drain_len) == ERROR_OK) + ; + mcp2515_modify_bit(s_dev, MCP_CANINTF, 0xFF, 0); + pthread_mutex_unlock(&s_spi_mtx); + } + /* consume initial level so first real edge is not missed */ lseek(pfd.fd, 0, SEEK_SET); read(pfd.fd, dummy, sizeof(dummy)); @@ -406,13 +419,22 @@ static void *can_rx_thread(void *arg) lseek(pfd.fd, 0, SEEK_SET); read(pfd.fd, dummy, sizeof(dummy)); - rid = 0; rlen = 0; memset(rdata, 0, 8); - pthread_mutex_lock(&s_spi_mtx); - int rc = mcp2515_readMessage(s_dev, &rid, rdata, &rlen); - pthread_mutex_unlock(&s_spi_mtx); - if (rc != ERROR_OK) continue; + /* Drain ALL frames from MCP2515 in one pass. + * Reading only one frame per interrupt leaves RX1IF set when both + * buffers are occupied, keeping INT LOW with no new falling edge. */ + for (;;) { + rid = 0; rlen = 0; memset(rdata, 0, 8); + pthread_mutex_lock(&s_spi_mtx); + int rc = mcp2515_readMessage(s_dev, &rid, rdata, &rlen); + if (rc != ERROR_OK) + mcp2515_modify_bit(s_dev, MCP_CANINTF, + CANINTF_ERRIF | CANINTF_MERRF, 0); + pthread_mutex_unlock(&s_spi_mtx); + + if (rc != ERROR_OK) break; + + if ((rid & CAN_SFF_MASK) != CAN_ECU_ID || rlen != 8) continue; - if ((rid & CAN_SFF_MASK) == CAN_ECU_ID && rlen == 8) { gettimeofday(&last_rx, NULL); uint16_t throttle_status = (uint16_t)rdata[0] | ((uint16_t)rdata[1] << 8); diff --git a/src/host_stream/mcp2515.c b/src/host_stream/mcp2515.c index 06b1554..d2981ac 100644 --- a/src/host_stream/mcp2515.c +++ b/src/host_stream/mcp2515.c @@ -389,8 +389,10 @@ uint8_t* spi_send(mcp2515_dev *mcp2515_device, uint8_t *data, int length) { int ret = 0; uint8_t *tx_buffer = (uint8_t *)malloc(sizeof(uint8_t) * length); + if (!tx_buffer) return NULL; memcpy(tx_buffer, data, sizeof(uint8_t) * length); uint8_t *rx_buffer = (uint8_t *)malloc(sizeof(uint8_t) * length); + if (!rx_buffer) { free(tx_buffer); return NULL; } memset(rx_buffer, 0, sizeof(uint8_t) * length); struct spi_ioc_transfer tr = { @@ -416,7 +418,9 @@ uint8_t* spi_send(mcp2515_dev *mcp2515_device, uint8_t *data, int length) if (ret < 1) { printf("[SPI] ioctl failed: ret=%d, errno=%d, fd=%d, len=%d\n", ret, errno, mcp2515_device->spi_dev->fd, length); - exit(0); + free(tx_buffer); + free(rx_buffer); + return NULL; } free(tx_buffer); return rx_buffer; @@ -963,21 +967,14 @@ int mcp2515_write_register(mcp2515_dev *mcp2515_device, uint8_t register_address void mcp2515_read_register(mcp2515_dev *mcp2515_device, uint8_t register_address, uint8_t values[], uint8_t data_length) { - // tx buffer - // uint8_t result = 0; uint8_t *tx_buffer = (uint8_t*)malloc(sizeof(uint8_t)*(data_length + 2)); tx_buffer[0] = INSTRUCTION_READ; tx_buffer[1] = register_address; - // rx - // uint8_t *rx_buffer = (uint8_t*)malloc(sizeof(uint8_t)*(data_length + 2)); - // - uint8_t *spi_ret = spi_send(mcp2515_device,tx_buffer,(data_length + 2)); - memcpy(values, spi_ret + 2, sizeof(uint8_t) * data_length); - // free(rx_buffer); + uint8_t *spi_ret = spi_send(mcp2515_device, tx_buffer, (data_length + 2)); free(tx_buffer); - // result = spi_ret[2]; + if (!spi_ret) { memset(values, 0, data_length); return; } + memcpy(values, spi_ret + 2, sizeof(uint8_t) * data_length); free(spi_ret); - // return 0; } uint8_t mcp2515_read_register_once(mcp2515_dev *mcp2515_device, uint8_t register_address) @@ -1005,8 +1002,9 @@ uint8_t mcp2515_read_register_once(mcp2515_dev *mcp2515_device, uint8_t register uint8_t mcp2515_read_status(mcp2515_dev *mcp2515_device) { - uint8_t read_status[2] = {INSTRUCTION_READ_STATUS,0x00}; + uint8_t read_status[2] = {INSTRUCTION_READ_STATUS,0x00}; uint8_t *spi_ret = spi_send(mcp2515_device, read_status, 2); + if (!spi_ret) return 0; uint8_t result = spi_ret[1]; free(spi_ret); return result;