From 7225e44725d7986e32a2ae4445ffbb3e7a79204f Mon Sep 17 00:00:00 2001 From: Zhe Weng Date: Tue, 11 Oct 2022 14:19:07 +0800 Subject: [PATCH] arch/sim: support multiple tapdev for sim Support more than one TAP device for debugging (future) NAT and FORWARD, left WPCAP and VPNKit unchanged (force number to 1). Although we can support at most 31 interfaces, limit to 8 like CONFIG_TUN_NINTERFACES. Signed-off-by: wengzhe --- arch/sim/Kconfig | 11 +++ arch/sim/src/sim/posix/up_tapdev.c | 101 ++++++++++++------------ arch/sim/src/sim/posix/up_vpnkit.c | 2 +- arch/sim/src/sim/posix/up_wpcap.c | 4 +- arch/sim/src/sim/up_internal.h | 57 +++++++------- arch/sim/src/sim/up_netdriver.c | 119 +++++++++++++++++++---------- 6 files changed, 176 insertions(+), 118 deletions(-) diff --git a/arch/sim/Kconfig b/arch/sim/Kconfig index 7df5c92654..e05043afbb 100644 --- a/arch/sim/Kconfig +++ b/arch/sim/Kconfig @@ -194,6 +194,17 @@ config SIM_NETDEV_VPNKIT endchoice +config SIM_NETDEV_NUMBER + int "Number of Simulated Network Device" + default 1 + range 1 8 + depends on SIM_NETDEV_TAP + ---help--- + The number of simulated network devices. + + Note that only one network device will be brought up by netinit automatically, + others will be kept in DOWN state by default. + endif config SIM_NETDEV_VPNKIT_PATH diff --git a/arch/sim/src/sim/posix/up_tapdev.c b/arch/sim/src/sim/posix/up_tapdev.c index d56b19286e..2be0568b76 100644 --- a/arch/sim/src/sim/posix/up_tapdev.c +++ b/arch/sim/src/sim/posix/up_tapdev.c @@ -108,14 +108,17 @@ struct sel_arg_struct #ifdef TAPDEV_DEBUG static int gdrop = 0; #endif -static int gtapdevfd = -1; -static char gdevname[IFNAMSIZ]; -static void *g_priv = NULL; -static void (*g_tx_done_intr_cb)(void *priv) = NULL; -static void (*g_rx_ready_intr_cb)(void *priv) = NULL; +static int gtapdevfd[CONFIG_SIM_NETDEV_NUMBER] = +{ + [0 ... CONFIG_SIM_NETDEV_NUMBER - 1] = -1 +}; +static char gdevname[CONFIG_SIM_NETDEV_NUMBER][IFNAMSIZ]; +static void *g_priv[CONFIG_SIM_NETDEV_NUMBER]; +static void (*g_tx_done_intr_cb[CONFIG_SIM_NETDEV_NUMBER])(void *priv); +static void (*g_rx_ready_intr_cb[CONFIG_SIM_NETDEV_NUMBER])(void *priv); #ifdef CONFIG_SIM_NET_HOST_ROUTE -static struct rtentry ghostroute; +static struct rtentry ghostroute[CONFIG_SIM_NETDEV_NUMBER]; #endif /**************************************************************************** @@ -142,7 +145,7 @@ static inline void dump_ethhdr(const char *msg, unsigned char *buf, # define dump_ethhdr(m,b,l) #endif -static void set_macaddr(void) +static void set_macaddr(int devidx) { unsigned char mac[7]; @@ -155,11 +158,15 @@ static void set_macaddr(void) * * With a unique MAC address, we get ALL the packets. * + * The generated MAC addresses will be same if we use timestamp as seed and + * create more than one device at the same time, so add index to make mac + * address different. + * * TODO: The generated MAC address should be checked to see if it * conflicts with something else on the network. */ - srand(time(NULL)); + srand(time(NULL) + devidx); mac[0] = 0x42; mac[1] = rand() % 256; mac[2] = rand() % 256; @@ -168,14 +175,14 @@ static void set_macaddr(void) mac[5] = rand() % 256; mac[6] = 0; - netdriver_setmacaddr(mac); + netdriver_setmacaddr(devidx, mac); } /**************************************************************************** * Public Functions ****************************************************************************/ -void tapdev_init(void *priv, +void tapdev_init(int devidx, void *priv, void (*tx_done_intr_cb)(void *priv), void (*rx_ready_intr_cb)(void *priv)) { @@ -210,7 +217,7 @@ void tapdev_init(void *priv, /* Save the tap device name */ - strncpy(gdevname, ifr.ifr_name, IFNAMSIZ); + strncpy(gdevname[devidx], ifr.ifr_name, IFNAMSIZ); #ifdef CONFIG_SIM_NET_BRIDGE /* Get a socket with which to manipulate the tap device; the remaining @@ -229,14 +236,14 @@ void tapdev_init(void *priv, memset(&ifr, 0, sizeof(ifr)); strncpy(ifr.ifr_name, CONFIG_SIM_NET_BRIDGE_DEVICE, IFNAMSIZ); - ifr.ifr_ifindex = if_nametoindex(gdevname); + ifr.ifr_ifindex = if_nametoindex(gdevname[devidx]); ret = ioctl(sockfd, SIOCBRADDIF, &ifr); if (ret < 0) { syslog(LOG_ERR, "TAPDEV: ioctl failed (can't add interface %s to " "bridge %s): %d\n", - gdevname, CONFIG_SIM_NET_BRIDGE_DEVICE, -ret); + gdevname[devidx], CONFIG_SIM_NET_BRIDGE_DEVICE, -ret); close(sockfd); close(tapdevfd); return; @@ -247,40 +254,40 @@ void tapdev_init(void *priv, if (ret < 0) { syslog(LOG_ERR, "TAPDEV: ioctl failed (can't get MTU " - "from %s): %d\n", gdevname, -ret); + "from %s): %d\n", gdevname[devidx], -ret); close(tapdevfd); return; } else { - netdriver_setmtu(ifr.ifr_mtu); + netdriver_setmtu(devidx, ifr.ifr_mtu); } #endif - gtapdevfd = tapdevfd; - g_priv = priv; + gtapdevfd[devidx] = tapdevfd; + g_priv[devidx] = priv; /* Register the emulated TX done interrupt callback */ - g_tx_done_intr_cb = tx_done_intr_cb; + g_tx_done_intr_cb[devidx] = tx_done_intr_cb; /* Register the emulated RX ready interrupt callback */ - g_rx_ready_intr_cb = rx_ready_intr_cb; + g_rx_ready_intr_cb[devidx] = rx_ready_intr_cb; /* Set the MAC address */ - set_macaddr(); + set_macaddr(devidx); } -int tapdev_avail(void) +int tapdev_avail(int devidx) { struct timeval tv; fd_set fdset; /* We can't do anything if we failed to open the tap device */ - if (gtapdevfd < 0) + if (gtapdevfd[devidx] < 0) { return 0; } @@ -291,21 +298,21 @@ int tapdev_avail(void) tv.tv_usec = 0; FD_ZERO(&fdset); - FD_SET(gtapdevfd, &fdset); + FD_SET(gtapdevfd[devidx], &fdset); - return select(gtapdevfd + 1, &fdset, NULL, NULL, &tv) > 0; + return select(gtapdevfd[devidx] + 1, &fdset, NULL, NULL, &tv) > 0; } -unsigned int tapdev_read(unsigned char *buf, unsigned int buflen) +unsigned int tapdev_read(int devidx, unsigned char *buf, unsigned int buflen) { int ret; - if (!tapdev_avail()) + if (!tapdev_avail(devidx)) { return 0; } - ret = read(gtapdevfd, buf, buflen); + ret = read(gtapdevfd[devidx], buf, buflen); if (ret < 0) { syslog(LOG_ERR, "TAPDEV: read failed: %d\n", -ret); @@ -316,11 +323,11 @@ unsigned int tapdev_read(unsigned char *buf, unsigned int buflen) return ret; } -void tapdev_send(unsigned char *buf, unsigned int buflen) +void tapdev_send(int devidx, unsigned char *buf, unsigned int buflen) { int ret; - if (gtapdevfd < 0) + if (gtapdevfd[devidx] < 0) { return; } @@ -336,7 +343,7 @@ void tapdev_send(unsigned char *buf, unsigned int buflen) } #endif - ret = write(gtapdevfd, buf, buflen); + ret = write(gtapdevfd[devidx], buf, buflen); if (ret < 0) { syslog(LOG_ERR, "TAPDEV: write failed: %d\n", -ret); @@ -347,20 +354,20 @@ void tapdev_send(unsigned char *buf, unsigned int buflen) /* Emulate TX done interrupt */ - if (g_tx_done_intr_cb != NULL) + if (g_tx_done_intr_cb[devidx] != NULL) { - g_tx_done_intr_cb(g_priv); + g_tx_done_intr_cb[devidx](g_priv[devidx]); } /* Emulate RX ready interrupt */ - if (g_rx_ready_intr_cb != NULL && tapdev_avail()) + if (g_rx_ready_intr_cb[devidx] != NULL && tapdev_avail(devidx)) { - g_rx_ready_intr_cb(g_priv); + g_rx_ready_intr_cb[devidx](g_priv[devidx]); } } -void tapdev_ifup(in_addr_t ifaddr) +void tapdev_ifup(int devidx, in_addr_t ifaddr) { struct ifreq ifr; int sockfd; @@ -370,7 +377,7 @@ void tapdev_ifup(in_addr_t ifaddr) struct sockaddr_in *addr; #endif - if (gtapdevfd < 0) + if (gtapdevfd[devidx] < 0) { return; } @@ -386,7 +393,7 @@ void tapdev_ifup(in_addr_t ifaddr) /* Bring the TAP interface up */ - strncpy(ifr.ifr_name, gdevname, IFNAMSIZ); + strncpy(ifr.ifr_name, gdevname[devidx], IFNAMSIZ); ret = ioctl(sockfd, SIOCGIFFLAGS, (unsigned long)&ifr); if (ret < 0) @@ -410,17 +417,17 @@ void tapdev_ifup(in_addr_t ifaddr) #ifdef CONFIG_SIM_NET_HOST_ROUTE /* Add host route */ - memset(&ghostroute, 0, sizeof(ghostroute)); + memset(&ghostroute[devidx], 0, sizeof(ghostroute[devidx])); - addr = (struct sockaddr_in *)&ghostroute.rt_dst; + addr = (struct sockaddr_in *)&ghostroute[devidx].rt_dst; addr->sin_family = AF_INET; addr->sin_addr.s_addr = ifaddr; - ghostroute.rt_dev = gdevname; - ghostroute.rt_flags = RTF_UP | RTF_HOST; - ghostroute.rt_metric = 0; + ghostroute[devidx].rt_dev = gdevname[devidx]; + ghostroute[devidx].rt_flags = RTF_UP | RTF_HOST; + ghostroute[devidx].rt_metric = 0; - ret = ioctl(sockfd, SIOCADDRT, (unsigned long)&ghostroute); + ret = ioctl(sockfd, SIOCADDRT, (unsigned long)&ghostroute[devidx]); if (ret < 0) { syslog(LOG_ERR, "TAPDEV: ioctl failed" @@ -433,18 +440,18 @@ void tapdev_ifup(in_addr_t ifaddr) close(sockfd); } -void tapdev_ifdown(void) +void tapdev_ifdown(int devidx) { #ifdef CONFIG_SIM_NET_HOST_ROUTE int sockfd; int ret; - if (gtapdevfd < 0) + if (gtapdevfd[devidx] < 0) { return; } - if (((struct sockaddr_in *)&ghostroute.rt_dst)->sin_addr.s_addr != 0) + if (((struct sockaddr_in *)&ghostroute[devidx].rt_dst)->sin_addr.s_addr) { /* Get a socket with which to manipulate the tap device */ @@ -455,7 +462,7 @@ void tapdev_ifdown(void) return; } - ret = ioctl(sockfd, SIOCDELRT, (unsigned long)&ghostroute); + ret = ioctl(sockfd, SIOCDELRT, (unsigned long)&ghostroute[devidx]); if (ret < 0) { syslog(LOG_ERR, "TAPDEV: ioctl failed " diff --git a/arch/sim/src/sim/posix/up_vpnkit.c b/arch/sim/src/sim/posix/up_vpnkit.c index 37408eb06c..28b99296ee 100644 --- a/arch/sim/src/sim/posix/up_vpnkit.c +++ b/arch/sim/src/sim/posix/up_vpnkit.c @@ -106,7 +106,7 @@ static int vpnkit_connect(void) INFO("Successfully negotiated with vpnkit"); g_vpnkit_fd = fd; g_connect_warned = false; - netdriver_setmacaddr(g_vifinfo.mac); + netdriver_setmacaddr(0, g_vifinfo.mac); return 0; } diff --git a/arch/sim/src/sim/posix/up_wpcap.c b/arch/sim/src/sim/posix/up_wpcap.c index 7915aa7d88..85018c4060 100644 --- a/arch/sim/src/sim/posix/up_wpcap.c +++ b/arch/sim/src/sim/posix/up_wpcap.c @@ -91,7 +91,7 @@ * definitions and Windows network definitions. */ -void netdriver_setmacaddr(unsigned char *macaddr); +void netdriver_setmacaddr(int devidx, unsigned char *macaddr); /**************************************************************************** * Private Types @@ -254,7 +254,7 @@ static void set_ethaddr(struct in_addr addr) adapters->PhysicalAddress[2], adapters->PhysicalAddress[3], adapters->PhysicalAddress[4], adapters->PhysicalAddress[5]); - netdriver_setmacaddr(adapters->PhysicalAddress); + netdriver_setmacaddr(0, adapters->PhysicalAddress); break; } } diff --git a/arch/sim/src/sim/up_internal.h b/arch/sim/src/sim/up_internal.h index 5c6247e6b1..d417226aa4 100644 --- a/arch/sim/src/sim/up_internal.h +++ b/arch/sim/src/sim/up_internal.h @@ -46,6 +46,10 @@ # define CONFIG_SMP_NCPUS 1 #endif +#ifndef CONFIG_SIM_NETDEV_NUMBER +# define CONFIG_SIM_NETDEV_NUMBER 1 +#endif + /* Determine which (if any) console driver to use */ #ifndef CONFIG_DEV_CONSOLE @@ -258,21 +262,22 @@ int sim_ajoy_initialize(void); /* up_tapdev.c **************************************************************/ #if defined(CONFIG_SIM_NETDEV_TAP) && !defined(__CYGWIN__) -void tapdev_init(void *priv, +void tapdev_init(int devidx, void *priv, void (*tx_done_intr_cb)(void *priv), void (*rx_ready_intr_cb)(void *priv)); -int tapdev_avail(void); -unsigned int tapdev_read(unsigned char *buf, unsigned int buflen); -void tapdev_send(unsigned char *buf, unsigned int buflen); -void tapdev_ifup(in_addr_t ifaddr); -void tapdev_ifdown(void); +int tapdev_avail(int devidx); +unsigned int tapdev_read(int devidx, unsigned char *buf, + unsigned int buflen); +void tapdev_send(int devidx, unsigned char *buf, unsigned int buflen); +void tapdev_ifup(int devidx, in_addr_t ifaddr); +void tapdev_ifdown(int devidx); -# define netdev_init(priv,txcb,rxcb) tapdev_init(priv,txcb,rxcb) -# define netdev_avail() tapdev_avail() -# define netdev_read(buf,buflen) tapdev_read(buf,buflen) -# define netdev_send(buf,buflen) tapdev_send(buf,buflen) -# define netdev_ifup(ifaddr) tapdev_ifup(ifaddr) -# define netdev_ifdown() tapdev_ifdown() +# define netdev_init(idx,priv,txcb,rxcb) tapdev_init(idx,priv,txcb,rxcb) +# define netdev_avail(idx) tapdev_avail(idx) +# define netdev_read(idx,buf,buflen) tapdev_read(idx,buf,buflen) +# define netdev_send(idx,buf,buflen) tapdev_send(idx,buf,buflen) +# define netdev_ifup(idx,ifaddr) tapdev_ifup(idx,ifaddr) +# define netdev_ifdown(idx) tapdev_ifdown(idx) #endif /* up_wpcap.c ***************************************************************/ @@ -284,12 +289,12 @@ void wpcap_init(void *priv, unsigned int wpcap_read(unsigned char *buf, unsigned int buflen); void wpcap_send(unsigned char *buf, unsigned int buflen); -# define netdev_init(priv,txcb,rxcb) wpcap_init(priv,txcb,rxcb) -# define netdev_avail() 1 -# define netdev_read(buf,buflen) wpcap_read(buf,buflen) -# define netdev_send(buf,buflen) wpcap_send(buf,buflen) -# define netdev_ifup(ifaddr) {} -# define netdev_ifdown() {} +# define netdev_init(idx,priv,txcb,rxcb) wpcap_init(priv,txcb,rxcb) +# define netdev_avail(idx) 1 +# define netdev_read(idx,buf,buflen) wpcap_read(buf,buflen) +# define netdev_send(idx,buf,buflen) wpcap_send(buf,buflen) +# define netdev_ifup(idx,ifaddr) {} +# define netdev_ifdown(idx) {} #endif /* up_vpnkit.c **************************************************************/ @@ -302,19 +307,19 @@ int vpnkit_avail(void); unsigned int vpnkit_read(unsigned char *buf, unsigned int buflen); void vpnkit_send(unsigned char *buf, unsigned int buflen); -# define netdev_init(priv,txcb,rxcb) vpnkit_init(priv,txcb,rxcb) -# define netdev_avail() vpnkit_avail() -# define netdev_read(buf,buflen) vpnkit_read(buf,buflen) -# define netdev_send(buf,buflen) vpnkit_send(buf,buflen) -# define netdev_ifup(ifaddr) {} -# define netdev_ifdown() {} +# define netdev_init(idx,priv,txcb,rxcb) vpnkit_init(priv,txcb,rxcb) +# define netdev_avail(idx) vpnkit_avail() +# define netdev_read(idx,buf,buflen) vpnkit_read(buf,buflen) +# define netdev_send(idx,buf,buflen) vpnkit_send(buf,buflen) +# define netdev_ifup(idx,ifaddr) {} +# define netdev_ifdown(idx) {} #endif /* up_netdriver.c ***********************************************************/ int netdriver_init(void); -void netdriver_setmacaddr(unsigned char *macaddr); -void netdriver_setmtu(int mtu); +void netdriver_setmacaddr(int devidx, unsigned char *macaddr); +void netdriver_setmtu(int devidx, int mtu); void netdriver_loop(void); /* up_rptun.c ***************************************************************/ diff --git a/arch/sim/src/sim/up_netdriver.c b/arch/sim/src/sim/up_netdriver.c index bd249bc463..168142730a 100644 --- a/arch/sim/src/sim/up_netdriver.c +++ b/arch/sim/src/sim/up_netdriver.c @@ -62,6 +62,7 @@ #include #include +#include #include #include #include @@ -82,7 +83,7 @@ static struct work_s g_recv_work; /* Ethernet peripheral state */ -static struct net_driver_s g_sim_dev; +static struct net_driver_s g_sim_dev[CONFIG_SIM_NETDEV_NUMBER]; /**************************************************************************** * Private Functions @@ -90,6 +91,10 @@ static struct net_driver_s g_sim_dev; static void netdriver_reply(struct net_driver_s *dev) { + int devidx = (intptr_t)dev->d_private; + + UNUSED(devidx); + /* If the receiving resulted in data that should be sent out on * the network, the field d_len is set to a value > 0. */ @@ -121,7 +126,7 @@ static void netdriver_reply(struct net_driver_s *dev) /* Send the packet */ NETDEV_TXPACKETS(dev); - netdev_send(dev->d_buf, dev->d_len); + netdev_send(devidx, dev->d_buf, dev->d_len); NETDEV_TXDONE(dev); } } @@ -130,6 +135,9 @@ static void netdriver_recv_work(void *arg) { struct net_driver_s *dev = arg; struct eth_hdr_s *eth; + int devidx = (intptr_t)dev->d_private; + + UNUSED(devidx); net_lock(); @@ -137,13 +145,14 @@ static void netdriver_recv_work(void *arg) * to prevent RX data stream congestion. */ - while (netdev_avail()) + while (netdev_avail(devidx)) { /* netdev_read will return 0 on a timeout event and > 0 * on a data received event */ - dev->d_len = netdev_read((unsigned char *)dev->d_buf, + dev->d_len = netdev_read(devidx, + (unsigned char *)dev->d_buf, dev->d_pktsize); if (dev->d_len > 0) { @@ -218,7 +227,7 @@ static void netdriver_recv_work(void *arg) if (dev->d_len > 0) { - netdev_send(dev->d_buf, dev->d_len); + netdev_send(devidx, dev->d_buf, dev->d_len); } } else @@ -241,6 +250,10 @@ static void netdriver_recv_work(void *arg) static int netdriver_txpoll(struct net_driver_s *dev) { + int devidx = (intptr_t)dev->d_private; + + UNUSED(devidx); + /* If the polling resulted in data that should be sent out on the network, * the field d_len is set to a value > 0. */ @@ -274,7 +287,7 @@ static int netdriver_txpoll(struct net_driver_s *dev) /* Send the packet */ NETDEV_TXPACKETS(dev); - netdev_send(dev->d_buf, dev->d_len); + netdev_send(devidx, dev->d_buf, dev->d_len); NETDEV_TXDONE(dev); } } @@ -288,15 +301,21 @@ static int netdriver_txpoll(struct net_driver_s *dev) static int netdriver_ifup(struct net_driver_s *dev) { - netdev_ifup(dev->d_ipaddr); + int devidx = (intptr_t)dev->d_private; + + UNUSED(devidx); + netdev_ifup(devidx, dev->d_ipaddr); netdev_carrier_on(dev); return OK; } static int netdriver_ifdown(struct net_driver_s *dev) { + int devidx = (intptr_t)dev->d_private; + + UNUSED(devidx); netdev_carrier_off(dev); - netdev_ifdown(); + netdev_ifdown(devidx); return OK; } @@ -347,55 +366,71 @@ static void netdriver_rxready_interrupt(void *priv) int netdriver_init(void) { - struct net_driver_s *dev = &g_sim_dev; + struct net_driver_s *dev; void *pktbuf; int pktsize; - - /* Internal initialization */ - - netdev_init(dev, - netdriver_txdone_interrupt, - netdriver_rxready_interrupt); - - /* Update the buffer size */ - - pktsize = dev->d_pktsize ? dev->d_pktsize : - (MAX_NETDEV_PKTSIZE + CONFIG_NET_GUARDSIZE); - - /* Allocate packet buffer */ - - pktbuf = kmm_malloc(pktsize); - if (pktbuf == NULL) + int devidx; + for (devidx = 0; devidx < CONFIG_SIM_NETDEV_NUMBER; devidx++) { - return -ENOMEM; + dev = &g_sim_dev[devidx]; + + /* Internal initialization */ + + netdev_init(devidx, dev, + netdriver_txdone_interrupt, + netdriver_rxready_interrupt); + + /* Update the buffer size */ + + pktsize = dev->d_pktsize ? dev->d_pktsize : + (MAX_NETDEV_PKTSIZE + CONFIG_NET_GUARDSIZE); + + /* Allocate packet buffer */ + + pktbuf = kmm_malloc(pktsize); + if (pktbuf == NULL) + { + return -ENOMEM; + } + + /* Set callbacks */ + + dev->d_buf = pktbuf; + dev->d_ifup = netdriver_ifup; + dev->d_ifdown = netdriver_ifdown; + dev->d_txavail = netdriver_txavail; + dev->d_private = (void *)(intptr_t)devidx; + + /* Register the device with the OS so that socket IOCTLs can be + * performed + */ + + netdev_register(dev, NET_LL_ETHERNET); } - /* Set callbacks */ - - dev->d_buf = pktbuf; - dev->d_ifup = netdriver_ifup; - dev->d_ifdown = netdriver_ifdown; - dev->d_txavail = netdriver_txavail; - - /* Register the device with the OS so that socket IOCTLs can be performed */ - - return netdev_register(dev, NET_LL_ETHERNET); + return OK; } -void netdriver_setmacaddr(unsigned char *macaddr) +void netdriver_setmacaddr(int devidx, unsigned char *macaddr) { - memcpy(g_sim_dev.d_mac.ether.ether_addr_octet, macaddr, IFHWADDRLEN); + memcpy(g_sim_dev[devidx].d_mac.ether.ether_addr_octet, macaddr, + IFHWADDRLEN); } -void netdriver_setmtu(int mtu) +void netdriver_setmtu(int devidx, int mtu) { - g_sim_dev.d_pktsize = mtu; + g_sim_dev[devidx].d_pktsize = mtu; } void netdriver_loop(void) { - if (work_available(&g_recv_work) && netdev_avail()) + int devidx; + for (devidx = 0; devidx < CONFIG_SIM_NETDEV_NUMBER; devidx++) { - work_queue(LPWORK, &g_recv_work, netdriver_recv_work, &g_sim_dev, 0); + if (work_available(&g_recv_work) && netdev_avail(devidx)) + { + work_queue(LPWORK, &g_recv_work, netdriver_recv_work, + &g_sim_dev[devidx], 0); + } } }