This is an automated email from the ASF dual-hosted git repository. xiaoxiang781216 pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/nuttx.git
commit 4486109f00c3f6c59e6a51ad06e277ef664aadcd Author: p-szafonimateusz <[email protected]> AuthorDate: Mon Jan 5 14:16:02 2026 +0100 net: add netpoll support Add netpoll support based on netdev_lowerhalf_s, which enables sending packets in contexts where the network stack is not usable (e.g. from interrupt context or during panic). For now only UDP packets are supported. [skip-cmake-parity]: net/CMakeLists.txt uses nuttx_add_subdirectory() which picks up the new net/netpoll/CMakeLists.txt automatically. Signed-off-by: p-szafonimateusz <[email protected]> --- Documentation/os/networking/index.rst | 1 + Documentation/os/networking/netpoll.rst | 14 ++ include/nuttx/net/netdev_lowerhalf.h | 14 ++ include/nuttx/net/netpoll.h | 226 +++++++++++++++++ net/Kconfig | 10 + net/Makefile | 1 + net/netpoll/CMakeLists.txt | 27 ++ net/netpoll/Make.defs | 31 +++ net/netpoll/netpoll_pkt.c | 78 ++++++ net/netpoll/netpoll_udp.c | 426 ++++++++++++++++++++++++++++++++ 10 files changed, 828 insertions(+) diff --git a/Documentation/os/networking/index.rst b/Documentation/os/networking/index.rst index 6d822299560..5e569eb8165 100644 --- a/Documentation/os/networking/index.rst +++ b/Documentation/os/networking/index.rst @@ -68,6 +68,7 @@ Network Support bridge.rst netdev.rst netdriver.rst + netpoll.rst mdio.rst netguardsize.rst netlink.rst diff --git a/Documentation/os/networking/netpoll.rst b/Documentation/os/networking/netpoll.rst new file mode 100644 index 00000000000..c93106229f9 --- /dev/null +++ b/Documentation/os/networking/netpoll.rst @@ -0,0 +1,14 @@ +======= +Netpoll +======= + +Netpoll is a tool inspired by a similar tool in Linux that allows you to use +network interfaces in places where the network stack is normally inaccessible, +such as interrupts. + +It allows sending and receiving UDP packets with minimal kernel involvement, +which can be used to implement kernel debugging features over network interface. + +The NuttX implementation is based on the ``struct netdev_lowerhalf_s`` which +means that the network interface using netpoll must be based on +the :ref:`Netdev Lowerhalf <netdriver>` implementation. diff --git a/include/nuttx/net/netdev_lowerhalf.h b/include/nuttx/net/netdev_lowerhalf.h index 74ac267ba16..04c4ed9d04e 100644 --- a/include/nuttx/net/netdev_lowerhalf.h +++ b/include/nuttx/net/netdev_lowerhalf.h @@ -180,6 +180,20 @@ struct netdev_ops_s /* reclaim - try to reclaim packets sent by netdev. */ CODE void (*reclaim)(FAR struct netdev_lowerhalf_s *dev); + +#ifdef CONFIG_NET_NETPOLL + /* txpoll - Complete pending transmissions and free the finished TX + * packets, so their TX quota can be reused. + * + * Used only by netpoll, which may run with interrupts disabled and + * without the network stack (e.g. on the panic path), where the + * driver's TX-done interrupt cannot run. It must not depend on + * interrupts or on the upper half. It is separate from reclaim, + * which is called by the upper half in normal context. + */ + + CODE void (*txpoll)(FAR struct netdev_lowerhalf_s *dev); +#endif }; /* This structure is a set of wireless handlers, leave unsupported operations diff --git a/include/nuttx/net/netpoll.h b/include/nuttx/net/netpoll.h new file mode 100644 index 00000000000..7d9e5917b43 --- /dev/null +++ b/include/nuttx/net/netpoll.h @@ -0,0 +1,226 @@ +/**************************************************************************** + * include/nuttx/net/netpoll.h + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __INCLUDE_NUTTX_NET_NETPOLL_H +#define __INCLUDE_NUTTX_NET_NETPOLL_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/net/netdev_lowerhalf.h> + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +struct netpoll_s +{ + /* Netdev lower-half */ + + FAR struct netdev_lowerhalf_s *lower; + + size_t maxlen; /* Maximum MTU size */ + size_t hdrlen; /* Total header length */ + uint16_t srcport; /* Source port number */ + uint16_t destport; /* Destination port number */ + in_addr_t destipaddr; /* Destination IP address */ + uint8_t destmac[ETHER_ADDR_LEN]; /* Ethernet destination address */ +}; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifdef __cplusplus +extern "C" +{ +#endif + +/**************************************************************************** + * Name: netpoll_pkt_alloc + * + * Description: + * Allocate a TX packet for netpoll. + * When there are no free IOBs, this function tries to free one netpkt. + * + * Input Parameters: + * dev - The lower half device driver structure + * + * Returned Value: + * Pointer to the packet, NULL on failure + * + ****************************************************************************/ + +FAR netpkt_t *netpoll_pkt_alloc(FAR struct netdev_lowerhalf_s *dev); + +/**************************************************************************** + * Name: netpoll_get_maxlen + * + * Description: + * Get maximum data length for netpoll + * + * Input Parameters: + * np - pointer to netpoll data + * + * Returned Value: + * Returns maximum data length. + * + ****************************************************************************/ + +#define netpoll_get_maxlen(np) ((np)->maxlen) + +/**************************************************************************** + * Name: netpoll_pkt_copy + * + * Description: + * Copy 'len' bytes of data from a buffer into the netpkt + * + * Input Parameters: + * np - pointer to netpoll data + * pkt - packet to send + * src - The source buffer + * len - How many bytes to copy + * offset - The offset of netpkt to put the data + * + * Returned Value: + * 0:Success; negated errno on failure + * + ****************************************************************************/ + +static inline_function int netpoll_pkt_copy(FAR struct netpoll_s *np, + FAR netpkt_t *pkt, + FAR const void *src, + size_t len, int offset) +{ + return netpkt_copyin(np->lower, pkt, src, len, np->hdrlen + offset); +} + +/**************************************************************************** + * Name: netpoll_udp_init + * + * Description: + * Initialize netpoll with UDP. + * + * Input Parameters: + * np - pointer to netpoll data + * lower - pointer to netdev lower + * srcport - source port number + * destipaddr - destination IP address + * destport - destination port number + * destmac - MAC address octets in string format: "ff:ff:ff:ff:ff:ff" + * Default: "ff:ff:ff:ff:ff:ff" + * + * Returned Value: + * Returns OK on success + * + ****************************************************************************/ + +int netpoll_udp_init(FAR struct netpoll_s *np, + FAR struct netdev_lowerhalf_s *lower, + uint16_t srcport, in_addr_t destipaddr, + uint16_t destport, FAR const char *destmac); + +/**************************************************************************** + * Name: netpoll_udp_pkt_alloc + * + * Description: + * Allocate UDP packet and prepare packet header (MAC, IPv4 and UDP). + * + * Input Parameters: + * np - pointer to netpoll data + * + * Returned Value: + * Pointer to PACKET on success; NULL on failure. + * + ****************************************************************************/ + +FAR netpkt_t *netpoll_udp_pkt_alloc(FAR struct netpoll_s *np); + +/**************************************************************************** + * Name: netpoll_upd_send + * + * Description: + * Send UDP packet with netpoll. + * + * Input Parameters: + * np - pointer to netpoll data + * buf - data to send + * len - length of data to send + * + * Returned Value: + * OK on success; Negated errno on failure. + * + * Assumptions: + * Same as netpoll_udp_pkt_send(). + * + ****************************************************************************/ + +int netpoll_udp_send(FAR struct netpoll_s *np, + FAR const void *buf, size_t len); + +/**************************************************************************** + * Name: netpoll_udp_recv + * + * Description: + * Receive UDP packet with netpoll (blocking). + * + * Input Parameters: + * np - pointer to netpoll data + * buf - receive buffer + * len - length of receive buffer + * + * Returned Value: + * On success, returns the number of characters received. + * + ****************************************************************************/ + +ssize_t netpoll_udp_recv(FAR struct netpoll_s *np, FAR void *buf, + size_t len); + +/**************************************************************************** + * Name: netpoll_udp_pkt_send + * + * Description: + * Send packet as UDP packet with netpoll. + * + * Input Parameters: + * np - pointer to netpoll data + * pkt - packet to send + * + * Returned Value: + * OK on success; Negated errno on failure. + * + * Assumptions: + * The caller holds netdev_lock() of the device, so the driver's + * transmit() is serialized with the upper half. The only exception is + * the final panic path, where nothing else runs anymore. + * + ****************************************************************************/ + +int netpoll_udp_pkt_send(FAR struct netpoll_s *np, FAR netpkt_t *pkt); + +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_NUTTX_NET_NETPOLL_H */ diff --git a/net/Kconfig b/net/Kconfig index 8cae8909f42..202820fc8aa 100644 --- a/net/Kconfig +++ b/net/Kconfig @@ -67,6 +67,16 @@ config NET_DEFAULT_MAX_PORT ---help--- Default Network max port +config NET_NETPOLL + bool "Support Netpoll interface" + depends on NET_IPv4 && NET_ETHERNET + default n + ---help--- + Support Netpoll interface based on NETDEV lower-half interface. + Netpoll allows sending and receiving packets bypassing the network + stack, which makes it possible to use its interface from the interrupt + context + menu "Driver buffer configuration" config NET_ETH_PKTSIZE diff --git a/net/Makefile b/net/Makefile index 476f0ea5cd8..5ffc81b02e4 100644 --- a/net/Makefile +++ b/net/Makefile @@ -53,6 +53,7 @@ include ipfilter/Make.defs include ipforward/Make.defs include nat/Make.defs include netfilter/Make.defs +include netpoll/Make.defs include route/Make.defs include procfs/Make.defs include usrsock/Make.defs diff --git a/net/netpoll/CMakeLists.txt b/net/netpoll/CMakeLists.txt new file mode 100644 index 00000000000..b659230e5e9 --- /dev/null +++ b/net/netpoll/CMakeLists.txt @@ -0,0 +1,27 @@ +# ############################################################################## +# net/netpoll/CMakeLists.txt +# +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed to the Apache Software Foundation (ASF) under one or more contributor +# license agreements. See the NOTICE file distributed with this work for +# additional information regarding copyright ownership. The ASF licenses this +# file to you under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. +# +# ############################################################################## + +if(CONFIG_NET_NETPOLL) + set(SRCS netpoll_pkt.c netpoll_udp.c) + + target_sources(net PRIVATE ${SRCS}) +endif() diff --git a/net/netpoll/Make.defs b/net/netpoll/Make.defs new file mode 100644 index 00000000000..41a846daa09 --- /dev/null +++ b/net/netpoll/Make.defs @@ -0,0 +1,31 @@ +############################################################################ +# net/netpoll/Make.defs +# +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed to the Apache Software Foundation (ASF) under one or more +# contributor license agreements. See the NOTICE file distributed with +# this work for additional information regarding copyright ownership. The +# ASF licenses this file to you under the Apache License, Version 2.0 (the +# "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +############################################################################ + +ifeq ($(CONFIG_NET_NETPOLL),y) +NET_CSRCS += netpoll_pkt.c netpoll_udp.c + +# Include netpoll build support + +DEPPATH += --dep-path netpoll +VPATH += :netpoll + +endif # CONFIG_NET_NETPOLL diff --git a/net/netpoll/netpoll_pkt.c b/net/netpoll/netpoll_pkt.c new file mode 100644 index 00000000000..a6b82fecc7c --- /dev/null +++ b/net/netpoll/netpoll_pkt.c @@ -0,0 +1,78 @@ +/**************************************************************************** + * net/netpoll/netpoll_pkt.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <nuttx/net/netpoll.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* How many times to reclaim TX packets from the driver before giving up */ + +#define NETPOLL_PKT_ALLOC_RETRY 8 + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: netpoll_pkt_alloc + * + * Description: + * Allocate a TX packet for netpoll. + * When the TX quota is used up, ask the driver to reclaim finished TX + * packets and try again a bounded number of times. + * + * Input Parameters: + * dev - The lower half device driver structure + * + * Returned Value: + * Pointer to the packet, NULL on failure + * + ****************************************************************************/ + +FAR netpkt_t *netpoll_pkt_alloc(FAR struct netdev_lowerhalf_s *dev) +{ + FAR netpkt_t *pkt; + int retry; + + for (retry = 0; retry < NETPOLL_PKT_ALLOC_RETRY; retry++) + { + pkt = netpkt_alloc(dev, NETPKT_TX); + if (pkt != NULL) + { + return pkt; + } + + /* Try to release TX packets the driver has finished with */ + + dev->ops->txpoll(dev); + } + + return NULL; +} diff --git a/net/netpoll/netpoll_udp.c b/net/netpoll/netpoll_udp.c new file mode 100644 index 00000000000..9dc336557bd --- /dev/null +++ b/net/netpoll/netpoll_udp.c @@ -0,0 +1,426 @@ +/**************************************************************************** + * net/netpoll/netpoll_udp.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <netinet/ether.h> + +#include <nuttx/init.h> +#include <nuttx/mutex.h> +#include <nuttx/net/netpoll.h> +#include <nuttx/net/udp.h> + +#include "inet/inet.h" + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: netpoll_udp_build_header + * + * Description: + * Fill packet with UDP header + * + * Input Parameters: + * np - pointer to netpoll data + * pkt - packet to send + * + * Returned Value: + * OK on success; Negated errno on failure. + * + ****************************************************************************/ + +static void netpoll_udp_build_header(FAR struct netpoll_s *np, + FAR netpkt_t *pkt) +{ + FAR struct netdev_lowerhalf_s *lower = np->lower; + FAR struct eth_hdr_s *eth; + FAR struct ipv4_hdr_s *ipv4; + FAR struct udp_hdr_s *udp; + FAR uint8_t *data; + size_t offset = 0; + + /* Get IO data */ + + data = netpkt_getdata(lower, pkt); + + /* Fill MAC header */ + + eth = (FAR struct eth_hdr_s *)&data[offset]; + eth->type = htons(ETHTYPE_IP); + memcpy(eth->dest, np->destmac, sizeof(eth->dest)); + memcpy(eth->src, &lower->netdev.d_mac, sizeof(eth->src)); + + /* Fill IPv4 header */ + + offset += ETH_HDRLEN; + ipv4 = (FAR struct ipv4_hdr_s *)&data[offset]; + ipv4_build_header(ipv4, pkt->io_pktlen, IP_PROTO_UDP, + &lower->netdev.d_ipaddr, &np->destipaddr, + IP_TTL_DEFAULT, 0, NULL); + + /* Fill UDP header */ + + offset += IPv4_HDRLEN; + udp = (FAR struct udp_hdr_s *)&data[offset]; + + udp->srcport = np->srcport; + udp->destport = np->destport; + udp->udplen = htons(pkt->io_pktlen - IPv4_HDRLEN); + udp->udpchksum = 0; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: netpoll_udp_init + * + * Description: + * Initialize netpoll with UDP. + * + * Input Parameters: + * np - pointer to netpoll data + * lower - pointer to netdev lower + * srcport - source port number + * destipaddr - destination IP address + * destport - destination port number + * destmac - MAC address octets in string format: "ff:ff:ff:ff:ff:ff". + * Default: "ff:ff:ff:ff:ff:ff" + * + * Returned Value: + * Returns OK on success + * + ****************************************************************************/ + +int netpoll_udp_init(FAR struct netpoll_s *np, + FAR struct netdev_lowerhalf_s *lower, + uint16_t srcport, in_addr_t destipaddr, + uint16_t destport, FAR const char *destmac) +{ + struct ether_addr addr; + + if (lower->ops->txpoll == NULL) + { + return -EINVAL; + } + + memset(np, 0, sizeof(struct netpoll_s)); + + if (destmac == NULL) + { + destmac = "ff:ff:ff:ff:ff:ff"; + } + + /* Decode MAC */ + + if (!ether_aton_r(destmac, &addr)) + { + return -EINVAL; + } + + memcpy(np->destmac, addr.ether_addr_octet, ETHER_ADDR_LEN); + + /* Parameters */ + + np->srcport = htons(srcport); + np->destipaddr = htonl(destipaddr); + np->destport = htons(destport); + + /* Store lower half reference */ + + np->lower = lower; + + /* Store header length for later */ + + np->hdrlen = NET_LL_HDRLEN(&lower->netdev) + IPv4UDP_HDRLEN; + + /* Get maximum UDP packet length for this device */ + + np->maxlen = NETDEV_PKTSIZE(&lower->netdev) - np->hdrlen; + + return OK; +} + +/**************************************************************************** + * Name: netpoll_udp_pkt_alloc + * + * Description: + * Allocate UDP packet. + * + * Input Parameters: + * np - pointer to netpoll data + * + * Returned Value: + * Pointer to packet on success; NULL on failure. + * + ****************************************************************************/ + +FAR netpkt_t *netpoll_udp_pkt_alloc(FAR struct netpoll_s *np) +{ + FAR struct netdev_lowerhalf_s *lower = np->lower; + FAR netpkt_t *pkt; + + /* Get packet */ + + pkt = netpoll_pkt_alloc(lower); + if (!pkt) + { + return NULL; + } + + /* Initialize packet with header length */ + + netpkt_setdatalen(lower, pkt, np->hdrlen); + + return pkt; +} + +/**************************************************************************** + * Name: netpoll_udp_pkt_send + * + * Description: + * Send packet as UDP packet with netpoll. + * + * Input Parameters: + * np - pointer to netpoll data + * pkt - packet to send + * + * Returned Value: + * Length of header if success, negative value on failure + * + * Assumption: + * Packet is ready to send, all packet headers are complete, only UDP + * payload len can change. + * + ****************************************************************************/ + +int netpoll_udp_pkt_send(FAR struct netpoll_s *np, FAR netpkt_t *pkt) +{ + FAR struct netdev_lowerhalf_s *lower = np->lower; + int ret; + + /* The upper half calls transmit() with netdev_lock() held, so the + * caller must hold it too, except on the final panic path where the + * rest of the system is stopped. + */ + + DEBUGASSERT(OSINIT_IS_PANIC() || + nxrmutex_is_hold(&lower->netdev.d_lock)); + + /* Don't send data if device is not ready */ + + if (!IFF_IS_RUNNING(lower->netdev.d_flags)) + { + return -ENODEV; + } + + /* Fill header */ + + netpoll_udp_build_header(np, pkt); + + /* Transmit packet */ + + ret = lower->ops->transmit(lower, pkt); + if (ret < 0) + { + return ret; + } + + NETDEV_TXDONE(&lower->netdev); + + return OK; +} + +/**************************************************************************** + * Name: netpoll_udp_send + * + * Description: + * Send UDP packet with netpoll. + * + * Input Parameters: + * np - pointer to netpoll data + * buf - data to send + * len - length of data to send + * + * Returned Value: + * OK on success; Negated errno on failure. + * + ****************************************************************************/ + +int netpoll_udp_send(FAR struct netpoll_s *np, FAR const void *buf, + size_t len) +{ + FAR netpkt_t *pkt; + int ret; + + /* Get packet */ + + pkt = netpoll_udp_pkt_alloc(np); + if (!pkt) + { + return -ENOMEM; + } + + /* Fill with data */ + + netpoll_pkt_copy(np, pkt, buf, len, 0); + + /* Send packet */ + + ret = netpoll_udp_pkt_send(np, pkt); + if (ret < 0) + { + netpkt_free(np->lower, pkt, NETPKT_TX); + } + + return ret; +} + +/**************************************************************************** + * Name: netpoll_udp_recv + * + * Description: + * Receive UDP packet for a given port and store source address. + * This function is blocking and returns only when a valid UDP packet + * is received so it must be used with caution. + * + * This function doesn't use the NET stack so it can be used in + * an interrupt context. + * + * Input Parameters: + * np - pointer to netpoll data + * buf - receive buffer + * len - length of receive buffer + * + * Returned Value: + * Returns the number of characters received. + * + ****************************************************************************/ + +ssize_t netpoll_udp_recv(FAR struct netpoll_s *np, FAR void *buf, size_t len) +{ + FAR struct netdev_lowerhalf_s *lower = np->lower; + FAR struct eth_hdr_s *eth; + FAR struct ipv4_hdr_s *ipv4; + FAR struct udp_hdr_s *udp; + FAR netpkt_t *pkt; + FAR uint8_t *data; + in_addr_t addr; + size_t offset; + bool found = false; + + DEBUGASSERT(lower->ops->receive); + + /* Don't recv data if device is not ready */ + + if (!IFF_IS_RUNNING(lower->netdev.d_flags)) + { + return -ENODEV; + } + + /* Loop until we get a valid UDP packet which is addressed to us. + * At this point all other packages are dropped. + */ + + while (!found) + { + /* Try to get packet */ + + pkt = lower->ops->receive(lower); + if (pkt == NULL) + { + continue; + } + + /* Get IO data */ + + data = netpkt_getdata(lower, pkt); + + /* Get MAC header */ + + offset = 0; + eth = (FAR struct eth_hdr_s *)&data[offset]; + + if (eth->type != htons(ETHTYPE_IP)) + { + goto errout; + } + + /* Get IPv4 header */ + + offset += ETH_HDRLEN; + ipv4 = (FAR struct ipv4_hdr_s *)&data[offset]; + + /* Check if the packet is destined for our IP address. */ + + addr = net_ip4addr_conv32(ipv4->destipaddr); + if (!net_ipv4addr_cmp(addr, lower->netdev.d_ipaddr)) + { + goto errout; + } + + /* Must be UDP */ + + if (ipv4->proto != IP_PROTO_UDP) + { + goto errout; + } + + /* Get UDP header */ + + offset += IPv4_HDRLEN; + udp = (FAR struct udp_hdr_s *)&data[offset]; + + if (udp->destport != np->srcport) + { + goto errout; + } + + /* Copy data to buffer */ + + len = MIN((ntohs(udp->udplen) - UDP_HDRLEN), len); + offset += UDP_HDRLEN; + netpkt_copyout(lower, buf, pkt, len, offset); + found = true; + + /* Store source info */ + + np->destipaddr = net_ip4addr_conv32(ipv4->srcipaddr); + np->destport = udp->srcport; + + /* Update destination MAC */ + + memcpy(np->destmac, eth->src, sizeof(np->destmac)); + +errout: + netpkt_free(lower, pkt, NETPKT_RX); + } + + return len; +}
