int csp_route_table_init(void) {
int prio;
/* Clear routing table */
memset(routes, 0, sizeof(csp_route_t) * CSP_ROUTE_COUNT);
/* Create router fifos for each priority */
for (prio = 0; prio < CSP_ROUTE_FIFOS; prio++) {
if (router_input_fifo[prio] == NULL) {
router_input_fifo[prio] = csp_queue_create(CSP_FIFO_INPUT, sizeof(csp_route_queue_t));
if (!router_input_fifo[prio])
return CSP_ERR_NOMEM;
}
}
#ifdef CSP_USE_QOS
/* Create QoS fifo notification queue */
router_input_event = csp_queue_create(CSP_FIFO_INPUT, sizeof(int));
if (!router_input_event)
return CSP_ERR_NOMEM;
#endif
/* Register loopback route */
csp_route_set(my_address, &csp_if_lo, CSP_NODE_MAC);
/* Also register loopback as default, until user redefines default route */
csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_lo, CSP_NODE_MAC);
return CSP_ERR_NONE;
}
int csp_route_table_init(void) {
int prio;
/* Clear rounting table */
memset(routes, 0, sizeof(csp_route_t) * (CSP_ID_HOST_MAX + 2));
/* Create routing table lock */
if (csp_mutex_create(&routes_lock) != CSP_MUTEX_OK)
return CSP_ERR_NOMEM;
/* Create router fifos for each priority */
for (prio = 0; prio < CSP_ROUTE_FIFOS; prio++) {
router_input_fifo[prio] = csp_queue_create(CSP_FIFO_INPUT, sizeof(csp_route_queue_t));
if (!router_input_fifo[prio])
return CSP_ERR_NOMEM;
}
#ifdef CSP_USE_QOS
/* Create QoS fifo notification queue */
router_input_event = csp_queue_create(CSP_FIFO_INPUT, sizeof(int));
if (!router_input_event)
return CSP_ERR_NOMEM;
#endif
return CSP_ERR_NONE;
}
csp_socket_t * csp_socket(uint32_t opts) {
/* Validate socket options */
#ifndef CSP_USE_RDP
if (opts & CSP_SO_RDPREQ) {
csp_log_error("Attempt to create socket that requires RDP, but CSP was compiled without RDP support\r\n");
return NULL;
}
#endif
#ifndef CSP_USE_XTEA
if (opts & CSP_SO_XTEAREQ) {
csp_log_error("Attempt to create socket that requires XTEA, but CSP was compiled without XTEA support\r\n");
return NULL;
}
#endif
#ifndef CSP_USE_HMAC
if (opts & CSP_SO_HMACREQ) {
csp_log_error("Attempt to create socket that requires HMAC, but CSP was compiled without HMAC support\r\n");
return NULL;
}
#endif
#ifndef CSP_USE_CRC32
if (opts & CSP_SO_CRC32REQ) {
csp_log_error("Attempt to create socket that requires CRC32, but CSP was compiled without CRC32 support\r\n");
return NULL;
}
#endif
/* Drop packet if reserved flags are set */
if (opts & ~(CSP_SO_RDPREQ | CSP_SO_XTEAREQ | CSP_SO_HMACREQ | CSP_SO_CRC32REQ | CSP_SO_CONN_LESS)) {
csp_log_error("Invalid socket option\r\n");
return NULL;
}
/* Use CSP buffers instead? */
csp_socket_t * sock = csp_conn_allocate(CONN_SERVER);
if (sock == NULL)
return NULL;
/* If connectionless, init the queue to a pre-defined size
* if not, the user must init the queue using csp_listen */
if (opts & CSP_SO_CONN_LESS) {
sock->socket = csp_queue_create(CSP_CONN_QUEUE_LENGTH, sizeof(csp_packet_t *));
if (sock->socket == NULL)
return NULL;
} else {
sock->socket = NULL;
}
sock->opts = opts;
return sock;
}
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;
}
int csp_listen(csp_socket_t * socket, size_t conn_queue_length) {
if (socket == NULL)
return CSP_ERR_INVAL;
socket->socket = csp_queue_create(conn_queue_length, sizeof(csp_conn_t *));
if (socket->socket == NULL)
return CSP_ERR_NOMEM;
return CSP_ERR_NONE;
}
int csp_can_init(uint8_t mode, struct csp_can_config *conf) {
int ret;
uint32_t mask;
/* Initialize packet buffer */
if (pbuf_init() != 0) {
csp_log_error("Failed to initialize CAN packet buffers\r\n");
return CSP_ERR_NOMEM;
}
/* Initialize CFP identifier */
if (id_init() != 0) {
csp_log_error("Failed to initialize CAN identification number\r\n");
return CSP_ERR_NOMEM;
}
if (mode == CSP_CAN_MASKED) {
mask = CFP_MAKE_DST((1 << CFP_HOST_SIZE) - 1);
} else if (mode == CSP_CAN_PROMISC) {
mask = 0;
csp_if_can.promisc = 1;
} else {
csp_log_error("Unknown CAN mode\r\n");
return CSP_ERR_INVAL;
}
can_rx_queue = csp_queue_create(CSP_CAN_RX_QUEUE_SIZE, sizeof(can_frame_t));
if (can_rx_queue == NULL) {
csp_log_error("Failed to create CAN RX queue\r\n");
return CSP_ERR_NOMEM;
}
ret = csp_thread_create(csp_can_rx_task, (signed char *) "CANRX",2048, NULL, 3, &can_rx_task);
if (ret != 0) {
csp_log_error("Failed to init CAN RX task\r\n");
return CSP_ERR_NOMEM;
}
/* Initialize CAN driver */
if (can_init(CFP_MAKE_DST(my_address), mask, csp_tx_callback, csp_rx_callback, conf) != 0) {
csp_log_error("Failed to initialize CAN driver\r\n");
return CSP_ERR_DRIVER;
}
/* Regsiter interface */
csp_route_add_if(&csp_if_can);
return CSP_ERR_NONE;
}
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;
}
int csp_promisc_enable(unsigned int buf_size) {
/* If queue already initialised */
if (csp_promisc_queue != NULL) {
csp_promisc_enabled = 1;
return CSP_ERR_NONE;
}
/* Create packet queue */
csp_promisc_queue = csp_queue_create(buf_size, sizeof(csp_packet_t *));
if (csp_promisc_queue == NULL)
return CSP_ERR_INVAL;
csp_promisc_enabled = 1;
return CSP_ERR_NONE;
}
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;
}
