/**
* When a frame is received, cast it to a csp_packet
* and send it directly to the CSP new packet function.
* Context: ISR only
* @param frame
*/
void csp_i2c_rx(i2c_frame_t * frame, void * pxTaskWoken) {
static csp_packet_t * packet;
/* Validate input */
if (frame == NULL)
return;
if ((frame->len < 4) || (frame->len > I2C_MTU)) {
csp_if_i2c.frame++;
csp_buffer_free_isr(frame);
return;
}
/* Strip the CSP header off the length field before converting to CSP packet */
frame->len -= sizeof(csp_id_t);
/* Convert the packet from network to host order */
packet = (csp_packet_t *) frame;
packet->id.ext = csp_ntoh32(packet->id.ext);
/* Receive the packet in CSP */
csp_new_packet(packet, &csp_if_i2c, pxTaskWoken);
}
/**
* Loopback interface transmit function
* @param packet Packet to transmit
* @param timeout Timout in ms
* @return 1 if packet was successfully transmitted, 0 on error
*/
int csp_lo_tx(csp_packet_t * packet, uint32_t timeout) {
/* Send back into CSP, notice calling from task so last argument must be NULL! */
csp_new_packet(packet, &csp_if_lo, NULL);
return CSP_ERR_NONE;
}
void * fifo_rx(void * parameters) {
csp_packet_t *buf = csp_buffer_get(BUF_SIZE);
/* Wait for packet on fifo */
while (read(rx_channel, &buf->length, BUF_SIZE) > 0) {
csp_new_packet(buf, &csp_if_fifo, NULL);
buf = csp_buffer_get(BUF_SIZE);
}
return NULL;
}
void csp_tiradio_rx (csp_iface_t *interface,
uint8_t *buf, int len, void *xTaskWoken) {
csp_packet_t *packet;
csp_tiradio_driver_handle_t* handle =
(csp_tiradio_driver_handle_t*) (interface->driver);
if (len < CSP_HEADER_LENGTH) {
csp_log_warn("Length less than minimum expected! Size %u,"
" expected %u; dropping message\r\n", len,
CSP_HEADER_LENGTH);
return;
}
packet = csp_buffer_get(interface->mtu);
if (packet != NULL) {
memcpy(&packet->id.ext, buf, len);
packet->length = len;
if (packet->length >= CSP_HEADER_LENGTH &&
packet->length <= interface->mtu + CSP_HEADER_LENGTH) {
/* Strip CSP header off the length field*/
packet->length -= CSP_HEADER_LENGTH;
/* Convert the packet from network to host order */
packet->id.ext = csp_ntoh32(packet->id.ext);
csp_new_packet(packet, interface, xTaskWoken);
if (handle->module_id < NUM_TIRADIO_MODULES)
latest_csp_transfer_id[handle->module_id] = packet->id;
}
else {
interface->frame++;
csp_buffer_free(packet);
}
}
else {
interface->frame++;
}
}
unsigned WINAPI fifo_rx(void *handle) {
printf("fifo_rx tid: %lu\n", GetCurrentThreadId());
HANDLE pipe = (HANDLE) handle;
csp_packet_t *buf = csp_buffer_get(BUF_SIZE);
DWORD bytesRead;
BOOL readSuccess;
while(1) {
readSuccess =
ReadFile(pipe, &buf->length, BUF_SIZE, &bytesRead, NULL);
if( !readSuccess || bytesRead == 0 ) {
csp_buffer_free(buf);
printError();
break;
}
csp_new_packet(buf, &csp_if_fifo, NULL);
buf = csp_buffer_get(BUF_SIZE);
}
printf("Closing pipe to client\n");
CloseHandle(pipe);
return 0;
}
static int csp_can_process_frame(can_frame_t *frame) {
pbuf_element_t *buf;
uint8_t offset;
can_id_t id = frame->id;
/* Bind incoming frame to a packet buffer */
buf = pbuf_find(id, CFP_ID_CONN_MASK, NULL);
/* Check returned buffer */
if (buf == NULL) {
if (CFP_TYPE(id) == CFP_BEGIN) {
buf = pbuf_new(id, NULL);
if (buf == NULL) {
csp_log_warn("No available packet buffer for CAN\r\n");
csp_if_can.rx_error++;
return CSP_ERR_NOMEM;
}
} else {
csp_log_warn("Out of order MORE frame received for can id 0x%"PRIx32"; remain is %u\r\n",
(uint32_t)id, CFP_REMAIN(id));
csp_if_can.frame++;
return CSP_ERR_INVAL;
}
}
/* Reset frame data offset */
offset = 0;
switch (CFP_TYPE(id)) {
case CFP_BEGIN:
/* Discard packet if DLC is less than CSP id + CSP length fields */
if (frame->dlc < sizeof(csp_id_t) + sizeof(uint16_t)) {
csp_log_warn("Short BEGIN frame received\r\n");
csp_if_can.frame++;
pbuf_free(buf, NULL);
break;
}
/* Check for incomplete frame */
if (buf->packet != NULL) {
/* Reuse the buffer */
csp_log_warn("Incomplete frame\r\n");
csp_if_can.frame++;
} else {
/* Allocate memory for frame */
buf->packet = csp_buffer_get(csp_buffer_size() - CSP_BUFFER_PACKET_OVERHEAD);
if (buf->packet == NULL) {
csp_log_error("Failed to get buffer for CSP_BEGIN packet\r\n");
csp_if_can.frame++;
pbuf_free(buf, NULL);
break;
}
}
/* Copy CSP identifier and length*/
memcpy(&(buf->packet->id), frame->data, sizeof(csp_id_t));
buf->packet->id.ext = csp_ntoh32(buf->packet->id.ext);
memcpy(&(buf->packet->length), frame->data + sizeof(csp_id_t), sizeof(uint16_t));
buf->packet->length = csp_ntoh16(buf->packet->length);
/* Reset RX count */
buf->rx_count = 0;
/* Set offset to prevent CSP header from being copied to CSP data */
offset = sizeof(csp_id_t) + sizeof(uint16_t);
/* Set remain field - increment to include begin packet */
buf->remain = CFP_REMAIN(id) + 1;
/* Note fall through! */
case CFP_MORE:
/* Check 'remain' field match */
if (CFP_REMAIN(id) != buf->remain - 1) {
csp_log_error("CAN frame lost in CSP packet, %u vs. %u\r\n",
CFP_REMAIN(id),buf->remain - 1);
pbuf_free(buf, NULL);
csp_if_can.frame++;
break;
}
/* Decrement remaining frames */
buf->remain--;
/* Check for overflow */
if ((buf->rx_count + frame->dlc - offset) > buf->packet->length) {
csp_log_error("RX buffer overflow\r\n");
csp_if_can.frame++;
pbuf_free(buf, NULL);
break;
}
/* Copy dlc bytes into buffer */
memcpy(&buf->packet->data[buf->rx_count], frame->data + offset, frame->dlc - offset);
buf->rx_count += frame->dlc - offset;
/* Check if more data is expected */
if (buf->rx_count != buf->packet->length)
break;
/* Data is available */
csp_new_packet(buf->packet, &csp_if_can, NULL);
/* Drop packet buffer reference */
buf->packet = NULL;
/* Free packet buffer */
pbuf_free(buf, NULL);
break;
default:
csp_log_warn("Received unknown CFP message type\r\n");
pbuf_free(buf, NULL);
break;
}
return CSP_ERR_NONE;
}
