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; }
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; }
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; }
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_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; }
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_lock(csp_mutex_t * mutex, uint32_t timeout) { return csp_bin_sem_wait(mutex, timeout); }
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_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; } }