int csp_bind(csp_socket_t * socket, uint8_t port) {
if (socket == NULL)
return CSP_ERR_INVAL;
if (port > CSP_ANY) {
csp_log_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_log_error("Port %d is already in use\r\n", port);
return CSP_ERR_USED;
}
csp_log_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;
}
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);
}
}
/**
* Check supported packet options
* @param interface pointer to incoming interface
* @param packet pointer to packet
* @return CSP_ERR_NONE is all options are supported, CSP_ERR_NOTSUP if not
*/
static int csp_route_check_options(csp_iface_t *interface, csp_packet_t *packet)
{
#ifndef CSP_USE_XTEA
/* Drop XTEA packets */
if (packet->id.flags & CSP_FXTEA) {
csp_log_error("Received XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet");
interface->autherr++;
return CSP_ERR_NOTSUP;
}
#endif
#ifndef CSP_USE_HMAC
/* Drop HMAC packets */
if (packet->id.flags & CSP_FHMAC) {
csp_log_error("Received packet with HMAC, but CSP was compiled without HMAC support. Discarding packet");
interface->autherr++;
return CSP_ERR_NOTSUP;
}
#endif
#ifndef CSP_USE_RDP
/* Drop RDP packets */
if (packet->id.flags & CSP_FRDP) {
csp_log_error("Received RDP packet, but CSP was compiled without RDP support. Discarding packet");
interface->rx_error++;
return CSP_ERR_NOTSUP;
}
#endif
return CSP_ERR_NONE;
}
int can_mbox_init(can_iface_ctx_t *iface_ctx) {
int i;
mbox_t *m, *mbox;
mbox = iface_ctx->mbox;
for (i = 0; i < MBOX_NUM; i++) {
m = &mbox[i];
m->state = MBOX_FREE;
m->csp_can_socket = &iface_ctx->csp_can_socket;
m->mbox_pool_sem = &iface_ctx->mbox_pool_sem;
/* Init signal semaphore */
if (csp_bin_sem_create(&(m->signal_sem)) != CSP_SEMAPHORE_OK) {
csp_log_error("sem create");
return -1;
} else {
/* Take signal semaphore so the thread waits for tx data */
csp_bin_sem_wait(&(m->signal_sem), CSP_MAX_DELAY);
}
/* Create mailbox */
if(csp_thread_create(mbox_tx_thread, (signed char *)"mbox_tx", 1024, (void *)m, 3, &m->thread) != CSP_ERR_NONE) { //TODO: Adjust priority
csp_log_error("thread creation");
return -1;
}
}
/* Init mailbox pool semaphore */
csp_bin_sem_create(&iface_ctx->mbox_pool_sem);
return 0;
}
int can_mbox_init(void) {
int i;
mbox_t * m;
for (i = 0; i < MBOX_NUM; i++) {
m = &mbox[i];
m->state = MBOX_FREE;
/* Init signal semaphore */
if (sem_init(&(m->signal_sem), 0, 1) != 0) {
csp_log_error("sem_init: %s\r\n", strerror(errno));
return -1;
} else {
/* Take signal semaphore so the thread waits for tx data */
sem_wait(&(m->signal_sem));
}
/* Create mailbox */
if (pthread_create(&m->thread, NULL, mbox_tx_thread, (void *)m) != 0) {
csp_log_error("pthread_create: %s\r\n", strerror(errno));
return -1;
}
}
/* Init mailbox pool semaphore */
sem_init(&mbox_sem, 0, 1);
return 0;
}
void *csp_buffer_get(size_t buf_size) {
csp_skbf_t * buffer = NULL;
if (buf_size + CSP_BUFFER_PACKET_OVERHEAD > size) {
csp_log_error("Attempt to allocate too large block %u\r\n", buf_size);
return NULL;
}
csp_queue_dequeue(csp_buffers, &buffer, 0);
if (buffer == NULL) {
csp_log_error("Out of buffers\r\n");
return NULL;
}
csp_log_buffer("GET: %p %p\r\n", buffer, buffer->skbf_addr);
if (buffer != buffer->skbf_addr) {
csp_log_error("Corrupt CSP buffer\r\n");
return NULL;
}
buffer->refcount++;
return buffer->skbf_data;
}
static int csp_rtable_parse(char * buffer, int dry_run) {
int valid_entries = 0;
/* Copy string before running strtok */
char * str = alloca(strlen(buffer) + 1);
memcpy(str, buffer, strlen(buffer) + 1);
/* Get first token */
str = strtok(str, ",");
while ((str) && (strlen(str) > 1)) {
int address = 0, netmask = 0, mac = 255;
char name[100] = {};
if (sscanf(str, "%u/%u %s %u", &address, &netmask, name, &mac) != 4) {
if (sscanf(str, "%u/%u %s", &address, &netmask, name) != 3) {
csp_log_error("Parse error %s", str);
return -1;
}
}
//printf("Parsed %u/%u %u %s\r\n", address, netmask, mac, name);
csp_iface_t * ifc = csp_iflist_get_by_name(name);
if (ifc) {
if (dry_run == 0)
csp_rtable_set(address, netmask, ifc, mac);
} else {
csp_log_error("Unknown interface %s", name);
return -1;
}
valid_entries++;
str = strtok(NULL, ",");
}
return valid_entries;
}
int can_configure_bitrate(unsigned long int afcpu, uint32_t bps) {
/* Set baud rate (500 kbps) */
if (bps != 500000) {
csp_log_error("CAN bitrate must be 500000 (was %d)\r\n");
return -1;
}
/* TODO: Set arbitrary bitrate */
if (afcpu == 8000000) {
CANTCON = 0x02; // Set CAN Timer Prescaler
CANBT1 = 0x02; //!< Tscl = 2x Tclkio = 250 ns
CANBT2 = 0x04; //!< Tsync = 1x Tscl, Tprs = 3x Tscl, Tsjw = 1x Tscl
CANBT3 = 0x13; //!< Tpsh1 = 2x Tscl, Tpsh2 = 2x Tscl, 3 sample points
} else if (afcpu == 12000000) {
CANTCON = 0x02; // Set CAN Timer Prescaler
CANBT1 = 0x02; //!< Tscl = 2x Tclkio = 166.666 ns
CANBT2 = 0x08; //!< Tsync = 1x Tscl, Tprs = 5x Tscl, Tsjw = 1x Tscl
CANBT3 = 0x25; //!< Tpsh1 = 3x Tscl, Tpsh2 = 3x Tscl, 3 sample points
} else if (afcpu == 16000000) {
CANTCON = 128; // Set CAN Timer Prescaler
CANBT1 = 0x06; //!< Tscl = 4x Tclkio = 250 ns
CANBT2 = 0x04; //!< Tsync = 1x Tscl, Tprs = 3x Tscl, Tsjw = 1x Tscl
CANBT3 = 0x13; //!< Tpsh1 = 2x Tscl, Tpsh2 = 2x Tscl, 3 sample points
} else {
csp_log_error("Error, missing CAN driver defines for that FCPU=%d\r\n", afcpu);
return -1;
}
return 0;
}
/** pbuf_init
* Initialize packet buffer.
* @return 0 on success, -1 on error.
*/
static int pbuf_init(void) {
/* Initialize packet buffers */
int i;
pbuf_element_t *buf;
for (i = 0; i < PBUF_ELEMENTS; i++) {
buf = &pbuf[i];
buf->rx_count = 0;
buf->tx_count = 0;
buf->cfpid = 0;
buf->packet = NULL;
buf->state = BUF_FREE;
buf->last_used = 0;
buf->remain = 0;
/* Create tx semaphore if blocking mode is enabled */
if (csp_bin_sem_create(&buf->tx_sem) != CSP_SEMAPHORE_OK) {
csp_log_error("Failed to allocate TX semaphore\r\n");
return CSP_ERR_NOMEM;
}
}
/* Initialize global lock */
if (CSP_INIT_CRITICAL(pbuf_sem) != CSP_ERR_NONE) {
csp_log_error("No more memory for packet buffer semaphore\r\n");
return CSP_ERR_NOMEM;
}
return CSP_ERR_NONE;
}
int csp_sendto(uint8_t prio, uint8_t dest, uint8_t dport, uint8_t src_port, uint32_t opts, csp_packet_t * packet, uint32_t timeout) {
packet->id.flags = 0;
if (opts & CSP_O_RDP) {
csp_log_error("Attempt to create RDP packet on connection-less socket\r\n");
return CSP_ERR_INVAL;
}
/** SEQNR */
if(opts & CSP_O_SEQNR) {
#ifdef CSP_USE_SEQNR
packet->id.flags |= CSP_SEQNR;
#else
csp_log_error("Attempt to create packet with SEQNR capabilities, but CSP was compiled without such support\r\n");
return CSP_ERR_NOTSUP;
#endif
}
if (opts & CSP_O_HMAC) {
#ifdef CSP_USE_HMAC
packet->id.flags |= CSP_FHMAC;
#else
csp_log_error("Attempt to create HMAC authenticated packet, but CSP was compiled without HMAC support\r\n");
return CSP_ERR_NOTSUP;
#endif
}
if (opts & CSP_O_XTEA) {
#ifdef CSP_USE_XTEA
packet->id.flags |= CSP_FXTEA;
#else
csp_log_error("Attempt to create XTEA encrypted packet, but CSP was compiled without XTEA support\r\n");
return CSP_ERR_NOTSUP;
#endif
}
if (opts & CSP_O_CRC32) {
#ifdef CSP_USE_CRC32
packet->id.flags |= CSP_FCRC32;
#else
csp_log_error("Attempt to create CRC32 validated packet, but CSP was compiled without CRC32 support\r\n");
return CSP_ERR_NOTSUP;
#endif
}
packet->id.dst = dest;
packet->id.dport = dport;
packet->id.src = my_address;
packet->id.sport = src_port;
packet->id.pri = prio;
if (csp_send_direct(packet->id, packet, timeout) != CSP_ERR_NONE)
return CSP_ERR_NOTSUP;
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_sendto(uint8_t prio, uint8_t dest, uint8_t dport, uint8_t src_port, uint32_t opts, csp_packet_t * packet, uint32_t timeout) {
packet->id.flags = 0;
if (opts & CSP_O_RDP) {
csp_log_error("Attempt to create RDP packet on connection-less socket");
return CSP_ERR_INVAL;
}
if (opts & CSP_O_HMAC) {
#ifdef CSP_USE_HMAC
packet->id.flags |= CSP_FHMAC;
#else
csp_log_error("Attempt to create HMAC authenticated packet, but CSP was compiled without HMAC support");
return CSP_ERR_NOTSUP;
#endif
}
if (opts & CSP_O_XTEA) {
#ifdef CSP_USE_XTEA
packet->id.flags |= CSP_FXTEA;
#else
csp_log_error("Attempt to create XTEA encrypted packet, but CSP was compiled without XTEA support");
return CSP_ERR_NOTSUP;
#endif
}
if (opts & CSP_O_CRC32) {
#ifdef CSP_USE_CRC32
packet->id.flags |= CSP_FCRC32;
#else
csp_log_error("Attempt to create CRC32 validated packet, but CSP was compiled without CRC32 support");
return CSP_ERR_NOTSUP;
#endif
}
packet->id.dst = dest;
packet->id.dport = dport;
packet->id.src = csp_get_address();
packet->id.sport = src_port;
packet->id.pri = prio;
csp_iface_t * ifout = csp_rtable_find_iface(dest);
if (csp_send_direct(packet->id, packet, ifout, timeout) != CSP_ERR_NONE)
return CSP_ERR_NOTSUP;
return CSP_ERR_NONE;
}
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;
}
#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);
return (ret == CSP_ERR_NONE) ? 1 : 0;
}
int csp_route_set(uint8_t node, csp_iface_t *ifc, uint8_t nexthop_mac_addr) {
/* Don't add nothing */
if (ifc == NULL)
return CSP_ERR_INVAL;
/**
* Check if the interface has been added.
*
* NOTE: For future implementations, interfaces should call
* csp_route_add_if in its csp_if_<name>_init function, instead
* of registering at first route_set, in order to make the interface
* available to network based (CMP) route configuration.
*/
csp_route_add_if(ifc);
/* Set route */
if (node <= CSP_DEFAULT_ROUTE) {
routes[node].interface = ifc;
routes[node].nexthop_mac_addr = nexthop_mac_addr;
} else {
csp_log_error("Failed to set route: invalid node id %u\r\n", node);
return CSP_ERR_INVAL;
}
return CSP_ERR_NONE;
}
void *csp_buffer_get(size_t buf_size) {
void *buffer;
if (buf_size + CSP_BUFFER_PACKET_OVERHEAD > size) {
csp_log_error("Attempt to allocate too large block %u\r\n", buf_size);
return NULL;
}
csp_queue_dequeue(csp_buffers, &buffer, 0);
if (buffer != NULL) {
csp_log_buffer("BUFFER: Using element at %p\r\n", buffer);
} else {
csp_log_error("Out of buffers\r\n");
}
return buffer;
}
int csp_buffer_get_refcount(void *packet) {
if (!packet) {
csp_log_error("Attempt to free null pointer\r\n");
return -1;
}
csp_skbf_t * buf = packet - sizeof(csp_skbf_t);
return buf->refcount;
}
int csp_close(csp_conn_t * conn) {
if (conn == NULL) {
csp_log_error("NULL Pointer given to csp_close\r\n");
return CSP_ERR_INVAL;
}
if (conn->state == CONN_CLOSED) {
csp_log_protocol("Conn already closed\r\n");
return CSP_ERR_NONE;
}
#ifdef CSP_USE_RDP
/* Ensure RDP knows this connection is closing */
if (conn->idin.flags & CSP_FRDP || conn->idout.flags & CSP_FRDP)
if (csp_rdp_close(conn) == CSP_ERR_AGAIN)
return CSP_ERR_NONE;
#endif
/* Lock connection array while closing connection */
if (csp_bin_sem_wait(&conn_lock, 100) != CSP_SEMAPHORE_OK) {
csp_log_error("Failed to lock conn array\r\n");
return CSP_ERR_TIMEDOUT;
}
/* Set to closed */
conn->state = CONN_CLOSED;
/* Ensure connection queue is empty */
csp_conn_flush_rx_queue(conn);
/* Reset RDP state */
#ifdef CSP_USE_RDP
if (conn->idin.flags & CSP_FRDP)
csp_rdp_flush_all(conn);
#endif
/* Unlock connection array */
csp_bin_sem_post(&conn_lock);
return CSP_ERR_NONE;
}
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_sys_shutdown(void) {
extern void __attribute__((weak)) cpu_shutdown(void);
if (cpu_shutdown) {
cpu_shutdown();
while (1);
}
csp_log_error("Failed to shutdown");
return CSP_ERR_INVAL;
}
int csp_route_start_task(unsigned int task_stack_size, unsigned int priority) {
int ret = csp_thread_create(csp_task_router, (signed char *) "RTE", task_stack_size, NULL, priority, &handle_router);
if (ret != 0) {
csp_log_error("Failed to start router task\n");
return CSP_ERR_NOMEM;
}
return CSP_ERR_NONE;
}
int can_init(uint32_t id, uint32_t mask, can_tx_callback_t atxcb, can_rx_callback_t arxcb, struct csp_can_config *conf) {
struct ifreq ifr;
struct sockaddr_can addr;
pthread_t rx_thread;
csp_assert(conf && conf->ifc);
txcb = atxcb;
rxcb = arxcb;
/* Create socket */
if ((can_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
csp_log_error("socket: %s\r\n", strerror(errno));
return -1;
}
/* Locate interface */
strncpy(ifr.ifr_name, conf->ifc, IFNAMSIZ - 1);
if (ioctl(can_socket, SIOCGIFINDEX, &ifr) < 0) {
csp_log_error("ioctl: %s\r\n", strerror(errno));
return -1;
}
/* Bind the socket to CAN interface */
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(can_socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
csp_log_error("bind: %s\r\n", strerror(errno));
return -1;
}
/* Set promiscuous mode */
if (mask) {
struct can_filter filter;
filter.can_id = id;
filter.can_mask = mask;
if (setsockopt(can_socket, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) {
csp_log_error("setsockopt: %s\r\n", strerror(errno));
return -1;
}
}
/* Create receive thread */
if (pthread_create(&rx_thread, NULL, mbox_rx_thread, NULL) != 0) {
csp_log_error("setsockopt: %s\r\n", strerror(errno));
return -1;
}
/* Create mailbox pool */
if (can_mbox_init() != 0) {
csp_log_error("Failed to create tx thread pool\n");
return -1;
}
return 0;
}
/* 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_conn_t * csp_conn_allocate(csp_conn_type_t type) {
int i, j;
static uint8_t csp_conn_last_given = 0;
csp_conn_t * conn;
if (csp_bin_sem_wait(&conn_lock, 100) != CSP_SEMAPHORE_OK) {
csp_log_error("Failed to lock conn array\r\n");
return NULL;
}
/* Search for free connection */
i = csp_conn_last_given;
i = (i + 1) % CSP_CONN_MAX;
for (j = 0; j < CSP_CONN_MAX; j++) {
conn = &arr_conn[i];
if (conn->state == CONN_CLOSED)
break;
i = (i + 1) % CSP_CONN_MAX;
}
if (conn->state == CONN_OPEN) {
csp_log_error("No more free connections\r\n");
csp_bin_sem_post(&conn_lock);
return NULL;
}
conn->state = CONN_OPEN;
conn->socket = NULL;
conn->type = type;
csp_conn_last_given = i;
csp_bin_sem_post(&conn_lock);
return conn;
}
int csp_sys_reboot(void) {
extern void __attribute__((weak)) cpu_set_reset_cause(unsigned int);
if (cpu_set_reset_cause)
cpu_set_reset_cause(1);
extern void __attribute__((weak)) cpu_reset(void);
if (cpu_reset) {
cpu_reset();
while (1);
}
csp_log_error("Failed to reboot");
return CSP_ERR_INVAL;
}
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);
}
}
}
}
int csp_bridge_start(unsigned int task_stack_size, unsigned int task_priority, csp_iface_t * _if_a, csp_iface_t * _if_b) {
/* Set static references to A/B side of bridge */
if_a = _if_a;
if_b = _if_b;
static csp_thread_handle_t handle;
int ret = csp_thread_create(csp_bridge, "BRIDGE", task_stack_size, NULL, task_priority, &handle);
if (ret != 0) {
csp_log_error("Failed to start task");
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;
}
/* Mailbox thread */
static void * mbox_tx_thread(void * parameters) {
/* Set thread parameters */
mbox_t * m = (mbox_t *)parameters;
uint32_t id;
while (1) {
/* Wait for a new packet to process */
sem_wait(&(m->signal_sem));
/* Send frame */
int tries = 0, error = CAN_NO_ERROR;
while (write(can_socket, &m->frame, sizeof(m->frame)) != sizeof(m->frame)) {
if (++tries < 1000 && errno == ENOBUFS) {
/* Wait 10 ms and try again */
usleep(10000);
} else {
csp_log_error("write: %s\r\n", strerror(errno));
error = CAN_ERROR;
break;
}
}
id = m->frame.can_id;
/* Free mailbox */
sem_wait(&mbox_sem);
m->state = MBOX_FREE;
sem_post(&mbox_sem);
/* Call tx callback */
if (txcb) txcb(id, error, NULL);
}
/* We should never reach this point */
pthread_exit(NULL);
}
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_transaction_persistent(csp_conn_t * conn, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen) {
int size = (inlen > outlen) ? inlen : outlen;
csp_packet_t * packet = csp_buffer_get(size);
if (packet == NULL)
return 0;
/* Copy the request */
if (outlen > 0 && outbuf != NULL)
memcpy(packet->data, outbuf, outlen);
packet->length = outlen;
if (!csp_send(conn, packet, timeout)) {
csp_buffer_free(packet);
return 0;
}
/* If no reply is expected, return now */
if (inlen == 0)
{
return 1;
}
packet = csp_read(conn, timeout);
if (packet == NULL)
return 0;
if ((inlen != -1) && ((int)packet->length != inlen)) {
csp_log_error("Reply length %u expected %u\r\n", packet->length, inlen);
csp_buffer_free(packet);
return 0;
}
memcpy(inbuf, packet->data, packet->length);
int length = packet->length;
csp_buffer_free(packet);
return length;
}
