/** * 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); }