void csp_buffer_free(void *packet) {
if (!packet) {
csp_log_error("Attempt to free null pointer\r\n");
return;
}
csp_skbf_t * buf = packet - sizeof(csp_skbf_t);
if (((uintptr_t) buf % CSP_BUFFER_ALIGN) > 0) {
csp_log_error("FREE: Unaligned CSP buffer pointer %p\r\n", packet);
return;
}
if (buf->skbf_addr != buf) {
csp_log_error("FREE: Invalid CSP buffer pointer %p\r\n", packet);
return;
}
if (buf->refcount == 0) {
csp_log_error("FREE: Buffer already free %p\r\n", buf);
return;
} else if (buf->refcount > 1) {
buf->refcount--;
csp_log_error("FREE: Buffer %p in use by %u users\r\n", buf, buf->refcount);
return;
} else {
buf->refcount = 0;
csp_log_buffer("FREE: %p\r\n", buf);
csp_queue_enqueue(csp_buffers, &buf, 0);
}
}
int csp_conn_enqueue_packet(csp_conn_t * conn, csp_packet_t * packet) {
if (!conn || !packet)
return CSP_ERR_INVAL;
int rxq = csp_conn_get_rxq(packet->id.pri);
if (csp_queue_enqueue(conn->rx_queue[rxq], &packet, 0) != CSP_QUEUE_OK)
return CSP_ERR_NOMEM;
#ifdef CSP_USE_QOS
int event = 0;
if (csp_queue_enqueue(conn->rx_event, &event, 0) != CSP_QUEUE_OK)
return CSP_ERR_NOMEM;
#endif
return CSP_ERR_NONE;
}
int csp_route_enqueue(csp_queue_handle_t handle, void * value, uint32_t timeout, CSP_BASE_TYPE * pxTaskWoken) {
int result;
if (pxTaskWoken == NULL)
result = csp_queue_enqueue(handle, value, timeout);
else
result = csp_queue_enqueue_isr(handle, value, pxTaskWoken);
#ifdef CSP_USE_QOS
static int event = 0;
if (result == CSP_QUEUE_OK) {
if (pxTaskWoken == NULL)
csp_queue_enqueue(router_input_event, &event, 0);
else
csp_queue_enqueue_isr(router_input_event, &event, pxTaskWoken);
}
#endif
return (result == CSP_QUEUE_OK) ? CSP_ERR_NONE : CSP_ERR_NOBUFS;
}
int csp_buffer_init(int buf_count, int buf_size) {
unsigned int i;
csp_skbf_t * buf;
count = buf_count;
size = buf_size;
unsigned int skbfsize = (sizeof(csp_skbf_t) + size);
skbfsize = CSP_BUFFER_ALIGN * ((skbfsize + CSP_BUFFER_ALIGN - 1) / CSP_BUFFER_ALIGN);
unsigned int poolsize = count * skbfsize;
csp_buffer_pool = csp_malloc(poolsize);
if (csp_buffer_pool == NULL)
goto fail_malloc;
csp_buffers = csp_queue_create(count, sizeof(void *));
if (!csp_buffers)
goto fail_queue;
if (CSP_INIT_CRITICAL(csp_critical_lock) != CSP_ERR_NONE)
goto fail_critical;
memset(csp_buffer_pool, 0, poolsize);
for (i = 0; i < count; i++) {
/* We have already taken care of pointer alignment since
* skbfsize is an integer multiple of sizeof(int *)
* but the explicit cast to a void * is still necessary
* to tell the compiler so.
*/
buf = (void *) &csp_buffer_pool[i * skbfsize];
buf->refcount = 0;
buf->skbf_addr = buf;
csp_queue_enqueue(csp_buffers, &buf, 0);
}
return CSP_ERR_NONE;
fail_critical:
csp_queue_remove(csp_buffers);
fail_queue:
csp_free(csp_buffer_pool);
fail_malloc:
return CSP_ERR_NOMEM;
}
void csp_promisc_add(csp_packet_t * packet, csp_queue_handle_t queue) {
if (csp_promisc_enabled == 0)
return;
if (queue != NULL) {
/* Make a copy of the message and queue it to the promiscuous task */
csp_packet_t *packet_copy = csp_buffer_clone(packet);
if (packet_copy != NULL) {
if (csp_queue_enqueue(queue, &packet_copy, 0) != CSP_QUEUE_OK) {
csp_log_error("Promiscuous mode input queue full\r\n");
csp_buffer_free(packet_copy);
}
}
}
}
void csp_udp_new_packet(csp_conn_t * conn, csp_packet_t * packet) {
/* Enqueue */
if (csp_conn_enqueue_packet(conn, packet) < 0) {
csp_log_error("Connection buffer queue full!\r\n");
csp_buffer_free(packet);
return;
}
/* Try to queue up the new connection pointer */
if (conn->socket != NULL) {
if (csp_queue_enqueue(conn->socket, &conn, 0) != CSP_QUEUE_OK) {
csp_log_warn("Warning socket connection queue full\r\n");
csp_close(conn);
return;
}
/* Ensure that this connection will not be posted to this socket again */
conn->socket = NULL;
}
}
int csp_buffer_init(int buf_count, int buf_size) {
unsigned int i;
void *element;
count = buf_count;
size = buf_size;
csp_buffer_list = csp_malloc(count * size);
if (csp_buffer_list == NULL)
goto fail_malloc;
csp_buffers = csp_queue_create(count, sizeof(void *));
if (!csp_buffers)
goto fail_queue;
if (CSP_INIT_CRITICAL(csp_critical_lock) != CSP_ERR_NONE)
goto fail_critical;
memset(csp_buffer_list, 0, count * size);
for (i = 0; i < count; i++) {
element = csp_buffer_list + i * size;
csp_queue_enqueue(csp_buffers, &element, 0);
}
return CSP_ERR_NONE;
fail_critical:
csp_queue_remove(csp_buffers);
fail_queue:
csp_free(csp_buffer_list);
fail_malloc:
return CSP_ERR_NOMEM;
}
int csp_route_work(uint32_t timeout) {
csp_qfifo_t input;
csp_packet_t * packet;
csp_conn_t * conn;
csp_socket_t * socket;
#ifdef CSP_USE_RDP
/* Check connection timeouts (currently only for RDP) */
csp_conn_check_timeouts();
#endif
/* Get next packet to route */
if (csp_qfifo_read(&input) != CSP_ERR_NONE)
return -1;
packet = input.packet;
csp_log_packet("INP: S %u, D %u, Dp %u, Sp %u, Pr %u, Fl 0x%02X, Sz %"PRIu16" VIA: %s",
packet->id.src, packet->id.dst, packet->id.dport,
packet->id.sport, packet->id.pri, packet->id.flags, packet->length, input.interface->name);
/* Here there be promiscuous mode */
#ifdef CSP_USE_PROMISC
csp_promisc_add(packet);
#endif
#ifdef CSP_USE_DEDUP
/* Check for duplicates */
if (csp_dedup_is_duplicate(packet)) {
/* Discard packet */
csp_log_packet("Duplicate packet discarded");
csp_buffer_free(packet);
return 0;
}
#endif
/* If the message is not to me, route the message to the correct interface */
if ((packet->id.dst != csp_get_address()) && (packet->id.dst != CSP_BROADCAST_ADDR)) {
/* Find the destination interface */
csp_iface_t * dstif = csp_rtable_find_iface(packet->id.dst);
/* If the message resolves to the input interface, don't loop it back out */
if ((dstif == NULL) || ((dstif == input.interface) && (input.interface->split_horizon_off == 0))) {
csp_buffer_free(packet);
return 0;
}
/* Otherwise, actually send the message */
if (csp_send_direct(packet->id, packet, dstif, 0) != CSP_ERR_NONE) {
csp_log_warn("Router failed to send");
csp_buffer_free(packet);
}
/* Next message, please */
return 0;
}
/* Discard packets with unsupported options */
if (csp_route_check_options(input.interface, packet) != CSP_ERR_NONE) {
csp_buffer_free(packet);
return 0;
}
/* The message is to me, search for incoming socket */
socket = csp_port_get_socket(packet->id.dport);
/* If the socket is connection-less, deliver now */
if (socket && (socket->opts & CSP_SO_CONN_LESS)) {
if (csp_route_security_check(socket->opts, input.interface, packet) < 0) {
csp_buffer_free(packet);
return 0;
}
if (csp_queue_enqueue(socket->socket, &packet, 0) != CSP_QUEUE_OK) {
csp_log_error("Conn-less socket queue full");
csp_buffer_free(packet);
return 0;
}
return 0;
}
/* Search for an existing connection */
conn = csp_conn_find(packet->id.ext, CSP_ID_CONN_MASK);
/* If this is an incoming packet on a new connection */
if (conn == NULL) {
/* Reject packet if no matching socket is found */
if (!socket) {
csp_buffer_free(packet);
return 0;
}
/* Run security check on incoming packet */
if (csp_route_security_check(socket->opts, input.interface, packet) < 0) {
csp_buffer_free(packet);
return 0;
}
/* New incoming connection accepted */
csp_id_t idout;
idout.pri = packet->id.pri;
idout.src = csp_get_address();
idout.dst = packet->id.src;
idout.dport = packet->id.sport;
idout.sport = packet->id.dport;
idout.flags = packet->id.flags;
/* Create connection */
conn = csp_conn_new(packet->id, idout);
if (!conn) {
csp_log_error("No more connections available");
csp_buffer_free(packet);
return 0;
}
/* Store the socket queue and options */
conn->socket = socket->socket;
conn->opts = socket->opts;
/* Packet to existing connection */
} else {
/* Run security check on incoming packet */
if (csp_route_security_check(conn->opts, input.interface, packet) < 0) {
csp_buffer_free(packet);
return 0;
}
}
#ifdef CSP_USE_RDP
/* Pass packet to RDP module */
if (packet->id.flags & CSP_FRDP) {
csp_rdp_new_packet(conn, packet);
return 0;
}
#endif
/* Pass packet to UDP module */
csp_udp_new_packet(conn, packet);
return 0;
}
void csp_buffer_free(void *packet) {
csp_log_buffer("BUFFER: Free element at %p\r\n", packet);
csp_queue_enqueue(csp_buffers, &packet, 0);
}
int csp_queue_enqueue_isr(csp_queue_handle_t handle, void * value, CSP_BASE_TYPE * task_woken) {
if (task_woken != NULL)
*task_woken = 0;
return csp_queue_enqueue(handle, value, 0);
}