/** * 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; }