/* called from read event created by tun_new_if */
static int
tun_read_callback (struct event *e, void *d)
{
struct iface_tun *iface = (struct iface_tun *) d;
struct pbuf *pkt;
#ifdef HAVE_LINUX_IF_TUN_H
pkt = pbuf_new (iface->iface.mtu + 4);
#else
pkt = pbuf_new (iface->iface.mtu);
#endif
if (tuntap_do_read (iface->fd, pkt) == 0) {
(*iface->pkt_handler) ((struct iface *) iface, pkt);
}
pbuf_delete (pkt);
return 1;
}
/* called by main code via pointer in struct iface */
static struct pbuf *
tun_get_buf (struct iface *iface, int size)
{
struct pbuf *p;
p = pbuf_new (size + 4);
pbuf_drop (p, 4);
return p;
}
/* called from read event created by tap_new_if */
static int
tap_read_callback (struct event *e, void *d)
{
struct iface_tap *iface = (struct iface_tap *) d;
struct pbuf *p;
p = pbuf_new (iface->ife.iface.mtu + iface->ife.head_size);
pbuf_drop (p, iface->ife.head_size - 14);
tuntap_do_read (iface->fd, p);
iface->ife.recv_frame (&iface->ife, p);
pbuf_delete (p);
return 1;
}
static inline enum rpc_stat
my_udp_send(struct udp_pcb* pcb, struct pbuf *pbuf)
{
int err;
struct pbuf *p;
/* LWIP does not preserve the pbuf so make a copy first */
p = pbuf_new(pbuf->tot_len);
if(p == NULL){
return RPCERR_NOBUF;
}
assert(!pbuf_copy(p, pbuf));
err = udp_send(pcb, p);
pbuf_free(p);
switch(err){
case ERR_MEM:
return RPCERR_NOMEM;
case ERR_RTE:
return RPCERR_COMM;
case ERR_OK:
return RPC_OK;
}
return err;
}
/*..........................................................................*/
QState LwIPMgr_running(LwIPMgr *me, QEvt const *e) {
switch (e->sig) {
case Q_ENTRY_SIG: {
#if (LWIP_DHCP != 0)
dhcp_start(me->netif); /* start DHCP if configured */
/* NOTE: If LWIP_AUTOIP is configured in lwipopts.h and
* LWIP_DHCP_AUTOIP_COOP is set as well, the DHCP process will
* start AutoIP after DHCP fails for 59 seconds.
*/
#elif (LWIP_AUTOIP != 0)
autoip_start(me->netif); /* start AutoIP if configured */
#endif
QTimeEvt_postEvery(&me->te_LWIP_SLOW_TICK, (QActive *)me,
(LWIP_SLOW_TICK_MS * BSP_TICKS_PER_SEC) / 1000U);
return Q_HANDLED();
}
case Q_EXIT_SIG: {
QTimeEvt_disarm(&me->te_LWIP_SLOW_TICK);
return Q_HANDLED();
}
case SEND_UDP_SIG: {
if (me->upcb->remote_port != 0U) {
struct pbuf *p = pbuf_new((u8_t *)((TextEvt const *)e)->text,
strlen(((TextEvt const *)e)->text) + 1U);
if (p != (struct pbuf *)0) {
udp_send(me->upcb, p);
pbuf_free(p); /* don't leak the pbuf! */
}
}
return Q_HANDLED();
}
case LWIP_RX_READY_SIG: {
eth_driver_read();
return Q_HANDLED();
}
case LWIP_TX_READY_SIG: {
eth_driver_write();
return Q_HANDLED();
}
case LWIP_SLOW_TICK_SIG: {
/* has IP address changed? */
if (me->ip_addr != me->netif->ip_addr.addr) {
me->ip_addr = me->netif->ip_addr.addr; /* save the IP addr. */
BSP_displyIP(ntohl(me->ip_addr));
}
#if LWIP_TCP
me->tcp_tmr += LWIP_SLOW_TICK_MS;
if (me->tcp_tmr >= TCP_TMR_INTERVAL) {
me->tcp_tmr = 0;
tcp_tmr();
}
#endif
#if LWIP_ARP
me->arp_tmr += LWIP_SLOW_TICK_MS;
if (me->arp_tmr >= ARP_TMR_INTERVAL) {
me->arp_tmr = 0;
etharp_tmr();
}
#endif
#if LWIP_DHCP
me->dhcp_fine_tmr += LWIP_SLOW_TICK_MS;
if (me->dhcp_fine_tmr >= DHCP_FINE_TIMER_MSECS) {
me->dhcp_fine_tmr = 0;
dhcp_fine_tmr();
}
me->dhcp_coarse_tmr += LWIP_SLOW_TICK_MS;
if (me->dhcp_coarse_tmr >= DHCP_COARSE_TIMER_MSECS) {
me->dhcp_coarse_tmr = 0;
dhcp_coarse_tmr();
}
#endif
#if LWIP_AUTOIP
me->auto_ip_tmr += LWIP_SLOW_TICK_MS;
if (me->auto_ip_tmr >= AUTOIP_TMR_INTERVAL) {
me->auto_ip_tmr = 0;
autoip_tmr();
}
#endif
return Q_HANDLED();
}
case LWIP_RX_OVERRUN_SIG: {
LINK_STATS_INC(link.err);
return Q_HANDLED();
}
}
return Q_SUPER(&QHsm_top);
}
/*..........................................................................*/
QState LwIPMgr_running(LwIPMgr *me, QEvent const *e) {
switch (e->sig) {
case Q_ENTRY_SIG: {
QTimeEvt_postEvery(&me->te_LWIP_SLOW_TICK, (QActive *)me,
(LWIP_SLOW_TICK_MS * BSP_TICKS_PER_SEC) / 1000);
return Q_HANDLED();
}
case Q_EXIT_SIG: {
QTimeEvt_disarm(&me->te_LWIP_SLOW_TICK);
return Q_HANDLED();
}
case SEND_UDP_SIG: {
if (me->upcb->remote_port != (uint16_t)0) {
struct pbuf *p = pbuf_new((u8_t *)((TextEvt const *)e)->text,
strlen(((TextEvt const *)e)->text) + 1);
if (p != (struct pbuf *)0) {
udp_send(me->upcb, p);
printf("Sent: %s\n", ((TextEvt const *)e)->text);
pbuf_free(p); /* don't leak the pbuf! */
}
}
return Q_HANDLED();
}
case LWIP_RX_READY_SIG: {
eth_driver_read();
return Q_HANDLED();
}
case LWIP_TX_READY_SIG: {
eth_driver_write();
return Q_HANDLED();
}
case LWIP_SLOW_TICK_SIG: {
/* has IP address changed? */
if (me->ip_addr != me->netif->ip_addr.addr) {
TextEvt *te;
uint32_t ip_net; /* IP address in the network byte order */
me->ip_addr = me->netif->ip_addr.addr; /* save the IP addr. */
ip_net = ntohl(me->ip_addr);
/* publish the text event to display the new IP address */
te = Q_NEW(TextEvt, DISPLAY_IPADDR_SIG);
snprintf(te->text, Q_DIM(te->text), "%d.%d.%d.%d",
((ip_net) >> 24) & 0xFF,
((ip_net) >> 16) & 0xFF,
((ip_net) >> 8) & 0xFF,
ip_net & 0xFF);
QF_PUBLISH((QEvent *)te, me);
}
#if LWIP_TCP
me->tcp_tmr += LWIP_SLOW_TICK_MS;
if (me->tcp_tmr >= TCP_TMR_INTERVAL) {
me->tcp_tmr = 0;
tcp_tmr();
}
#endif
#if LWIP_ARP
me->arp_tmr += LWIP_SLOW_TICK_MS;
if (me->arp_tmr >= ARP_TMR_INTERVAL) {
me->arp_tmr = 0;
etharp_tmr();
}
#endif
#if LWIP_DHCP
me->dhcp_fine_tmr += LWIP_SLOW_TICK_MS;
if (me->dhcp_fine_tmr >= DHCP_FINE_TIMER_MSECS) {
me->dhcp_fine_tmr = 0;
dhcp_fine_tmr();
}
me->dhcp_coarse_tmr += LWIP_SLOW_TICK_MS;
if (me->dhcp_coarse_tmr >= DHCP_COARSE_TIMER_MSECS) {
me->dhcp_coarse_tmr = 0;
dhcp_coarse_tmr();
}
#endif
#if LWIP_AUTOIP
me->auto_ip_tmr += LWIP_SLOW_TICK_MS;
if (me->auto_ip_tmr >= AUTOIP_TMR_INTERVAL) {
me->auto_ip_tmr = 0;
autoip_tmr();
}
#endif
return Q_HANDLED();
}
case LWIP_RX_OVERRUN_SIG: {
LINK_STATS_INC(link.err);
return Q_HANDLED();
}
}
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;
}
}
static int csp_can_process_frame(can_frame_t *frame) {
pbuf_element_t *buf;
uint8_t offset;
can_id_t id = frame->id;
/* Bind incoming frame to a packet buffer */
buf = pbuf_find(id, CFP_ID_CONN_MASK, NULL);
/* Check returned buffer */
if (buf == NULL) {
if (CFP_TYPE(id) == CFP_BEGIN) {
buf = pbuf_new(id, NULL);
if (buf == NULL) {
csp_log_warn("No available packet buffer for CAN\r\n");
csp_if_can.rx_error++;
return CSP_ERR_NOMEM;
}
} else {
csp_log_warn("Out of order MORE frame received for can id 0x%"PRIx32"; remain is %u\r\n",
(uint32_t)id, CFP_REMAIN(id));
csp_if_can.frame++;
return CSP_ERR_INVAL;
}
}
/* Reset frame data offset */
offset = 0;
switch (CFP_TYPE(id)) {
case CFP_BEGIN:
/* Discard packet if DLC is less than CSP id + CSP length fields */
if (frame->dlc < sizeof(csp_id_t) + sizeof(uint16_t)) {
csp_log_warn("Short BEGIN frame received\r\n");
csp_if_can.frame++;
pbuf_free(buf, NULL);
break;
}
/* Check for incomplete frame */
if (buf->packet != NULL) {
/* Reuse the buffer */
csp_log_warn("Incomplete frame\r\n");
csp_if_can.frame++;
} else {
/* Allocate memory for frame */
buf->packet = csp_buffer_get(csp_buffer_size() - CSP_BUFFER_PACKET_OVERHEAD);
if (buf->packet == NULL) {
csp_log_error("Failed to get buffer for CSP_BEGIN packet\r\n");
csp_if_can.frame++;
pbuf_free(buf, NULL);
break;
}
}
/* Copy CSP identifier and length*/
memcpy(&(buf->packet->id), frame->data, sizeof(csp_id_t));
buf->packet->id.ext = csp_ntoh32(buf->packet->id.ext);
memcpy(&(buf->packet->length), frame->data + sizeof(csp_id_t), sizeof(uint16_t));
buf->packet->length = csp_ntoh16(buf->packet->length);
/* Reset RX count */
buf->rx_count = 0;
/* Set offset to prevent CSP header from being copied to CSP data */
offset = sizeof(csp_id_t) + sizeof(uint16_t);
/* Set remain field - increment to include begin packet */
buf->remain = CFP_REMAIN(id) + 1;
/* Note fall through! */
case CFP_MORE:
/* Check 'remain' field match */
if (CFP_REMAIN(id) != buf->remain - 1) {
csp_log_error("CAN frame lost in CSP packet, %u vs. %u\r\n",
CFP_REMAIN(id),buf->remain - 1);
pbuf_free(buf, NULL);
csp_if_can.frame++;
break;
}
/* Decrement remaining frames */
buf->remain--;
/* Check for overflow */
if ((buf->rx_count + frame->dlc - offset) > buf->packet->length) {
csp_log_error("RX buffer overflow\r\n");
csp_if_can.frame++;
pbuf_free(buf, NULL);
break;
}
/* Copy dlc bytes into buffer */
memcpy(&buf->packet->data[buf->rx_count], frame->data + offset, frame->dlc - offset);
buf->rx_count += frame->dlc - offset;
/* Check if more data is expected */
if (buf->rx_count != buf->packet->length)
break;
/* Data is available */
csp_new_packet(buf->packet, &csp_if_can, NULL);
/* Drop packet buffer reference */
buf->packet = NULL;
/* Free packet buffer */
pbuf_free(buf, NULL);
break;
default:
csp_log_warn("Received unknown CFP message type\r\n");
pbuf_free(buf, NULL);
break;
}
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;
}
}
