int can_send(csp_iface_t *csp_if_can, can_id_t id, uint8_t data[], uint8_t dlc, CSP_BASE_TYPE * task_woken) {
int i, found = 0;
mbox_t * m;
can_iface_ctx_t *iface_ctx;
csp_bin_sem_handle_t *mbox_pool_sem;
if (dlc > 8)
return -1;
iface_ctx = (can_iface_ctx_t *)csp_if_can->driver;
if (NULL == iface_ctx) {
return -1;
}
mbox_pool_sem = &iface_ctx->mbox_pool_sem;
/* Find free mailbox */
csp_bin_sem_wait(mbox_pool_sem, CSP_MAX_DELAY);
for (i = 0; i < MBOX_NUM; i++) {
m = &(iface_ctx->mbox[i]);
if(m->state == MBOX_FREE) {
m->state = MBOX_USED;
found = 1;
break;
}
}
csp_bin_sem_post(mbox_pool_sem);
if (!found)
return -1;
/* Copy identifier */
m->frame.can_id = id | CAN_EFF_FLAG;
/* Copy data to frame */
for (i = 0; i < dlc; i++)
m->frame.data[i] = data[i];
/* Set DLC */
m->frame.can_dlc = dlc;
/* Signal thread to start */
csp_bin_sem_post(&(m->signal_sem));
return 0;
}
static int id_get(void) {
int id;
if (csp_bin_sem_wait(&id_sem, 1000) != CSP_SEMAPHORE_OK)
return CSP_ERR_TIMEDOUT;
id = cfp_id++;
cfp_id = cfp_id & ((1 << CFP_ID_SIZE) - 1);
csp_bin_sem_post(&id_sem);
return id;
}
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_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;
}
csp_conn_t * csp_connect(uint8_t prio, uint8_t dest, uint8_t dport, uint32_t timeout, uint32_t opts) {
/* Generate identifier */
csp_id_t incoming_id, outgoing_id;
incoming_id.pri = prio;
incoming_id.dst = my_address;
incoming_id.src = dest;
incoming_id.sport = dport;
incoming_id.flags = 0;
outgoing_id.pri = prio;
outgoing_id.dst = dest;
outgoing_id.src = my_address;
outgoing_id.dport = dport;
outgoing_id.flags = 0;
/* Set connection options */
if (opts & CSP_O_RDP) {
#ifdef CSP_USE_RDP
incoming_id.flags |= CSP_FRDP;
outgoing_id.flags |= CSP_FRDP;
#else
csp_log_error("Attempt to create RDP connection, but CSP was compiled without RDP support\r\n");
return NULL;
#endif
}
if (opts & CSP_O_HMAC) {
#ifdef CSP_USE_HMAC
outgoing_id.flags |= CSP_FHMAC;
incoming_id.flags |= CSP_FHMAC;
#else
csp_log_error("Attempt to create HMAC authenticated connection, but CSP was compiled without HMAC support\r\n");
return NULL;
#endif
}
if (opts & CSP_O_XTEA) {
#ifdef CSP_USE_XTEA
outgoing_id.flags |= CSP_FXTEA;
incoming_id.flags |= CSP_FXTEA;
#else
csp_log_error("Attempt to create XTEA encrypted connection, but CSP was compiled without XTEA support\r\n");
return NULL;
#endif
}
if (opts & CSP_O_CRC32) {
#ifdef CSP_USE_CRC32
outgoing_id.flags |= CSP_FCRC32;
incoming_id.flags |= CSP_FCRC32;
#else
csp_log_error("Attempt to create CRC32 validated connection, but CSP was compiled without CRC32 support\r\n");
return NULL;
#endif
}
/* Find an unused ephemeral port */
csp_conn_t * conn;
/* Wait for sport lock */
if (csp_bin_sem_wait(&sport_lock, 1000) != CSP_SEMAPHORE_OK)
return NULL;
uint8_t start = sport;
while (++sport != start) {
if (sport > CSP_ID_PORT_MAX)
sport = CSP_MAX_BIND_PORT + 1;
outgoing_id.sport = sport;
incoming_id.dport = sport;
/* Match on destination port of _incoming_ identifier */
conn = csp_conn_find(incoming_id.ext, CSP_ID_DPORT_MASK);
/* Break if we found an unused ephemeral port */
if (conn == NULL)
break;
}
/* Post sport lock */
csp_bin_sem_post(&sport_lock);
/* If no available ephemeral port was found */
if (sport == start)
return NULL;
/* Get storage for new connection */
conn = csp_conn_new(incoming_id, outgoing_id);
if (conn == NULL)
return NULL;
/* Set connection options */
conn->opts = opts;
#ifdef CSP_USE_RDP
/* Call Transport Layer connect */
if (outgoing_id.flags & CSP_FRDP) {
/* If the transport layer has failed to connect
* deallocate connection structure again and return NULL */
if (csp_rdp_connect(conn, timeout) != CSP_ERR_NONE) {
csp_close(conn);
return NULL;
}
}
#endif
/* We have a successful connection */
return conn;
}
int csp_mutex_unlock(csp_mutex_t * mutex) {
return csp_bin_sem_post(mutex);
}
int csp_can_tx(csp_iface_t * interface, csp_packet_t *packet, uint32_t timeout) {
uint8_t bytes, overhead, avail, dest;
uint8_t frame_buf[8];
/* Get CFP identification number */
int ident = id_get();
if (ident < 0) {
csp_log_warn("Failed to get CFP identification number\r\n");
return CSP_ERR_INVAL;
}
/* Calculate overhead */
overhead = sizeof(csp_id_t) + sizeof(uint16_t);
/* Insert destination node mac address into the CFP destination field */
dest = csp_route_get_nexthop_mac(packet->id.dst);
if (dest == CSP_NODE_MAC)
dest = packet->id.dst;
/* Create CAN identifier */
can_id_t id = 0;
id |= CFP_MAKE_SRC(packet->id.src);
id |= CFP_MAKE_DST(dest);
id |= CFP_MAKE_ID(ident);
id |= CFP_MAKE_TYPE(CFP_BEGIN);
id |= CFP_MAKE_REMAIN((packet->length + overhead - 1) / 8);
/* Get packet buffer */
pbuf_element_t *buf = pbuf_new(id, NULL);
if (buf == NULL) {
csp_log_warn("Failed to get packet buffer for CAN\r\n");
return CSP_ERR_NOMEM;
}
/* Set packet */
buf->packet = packet;
/* Calculate first frame data bytes */
avail = 8 - overhead;
bytes = (packet->length <= avail) ? packet->length : avail;
/* Copy CSP headers and data */
uint32_t csp_id_be = csp_hton32(packet->id.ext);
uint16_t csp_length_be = csp_hton16(packet->length);
memcpy(frame_buf, &csp_id_be, sizeof(csp_id_be));
memcpy(frame_buf + sizeof(csp_id_be), &csp_length_be, sizeof(csp_length_be));
memcpy(frame_buf + overhead, packet->data, bytes);
/* Increment tx counter */
buf->tx_count += bytes;
/* Take semaphore so driver can post it later */
csp_bin_sem_wait(&buf->tx_sem, 0);
/* Send frame */
if (can_send(id, frame_buf, overhead + bytes, NULL) != 0) {
csp_log_info("Failed to send CAN frame in csp_tx_can\r\n");
return CSP_ERR_DRIVER;
}
/* Non blocking mode */
if (timeout == 0)
return CSP_ERR_NONE;
/* Blocking mode */
if (csp_bin_sem_wait(&buf->tx_sem, timeout) != CSP_SEMAPHORE_OK) {
csp_bin_sem_post(&buf->tx_sem);
return CSP_ERR_TIMEDOUT;
} else {
csp_bin_sem_post(&buf->tx_sem);
return CSP_ERR_NONE;
}
}
int csp_tx_callback(can_id_t canid, can_error_t error, CSP_BASE_TYPE *task_woken) {
int bytes;
uint8_t dest;
/* Match buffer element */
pbuf_element_t *buf = pbuf_find(canid, CFP_ID_CONN_MASK, task_woken);
if (buf == NULL) {
csp_log_info("Failed to match buffer element in tx callback\r\n");
csp_if_can.tx_error++;
return CSP_ERR_INVAL;
}
if (buf->packet == NULL) {
csp_log_warn("Buffer packet was NULL\r\n");
csp_if_can.tx_error++;
pbuf_free(buf, task_woken);
return CSP_ERR_INVAL;
}
/* Free packet buffer if send failed */
if (error != CAN_NO_ERROR) {
csp_log_warn("Error in transmit callback\r\n");
csp_if_can.tx_error++;
pbuf_free(buf, task_woken);
return CSP_ERR_DRIVER;
}
/* Send next frame if not complete */
if (buf->tx_count < buf->packet->length) {
/* Calculate frame data bytes */
bytes = (buf->packet->length - buf->tx_count >= 8) ? 8 : buf->packet->length - buf->tx_count;
/* Insert destination node mac address into the CFP destination field */
dest = csp_route_get_nexthop_mac(buf->packet->id.dst);
if (dest == CSP_NODE_MAC)
dest = buf->packet->id.dst;
/* Prepare identifier */
can_id_t id = 0;
id |= CFP_MAKE_SRC(buf->packet->id.src);
id |= CFP_MAKE_DST(dest);
id |= CFP_MAKE_ID(CFP_ID(canid));
id |= CFP_MAKE_TYPE(CFP_MORE);
id |= CFP_MAKE_REMAIN((buf->packet->length - buf->tx_count - bytes + 7) / 8);
/* Increment tx counter */
buf->tx_count += bytes;
/* Send frame */
if (can_send(id, buf->packet->data + buf->tx_count - bytes, bytes, task_woken) != 0) {
csp_log_info("Failed to send CAN frame in Tx callback\r\n");
csp_if_can.tx_error++;
pbuf_free(buf, task_woken);
return CSP_ERR_DRIVER;
}
} else {
/* Free packet buffer */
pbuf_free(buf, task_woken);
/* Post semaphore if blocking mode is enabled */
if (task_woken != NULL) {
csp_bin_sem_post_isr(&buf->tx_sem, task_woken);
} else {
csp_bin_sem_post(&buf->tx_sem);
}
}
return CSP_ERR_NONE;
}
int csp_can_tx(csp_iface_t * interface, csp_packet_t *packet, uint32_t timeout) {
uint8_t bytes, overhead, avail, dest;
uint8_t frame_buf[8];
/* Get CFP identification number */
int ident = id_get();
if (ident < 0) {
csp_log_warn("Failed to get CFP identification number");
return CSP_ERR_INVAL;
}
/* Calculate overhead */
overhead = sizeof(csp_id_t) + sizeof(uint16_t);
/* Insert destination node mac address into the CFP destination field */
dest = csp_rtable_find_mac(packet->id.dst);
if (dest == CSP_NODE_MAC)
dest = packet->id.dst;
/* Create CAN identifier */
can_id_t id = 0;
id |= CFP_MAKE_SRC(packet->id.src);
id |= CFP_MAKE_DST(dest);
id |= CFP_MAKE_ID(ident);
id |= CFP_MAKE_TYPE(CFP_BEGIN);
id |= CFP_MAKE_REMAIN((packet->length + overhead - 1) / 8);
/* Get packet buffer */
pbuf_element_t *buf = pbuf_new(id, NULL);
if (buf == NULL) {
csp_log_warn("Failed to get packet buffer for CAN");
return CSP_ERR_NOMEM;
}
/* Set packet */
buf->packet = packet;
/* Calculate first frame data bytes */
avail = 8 - overhead;
bytes = (packet->length <= avail) ? packet->length : avail;
/* Copy CSP headers and data */
uint32_t csp_id_be = csp_hton32(packet->id.ext);
uint16_t csp_length_be = csp_hton16(packet->length);
memcpy(frame_buf, &csp_id_be, sizeof(csp_id_be));
memcpy(frame_buf + sizeof(csp_id_be), &csp_length_be, sizeof(csp_length_be));
memcpy(frame_buf + overhead, packet->data, bytes);
/* Increment tx counter */
buf->tx_count += bytes;
/* Take semaphore so driver can post it later */
if (csp_bin_sem_wait(&buf->tx_sem, 0) != CSP_SEMAPHORE_OK) {
csp_log_error("Failed to take CAN pbuf TX sem!");
pbuf_free(buf, NULL, false);
return CSP_ERR_DRIVER;
}
/* Send frame. We must free packet buffer is this fails,
* but the packet itself should be freed by the caller */
if (can_send(id, frame_buf, overhead + bytes, NULL) != 0) {
csp_log_warn("Failed to send CAN frame in csp_tx_can");
csp_bin_sem_post(&buf->tx_sem);
pbuf_free(buf, NULL, false);
return CSP_ERR_DRIVER;
}
/* NOTE: The transmit packet is now owned by the transmission MOB and
* must NOT be freed by the calling thread. */
/* Non blocking mode */
if (timeout == 0)
return CSP_ERR_NONE;
/* Blocking mode */
if (csp_bin_sem_wait(&buf->tx_sem, timeout) != CSP_SEMAPHORE_OK) {
/* tx_sem is posted by transmission callback. The packet
* could still be in use by the transmission MOB, so
* we can not return CSP_ERR_TIMEOUT and risk that the
* calling thread frees the packet. */
return CSP_ERR_NONE;
} else {
csp_bin_sem_post(&buf->tx_sem);
return CSP_ERR_NONE;
}
}
