/**
* Send a packet on an already established connection
* @param conn pointer to connection
* @param packet pointer to packet,
* @param timeout a timeout to wait for TX to complete. NOTE: not all underlying drivers supports flow-control.
* @return returns 1 if successful and 0 otherwise. you MUST free the frame yourself if the transmission was not successful.
*/
int csp_send(csp_conn_t * conn, csp_packet_t * packet, unsigned int timeout) {
if ((conn == NULL) || (packet == NULL) || (conn->state != CONN_OPEN)) {
csp_debug(CSP_ERROR, "Invalid call to csp_send\r\n");
return 0;
}
#if CSP_USE_RDP
int result = 1;
switch(conn->idout.protocol) {
case CSP_RDP:
result = csp_rdp_send(conn, packet, timeout);
break;
default:
break;
}
if (result == 0) {
csp_debug(CSP_WARN, "RPD send failed\r\n!");
return 0;
}
#endif
return csp_send_direct(conn->idout, packet, timeout);
}
void * csp_buffer_get_isr(size_t buf_size) {
static uint8_t csp_buffer_last_given = 0;
if (buf_size + CSP_BUFFER_PACKET_OVERHEAD > size) {
csp_debug(CSP_ERROR, "Attempt to allocate too large block %u\r\n", buf_size);
return NULL;
}
int i = csp_buffer_last_given; // Start with the last given element
i = (i + 1) % count; // Increment by one
while (i != csp_buffer_last_given) { // Loop till we have checked all
if (csp_buffer_list[i] == CSP_BUFFER_FREE) { // Check the buffer list
csp_buffer_list[i] = CSP_BUFFER_USED; // Mark as used
csp_buffer_last_given = i; // Remember the progress
csp_debug(CSP_BUFFER, "BUFFER: Using element %u at %p\r\n", i, csp_buffer_p + (i * size));
return csp_buffer_p + (i * size); // Return poniter
}
i = (i + 1) % count; // Increment by one
}
csp_debug(CSP_ERROR, "Out of buffers\r\n");
return NULL; // If we are out of memory, return NULL
}
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;
}
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, 100) != 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_debug(CSP_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, 100) != CSP_QUEUE_OK)
return CSP_ERR_TIMEDOUT;
#endif
return CSP_ERR_NONE;
}
/**
* Instantly free's the packet buffer
* This call is safe from both ISR and Task context
* @param packet
*/
void csp_buffer_free(void * packet) {
int i = ((uint8_t *) packet - csp_buffer_p) / size; // Find number in array by math (wooo)
csp_debug(CSP_BUFFER, "BUFFER: Free element %u\r\n", i);
if (i < 0 || i > count)
return;
csp_buffer_list[i] = CSP_BUFFER_FREE; // Mark this as free now
}
int csp_bind(csp_socket_t * socket, uint8_t port) {
if (port > CSP_ANY) {
csp_debug(CSP_ERROR, "Only ports from 0-%u (and CSP_ANY for default) are available for incoming ports\r\n", CSP_ANY);
return CSP_ERR_INVAL;
}
/* Check if port number is valid */
if (ports[port].state != PORT_CLOSED) {
csp_debug(CSP_ERROR, "Port %d is already in use\r\n", port);
return CSP_ERR_USED;
}
csp_debug(CSP_INFO, "Binding socket %p to port %u\r\n", socket, port);
/* Save listener */
ports[port].socket = socket;
ports[port].state = PORT_OPEN;
return CSP_ERR_NONE;
}
static csp_rtable_t * csp_rtable_find(uint8_t addr, uint8_t netmask, uint8_t exact) {
/* Remember best result */
csp_rtable_t * best_result = NULL;
uint8_t best_result_mask = 0;
/* Start search */
csp_rtable_t * i = rtable;
while(i) {
/* Look for exact match */
if (i->address == addr && i->netmask == netmask) {
best_result = i;
break;
}
/* Try a CIDR netmask match */
if (!exact) {
uint8_t hostbits = (1 << (CSP_ID_HOST_SIZE - i->netmask)) - 1;
uint8_t netbits = ~hostbits;
//printf("Netbits %x Hostbits %x\r\n", netbits, hostbits);
/* Match network addresses */
uint8_t net_a = i->address & netbits;
uint8_t net_b = addr & netbits;
//printf("A: %hhx, B: %hhx\r\n", net_a, net_b);
/* We have a match */
if (net_a == net_b) {
if (i->netmask >= best_result_mask) {
//printf("Match best result %u %u\r\n", best_result_mask, i->netmask);
best_result = i;
best_result_mask = i->netmask;
}
}
}
i = i->next;
}
#if 0
if (best_result)
csp_debug(CSP_PACKET, "Using routing entry: %u/%u dev %s m:%u\r\n", best_result->address, best_result->netmask, best_result->interface->name, best_result->mac);
#endif
return best_result;
}
/** csp_socket
* Create CSP socket endpoint
* @param opts Socket options
* @return Pointer to socket on success, NULL on failure
*/
csp_socket_t * csp_socket(uint32_t opts) {
/* Validate socket options */
if ((opts & CSP_SO_RDPREQ) && !CSP_USE_RDP) {
csp_debug(CSP_ERROR, "Attempt to create socket that requires RDP, but CSP was compiled without RDP support\r\n");
return NULL;
} else if ((opts & CSP_SO_XTEAREQ) && !CSP_ENABLE_XTEA) {
csp_debug(CSP_ERROR, "Attempt to create socket that requires XTEA, but CSP was compiled without XTEA support\r\n");
return NULL;
} else if ((opts & CSP_SO_HMACREQ) && !CSP_ENABLE_HMAC) {
csp_debug(CSP_ERROR, "Attempt to create socket that requires XTEA, but CSP was compiled without XTEA support\r\n");
return NULL;
}
/* Use CSP buffers instead? */
csp_socket_t * sock = csp_malloc(sizeof(csp_socket_t));
if (sock != NULL) {
sock->conn_queue = NULL;
sock->opts = opts;
}
return sock;
}
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;
}
int csp_buffer_init(int buf_count, int buf_size) {
#ifndef CSP_BUFFER_STATIC
/* Remember size */
count = buf_count;
size = buf_size;
/* Allocate main memory */
csp_buffer_p = csp_malloc(count * size);
if (csp_buffer_p == NULL)
return CSP_ERR_NOMEM;
/* Allocate housekeeping memory */
csp_buffer_list = (csp_buffer_state_t *) csp_malloc(count * sizeof(csp_buffer_state_t));
if (csp_buffer_list == NULL) {
csp_free(csp_buffer_p);
return CSP_ERR_NOMEM;
}
#endif
#if defined(CSP_POSIX) || defined(CSP_WINDOWS)
/* Initialize critical lock */
if (csp_bin_sem_create(&csp_critical_lock) != CSP_SEMAPHORE_OK) {
csp_debug(CSP_ERROR, "No more memory for buffer semaphore\r\n");
if (csp_buffer_list)
csp_free(csp_buffer_list);
if (csp_buffer_p)
csp_free(csp_buffer_p);
return CSP_ERR_NOMEM;
}
#endif
/* Clear housekeeping memory = all free mem */
memset(csp_buffer_list, 0, count * sizeof(csp_buffer_state_t));
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_debug(CSP_ERROR, "Decryption failed! Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_XTEA;
}
} else if (security_opts & CSP_SO_XTEAREQ) {
csp_debug(CSP_WARN, "Received packet without XTEA encryption. Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_XTEA;
#else
csp_debug(CSP_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_debug(CSP_ERROR, "CRC32 verification error! Discarding packet\r\n");
interface->rx_error++;
return CSP_ERR_CRC32;
}
} else if (security_opts & CSP_SO_CRC32REQ) {
csp_debug(CSP_WARN, "Received packet without CRC32. Accepting packet\r\n");
packet->length -= sizeof(uint32_t);
#else
/* Strip CRC32 field and accept the packet */
csp_debug(CSP_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_debug(CSP_ERROR, "HMAC verification error! Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_HMAC;
}
} else if (security_opts & CSP_SO_HMACREQ) {
csp_debug(CSP_WARN, "Received packet without HMAC. Discarding packet\r\n");
interface->autherr++;
return CSP_ERR_HMAC;
#else
csp_debug(CSP_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;
}
/**
* Function to transmit a frame without an existing connection structure.
* This function is used for stateless transmissions
* @param idout 32bit CSP identifier
* @param packet pointer to packet,
* @param timeout a timeout to wait for TX to complete. NOTE: not all underlying drivers supports flow-control.
* @return returns 1 if successful and 0 otherwise. you MUST free the frame yourself if the transmission was not successful.
*/
int csp_send_direct(csp_id_t idout, csp_packet_t * packet, unsigned int timeout) {
if (packet == NULL) {
csp_debug(CSP_ERROR, "csp_send_direct: packet == NULL\r\n");
return 0;
}
csp_iface_t * ifout = csp_route_if(idout.dst);
if ((ifout == NULL) || (*ifout->nexthop == NULL)) {
csp_debug(CSP_ERROR, "No route to host: %#08x\r\n", idout.ext);
return 0;
}
csp_debug(CSP_PACKET, "Sending packet from %u to %u port %u via interface %s\r\n", idout.src, idout.dst, idout.dport, ifout->name);
ifout->count++;
#if 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
/* Only encrypt packets from the current node */
if (idout.src == my_address && (idout.flags & CSP_FXTEA)) {
#if CSP_ENABLE_XTEA
/* Create nonce */
uint32_t nonce, nonce_n;
nonce = (uint32_t)rand();
nonce_n = htonl(nonce);
memcpy(&packet->data[packet->length], &nonce_n, sizeof(nonce_n));
/* Create initialization vector */
uint32_t iv[2] = {nonce, 1};
/* Encrypt data */
if (xtea_encrypt(packet->data, packet->length, (uint32_t *)CSP_CRYPTO_KEY, iv) != 0) {
/* Encryption failed */
csp_debug(CSP_WARN, "Encryption failed! Discarding packet\r\n");
csp_buffer_free(packet);
return 0;
}
packet->length += sizeof(nonce_n);
#else
csp_debug(CSP_WARN, "Attempt to send XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet\r\n");
return 0;
#endif
}
/* Only append HMAC to packets from the current node */
if (idout.src == my_address && (idout.flags & CSP_FHMAC)) {
#if CSP_ENABLE_HMAC
/* Calculate and add HMAC */
if (hmac_append(packet, (uint8_t *)CSP_CRYPTO_KEY, CSP_CRYPTO_KEY_LENGTH) != 0) {
/* HMAC append failed */
csp_debug(CSP_WARN, "HMAC append failed!\r\n");
csp_buffer_free(packet);
return 0;
}
#else
csp_debug(CSP_WARN, "Attempt to send packet with HMAC, but CSP was compiled without HMAC support. Discarding packet\r\n");
return 0;
#endif
}
return (*ifout->nexthop)(idout, packet, timeout);
}
