int csp_send(csp_conn_t * conn, csp_packet_t * packet, uint32_t timeout) {
int ret;
if ((conn == NULL) || (packet == NULL) || (conn->state != CONN_OPEN)) {
csp_log_error("Invalid call to csp_send\r\n");
return 0;
}
#ifdef CSP_USE_RDP
if (conn->idout.flags & CSP_FRDP) {
if (csp_rdp_send(conn, packet, timeout) != CSP_ERR_NONE) {
csp_route_t * ifout = csp_route_if(conn->idout.dst);
if (ifout != NULL && ifout->interface != NULL)
ifout->interface->tx_error++;
csp_log_warn("RDP send failed\r\n!");
return 0;
}
}
#endif
ret = csp_send_direct(conn->idout, packet, timeout);
return (ret == CSP_ERR_NONE) ? 1 : 0;
}
int csp_route_next_packet(csp_route_queue_t * input) {
#ifdef CSP_USE_QOS
int prio, found, event;
/* Wait for packet in any queue */
if (csp_queue_dequeue(router_input_event, &event, CSP_ROUTER_RX_TIMEOUT) != CSP_QUEUE_OK)
return CSP_ERR_TIMEDOUT;
/* Find packet with highest priority */
found = 0;
for (prio = 0; prio < CSP_ROUTE_FIFOS; prio++) {
if (csp_queue_dequeue(router_input_fifo[prio], input, 0) == CSP_QUEUE_OK) {
found = 1;
break;
}
}
if (!found) {
csp_log_warn("Spurious wakeup of router task. No packet found\r\n");
return CSP_ERR_TIMEDOUT;
}
#else
if (csp_queue_dequeue(router_input_fifo[0], input, CSP_ROUTER_RX_TIMEOUT) != CSP_QUEUE_OK)
return CSP_ERR_TIMEDOUT;
#endif
return CSP_ERR_NONE;
}
/** pbuf_cleanup
* Purge all packets buffers that have timed out
*/
static void pbuf_cleanup(void) {
int i;
pbuf_element_t *buf;
/* Lock packet buffer */
CSP_ENTER_CRITICAL(pbuf_sem);
/* Loop through buffer elements */
for (i = 0; i < PBUF_ELEMENTS; i++) {
buf = &pbuf[i];
if (buf->state == BUF_USED) {
/* Check timeout */
uint32_t now = csp_get_ms();
if (now - buf->last_used > PBUF_TIMEOUT_MS) {
csp_log_warn("CAN Buffer element timed out");
/* Recycle packet buffer */
pbuf_free_locked(buf, NULL, true);
}
}
}
/* Unlock packet buffer */
CSP_EXIT_CRITICAL(pbuf_sem);
}
int can_send(can_id_t id, uint8_t data[], uint8_t dlc, CSP_BASE_TYPE * task_woken) {
int i, m = -1;
/* Disable CAN interrupt while looping MOBs */
CAN_CLEAR_INTERRUPT();
/* Disable interrupts while looping mailboxes */
if (task_woken == NULL) {
portENTER_CRITICAL();
}
/* Search free MOB from 0 -> CAN_TX_MOBs */
for(i = 0; i < CAN_TX_MOBS; i++) {
if (mbox[i] == MBOX_FREE && !(CANEN2 & (1 << i))) {
mbox[i] = MBOX_USED;
m = i;
break;
}
}
/* Enable interrupts */
if (task_woken == NULL) {
portEXIT_CRITICAL();
}
/* Enable CAN interrupt */
CAN_SET_INTERRUPT();
/* Return if no available MOB was found */
if (m < 0) {
csp_log_warn("TX overflow, no available MOB\r\n");
return -1;
}
/* Select and clear mob */
CAN_SET_MOB(m);
CAN_MOB_ABORT();
CAN_CLEAR_STATUS_MOB();
/* Set identifier */
CAN_SET_EXT_ID(id);
/* Set data - CANMSG is auto incrementing */
for (i = 0; i < dlc; i++)
CANMSG = data[i];
/* Set DLC */
CAN_CLEAR_DLC();
CAN_SET_DLC(dlc);
/* Start TX */
CAN_CONFIG_TX();
return 0;
}
void csp_new_packet(csp_packet_t * packet, csp_iface_t * interface, CSP_BASE_TYPE * pxTaskWoken) {
int result, fifo;
if (packet == NULL) {
csp_log_warn("csp_new packet called with NULL packet\r\n");
return;
} else if (interface == NULL) {
csp_log_warn("csp_new packet called with NULL interface\r\n");
if (pxTaskWoken == NULL)
csp_buffer_free(packet);
else
csp_buffer_free_isr(packet);
return;
}
csp_route_queue_t queue_element;
queue_element.interface = interface;
queue_element.packet = packet;
fifo = csp_route_get_fifo(packet->id.pri);
result = csp_route_enqueue(router_input_fifo[fifo], &queue_element, 0, pxTaskWoken);
if (result != CSP_ERR_NONE) {
csp_log_warn("ERROR: Routing input FIFO is FULL. Dropping packet.\r\n");
interface->drop++;
if (pxTaskWoken == NULL)
csp_buffer_free(packet);
else
csp_buffer_free_isr(packet);
} else {
interface->rx++;
interface->rxbytes += packet->length;
}
}
static void * mbox_rx_thread(void * parameters) {
struct can_frame frame;
int nbytes;
while (1) {
/* Read CAN frame */
nbytes = read(can_socket, &frame, sizeof(frame));
if (nbytes < 0) {
csp_log_error("read: %s\r\n", strerror(errno));
break;
}
if (nbytes != sizeof(frame)) {
csp_log_warn("Read incomplete CAN frame\n");
continue;
}
/* Frame type */
if (frame.can_id & (CAN_ERR_FLAG | CAN_RTR_FLAG) || !(frame.can_id & CAN_EFF_FLAG)) {
/* Drop error and remote frames */
csp_log_warn("Discarding ERR/RTR/SFF frame\r\n");
} else {
/* Strip flags */
frame.can_id &= CAN_EFF_MASK;
}
/* Call RX callback */
if (rxcb) rxcb((can_frame_t *)&frame, NULL);
}
/* We should never reach this point */
pthread_exit(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++;
}
}
void csp_udp_new_packet(csp_conn_t * conn, csp_packet_t * packet) {
/* Enqueue */
if (csp_conn_enqueue_packet(conn, packet) < 0) {
csp_log_error("Connection buffer queue full!\r\n");
csp_buffer_free(packet);
return;
}
/* Try to queue up the new connection pointer */
if (conn->socket != NULL) {
if (csp_queue_enqueue(conn->socket, &conn, 0) != CSP_QUEUE_OK) {
csp_log_warn("Warning socket connection queue full\r\n");
csp_close(conn);
return;
}
/* Ensure that this connection will not be posted to this socket again */
conn->socket = NULL;
}
}
/**
* Helper function to decrypt, check auth and CRC32
* @param security_opts either socket_opts or conn_opts
* @param interface pointer to incoming interface
* @param packet pointer to packet
* @return -1 Missing feature, -2 XTEA error, -3 CRC error, -4 HMAC error, 0 = OK.
*/
static int csp_route_security_check(uint32_t security_opts, csp_iface_t * interface, csp_packet_t * packet) {
#ifdef CSP_USE_XTEA
/* XTEA encrypted packet */
if (packet->id.flags & CSP_FXTEA) {
/* Read nonce */
uint32_t nonce;
memcpy(&nonce, &packet->data[packet->length - sizeof(nonce)], sizeof(nonce));
nonce = csp_ntoh32(nonce);
packet->length -= sizeof(nonce);
/* Create initialization vector */
uint32_t iv[2] = {nonce, 1};
/* Decrypt data */
if (csp_xtea_decrypt(packet->data, packet->length, iv) != 0) {
/* Decryption failed */
csp_log_error("Decryption failed! Discarding packet");
interface->autherr++;
return CSP_ERR_XTEA;
}
} else if (security_opts & CSP_SO_XTEAREQ) {
csp_log_warn("Received packet without XTEA encryption. Discarding packet");
interface->autherr++;
return CSP_ERR_XTEA;
}
#endif
/* CRC32 verified packet */
if (packet->id.flags & CSP_FCRC32) {
#ifdef CSP_USE_CRC32
if (packet->length < 4)
csp_log_error("Too short packet for CRC32, %u", packet->length);
/* Verify CRC32 (does not include header for backwards compatability with csp1.x) */
if (csp_crc32_verify(packet, false) != 0) {
/* Checksum failed */
csp_log_error("CRC32 verification error! Discarding packet");
interface->rx_error++;
return CSP_ERR_CRC32;
}
} else if (security_opts & CSP_SO_CRC32REQ) {
csp_log_warn("Received packet without CRC32. Accepting packet");
#else
/* Strip CRC32 field and accept the packet */
csp_log_warn("Received packet with CRC32, but CSP was compiled without CRC32 support. Accepting packet");
packet->length -= sizeof(uint32_t);
#endif
}
#ifdef CSP_USE_HMAC
/* HMAC authenticated packet */
if (packet->id.flags & CSP_FHMAC) {
/* Verify HMAC (does not include header for backwards compatability with csp1.x) */
if (csp_hmac_verify(packet, false) != 0) {
/* HMAC failed */
csp_log_error("HMAC verification error! Discarding packet");
interface->autherr++;
return CSP_ERR_HMAC;
}
} else if (security_opts & CSP_SO_HMACREQ) {
csp_log_warn("Received packet without HMAC. Discarding packet");
interface->autherr++;
return CSP_ERR_HMAC;
}
#endif
#ifdef CSP_USE_RDP
/* RDP packet */
if (!(packet->id.flags & CSP_FRDP)) {
if (security_opts & CSP_SO_RDPREQ) {
csp_log_warn("Received packet without RDP header. Discarding packet");
interface->rx_error++;
return CSP_ERR_INVAL;
}
}
#endif
return CSP_ERR_NONE;
}
int csp_route_work(uint32_t timeout) {
csp_qfifo_t input;
csp_packet_t * packet;
csp_conn_t * conn;
csp_socket_t * socket;
#ifdef CSP_USE_RDP
/* Check connection timeouts (currently only for RDP) */
csp_conn_check_timeouts();
#endif
/* Get next packet to route */
if (csp_qfifo_read(&input) != CSP_ERR_NONE)
return -1;
packet = input.packet;
csp_log_packet("INP: S %u, D %u, Dp %u, Sp %u, Pr %u, Fl 0x%02X, Sz %"PRIu16" VIA: %s",
packet->id.src, packet->id.dst, packet->id.dport,
packet->id.sport, packet->id.pri, packet->id.flags, packet->length, input.interface->name);
/* Here there be promiscuous mode */
#ifdef CSP_USE_PROMISC
csp_promisc_add(packet);
#endif
#ifdef CSP_USE_DEDUP
/* Check for duplicates */
if (csp_dedup_is_duplicate(packet)) {
/* Discard packet */
csp_log_packet("Duplicate packet discarded");
csp_buffer_free(packet);
return 0;
}
#endif
/* If the message is not to me, route the message to the correct interface */
if ((packet->id.dst != csp_get_address()) && (packet->id.dst != CSP_BROADCAST_ADDR)) {
/* Find the destination interface */
csp_iface_t * dstif = csp_rtable_find_iface(packet->id.dst);
/* If the message resolves to the input interface, don't loop it back out */
if ((dstif == NULL) || ((dstif == input.interface) && (input.interface->split_horizon_off == 0))) {
csp_buffer_free(packet);
return 0;
}
/* Otherwise, actually send the message */
if (csp_send_direct(packet->id, packet, dstif, 0) != CSP_ERR_NONE) {
csp_log_warn("Router failed to send");
csp_buffer_free(packet);
}
/* Next message, please */
return 0;
}
/* Discard packets with unsupported options */
if (csp_route_check_options(input.interface, packet) != CSP_ERR_NONE) {
csp_buffer_free(packet);
return 0;
}
/* The message is to me, search for incoming socket */
socket = csp_port_get_socket(packet->id.dport);
/* If the socket is connection-less, deliver now */
if (socket && (socket->opts & CSP_SO_CONN_LESS)) {
if (csp_route_security_check(socket->opts, input.interface, packet) < 0) {
csp_buffer_free(packet);
return 0;
}
if (csp_queue_enqueue(socket->socket, &packet, 0) != CSP_QUEUE_OK) {
csp_log_error("Conn-less socket queue full");
csp_buffer_free(packet);
return 0;
}
return 0;
}
/* Search for an existing connection */
conn = csp_conn_find(packet->id.ext, CSP_ID_CONN_MASK);
/* If this is an incoming packet on a new connection */
if (conn == NULL) {
/* Reject packet if no matching socket is found */
if (!socket) {
csp_buffer_free(packet);
return 0;
}
/* Run security check on incoming packet */
if (csp_route_security_check(socket->opts, input.interface, packet) < 0) {
csp_buffer_free(packet);
return 0;
}
/* New incoming connection accepted */
csp_id_t idout;
idout.pri = packet->id.pri;
idout.src = csp_get_address();
idout.dst = packet->id.src;
idout.dport = packet->id.sport;
idout.sport = packet->id.dport;
idout.flags = packet->id.flags;
/* Create connection */
conn = csp_conn_new(packet->id, idout);
if (!conn) {
csp_log_error("No more connections available");
csp_buffer_free(packet);
return 0;
}
/* Store the socket queue and options */
conn->socket = socket->socket;
conn->opts = socket->opts;
/* Packet to existing connection */
} else {
/* Run security check on incoming packet */
if (csp_route_security_check(conn->opts, input.interface, packet) < 0) {
csp_buffer_free(packet);
return 0;
}
}
#ifdef CSP_USE_RDP
/* Pass packet to RDP module */
if (packet->id.flags & CSP_FRDP) {
csp_rdp_new_packet(conn, packet);
return 0;
}
#endif
/* Pass packet to UDP module */
csp_udp_new_packet(conn, packet);
return 0;
}
/**
* Helper function to decrypt, check auth and CRC32
* @param security_opts either socket_opts or conn_opts
* @param interface pointer to incoming interface
* @param packet pointer to packet
* @return -1 Missing feature, -2 XTEA error, -3 CRC error, -4 HMAC error, 0 = OK.
*/
static int csp_route_security_check(uint32_t security_opts, csp_iface_t * interface, csp_packet_t * packet) {
/* XTEA encrypted packet */
if (packet->id.flags & CSP_FXTEA) {
#ifdef CSP_USE_XTEA
/* Read nonce */
uint32_t nonce;
memcpy(&nonce, &packet->data[packet->length - sizeof(nonce)], sizeof(nonce));
nonce = csp_ntoh32(nonce);
packet->length -= sizeof(nonce);
/* Create initialization vector */
uint32_t iv[2] = {nonce, 1};
/* Decrypt data */
if (csp_xtea_decrypt(packet->data, packet->length, iv) != 0) {
/* Decryption failed */
csp_log_error("Decryption failed! Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_XTEA;
}
} else if (security_opts & CSP_SO_XTEAREQ) {
csp_log_warn("Received packet without XTEA encryption. Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_XTEA;
#else
csp_log_error("Received XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_NOTSUP;
#endif
}
/* CRC32 verified packet */
if (packet->id.flags & CSP_FCRC32) {
#ifdef CSP_USE_CRC32
/* Verify CRC32 */
if (csp_crc32_verify(packet) != 0) {
/* Checksum failed */
csp_log_error("CRC32 verification error! Discarding packet\r\n");
interface->rx_error++;
return CSP_ERR_CRC32;
}
} else if (security_opts & CSP_SO_CRC32REQ) {
csp_log_warn("Received packet without CRC32. Accepting packet\r\n");
packet->length -= sizeof(uint32_t);
#else
/* Strip CRC32 field and accept the packet */
csp_log_warn("Received packet with CRC32, but CSP was compiled without CRC32 support. Accepting packet\r\n");
packet->length -= sizeof(uint32_t);
#endif
}
/* HMAC authenticated packet */
if (packet->id.flags & CSP_FHMAC) {
#ifdef CSP_USE_HMAC
/* Verify HMAC */
if (csp_hmac_verify(packet) != 0) {
/* HMAC failed */
csp_log_error("HMAC verification error! Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_HMAC;
}
} else if (security_opts & CSP_SO_HMACREQ) {
csp_log_warn("Received packet without HMAC. Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_HMAC;
#else
csp_log_error("Received packet with HMAC, but CSP was compiled without HMAC support. Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_NOTSUP;
#endif
}
/*SEQNR enabled packet */
if(packet->id.flags & CSP_SEQNR) {
#ifdef CSP_USE_SEQNR
if(csp_seqnr_verify(packet) != 0) {
/*Fail*/
csp_log_error("SEQNR verification failed \r\n");
interface->autherr++;
return CSP_ERR_SEQNR;
}
} else if (security_opts & CSP_SO_SEQNR) {
csp_log_warn("SEQNR enabled but received packet without SEQNR. Discarding packet \r\n");
interface->autherr++;
return CSP_ERR_SEQNR;
#else
csp_log_error("Received packet with SEQNR, but CSP was compiled without SEQNR support. Discarding packet \r\n");
interface->autherr++;
return CSP_ERR_NOTSUP;
#endif
}
return CSP_ERR_NONE;
}
int csp_tx_callback(can_id_t canid, can_error_t error, CSP_BASE_TYPE *task_woken) {
int bytes;
uint8_t dest;
/* Match buffer element */
pbuf_element_t *buf = pbuf_find(canid, CFP_ID_CONN_MASK, task_woken);
if (buf == NULL) {
csp_log_info("Failed to match buffer element in tx callback\r\n");
csp_if_can.tx_error++;
return CSP_ERR_INVAL;
}
if (buf->packet == NULL) {
csp_log_warn("Buffer packet was NULL\r\n");
csp_if_can.tx_error++;
pbuf_free(buf, task_woken);
return CSP_ERR_INVAL;
}
/* Free packet buffer if send failed */
if (error != CAN_NO_ERROR) {
csp_log_warn("Error in transmit callback\r\n");
csp_if_can.tx_error++;
pbuf_free(buf, task_woken);
return CSP_ERR_DRIVER;
}
/* Send next frame if not complete */
if (buf->tx_count < buf->packet->length) {
/* Calculate frame data bytes */
bytes = (buf->packet->length - buf->tx_count >= 8) ? 8 : buf->packet->length - buf->tx_count;
/* Insert destination node mac address into the CFP destination field */
dest = csp_route_get_nexthop_mac(buf->packet->id.dst);
if (dest == CSP_NODE_MAC)
dest = buf->packet->id.dst;
/* Prepare identifier */
can_id_t id = 0;
id |= CFP_MAKE_SRC(buf->packet->id.src);
id |= CFP_MAKE_DST(dest);
id |= CFP_MAKE_ID(CFP_ID(canid));
id |= CFP_MAKE_TYPE(CFP_MORE);
id |= CFP_MAKE_REMAIN((buf->packet->length - buf->tx_count - bytes + 7) / 8);
/* Increment tx counter */
buf->tx_count += bytes;
/* Send frame */
if (can_send(id, buf->packet->data + buf->tx_count - bytes, bytes, task_woken) != 0) {
csp_log_info("Failed to send CAN frame in Tx callback\r\n");
csp_if_can.tx_error++;
pbuf_free(buf, task_woken);
return CSP_ERR_DRIVER;
}
} else {
/* Free packet buffer */
pbuf_free(buf, task_woken);
/* Post semaphore if blocking mode is enabled */
if (task_woken != NULL) {
csp_bin_sem_post_isr(&buf->tx_sem, task_woken);
} else {
csp_bin_sem_post(&buf->tx_sem);
}
}
return CSP_ERR_NONE;
}
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;
}
int csp_send_direct(csp_id_t idout, csp_packet_t * packet, uint32_t timeout) {
if (packet == NULL) {
csp_log_error("csp_send_direct called with NULL packet\r\n");
goto err;
}
csp_route_t * ifout = csp_route_if(idout.dst);
if ((ifout == NULL) || (ifout->interface == NULL) || (ifout->interface->nexthop == NULL)) {
csp_log_error("No route to host: %#08x\r\n", idout.ext);
goto err;
}
csp_log_packet("Sending packet size %u from %u to %u port %u via interface %s\r\n", packet->length, idout.src, idout.dst, idout.dport, ifout->interface->name);
#ifdef CSP_USE_PROMISC
/* Loopback traffic is added to promisc queue by the router */
if (idout.dst != my_address) {
packet->id.ext = idout.ext;
csp_promisc_add(packet, csp_promisc_queue);
}
#endif
if (idout.flags & CSP_SEQNR) {
#ifdef CSP_USE_SEQNR
/*Append the gloal seqnr. to the packet */
if(csp_seqnr_append(packet) != 0) {
/* SEQNR append failed */
csp_log_warn("SEQNR append failed !\r\n");
goto tx_err;
}
#else
csp_log_warn("Attempt to send packet with SEQNR, but csp was compiled without SEQNR support. Discarding packet\r\n");
goto tx_err;
#endif
}
/* Only encrypt packets from the current node */
if (idout.src == my_address) {
/* Append HMAC */
if (idout.flags & CSP_FHMAC) {
#ifdef CSP_USE_HMAC
/* Calculate and add HMAC */
if (csp_hmac_append(packet) != 0) {
/* HMAC append failed */
csp_log_warn("HMAC append failed!\r\n");
goto tx_err;
}
#else
csp_log_warn("Attempt to send packet with HMAC, but CSP was compiled without HMAC support. Discarding packet\r\n");
goto tx_err;
#endif
}
/* Append CRC32 */
if (idout.flags & CSP_FCRC32) {
#ifdef CSP_USE_CRC32
/* Calculate and add CRC32 */
if (csp_crc32_append(packet) != 0) {
/* CRC32 append failed */
csp_log_warn("CRC32 append failed!\r\n");
goto tx_err;
}
#else
csp_log_warn("Attempt to send packet with CRC32, but CSP was compiled without CRC32 support. Sending without CRC32r\n");
idout.flags &= ~(CSP_FCRC32);
#endif
}
if (idout.flags & CSP_FXTEA) {
#ifdef CSP_USE_XTEA
/* Create nonce */
uint32_t nonce, nonce_n;
nonce = (uint32_t)rand();
nonce_n = csp_hton32(nonce);
memcpy(&packet->data[packet->length], &nonce_n, sizeof(nonce_n));
/* Create initialization vector */
uint32_t iv[2] = {nonce, 1};
/* Encrypt data */
if (csp_xtea_encrypt(packet->data, packet->length, iv) != 0) {
/* Encryption failed */
csp_log_warn("Encryption failed! Discarding packet\r\n");
goto tx_err;
}
packet->length += sizeof(nonce_n);
#else
csp_log_warn("Attempt to send XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet\r\n");
goto tx_err;
#endif
}
}
/* Copy identifier to packet */
packet->id.ext = idout.ext;
/* Store length before passing to interface */
uint16_t bytes = packet->length;
uint16_t mtu = ifout->interface->mtu;
if (mtu > 0 && bytes > mtu)
goto tx_err;
if ((*ifout->interface->nexthop)(packet, timeout) != CSP_ERR_NONE)
goto tx_err;
ifout->interface->tx++;
ifout->interface->txbytes += bytes;
return CSP_ERR_NONE;
tx_err:
ifout->interface->tx_error++;
err:
return CSP_ERR_TX;
}
int csp_send_direct(csp_id_t idout, csp_packet_t * packet, uint32_t timeout) {
if (packet == NULL) {
csp_log_error("csp_send_direct called with NULL packet\r\n");
goto err;
}
csp_route_t * ifout = csp_route_if(idout.dst);
if ((ifout == NULL) || (ifout->interface == NULL) || (ifout->interface->nexthop == NULL)) {
csp_log_error("No route to host: %#08x\r\n", idout.ext);
goto err;
}
csp_log_packet("Output: Src %u, Dst %u, Dport %u, Sport %u, Pri %u, Flags 0x%02X, Size %u VIA: %s\r\n",
idout.src, idout.dst, idout.dport, idout.sport, idout.pri, idout.flags, packet->length, ifout->interface->name);
/*
* Copy identifier to packet. This originally happened below (after
* encryption), but doing it here makes logging simpler and doesn't
* affect the encryption stuff at all.
*/
packet->id.ext = idout.ext;
/* Log the packet as sent by the application (before encryption) */
if (csp_packet_callback)
csp_packet_callback(CSP_OUTPUT, ifout->interface->name, packet);
#if 0
#ifdef __linux__
struct timespec ts;
clock_gettime(CLOCK_REALTIME, &ts);
double sec = ts.tv_sec + ts.tv_nsec / 1e9;
#else
double sec = (float)xTaskGetTickCount() / configTICK_RATE_HZ;
#endif
printf("%.3f: packet contents (%d bytes):", sec,
packet->length);
for (int i = 0; i < packet->length; i++) {
if (i % 16 == 0) printf("\n");
printf("%02x ", packet->data[i]);
}
printf("\n\n");
#endif
#ifdef CSP_USE_PROMISC
/* Loopback traffic is added to promisc queue by the router */
if (idout.dst != my_address && idout.src == my_address) {
packet->id.ext = idout.ext;
csp_promisc_add(packet, csp_promisc_queue);
}
#endif
/* Only encrypt packets from the current node */
if (idout.src == my_address) {
/* Append HMAC */
if (idout.flags & CSP_FHMAC) {
#ifdef CSP_USE_HMAC
/* Calculate and add HMAC */
if (csp_hmac_append(packet) != 0) {
/* HMAC append failed */
csp_log_warn("HMAC append failed!\r\n");
goto tx_err;
}
#else
csp_log_warn("Attempt to send packet with HMAC, but CSP was compiled without HMAC support. Discarding packet\r\n");
goto tx_err;
#endif
}
/* Append CRC32 */
if (idout.flags & CSP_FCRC32) {
#ifdef CSP_USE_CRC32
/* Calculate and add CRC32 */
if (csp_crc32_append(packet) != 0) {
/* CRC32 append failed */
csp_log_warn("CRC32 append failed!\r\n");
goto tx_err;
}
#else
csp_log_warn("Attempt to send packet with CRC32, but CSP was compiled without CRC32 support. Sending without CRC32r\n");
idout.flags &= ~(CSP_FCRC32);
#endif
}
if (idout.flags & CSP_FXTEA) {
#ifdef CSP_USE_XTEA
/* Create nonce */
uint32_t nonce, nonce_n;
nonce = (uint32_t)rand();
nonce_n = csp_hton32(nonce);
memcpy(&packet->data[packet->length], &nonce_n, sizeof(nonce_n));
/* Create initialization vector */
uint32_t iv[2] = {nonce, 1};
/* Encrypt data */
if (csp_xtea_encrypt(packet->data, packet->length, iv) != 0) {
/* Encryption failed */
csp_log_warn("Encryption failed! Discarding packet\r\n");
goto tx_err;
}
packet->length += sizeof(nonce_n);
#else
csp_log_warn("Attempt to send XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet\r\n");
goto tx_err;
#endif
}
}
/* Store length before passing to interface */
uint16_t bytes = packet->length;
uint16_t mtu = ifout->interface->mtu;
if (mtu > 0 && bytes > mtu) {
csp_log_warn("Attempt to send a packet larger than the interface's mtu.\r\n");
goto tx_err;
}
/* Log the packet as sent over the interface */
if (csp_packet_callback)
csp_packet_callback(CSP_OUTPUT_RAW, ifout->interface->name, packet);
if ((*ifout->interface->nexthop)(ifout->interface, packet, timeout) != CSP_ERR_NONE)
goto tx_err;
/* Update our transmit-time estimates, if the interface supports it */
if (ifout->interface->tx_ms_per_byte) {
/*
* If the completion time is in the past, that means
* the interface is not currently transmitting.
*/
uint32_t time_now = csp_get_ms();
if (csp_time_after(time_now, ifout->interface->tx_done_time))
ifout->interface->tx_done_time = time_now;
ifout->interface->tx_done_time +=
ifout->interface->tx_ms_per_packet
+ bytes * ifout->interface->tx_ms_per_byte;
#if 0
printf("DEBUG: now %u, expected completion at %u\n",
time_now, ifout->interface->tx_done_time);
#endif
}
ifout->interface->tx++;
ifout->interface->txbytes += bytes;
return CSP_ERR_NONE;
tx_err:
ifout->interface->tx_error++;
err:
return CSP_ERR_TX;
}
int csp_can_tx(csp_iface_t * interface, csp_packet_t *packet, uint32_t timeout) {
uint8_t bytes, overhead, avail, dest;
uint8_t frame_buf[8];
/* Get CFP identification number */
int ident = id_get();
if (ident < 0) {
csp_log_warn("Failed to get CFP identification number");
return CSP_ERR_INVAL;
}
/* Calculate overhead */
overhead = sizeof(csp_id_t) + sizeof(uint16_t);
/* Insert destination node mac address into the CFP destination field */
dest = csp_rtable_find_mac(packet->id.dst);
if (dest == CSP_NODE_MAC)
dest = packet->id.dst;
/* Create CAN identifier */
can_id_t id = 0;
id |= CFP_MAKE_SRC(packet->id.src);
id |= CFP_MAKE_DST(dest);
id |= CFP_MAKE_ID(ident);
id |= CFP_MAKE_TYPE(CFP_BEGIN);
id |= CFP_MAKE_REMAIN((packet->length + overhead - 1) / 8);
/* Get packet buffer */
pbuf_element_t *buf = pbuf_new(id, NULL);
if (buf == NULL) {
csp_log_warn("Failed to get packet buffer for CAN");
return CSP_ERR_NOMEM;
}
/* Set packet */
buf->packet = packet;
/* Calculate first frame data bytes */
avail = 8 - overhead;
bytes = (packet->length <= avail) ? packet->length : avail;
/* Copy CSP headers and data */
uint32_t csp_id_be = csp_hton32(packet->id.ext);
uint16_t csp_length_be = csp_hton16(packet->length);
memcpy(frame_buf, &csp_id_be, sizeof(csp_id_be));
memcpy(frame_buf + sizeof(csp_id_be), &csp_length_be, sizeof(csp_length_be));
memcpy(frame_buf + overhead, packet->data, bytes);
/* Increment tx counter */
buf->tx_count += bytes;
/* Take semaphore so driver can post it later */
if (csp_bin_sem_wait(&buf->tx_sem, 0) != CSP_SEMAPHORE_OK) {
csp_log_error("Failed to take CAN pbuf TX sem!");
pbuf_free(buf, NULL, false);
return CSP_ERR_DRIVER;
}
/* Send frame. We must free packet buffer is this fails,
* but the packet itself should be freed by the caller */
if (can_send(id, frame_buf, overhead + bytes, NULL) != 0) {
csp_log_warn("Failed to send CAN frame in csp_tx_can");
csp_bin_sem_post(&buf->tx_sem);
pbuf_free(buf, NULL, false);
return CSP_ERR_DRIVER;
}
/* NOTE: The transmit packet is now owned by the transmission MOB and
* must NOT be freed by the calling thread. */
/* Non blocking mode */
if (timeout == 0)
return CSP_ERR_NONE;
/* Blocking mode */
if (csp_bin_sem_wait(&buf->tx_sem, timeout) != CSP_SEMAPHORE_OK) {
/* tx_sem is posted by transmission callback. The packet
* could still be in use by the transmission MOB, so
* we can not return CSP_ERR_TIMEOUT and risk that the
* calling thread frees the packet. */
return CSP_ERR_NONE;
} else {
csp_bin_sem_post(&buf->tx_sem);
return CSP_ERR_NONE;
}
}
int csp_send_direct(csp_id_t idout, csp_packet_t * packet, csp_iface_t * ifout, uint32_t timeout) {
if (packet == NULL) {
csp_log_error("csp_send_direct called with NULL packet");
goto err;
}
if ((ifout == NULL) || (ifout->nexthop == NULL)) {
csp_log_error("No route to host: %#08x", idout.ext);
goto err;
}
csp_log_packet("OUT: S %u, D %u, Dp %u, Sp %u, Pr %u, Fl 0x%02X, Sz %u VIA: %s",
idout.src, idout.dst, idout.dport, idout.sport, idout.pri, idout.flags, packet->length, ifout->name);
/* Copy identifier to packet (before crc, xtea and hmac) */
packet->id.ext = idout.ext;
#ifdef CSP_USE_PROMISC
/* Loopback traffic is added to promisc queue by the router */
if (idout.dst != csp_get_address() && idout.src == csp_get_address()) {
packet->id.ext = idout.ext;
csp_promisc_add(packet);
}
#endif
/* Only encrypt packets from the current node */
if (idout.src == csp_get_address()) {
/* Append HMAC */
if (idout.flags & CSP_FHMAC) {
#ifdef CSP_USE_HMAC
/* Calculate and add HMAC (does not include header for backwards compatability with csp1.x) */
if (csp_hmac_append(packet, false) != 0) {
/* HMAC append failed */
csp_log_warn("HMAC append failed!");
goto tx_err;
}
#else
csp_log_warn("Attempt to send packet with HMAC, but CSP was compiled without HMAC support. Discarding packet");
goto tx_err;
#endif
}
/* Append CRC32 */
if (idout.flags & CSP_FCRC32) {
#ifdef CSP_USE_CRC32
/* Calculate and add CRC32 (does not include header for backwards compatability with csp1.x) */
if (csp_crc32_append(packet, false) != 0) {
/* CRC32 append failed */
csp_log_warn("CRC32 append failed!");
goto tx_err;
}
#else
csp_log_warn("Attempt to send packet with CRC32, but CSP was compiled without CRC32 support. Sending without CRC32r");
idout.flags &= ~(CSP_FCRC32);
#endif
}
if (idout.flags & CSP_FXTEA) {
#ifdef CSP_USE_XTEA
/* Create nonce */
uint32_t nonce, nonce_n;
nonce = (uint32_t)rand();
nonce_n = csp_hton32(nonce);
memcpy(&packet->data[packet->length], &nonce_n, sizeof(nonce_n));
/* Create initialization vector */
uint32_t iv[2] = {nonce, 1};
/* Encrypt data */
if (csp_xtea_encrypt(packet->data, packet->length, iv) != 0) {
/* Encryption failed */
csp_log_warn("Encryption failed! Discarding packet");
goto tx_err;
}
packet->length += sizeof(nonce_n);
#else
csp_log_warn("Attempt to send XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet");
goto tx_err;
#endif
}
}
/* Store length before passing to interface */
uint16_t bytes = packet->length;
uint16_t mtu = ifout->mtu;
if (mtu > 0 && bytes > mtu)
goto tx_err;
if ((*ifout->nexthop)(ifout, packet, timeout) != CSP_ERR_NONE)
goto tx_err;
ifout->tx++;
ifout->txbytes += bytes;
return CSP_ERR_NONE;
tx_err:
ifout->tx_error++;
err:
return CSP_ERR_TX;
}
/**
* Helper function to decrypt, check auth and CRC32
* @param security_opts either socket_opts or conn_opts
* @param interface pointer to incoming interface
* @param packet pointer to packet
* @return -1 Missing feature, -2 XTEA error, -3 CRC error, -4 HMAC error, 0 = OK.
*/
static int csp_route_security_check(uint32_t security_opts, csp_iface_t * interface, csp_packet_t * packet) {
/* XTEA encrypted packet */
if (packet->id.flags & CSP_FXTEA) {
if (security_opts & CSP_SO_XTEAPROHIB) {
csp_log_error("Received packet with XTEA encryption, but XTEA encryption prohibited on socket");
return CSP_ERR_XTEA;
}
#ifdef CSP_USE_XTEA
/* Read nonce */
uint32_t nonce;
memcpy(&nonce, &packet->data[packet->length - sizeof(nonce)], sizeof(nonce));
nonce = csp_ntoh32(nonce);
packet->length -= sizeof(nonce);
/* Create initialization vector */
uint32_t iv[2] = {nonce, 1};
/* Decrypt data */
if (csp_xtea_decrypt(packet->data, packet->length, iv) != 0) {
/* Decryption failed */
csp_log_error("Decryption failed! Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_XTEA;
}
} else if (security_opts & CSP_SO_XTEAREQ) {
csp_log_warn("Received packet without XTEA encryption from %u. Discarding packet\r\n", packet->id.src);
interface->autherr++;
return CSP_ERR_XTEA;
#else
csp_log_error("Received XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_NOTSUP;
#endif
}
/* CRC32 verified packet */
if (packet->id.flags & CSP_FCRC32) {
#ifdef CSP_USE_CRC32
if (packet->length < 4)
csp_log_error("Too short packet for CRC32, %u", packet->length);
/* Verify CRC32 */
if (csp_crc32_verify(packet) != 0) {
/* Checksum failed */
csp_log_error("CRC32 verification error! Discarding packet\r\n");
interface->rx_error++;
return CSP_ERR_CRC32;
}
} else if (security_opts & CSP_SO_CRC32REQ) {
csp_log_warn("Received packet without CRC32 from %u. Accepting packet\r\n", packet->id.src);
#else
/* Strip CRC32 field and accept the packet */
csp_log_warn("Received packet with CRC32, but CSP was compiled without CRC32 support. Accepting packet\r\n");
packet->length -= sizeof(uint32_t);
#endif
}
/* HMAC authenticated packet */
if (packet->id.flags & CSP_FHMAC) {
if (security_opts & CSP_SO_HMACPROHIB) {
csp_log_error("Received packet with HMAC, but HMAC prohibited on socket");
return CSP_ERR_HMAC;
}
#ifdef CSP_USE_HMAC
/* Verify HMAC */
if (csp_hmac_verify(packet) != 0) {
/* HMAC failed */
csp_log_error("HMAC verification error! Discarding packet (src: %u, dest: %u)\r\n", packet->id.src, packet->id.dst);
interface->autherr++;
return CSP_ERR_HMAC;
}
} else if (security_opts & CSP_SO_HMACREQ) {
csp_log_warn("Received packet without HMAC from %u. Discarding packet\r\n", packet->id.src);
interface->autherr++;
return CSP_ERR_HMAC;
#else
csp_log_error("Received packet with HMAC, but CSP was compiled without HMAC support. Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_NOTSUP;
#endif
}
return CSP_ERR_NONE;
}
int csp_can_tx(csp_iface_t * interface, csp_packet_t *packet, uint32_t timeout) {
uint8_t bytes, overhead, avail, dest;
uint8_t frame_buf[8];
/* Get CFP identification number */
int ident = id_get();
if (ident < 0) {
csp_log_warn("Failed to get CFP identification number\r\n");
return CSP_ERR_INVAL;
}
/* Calculate overhead */
overhead = sizeof(csp_id_t) + sizeof(uint16_t);
/* Insert destination node mac address into the CFP destination field */
dest = csp_route_get_nexthop_mac(packet->id.dst);
if (dest == CSP_NODE_MAC)
dest = packet->id.dst;
/* Create CAN identifier */
can_id_t id = 0;
id |= CFP_MAKE_SRC(packet->id.src);
id |= CFP_MAKE_DST(dest);
id |= CFP_MAKE_ID(ident);
id |= CFP_MAKE_TYPE(CFP_BEGIN);
id |= CFP_MAKE_REMAIN((packet->length + overhead - 1) / 8);
/* Get packet buffer */
pbuf_element_t *buf = pbuf_new(id, NULL);
if (buf == NULL) {
csp_log_warn("Failed to get packet buffer for CAN\r\n");
return CSP_ERR_NOMEM;
}
/* Set packet */
buf->packet = packet;
/* Calculate first frame data bytes */
avail = 8 - overhead;
bytes = (packet->length <= avail) ? packet->length : avail;
/* Copy CSP headers and data */
uint32_t csp_id_be = csp_hton32(packet->id.ext);
uint16_t csp_length_be = csp_hton16(packet->length);
memcpy(frame_buf, &csp_id_be, sizeof(csp_id_be));
memcpy(frame_buf + sizeof(csp_id_be), &csp_length_be, sizeof(csp_length_be));
memcpy(frame_buf + overhead, packet->data, bytes);
/* Increment tx counter */
buf->tx_count += bytes;
/* Take semaphore so driver can post it later */
csp_bin_sem_wait(&buf->tx_sem, 0);
/* Send frame */
if (can_send(id, frame_buf, overhead + bytes, NULL) != 0) {
csp_log_info("Failed to send CAN frame in csp_tx_can\r\n");
return CSP_ERR_DRIVER;
}
/* Non blocking mode */
if (timeout == 0)
return CSP_ERR_NONE;
/* Blocking mode */
if (csp_bin_sem_wait(&buf->tx_sem, timeout) != CSP_SEMAPHORE_OK) {
csp_bin_sem_post(&buf->tx_sem);
return CSP_ERR_TIMEDOUT;
} else {
csp_bin_sem_post(&buf->tx_sem);
return CSP_ERR_NONE;
}
}
