/**
* Execute an OBC timesync
* Sends a timespec to the OBC and returns the value back from the OBC.
* Use the value tv_sec = 0, if you don't wish to update time but only get time back.
* @param time timespec_t seconds and nanoseconds
* @param timeout timeout in [ms]
*/
void obc_timesync(timestamp_t * time, int timeout) {
time->tv_sec = csp_hton32(time->tv_sec);
time->tv_nsec = csp_hton32(time->tv_nsec);
if (csp_transaction(CSP_PRIO_NORM, node_obc, OBC_PORT_TIMESYNC, timeout, time, sizeof(timestamp_t), time, sizeof(timestamp_t)) > 0) {
time->tv_sec = csp_ntoh32(time->tv_sec);
time->tv_nsec = csp_ntoh32(time->tv_nsec);
} else {
time->tv_sec = 0;
time->tv_nsec = 0;
}
}
int csp_sfp_recv(csp_conn_t * conn, void ** dataout, int * datasize, uint32_t timeout) {
unsigned int last_byte = 0;
csp_packet_t * packet;
while((packet = csp_read(conn, timeout)) != NULL) {
/* Check that SFP header is present */
if ((packet->id.flags & CSP_FFRAG) == 0) {
csp_debug(CSP_ERROR, "Missing SFP header\r\n");
return -1;
}
/* Read SFP header */
sfp_header_t * sfp_header = csp_sfp_header_remove(packet);
sfp_header->offset = csp_ntoh32(sfp_header->offset);
sfp_header->totalsize = csp_ntoh32(sfp_header->totalsize);
csp_debug(CSP_PROTOCOL, "SFP fragment %u/%u\r\n", sfp_header->offset + packet->length, sfp_header->totalsize);
if (sfp_header->offset > last_byte + 1) {
csp_debug(CSP_ERROR, "SFP missing %u bytes\r\n", sfp_header->offset - last_byte);
csp_buffer_free(packet);
return -1;
} else {
last_byte = sfp_header->offset + packet->length;
}
/* Allocate memory */
if (*dataout == NULL)
*dataout = csp_malloc(sfp_header->totalsize);
*datasize = sfp_header->totalsize;
/* Copy data to output */
if (*dataout != NULL)
memcpy(*dataout + sfp_header->offset, packet->data, packet->length);
if (sfp_header->offset + packet->length >= sfp_header->totalsize) {
csp_debug(CSP_PROTOCOL, "SFP complete\r\n");
csp_buffer_free(packet);
return 0;
} else {
csp_buffer_free(packet);
}
}
return -1;
}
static int do_cmp_clock(struct csp_cmp_message *cmp) {
cmp->clock.tv_sec = csp_ntoh32(cmp->clock.tv_sec);
cmp->clock.tv_nsec = csp_ntoh32(cmp->clock.tv_nsec);
if (cmp->clock.tv_sec != 0) {
clock_set_time(&cmp->clock);
}
clock_get_time(&cmp->clock);
cmp->clock.tv_sec = csp_hton32(cmp->clock.tv_sec);
cmp->clock.tv_nsec = csp_hton32(cmp->clock.tv_nsec);
return CSP_ERR_NONE;
}
/**
* 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);
}
int obc_boot_count_get(uint32_t *boot_count, int timeout) {
int ret = csp_transaction(CSP_PRIO_NORM, node_obc, OBC_PORT_BOOT_COUNT, timeout, NULL, 0, boot_count, sizeof(uint32_t));
if (ret > 0)
*boot_count = csp_ntoh32(*boot_count);
else
*boot_count = 0;
return ret;
}
void obc_load_image(const char * path) {
uint32_t remote_crc;
if (!csp_transaction(CSP_PRIO_NORM, node_obc, OBC_PORT_LOAD_IMG, 10000, (void *) path, strlen(path)+1, &remote_crc, sizeof(uint32_t)))
return;
remote_crc = csp_ntoh32(remote_crc);
printf("Remote CRC %"PRIX32"\r\n", remote_crc);
}
void csp_uptime(uint8_t node, uint32_t timeout) {
uint32_t uptime = 0;
int status = csp_transaction(CSP_PRIO_NORM, node, CSP_UPTIME, timeout, NULL, 0, &uptime, sizeof(uptime));
if (status == 0) {
printf("Network error\r\n");
return;
}
uptime = csp_ntoh32(uptime);
printf("Uptime of node %u is %u s\r\n", (unsigned int) node, (unsigned int) uptime);
}
void csp_buf_free(uint8_t node, uint32_t timeout) {
uint32_t size = 0;
int status = csp_transaction(CSP_PRIO_NORM, node, CSP_BUF_FREE, timeout, NULL, 0, &size, sizeof(size));
if (status == 0) {
printf("Network error\r\n");
return;
}
size = csp_ntoh32(size);
printf("Free buffers at node %u is %u\r\n", (unsigned int) node, (unsigned int) size);
}
void csp_memfree(uint8_t node, uint32_t timeout) {
uint32_t memfree;
int status = csp_transaction(CSP_PRIO_NORM, node, CSP_MEMFREE, timeout, NULL, 0, &memfree, sizeof(memfree));
if (status == 0) {
printf("Network error\r\n");
return;
}
/* Convert from network to host order */
memfree = csp_ntoh32(memfree);
printf("Free Memory at node %u is %u bytes\r\n", node, (unsigned int) memfree);
}
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++;
}
}
/**
* 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;
}
/**
* 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;
}
void csp_service_handler(csp_conn_t * conn, csp_packet_t * packet) {
switch (csp_conn_dport(conn)) {
case CSP_CMP:
/* Pass to CMP handler */
if (csp_cmp_handler(conn, packet) != CSP_ERR_NONE) {
csp_buffer_free(packet);
return;
}
break;
case CSP_PING:
/* A ping means, just echo the packet, so no changes */
csp_log_info("SERVICE: Ping received\r\n");
break;
case CSP_PS: {
csp_sys_tasklist((char *)packet->data);
packet->length = strlen((char *)packet->data);
packet->data[packet->length] = '\0';
packet->length++;
break;
}
case CSP_MEMFREE: {
uint32_t total = csp_sys_memfree();
total = csp_hton32(total);
memcpy(packet->data, &total, sizeof(total));
packet->length = sizeof(total);
break;
}
case CSP_REBOOT: {
uint32_t magic_word;
memcpy(&magic_word, packet->data, sizeof(magic_word));
magic_word = csp_ntoh32(magic_word);
/* If the magic word is invalid, return */
if (magic_word != 0x80078007) {
csp_buffer_free(packet);
return;
}
/* Otherwise Reboot */
csp_sys_reboot();
csp_buffer_free(packet);
return;
}
case CSP_BUF_FREE: {
uint32_t size = csp_buffer_remaining();
size = csp_hton32(size);
memcpy(packet->data, &size, sizeof(size));
packet->length = sizeof(size);
break;
}
case CSP_UPTIME: {
uint32_t time = csp_get_s();
time = csp_hton32(time);
memcpy(packet->data, &time, sizeof(time));
packet->length = sizeof(time);
break;
}
/** @brief Returns route information
* If no data received, sends back number of routes that exist,
* otherwise returns up to nodes_requested, starting at index parameter
* First parameter is index of table to start at, and
* second parameter is number of indeces to return
*/
case CSP_GET_ROUTE: {
//if there is no data, return number of routes that exist
if(packet->length == 0)
{
uint8_t routes_left = csp_num_routes_left(0);
packet->length = sizeof(routes_left);
memcpy(&packet->data[0], &routes_left, sizeof(routes_left));
break;
}
//otherwise, return node information
csp_route_t * route;
uint8_t start_index,i, j, nodes_requested, routes_left;
memcpy(&start_index, &packet->data[0], sizeof(start_index));
memcpy(&nodes_requested, &packet->data[1], sizeof(nodes_requested));
csp_route_info return_info[nodes_requested];
memset(return_info, 0x00, sizeof(return_info));
//don't attempt to read beyond size of routes array
if(start_index >= CSP_ROUTE_COUNT){
csp_buffer_free(packet);
printf("CSP_GET_ROUTE: won't read past array\r\n");
return;
}
i = start_index;
j = 0;
//populated return_info while j index less than number
//of nodes requested and our i index is less than CSP_ROUTE_COUNT
while((j < nodes_requested) && (i < CSP_ROUTE_COUNT)){
route = csp_route_struct(i);
if(route != NULL){
return_info[j].node = i;
strncpy(&return_info[j].name_buffer[0],route->interface->name,
(sizeof(return_info[j].name_buffer)-1));
return_info[j].nexthop_mac_addr = route->nexthop_mac_addr;
if(i != CSP_DEFAULT_ROUTE){
return_info[j].nexthop_mac_addr = route->nexthop_mac_addr
== CSP_NODE_MAC ?(i) : route->nexthop_mac_addr;
}
else return_info[j].nexthop_mac_addr = route->nexthop_mac_addr;
j++;
}
i++;
}
routes_left = csp_num_routes_left(i);
//return data contains number of remaining nodes and node info
packet->length = sizeof(return_info)+sizeof(routes_left);
memcpy(&packet->data[0], &routes_left,sizeof(routes_left));
memcpy(&packet->data[1], return_info, sizeof(return_info));
break;
}
default:
csp_buffer_free(packet);
return;
}
if (!csp_send(conn, packet, 0))
csp_buffer_free(packet);
}
void csp_service_handler(csp_conn_t * conn, csp_packet_t * packet) {
switch (csp_conn_dport(conn)) {
case CSP_CMP:
/* Pass to CMP handler */
if (csp_cmp_handler(conn, packet) != CSP_ERR_NONE) {
csp_buffer_free(packet);
return;
}
break;
case CSP_PING:
/* A ping means, just echo the packet, so no changes */
csp_log_info("SERVICE: Ping received");
break;
case CSP_PS: {
/* Sanity check on request */
if ((packet->length != 1) || (packet->data[0] != 0x55)) {
/* Sanity check failed */
csp_buffer_free(packet);
/* Clear the packet, it has been freed */
packet = NULL;
break;
}
/* Start by allocating just the right amount of memory */
int task_list_size = csp_sys_tasklist_size();
char * pslist = csp_malloc(task_list_size);
/* Check for malloc fail */
if (pslist == NULL) {
/* Send out the data */
strcpy((char *)packet->data, "Not enough memory");
packet->length = strlen((char *)packet->data);
/* Break and let the default handling send packet */
break;
}
/* Retrieve the tasklist */
csp_sys_tasklist(pslist);
int pslen = strnlen(pslist, task_list_size);
/* Split the potentially very long string into packets */
int i = 0;
while(i < pslen) {
/* Allocate packet buffer, if need be */
if (packet == NULL)
packet = csp_buffer_get(CSP_RPS_MTU);
if (packet == NULL)
break;
/* Calculate length, either full MTU or the remainder */
packet->length = (pslen - i > CSP_RPS_MTU) ? CSP_RPS_MTU : (pslen - i);
/* Send out the data */
memcpy(packet->data, &pslist[i], packet->length);
i += packet->length;
if (!csp_send(conn, packet, 0))
csp_buffer_free(packet);
/* Clear the packet reference when sent */
packet = NULL;
}
csp_free(pslist);
break;
}
case CSP_MEMFREE: {
uint32_t total = csp_sys_memfree();
total = csp_hton32(total);
memcpy(packet->data, &total, sizeof(total));
packet->length = sizeof(total);
break;
}
case CSP_REBOOT: {
uint32_t magic_word;
memcpy(&magic_word, packet->data, sizeof(magic_word));
magic_word = csp_ntoh32(magic_word);
/* If the magic word is valid, reboot */
if (magic_word == CSP_REBOOT_MAGIC) {
csp_sys_reboot();
} else if (magic_word == CSP_REBOOT_SHUTDOWN_MAGIC) {
csp_sys_shutdown();
}
csp_buffer_free(packet);
return;
}
case CSP_BUF_FREE: {
uint32_t size = csp_buffer_remaining();
size = csp_hton32(size);
memcpy(packet->data, &size, sizeof(size));
packet->length = sizeof(size);
break;
}
case CSP_UPTIME: {
uint32_t time = csp_get_s();
time = csp_hton32(time);
memcpy(packet->data, &time, sizeof(time));
packet->length = sizeof(time);
break;
}
default:
csp_buffer_free(packet);
return;
}
if (packet != NULL) {
if (!csp_send(conn, packet, 0))
csp_buffer_free(packet);
}
}
void csp_service_handler(csp_conn_t * conn, csp_packet_t * packet) {
switch (csp_conn_dport(conn)) {
case CSP_CMP:
/* Pass to CMP handler */
if (csp_cmp_handler(conn, packet) != CSP_ERR_NONE) {
csp_buffer_free(packet);
return;
}
break;
case CSP_PING:
/* A ping means, just echo the packet, so no changes */
csp_log_info("SERVICE: Ping received\r\n");
break;
case CSP_PS: {
csp_sys_tasklist((char *)packet->data);
packet->length = strlen((char *)packet->data);
packet->data[packet->length] = '\0';
packet->length++;
break;
}
case CSP_MEMFREE: {
uint32_t total = csp_sys_memfree();
total = csp_hton32(total);
memcpy(packet->data, &total, sizeof(total));
packet->length = sizeof(total);
break;
}
case CSP_REBOOT: {
uint32_t magic_word;
memcpy(&magic_word, packet->data, sizeof(magic_word));
magic_word = csp_ntoh32(magic_word);
/* If the magic word is invalid, return */
if (magic_word != 0x80078007) {
csp_buffer_free(packet);
return;
}
/* Otherwise Reboot */
csp_sys_reboot();
csp_buffer_free(packet);
return;
}
case CSP_BUF_FREE: {
uint32_t size = csp_buffer_remaining();
size = csp_hton32(size);
memcpy(packet->data, &size, sizeof(size));
packet->length = sizeof(size);
break;
}
case CSP_UPTIME: {
uint32_t time = csp_get_s();
time = csp_hton32(time);
memcpy(packet->data, &time, sizeof(time));
packet->length = sizeof(time);
break;
}
default:
csp_buffer_free(packet);
return;
}
if (!csp_send(conn, packet, 0))
csp_buffer_free(packet);
}
/**
* 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;
}
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;
}
