| /* |
| * Copyright 2017, Data61, CSIRO (ABN 41 687 119 230) |
| * |
| * SPDX-License-Identifier: GPL-2.0-only |
| */ |
| |
| #include <autoconf.h> |
| #include <ethdrivers/gen_config.h> |
| #include <lwip/gen_config.h> |
| |
| #ifdef CONFIG_LIB_LWIP |
| |
| #include <ethdrivers/lwip.h> |
| #include <ethdrivers/helpers.h> |
| #include <string.h> |
| #include <lwip/netif.h> |
| #include <netif/etharp.h> |
| #include <lwip/stats.h> |
| #include <lwip/snmp.h> |
| #include "debug.h" |
| |
| static void initialize_free_bufs(lwip_iface_t *iface) |
| { |
| dma_addr_t *dma_bufs = NULL; |
| dma_bufs = malloc(sizeof(dma_addr_t) * CONFIG_LIB_ETHDRIVER_NUM_PREALLOCATED_BUFFERS); |
| if (!dma_bufs) { |
| goto error; |
| } |
| memset(dma_bufs, 0, sizeof(dma_addr_t) * CONFIG_LIB_ETHDRIVER_NUM_PREALLOCATED_BUFFERS); |
| iface->bufs = malloc(sizeof(dma_addr_t *) * CONFIG_LIB_ETHDRIVER_NUM_PREALLOCATED_BUFFERS); |
| if (!iface->bufs) { |
| goto error; |
| } |
| for (int i = 0; i < CONFIG_LIB_ETHDRIVER_NUM_PREALLOCATED_BUFFERS; i++) { |
| dma_bufs[i] = dma_alloc_pin(&iface->dma_man, CONFIG_LIB_ETHDRIVER_PREALLOCATED_BUF_SIZE, 1, |
| iface->driver.dma_alignment); |
| if (!dma_bufs[i].phys) { |
| goto error; |
| } |
| ps_dma_cache_clean_invalidate(&iface->dma_man, dma_bufs[i].virt, CONFIG_LIB_ETHDRIVER_PREALLOCATED_BUF_SIZE); |
| iface->bufs[i] = &dma_bufs[i]; |
| } |
| iface->num_free_bufs = CONFIG_LIB_ETHDRIVER_NUM_PREALLOCATED_BUFFERS; |
| return; |
| error: |
| if (iface->bufs) { |
| free(iface->bufs); |
| } |
| if (dma_bufs) { |
| for (int i = 0; i < CONFIG_LIB_ETHDRIVER_NUM_PREALLOCATED_BUFFERS; i++) { |
| if (dma_bufs[i].virt) { |
| dma_unpin_free(&iface->dma_man, dma_bufs[i].virt, CONFIG_LIB_ETHDRIVER_PREALLOCATED_BUF_SIZE); |
| } |
| } |
| free(dma_bufs); |
| } |
| iface->bufs = NULL; |
| } |
| |
| static uintptr_t lwip_allocate_rx_buf(void *iface, size_t buf_size, void **cookie) |
| { |
| lwip_iface_t *lwip_iface = (lwip_iface_t *)iface; |
| if (buf_size > CONFIG_LIB_ETHDRIVER_PREALLOCATED_BUF_SIZE) { |
| LOG_ERROR("Requested RX buffer of size %zu which can never be fullfilled by preallocated buffers of size %d", buf_size, |
| CONFIG_LIB_ETHDRIVER_PREALLOCATED_BUF_SIZE); |
| return 0; |
| } |
| if (lwip_iface->num_free_bufs == 0) { |
| if (!lwip_iface->bufs) { |
| initialize_free_bufs(lwip_iface); |
| if (!lwip_iface->bufs) { |
| LOG_ERROR("Failed lazy initialization of preallocated free buffers"); |
| return 0; |
| } |
| } else { |
| return 0; |
| } |
| } |
| lwip_iface->num_free_bufs--; |
| dma_addr_t *buf = lwip_iface->bufs[lwip_iface->num_free_bufs]; |
| ps_dma_cache_invalidate(&lwip_iface->dma_man, buf->virt, buf_size); |
| *cookie = (void *)buf; |
| return buf->phys; |
| } |
| |
| static void lwip_tx_complete(void *iface, void *cookie) |
| { |
| lwip_iface_t *lwip_iface = (lwip_iface_t *)iface; |
| lwip_iface->bufs[lwip_iface->num_free_bufs] = cookie; |
| lwip_iface->num_free_bufs++; |
| } |
| |
| static void lwip_rx_complete(void *iface, unsigned int num_bufs, void **cookies, unsigned int *lens) |
| { |
| struct pbuf *p; |
| int len; |
| lwip_iface_t *lwip_iface = (lwip_iface_t *)iface; |
| int i; |
| len = 0; |
| for (i = 0; i < num_bufs; i++) { |
| ps_dma_cache_invalidate(&lwip_iface->dma_man, ((dma_addr_t *)cookies[i])->virt, lens[i]); |
| len += lens[i]; |
| } |
| #if ETH_PAD_SIZE |
| len += ETH_PAD_SIZE; /* allow room for Ethernet padding */ |
| #endif |
| /* Get a buffer from the pool */ |
| p = pbuf_alloc(PBUF_RAW, len, PBUF_POOL); |
| if (p == NULL) { |
| for (i = 0; i < num_bufs; i++) { |
| lwip_tx_complete(iface, cookies[i]); |
| } |
| return; |
| } |
| |
| #if ETH_PAD_SIZE |
| pbuf_header(p, -ETH_PAD_SIZE); /* drop the padding word */ |
| len -= ETH_PAD_SIZE; |
| #endif |
| |
| /* fill the pbuf chain */ |
| struct pbuf *q = p; |
| unsigned int copied = 0; |
| unsigned int buf = 0; |
| unsigned int buf_done = 0; |
| unsigned int pbuf_done = 0; |
| while (copied < len) { |
| unsigned int next = MIN(q->len - pbuf_done, lens[buf] - buf_done); |
| memcpy(q->payload + pbuf_done, ((dma_addr_t *)cookies[buf])->virt + buf_done, next); |
| buf_done += next; |
| pbuf_done += next; |
| copied += next; |
| if (buf_done == lens[buf]) { |
| buf++; |
| buf_done = 0; |
| } |
| if (pbuf_done == q->len) { |
| q = q->next; |
| pbuf_done = 0; |
| } |
| } |
| |
| // PKT_DEBUG(printf("Receiving packet\n")); |
| // PKT_DEBUG(print_packet(COL_RX, p->payload, len)); |
| |
| #if ETH_PAD_SIZE |
| pbuf_header(p, ETH_PAD_SIZE); /* reclaim the padding word */ |
| #endif |
| LINK_STATS_INC(link.recv); |
| |
| for (i = 0; i < num_bufs; i++) { |
| lwip_tx_complete(iface, cookies[i]); |
| } |
| |
| struct eth_hdr *ethhdr; |
| ethhdr = p->payload; |
| |
| switch (htons(ethhdr->type)) { |
| /* IP or ARP packet? */ |
| case ETHTYPE_IP: |
| case ETHTYPE_ARP: |
| #if PPPOE_SUPPORT |
| /* PPPoE packet? */ |
| case ETHTYPE_PPPOEDISC: |
| case ETHTYPE_PPPOE: |
| #endif /* PPPOE_SUPPORT */ |
| /* full packet send to tcpip_thread to process */ |
| if (lwip_iface->netif->input(p, lwip_iface->netif) != ERR_OK) { |
| LWIP_DEBUGF(NETIF_DEBUG, ("ethernetif_input: IP input error\n")); |
| pbuf_free(p); |
| p = NULL; |
| } |
| break; |
| |
| default: |
| pbuf_free(p); |
| break; |
| } |
| } |
| |
| static err_t ethif_link_output(struct netif *netif, struct pbuf *p) |
| { |
| lwip_iface_t *iface = (lwip_iface_t *)netif->state; |
| dma_addr_t buf; |
| struct pbuf *q; |
| int status; |
| |
| #if ETH_PAD_SIZE |
| pbuf_header(p, -ETH_PAD_SIZE); /* drop the padding word */ |
| #endif |
| |
| if (p->tot_len > CONFIG_LIB_ETHDRIVER_PREALLOCATED_BUF_SIZE) { |
| return ERR_MEM; |
| } |
| if (iface->num_free_bufs == 0) { |
| return ERR_MEM; |
| } |
| iface->num_free_bufs--; |
| dma_addr_t *orig_buf = iface->bufs[iface->num_free_bufs]; |
| buf = *orig_buf; |
| |
| char *pkt_pos = (char *)buf.virt; |
| for (q = p; q != NULL; q = q->next) { |
| memcpy(pkt_pos, q->payload, q->len); |
| pkt_pos += q->len; |
| } |
| ps_dma_cache_clean(&iface->dma_man, buf.virt, p->tot_len); |
| // PKT_DEBUG(cprintf(COL_TX, "Sending packet")); |
| // PKT_DEBUG(print_packet(COL_TX, (void*)buf.virt, p->tot_len)); |
| |
| #if ETH_PAD_SIZE |
| pbuf_header(p, ETH_PAD_SIZE); /* reclaim the padding word */ |
| #endif |
| |
| unsigned int length = p->tot_len; |
| status = iface->driver.i_fn.raw_tx(&iface->driver, 1, &buf.phys, &length, orig_buf); |
| switch (status) { |
| case ETHIF_TX_FAILED: |
| lwip_tx_complete(iface, orig_buf); |
| return ERR_WOULDBLOCK; |
| case ETHIF_TX_COMPLETE: |
| lwip_tx_complete(iface, orig_buf); |
| case ETHIF_TX_ENQUEUED: |
| break; |
| } |
| |
| LINK_STATS_INC(link.xmit); |
| |
| return ERR_OK; |
| } |
| |
| static uintptr_t lwip_pbuf_allocate_rx_buf(void *iface, size_t buf_size, void **cookie) |
| { |
| lwip_iface_t *lwip_iface = (lwip_iface_t *)iface; |
| #if ETH_PAD_SIZE |
| buf_size += ETH_PAD_SIZE; /* allow room for Ethernet padding */ |
| #endif |
| /* add space for alignment */ |
| buf_size += lwip_iface->driver.dma_alignment; |
| struct pbuf *p = pbuf_alloc(PBUF_RAW, buf_size, PBUF_RAM); |
| if (!p) { |
| return 0; |
| } |
| /* we cannot support chained pbufs when doing this */ |
| if (p->next) { |
| pbuf_free(p); |
| return 0; |
| } |
| #if ETH_PAD_SIZE |
| pbuf_header(p, -ETH_PAD_SIZE); /* drop the padding word */ |
| #endif |
| uintptr_t new_payload = (uintptr_t)p->payload; |
| /* round up to dma_alignment */ |
| new_payload = ROUND_UP(new_payload, lwip_iface->driver.dma_alignment); |
| pbuf_header(p, -(new_payload - (uintptr_t)p->payload)); |
| uintptr_t phys = ps_dma_pin(&lwip_iface->dma_man, p->payload, buf_size); |
| if (!phys) { |
| pbuf_free(p); |
| return 0; |
| } |
| ps_dma_cache_invalidate(&lwip_iface->dma_man, p->payload, buf_size); |
| *cookie = p; |
| return phys; |
| } |
| |
| static void lwip_pbuf_tx_complete(void *iface, void *cookie) |
| { |
| lwip_iface_t *lwip_iface = (lwip_iface_t *)iface; |
| struct pbuf *p = (struct pbuf *)cookie; |
| for (; p; p = p->next) { |
| uintptr_t loc = (uintptr_t)p->payload; |
| uintptr_t end = (uintptr_t)p->payload + p->len; |
| while (loc < end) { |
| uintptr_t next = ROUND_UP(loc + 1, PAGE_SIZE_4K); |
| if (next > end) { |
| next = end; |
| } |
| ps_dma_unpin(&lwip_iface->dma_man, (void *)loc, next - loc); |
| loc = next; |
| } |
| } |
| pbuf_free(cookie); |
| } |
| |
| static void lwip_pbuf_rx_complete(void *iface, unsigned int num_bufs, void **cookies, unsigned int *lens) |
| { |
| struct pbuf *p = NULL; |
| int i; |
| lwip_iface_t *lwip_iface = (lwip_iface_t *)iface; |
| |
| assert(num_bufs > 0); |
| /* staple all the bufs together, do it in reverse order for efficiency |
| * of traversing pbuf chains */ |
| for (i = num_bufs - 1; i >= 0; i--) { |
| struct pbuf *q = (struct pbuf *)cookies[i]; |
| ps_dma_cache_invalidate(&lwip_iface->dma_man, q->payload, lens[i]); |
| pbuf_realloc(q, lens[i]); |
| if (p) { |
| pbuf_cat(q, p); |
| } |
| p = q; |
| } |
| |
| #if ETH_PAD_SIZE |
| pbuf_header(p, ETH_PAD_SIZE); /* reclaim the padding word */ |
| #endif |
| |
| LINK_STATS_INC(link.recv); |
| |
| struct eth_hdr *ethhdr; |
| ethhdr = p->payload; |
| |
| switch (htons(ethhdr->type)) { |
| /* IP or ARP packet? */ |
| case ETHTYPE_IP: |
| case ETHTYPE_ARP: |
| #if PPPOE_SUPPORT |
| /* PPPoE packet? */ |
| case ETHTYPE_PPPOEDISC: |
| case ETHTYPE_PPPOE: |
| #endif /* PPPOE_SUPPORT */ |
| /* full packet send to tcpip_thread to process */ |
| if (lwip_iface->netif->input(p, lwip_iface->netif) != ERR_OK) { |
| LWIP_DEBUGF(NETIF_DEBUG, ("ethernetif_input: IP input error\n")); |
| LOG_INFO("failed to input\n"); |
| pbuf_free(p); |
| } |
| break; |
| |
| default: |
| pbuf_free(p); |
| break; |
| } |
| } |
| |
| static err_t ethif_pbuf_link_output(struct netif *netif, struct pbuf *p) |
| { |
| lwip_iface_t *iface = (lwip_iface_t *)netif->state; |
| struct pbuf *q; |
| int status; |
| |
| /* grab a reference to the pbuf */ |
| pbuf_ref(p); |
| |
| #if ETH_PAD_SIZE |
| pbuf_header(p, -ETH_PAD_SIZE); /* drop the padding word */ |
| #endif |
| int max_frames = 0; |
| |
| /* work out how many pieces this buffer could potentially take up */ |
| for (q = p; q; q = q->next) { |
| uintptr_t base = PAGE_ALIGN_4K((uintptr_t)q->payload); |
| uintptr_t top = PAGE_ALIGN_4K((uintptr_t)q->payload + q->len - 1); |
| max_frames += ((top - base) / PAGE_SIZE_4K) + 1; |
| } |
| int num_frames = 0; |
| unsigned int lengths[max_frames]; |
| uintptr_t phys[max_frames]; |
| for (q = p; q; q = q->next) { |
| uintptr_t loc = (uintptr_t)q->payload; |
| uintptr_t end = (uintptr_t)q->payload + q->len; |
| while (loc < end) { |
| uintptr_t next = ROUND_UP(loc + 1, PAGE_SIZE_4K); |
| if (next > end) { |
| next = end; |
| } |
| lengths[num_frames] = next - loc; |
| phys[num_frames] = ps_dma_pin(&iface->dma_man, (void *)loc, lengths[num_frames]); |
| ps_dma_cache_clean(&iface->dma_man, (void *)loc, lengths[num_frames]); |
| assert(phys[num_frames]); |
| num_frames++; |
| loc = next; |
| } |
| } |
| |
| #if ETH_PAD_SIZE |
| pbuf_header(p, ETH_PAD_SIZE); /* reclaim the padding word */ |
| #endif |
| |
| status = iface->driver.i_fn.raw_tx(&iface->driver, num_frames, phys, lengths, p); |
| switch (status) { |
| case ETHIF_TX_FAILED: |
| lwip_pbuf_tx_complete(iface, p); |
| return ERR_WOULDBLOCK; |
| case ETHIF_TX_COMPLETE: |
| lwip_pbuf_tx_complete(iface, p); |
| case ETHIF_TX_ENQUEUED: |
| break; |
| } |
| |
| LINK_STATS_INC(link.xmit); |
| |
| return ERR_OK; |
| } |
| |
| static struct raw_iface_callbacks lwip_prealloc_callbacks = { |
| .tx_complete = lwip_tx_complete, |
| .rx_complete = lwip_rx_complete, |
| .allocate_rx_buf = lwip_allocate_rx_buf |
| }; |
| |
| static struct raw_iface_callbacks lwip_pbuf_callbacks = { |
| .tx_complete = lwip_pbuf_tx_complete, |
| .rx_complete = lwip_pbuf_rx_complete, |
| .allocate_rx_buf = lwip_pbuf_allocate_rx_buf |
| }; |
| |
| static err_t ethif_init(struct netif *netif) |
| { |
| if (netif -> state == NULL) { |
| return ERR_ARG; |
| } |
| |
| lwip_iface_t *iface = (lwip_iface_t *)netif->state; |
| int mtu; |
| iface->driver.i_fn.low_level_init(&iface->driver, netif->hwaddr, &mtu); |
| netif->mtu = mtu; |
| |
| netif->hwaddr_len = ETHARP_HWADDR_LEN; |
| netif->output = etharp_output; |
| if (iface->bufs == NULL) { |
| netif->linkoutput = ethif_pbuf_link_output; |
| } else { |
| netif->linkoutput = ethif_link_output; |
| } |
| |
| NETIF_INIT_SNMP(netif, snmp_ifType_ethernet_csmacd, |
| LINK_SPEED_OF_YOUR_NETIF_IN_BPS); |
| |
| netif -> flags = NETIF_FLAG_BROADCAST | NETIF_FLAG_ETHARP | |
| NETIF_FLAG_LINK_UP | NETIF_FLAG_IGMP; |
| |
| iface->netif = netif; |
| return ERR_OK; |
| } |
| |
| lwip_iface_t *ethif_new_lwip_driver_no_malloc(ps_io_ops_t io_ops, ps_dma_man_t *pbuf_dma, ethif_driver_init driver, |
| void *driver_config, lwip_iface_t *iface) |
| { |
| memset(iface, 0, sizeof(*iface)); |
| iface->driver.cb_cookie = iface; |
| if (pbuf_dma) { |
| iface->driver.i_cb = lwip_pbuf_callbacks; |
| iface->dma_man = *pbuf_dma; |
| } else { |
| iface->driver.i_cb = lwip_prealloc_callbacks; |
| iface->dma_man = io_ops.dma_manager; |
| } |
| int err; |
| err = driver(&iface->driver, io_ops, driver_config); |
| if (err) { |
| goto error; |
| } |
| /* if the driver did not already cause it to happen, allocate the preallocated buffers */ |
| if (!pbuf_dma && !iface->bufs) { |
| initialize_free_bufs(iface); |
| if (iface->bufs == NULL) { |
| LOG_ERROR("Fault preallocating bufs"); |
| goto error; |
| } |
| } |
| iface->ethif_init = ethif_init; |
| return iface; |
| error: |
| return NULL; |
| } |
| |
| lwip_iface_t *ethif_new_lwip_driver(ps_io_ops_t io_ops, ps_dma_man_t *pbuf_dma, ethif_driver_init driver, |
| void *driver_config) |
| { |
| lwip_iface_t *ret; |
| lwip_iface_t *iface = malloc(sizeof(*iface)); |
| if (!iface) { |
| LOG_ERROR("Failed to malloc"); |
| return NULL; |
| } |
| ret = ethif_new_lwip_driver_no_malloc(io_ops, pbuf_dma, driver, driver_config, iface); |
| if (!ret) { |
| free(iface); |
| } |
| return ret; |
| } |
| |
| #endif /* CONFIG_LIB_LWIP */ |
