/** Copy image from RAM (src) to ROM (typically 0x48000000) */
void obc_ram_to_rom(uint32_t size, uint32_t checksum, uint32_t src, uint32_t dst) {
obc_ram_to_rom_t txbuf;
txbuf.size = csp_hton32(size);
txbuf.checksum = csp_hton32(checksum);
txbuf.src = csp_hton32(src);
txbuf.dst = csp_hton32(dst);
csp_transaction(CSP_PRIO_NORM, node_obc, OBC_PORT_RAM_TO_ROM, 0, &txbuf, sizeof(obc_ram_to_rom_t), NULL, 0);
}
/**
* 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;
}
}
void obc_boot_conf(uint32_t checksum, uint32_t boot_counts, const char * path) {
char buf[100];
checksum = csp_hton32(checksum);
boot_counts = csp_hton32(boot_counts);
memcpy(buf, &checksum, 4);
memcpy(&buf[4], &boot_counts, 4);
strncpy(buf + 8, path, 100-8);
csp_transaction(CSP_PRIO_NORM, node_obc, OBC_PORT_BOOT_CONF, 0, buf, strlen(path)+1+4+4, NULL, 0);
}
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;
}
int csp_i2c_tx(csp_packet_t * packet, uint32_t timeout) {
/* Cast the CSP packet buffer into an i2c frame */
i2c_frame_t * frame = (i2c_frame_t *) packet;
/* Insert destination node into the i2c destination field */
if (csp_route_get_nexthop_mac(packet->id.dst) == CSP_NODE_MAC) {
frame->dest = packet->id.dst;
} else {
frame->dest = csp_route_get_nexthop_mac(packet->id.dst);
}
/* Save the outgoing id in the buffer */
packet->id.ext = csp_hton32(packet->id.ext);
/* Add the CSP header to the I2C length field */
frame->len += sizeof(packet->id);
frame->len_rx = 0;
/* Some I2C drivers support X number of retries
* CSP don't care about this. If it doesn't work the first
* time, don'y use time on it.
*/
frame->retries = 0;
/* enqueue the frame */
if (i2c_send(csp_i2c_handle, frame, timeout) != E_NO_ERR)
return CSP_ERR_DRIVER;
return CSP_ERR_NONE;
}
static int csp_tiradio_tx (csp_iface_t *interface,
csp_packet_t *packet, uint32_t timeout) {
int ret = CSP_ERR_NONE;
int txbufin = packet->length + CSP_HEADER_LENGTH;
uint8_t *txbuf = csp_malloc(txbufin);
csp_tiradio_driver_handle_t *driver = interface->driver;
if (txbuf == NULL)
return CSP_ERR_NOMEM;
if (driver->module_id < NUM_TIRADIO_MODULES)
latest_csp_transfer_id[driver->module_id] = packet->id;
/* Save the outgoing id in the buffer */
packet->id.ext = csp_hton32(packet->id.ext);
memcpy(txbuf, &packet->id.ext, txbufin);
/* The packet goes straight to the radio. */
if (driver->putstr(driver->module_id,txbuf, txbufin) != 0) {
interface->tx_error++;
ret = CSP_ERR_TIMEDOUT;
} else {
csp_buffer_free(packet);
}
csp_free(txbuf);
return ret;
}
int csp_sfp_send(csp_conn_t * conn, void * data, int totalsize, int mtu, uint32_t timeout) {
int count = 0;
while(count < totalsize) {
/* Allocate packet */
csp_packet_t * packet = csp_buffer_get(mtu);
if (packet == NULL)
return -1;
/* Calculate sending size */
int size = totalsize - count;
if (size > mtu)
size = mtu;
/* Print debug */
csp_debug(CSP_PROTOCOL, "Sending SFP at %x size %u\r\n", data + count, size);
/* Copy data */
memcpy(packet->data, data + count, size);
packet->length = size;
/* Set fragment flag */
conn->idout.flags |= CSP_FFRAG;
/* Add SFP header */
sfp_header_t * sfp_header = csp_sfp_header_add(packet);
sfp_header->totalsize = csp_hton32(totalsize);
sfp_header->offset = csp_hton32(count);
/* Send data */
if (!csp_send(conn, packet, timeout)) {
csp_buffer_free(packet);
return -1;
}
/* Increment count */
count += size;
}
return 0;
}
static int do_cmp_if_stats(struct csp_cmp_message *cmp) {
csp_iface_t *ifc = csp_route_get_if_by_name(cmp->if_stats.interface);
if (ifc == NULL)
return CSP_ERR_INVAL;
cmp->if_stats.tx = csp_hton32(ifc->tx);
cmp->if_stats.rx = csp_hton32(ifc->rx);
cmp->if_stats.tx_error = csp_hton32(ifc->tx_error);
cmp->if_stats.rx_error = csp_hton32(ifc->rx_error);
cmp->if_stats.drop = csp_hton32(ifc->drop);
cmp->if_stats.autherr = csp_hton32(ifc->autherr);
cmp->if_stats.frame = csp_hton32(ifc->frame);
cmp->if_stats.txbytes = csp_hton32(ifc->txbytes);
cmp->if_stats.rxbytes = csp_hton32(ifc->rxbytes);
cmp->if_stats.irq = csp_hton32(ifc->irq);
return CSP_ERR_NONE;
}
static int do_cmp_peek(struct csp_cmp_message *cmp) {
cmp->peek.addr = csp_hton32(cmp->peek.addr);
if (cmp->peek.len > CSP_CMP_PEEK_MAX_LEN)
return CSP_ERR_INVAL;
/* Dangerous, you better know what you are doing */
memcpy(cmp->peek.data, (void *) (uintptr_t) cmp->peek.addr, cmp->peek.len);
return CSP_ERR_NONE;
}
static int do_cmp_poke(struct csp_cmp_message *cmp) {
cmp->poke.addr = csp_hton32(cmp->poke.addr);
if (cmp->poke.len > CSP_CMP_POKE_MAX_LEN)
return CSP_ERR_INVAL;
/* Extremely dangerous, you better know what you are doing */
memcpy((void *) (uintptr_t) cmp->poke.addr, cmp->poke.data, cmp->poke.len);
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;
}
/**
* Jump to an address in RAM
* This function is only useful after the load command has been executed, or an image
* has been uploaded to RAM.
* @param addr
*/
void obc_jump_ram(uint32_t addr) {
addr = csp_hton32(addr);
csp_transaction(CSP_PRIO_NORM, node_obc, OBC_PORT_JUMP, 0, &addr, sizeof(uint32_t), NULL, 0);
}
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);
}
}
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;
}
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);
}
void csp_reboot(uint8_t node) {
uint32_t magic_word = csp_hton32(0x80078007);
csp_transaction(CSP_PRIO_NORM, node, CSP_REBOOT, 0, &magic_word, sizeof(magic_word), NULL, 0);
}
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;
}
void obc_boot_del(void) {
uint32_t checksum = csp_hton32(0x80078007);
csp_transaction(CSP_PRIO_NORM, node_obc, OBC_PORT_BOOT_CONF, 0, &checksum, 4, NULL, 0);
}
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_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;
}
}