int csp_ping(uint8_t node, uint32_t timeout, unsigned int size, uint8_t conn_options) {
int i;
uint32_t start, time, status = 0;
/* Counter */
start = csp_get_ms();
/* Open connection */
csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PING, timeout, conn_options);
if (conn == NULL)
return -1;
/* Prepare data */
csp_packet_t * packet;
packet = csp_buffer_get(size);
if (packet == NULL)
goto out;
/* Set data to increasing numbers */
packet->length = size;
for (i = 0; i < size; i++)
packet->data[i] = i;
/* Try to send frame */
if (!csp_send(conn, packet, 0))
goto out;
/* Read incoming frame */
packet = csp_read(conn, timeout);
if (packet == NULL)
goto out;
/* Ensure that the data was actually echoed */
for (i = 0; i < size; i++)
if (packet->data[i] != i)
goto out;
status = 1;
out:
/* Clean up */
if (packet != NULL)
csp_buffer_free(packet);
csp_close(conn);
/* We have a reply */
time = (csp_get_ms() - start);
if (status) {
return time;
} else {
return -1;
}
}
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;
}
conn->in_send = 1;
#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);
conn->in_send = 0;
conn->last_send_time = csp_get_ms();
return (ret == CSP_ERR_NONE) ? 1 : 0;
}
/** 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_info("CAN Buffer element timed out\r\n");
/* Reuse packet buffer */
pbuf_free(buf, NULL);
}
}
}
/* Unlock packet buffer */
CSP_EXIT_CRITICAL(pbuf_sem);
}
/** pbuf_timestamp
* Update packet buffer timestamp of last use.
* @param buf Buffer element to update
* @param task_woken
* @return
*/
int pbuf_timestamp(pbuf_element_t *buf, CSP_BASE_TYPE *task_woken) {
if (buf != NULL) {
buf->last_used = task_woken ? csp_get_ms_isr() : csp_get_ms();
return CSP_ERR_NONE;
} else {
return CSP_ERR_INVAL;
}
}
/* Identification number */
static int id_init(void) {
/* Init ID field to random number */
srand((int)csp_get_ms());
cfp_id = rand() & ((1 << CFP_ID_SIZE) - 1);
if (csp_bin_sem_create(&id_sem) == CSP_SEMAPHORE_OK) {
return CSP_ERR_NONE;
} else {
csp_log_error("Could not initialize CFP id semaphore\r\n");
return CSP_ERR_NOMEM;
}
}
/** csp_init
* Start up the can-space protocol
* @param address The CSP node address
*/
void csp_init(unsigned char address) {
/* Initialize CSP */
my_address = address;
csp_conn_init();
csp_port_init();
csp_route_table_init();
/* Initialize random number generator */
srand(csp_get_ms());
/* Register loopback route */
csp_route_set("LOOP", address, csp_lo_tx, CSP_NODE_MAC);
}
csp_conn_t * csp_conn_new(csp_id_t idin, csp_id_t idout) {
/* Allocate connection structure */
csp_conn_t * conn = csp_conn_allocate(CONN_CLIENT);
if (conn) {
/* No lock is needed here, because nobody else *
* has a reference to this connection yet. */
conn->idin.ext = idin.ext;
conn->idout.ext = idout.ext;
conn->timestamp = csp_get_ms();
/* Ensure connection queue is empty */
csp_conn_flush_rx_queue(conn);
}
return conn;
}
int csp_conn_init(void) {
/* Initialize source port */
srand(csp_get_ms());
sport = (rand() % (CSP_ID_PORT_MAX - CSP_MAX_BIND_PORT)) + (CSP_MAX_BIND_PORT + 1);
if (csp_bin_sem_create(&sport_lock) != CSP_SEMAPHORE_OK) {
csp_log_error("No more memory for sport semaphore\r\n");
return CSP_ERR_NOMEM;
}
int i, prio;
for (i = 0; i < CSP_CONN_MAX; i++) {
for (prio = 0; prio < CSP_RX_QUEUES; prio++)
arr_conn[i].rx_queue[prio] = csp_queue_create(CSP_RX_QUEUE_LENGTH, sizeof(csp_packet_t *));
#ifdef CSP_USE_QOS
arr_conn[i].rx_event = csp_queue_create(CSP_CONN_QUEUE_LENGTH, sizeof(int));
#endif
arr_conn[i].state = CONN_CLOSED;
if (csp_mutex_create(&arr_conn[i].lock) != CSP_MUTEX_OK) {
csp_log_error("Failed to create connection lock\r\n");
return CSP_ERR_NOMEM;
}
#ifdef CSP_USE_RDP
if (csp_rdp_allocate(&arr_conn[i]) != CSP_ERR_NONE) {
csp_log_error("Failed to create queues for RDP in csp_conn_init\r\n");
return CSP_ERR_NOMEM;
}
#endif
}
if (csp_bin_sem_create(&conn_lock) != CSP_SEMAPHORE_OK) {
csp_log_error("No more memory for conn semaphore\r\n");
return CSP_ERR_NOMEM;
}
return CSP_ERR_NONE;
}
void csp_route_add_if(csp_iface_t *ifc) {
/* Add interface to pool */
if (interfaces == NULL) {
/* This is the first interface to be added */
interfaces = ifc;
ifc->next = NULL;
} else {
/* One or more interfaces were already added */
csp_iface_t * i = interfaces;
while (i != ifc && i->next)
i = i->next;
/* Insert interface last if not already in pool */
if (i != ifc && i->next == NULL) {
i->next = ifc;
ifc->next = NULL;
}
}
/* Initialize the estimated TX completion time */
ifc->tx_done_time = csp_get_ms();
}
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;
}
uint32_t csp_get_ms_isr(void) {
return csp_get_ms();
}
