/* Copyright (c) 2018, The Linux Foundation. All rights reserved. * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 and * only version 2 as published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "../core/mhi.h" #include "mhi_netdev.h" struct qmap_hdr { u8 cd_rsvd_pad; u8 mux_id; u16 pkt_len; } __packed; #define FIBO_QMAP_MUX_ID 0x81 #ifdef CONFIG_MHI_NETDEV_MBIM #else static uint __read_mostly qmap_mode = 1; module_param(qmap_mode, uint, S_IRUGO); #endif #define MHI_NETDEV_DRIVER_NAME "mhi_netdev" #define WATCHDOG_TIMEOUT (30 * HZ) #define MSG_VERB(fmt, ...) do { \ if (mhi_netdev->msg_lvl <= MHI_MSG_LVL_VERBOSE) \ pr_err("[D][%s] " fmt, __func__, ##__VA_ARGS__);\ } while (0) #define MHI_ASSERT(cond, msg) do { \ if (cond) { \ MSG_ERR(msg); \ WARN_ON(cond); \ } \ } while (0) #define MSG_LOG(fmt, ...) do { \ if (mhi_netdev->msg_lvl <= MHI_MSG_LVL_INFO) \ pr_err("[I][%s] " fmt, __func__, ##__VA_ARGS__);\ } while (0) #define MSG_ERR(fmt, ...) do { \ if (mhi_netdev->msg_lvl <= MHI_MSG_LVL_ERROR) \ pr_err("[E][%s] " fmt, __func__, ##__VA_ARGS__); \ } while (0) struct mhi_stats { u32 rx_int; u32 tx_full; u32 tx_pkts; u32 rx_budget_overflow; u32 rx_frag; u32 alloc_failed; }; /* important: do not exceed sk_buf->cb (48 bytes) */ struct mhi_skb_priv { void *buf; size_t size; struct mhi_netdev *mhi_netdev; }; struct mhi_netdev { int alias; struct mhi_device *mhi_dev; spinlock_t rx_lock; bool enabled; rwlock_t pm_lock; /* state change lock */ int (*rx_queue)(struct mhi_netdev *mhi_netdev, gfp_t gfp_t); struct work_struct alloc_work; int wake; struct sk_buff_head rx_allocated; u32 mru; const char *interface_name; struct napi_struct napi; struct net_device *ndev; struct sk_buff *frag_skb; bool recycle_buf; struct mhi_stats stats; struct dentry *dentry; enum MHI_DEBUG_LEVEL msg_lvl; #ifdef CONFIG_MHI_NETDEV_MBIM u16 tx_seq; u16 rx_seq; u32 rx_max; #endif }; struct mhi_netdev_priv { struct mhi_netdev *mhi_netdev; }; static struct mhi_driver mhi_netdev_driver; static void mhi_netdev_create_debugfs(struct mhi_netdev *mhi_netdev); static struct mhi_netdev * g_mhi_netdev = NULL; static inline void qmap_hex_dump(const char *tag, unsigned char *data, unsigned len) { //#define MHI_NETDEV_DEBUG #ifdef MHI_NETDEV_DEBUG if (g_mhi_netdev && g_mhi_netdev->msg_lvl > MHI_MSG_LVL_CRITICAL) { int i; printk("dump %s,%s:len=%d \n", tag, g_mhi_netdev->ndev->name, len); for (i = 0; i < len; i++) { printk(" 0x%02x", data[i]); if (((i+1) % 16) == 0) { printk("\n"); } } printk("\n"); } #endif } static int macaddr_check = 0; static int mhi_netdev_macaddr_check_get(char * buffer, const struct kernel_param * kp) { char mac_str[32]; if (g_mhi_netdev == NULL) { return sprintf(buffer, "%s\n", "null");; } snprintf(mac_str, sizeof(mac_str), "%02x:%02x:%02x:%02x:%02x:%02x\n", g_mhi_netdev->ndev->dev_addr[0], g_mhi_netdev->ndev->dev_addr[1], g_mhi_netdev->ndev->dev_addr[2], g_mhi_netdev->ndev->dev_addr[3], g_mhi_netdev->ndev->dev_addr[4], g_mhi_netdev->ndev->dev_addr[5]); return sprintf(buffer, "%s", mac_str); } static int mhi_netdev_macaddr_check_set(const char * val, const struct kernel_param * kp) { if (g_mhi_netdev == NULL) { return 0; } if (val[0] == '1') { if (!is_valid_ether_addr(g_mhi_netdev->ndev->dev_addr)) { eth_random_addr(g_mhi_netdev->ndev->dev_addr); g_mhi_netdev->ndev->addr_assign_type = NET_ADDR_RANDOM; if (!is_valid_ether_addr(g_mhi_netdev->ndev->dev_addr)) { eth_random_addr(g_mhi_netdev->ndev->dev_addr); } else { printk("invalid ether addr\n"); } } return 0; } return -EINVAL; } module_param_call(macaddr_check, mhi_netdev_macaddr_check_set, mhi_netdev_macaddr_check_get, &macaddr_check, 0644); static void mhi_netdev_skb_destructor(struct sk_buff *skb) { struct mhi_skb_priv *skb_priv = (struct mhi_skb_priv *)(skb->cb); struct mhi_netdev *mhi_netdev = skb_priv->mhi_netdev; skb->data = skb->head; skb_reset_tail_pointer(skb); skb->len = 0; MHI_ASSERT(skb->data != skb_priv->buf, "incorrect buf"); skb_queue_tail(&mhi_netdev->rx_allocated, skb); } static int mhi_netdev_alloc_skb(struct mhi_netdev *mhi_netdev, gfp_t gfp_t) { u32 cur_mru = mhi_netdev->mru; struct mhi_device *mhi_dev = mhi_netdev->mhi_dev; struct mhi_skb_priv *skb_priv; int ret; struct sk_buff *skb; int no_tre = mhi_get_no_free_descriptors(mhi_dev, DMA_FROM_DEVICE); int i; for (i = 0; i < no_tre; i++) { skb = alloc_skb(cur_mru + ETH_HLEN, gfp_t); if (!skb) return -ENOMEM; skb_reserve(skb, ETH_HLEN); read_lock_bh(&mhi_netdev->pm_lock); if (unlikely(!mhi_netdev->enabled)) { MSG_ERR("Interface not enabled\n"); ret = -EIO; goto error_queue; } skb_priv = (struct mhi_skb_priv *)skb->cb; skb_priv->buf = skb->data; skb_priv->size = cur_mru; skb_priv->mhi_netdev = mhi_netdev; skb->dev = mhi_netdev->ndev; if (mhi_netdev->recycle_buf) skb->destructor = mhi_netdev_skb_destructor; spin_lock_bh(&mhi_netdev->rx_lock); ret = mhi_queue_transfer(mhi_dev, DMA_FROM_DEVICE, skb, skb_priv->size, MHI_EOT); spin_unlock_bh(&mhi_netdev->rx_lock); if (ret) { MSG_ERR("Failed to queue skb, ret:%d\n", ret); ret = -EIO; goto error_queue; } read_unlock_bh(&mhi_netdev->pm_lock); } return 0; error_queue: skb->destructor = NULL; read_unlock_bh(&mhi_netdev->pm_lock); dev_kfree_skb_any(skb); return ret; } static void mhi_netdev_alloc_work(struct work_struct *work) { struct mhi_netdev *mhi_netdev = container_of(work, struct mhi_netdev, alloc_work); /* sleep about 1 sec and retry, that should be enough time * for system to reclaim freed memory back. */ const int sleep_ms = 1000; int retry = 60; int ret; MSG_LOG("Entered\n"); do { ret = mhi_netdev_alloc_skb(mhi_netdev, GFP_KERNEL); /* sleep and try again */ if (ret == -ENOMEM) { msleep(sleep_ms); retry--; } } while (ret == -ENOMEM && retry); MSG_LOG("Exit with status:%d retry:%d\n", ret, retry); } /* we will recycle buffers */ static int mhi_netdev_skb_recycle(struct mhi_netdev *mhi_netdev, gfp_t gfp_t) { struct mhi_device *mhi_dev = mhi_netdev->mhi_dev; int no_tre; int ret = 0; struct sk_buff *skb; struct mhi_skb_priv *skb_priv; read_lock_bh(&mhi_netdev->pm_lock); if (!mhi_netdev->enabled) { read_unlock_bh(&mhi_netdev->pm_lock); return -EIO; } no_tre = mhi_get_no_free_descriptors(mhi_dev, DMA_FROM_DEVICE); spin_lock_bh(&mhi_netdev->rx_lock); while (no_tre) { skb = skb_dequeue(&mhi_netdev->rx_allocated); /* no free buffers to recycle, reschedule work */ if (!skb) { ret = -ENOMEM; goto error_queue; } skb_priv = (struct mhi_skb_priv *)(skb->cb); ret = mhi_queue_transfer(mhi_dev, DMA_FROM_DEVICE, skb, skb_priv->size, MHI_EOT); /* failed to queue buffer */ if (ret) { MSG_ERR("Failed to queue skb, ret:%d\n", ret); skb_queue_tail(&mhi_netdev->rx_allocated, skb); goto error_queue; } no_tre--; } error_queue: spin_unlock_bh(&mhi_netdev->rx_lock); read_unlock_bh(&mhi_netdev->pm_lock); return ret; } static void mhi_netdev_dealloc(struct mhi_netdev *mhi_netdev) { struct sk_buff *skb; skb = skb_dequeue(&mhi_netdev->rx_allocated); while (skb) { skb->destructor = NULL; kfree_skb(skb); skb = skb_dequeue(&mhi_netdev->rx_allocated); } } static int mhi_netdev_poll(struct napi_struct *napi, int budget) { struct net_device *dev = napi->dev; struct mhi_netdev_priv *mhi_netdev_priv = netdev_priv(dev); struct mhi_netdev *mhi_netdev = mhi_netdev_priv->mhi_netdev; struct mhi_device *mhi_dev = mhi_netdev->mhi_dev; int rx_work = 0; int ret; MSG_VERB("Entered\n"); read_lock_bh(&mhi_netdev->pm_lock); if (!mhi_netdev->enabled) { MSG_LOG("interface is disabled!\n"); napi_complete(napi); read_unlock_bh(&mhi_netdev->pm_lock); return 0; } mhi_device_get(mhi_dev); rx_work = mhi_poll(mhi_dev, budget); if (rx_work < 0) { MSG_ERR("Error polling ret:%d\n", rx_work); rx_work = 0; napi_complete(napi); goto exit_poll; } /* queue new buffers */ ret = mhi_netdev->rx_queue(mhi_netdev, GFP_ATOMIC); if (ret == -ENOMEM) { MSG_LOG("out of tre, queuing bg worker\n"); mhi_netdev->stats.alloc_failed++; schedule_work(&mhi_netdev->alloc_work); } /* complete work if # of packet processed less than allocated budget */ if (rx_work < budget) napi_complete(napi); else mhi_netdev->stats.rx_budget_overflow++; exit_poll: mhi_device_put(mhi_dev); read_unlock_bh(&mhi_netdev->pm_lock); MSG_VERB("polled %d pkts\n", rx_work); return rx_work; } static int mhi_netdev_open(struct net_device *dev) { struct mhi_netdev_priv *mhi_netdev_priv = netdev_priv(dev); struct mhi_netdev *mhi_netdev = mhi_netdev_priv->mhi_netdev; struct mhi_device *mhi_dev = mhi_netdev->mhi_dev; MSG_LOG("Opened net dev interface\n"); /* tx queue may not necessarily be stopped already * so stop the queue if tx path is not enabled */ if (!mhi_dev->ul_chan) netif_stop_queue(dev); else netif_start_queue(dev); return 0; } static int mhi_netdev_change_mtu(struct net_device *dev, int new_mtu) { struct mhi_netdev_priv *mhi_netdev_priv = netdev_priv(dev); struct mhi_netdev *mhi_netdev = mhi_netdev_priv->mhi_netdev; struct mhi_device *mhi_dev = mhi_netdev->mhi_dev; if (new_mtu < 0 || mhi_dev->mtu < new_mtu) return -EINVAL; dev->mtu = new_mtu; return 0; } #ifdef CONFIG_MHI_NETDEV_MBIM static struct sk_buff *mhi_mbim_tx_fixup(struct mhi_netdev *mhi_netdev, struct sk_buff *skb, struct net_device *dev) { struct usb_cdc_ncm_nth16 *nth16; struct usb_cdc_ncm_ndp16 *ndp16; __le32 sign; u8 *c; u16 tci = 0; unsigned int skb_len; qmap_hex_dump(__func__, skb->data, skb->len); if (skb->len > VLAN_ETH_HLEN && __vlan_get_tag(skb, &tci) == 0) { skb_pull(skb, VLAN_ETH_HLEN); } else { skb_pull(skb, ETH_HLEN); } skb_len = skb->len; if (skb_headroom(skb) < sizeof(struct usb_cdc_ncm_nth16)) { printk("skb_headroom small!\n"); return NULL; } if (skb_tailroom(skb) < (sizeof(struct usb_cdc_ncm_ndp16) + sizeof(struct usb_cdc_ncm_dpe16) * 2)) { if (skb_pad(skb, (sizeof(struct usb_cdc_ncm_ndp16) + sizeof(struct usb_cdc_ncm_dpe16) * 2))) { printk("skb_tailroom small!\n"); return NULL; } } skb_push(skb, sizeof(struct usb_cdc_ncm_nth16)); skb_put(skb, sizeof(struct usb_cdc_ncm_ndp16) + sizeof(struct usb_cdc_ncm_dpe16) * 2); nth16 = (struct usb_cdc_ncm_nth16 *)skb->data; nth16->dwSignature = cpu_to_le32(USB_CDC_NCM_NTH16_SIGN); nth16->wHeaderLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16)); nth16->wSequence = cpu_to_le16(mhi_netdev->tx_seq++); nth16->wBlockLength = cpu_to_le16(skb->len); nth16->wNdpIndex = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16) + skb_len); sign = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN); c = (u8 *)&sign; //tci = 0; c[3] = tci; ndp16 = (struct usb_cdc_ncm_ndp16 *)(skb->data + nth16->wNdpIndex); ndp16->dwSignature = sign; ndp16->wLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_ndp16) + sizeof(struct usb_cdc_ncm_dpe16) * 2); ndp16->wNextNdpIndex = 0; ndp16->dpe16[0].wDatagramIndex = sizeof(struct usb_cdc_ncm_nth16); ndp16->dpe16[0].wDatagramLength = skb_len; ndp16->dpe16[1].wDatagramIndex = 0; ndp16->dpe16[1].wDatagramLength = 0; return skb; } static int mhi_mbim_rx_fixup(struct mhi_netdev *mhi_netdev, struct sk_buff *skb_in, struct net_device *dev) { struct usb_cdc_ncm_nth16 *nth16; int ndpoffset, len; u16 wSequence; struct mhi_netdev *ctx = mhi_netdev; if (skb_in->len < (sizeof(struct usb_cdc_ncm_nth16) + sizeof(struct usb_cdc_ncm_ndp16))) { MSG_ERR("frame too short\n"); goto error; } nth16 = (struct usb_cdc_ncm_nth16 *)skb_in->data; if (nth16->dwSignature != cpu_to_le32(USB_CDC_NCM_NTH16_SIGN)) { MSG_ERR("invalid NTH16 signature <%#010x>\n", le32_to_cpu(nth16->dwSignature)); goto error; } len = le16_to_cpu(nth16->wBlockLength); if (len > ctx->rx_max) { MSG_ERR("unsupported NTB block length %u/%u\n", len, ctx->rx_max); goto error; } wSequence = le16_to_cpu(nth16->wSequence); if (ctx->rx_seq != wSequence) { MSG_ERR("sequence number glitch prev=%d curr=%d\n", ctx->rx_seq, wSequence); } ctx->rx_seq = wSequence + 1; ndpoffset = nth16->wNdpIndex; while (ndpoffset > 0) { struct usb_cdc_ncm_ndp16 *ndp16 ; struct usb_cdc_ncm_dpe16 *dpe16; int nframes, x; u8 *c; u16 tci = 0; if (skb_in->len < (ndpoffset + sizeof(struct usb_cdc_ncm_ndp16))) { MSG_ERR("invalid NDP offset <%u>\n", ndpoffset); goto error; } ndp16 = (struct usb_cdc_ncm_ndp16 *)(skb_in->data + ndpoffset); if (le16_to_cpu(ndp16->wLength) < 0x10) { MSG_ERR("invalid DPT16 length <%u>\n", le16_to_cpu(ndp16->wLength)); goto error; } nframes = ((le16_to_cpu(ndp16->wLength) - sizeof(struct usb_cdc_ncm_ndp16)) / sizeof(struct usb_cdc_ncm_dpe16)); if (skb_in->len < (sizeof(struct usb_cdc_ncm_ndp16) + nframes * (sizeof(struct usb_cdc_ncm_dpe16)))) { MSG_ERR("Invalid nframes = %d\n", nframes); goto error; } switch (ndp16->dwSignature & cpu_to_le32(0x00ffffff)) { case cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN): c = (u8 *)&ndp16->dwSignature; tci = c[3]; /* tag IPS<0> packets too if MBIM_IPS0_VID exists */ //if (!tci && info->flags & FLAG_IPS0_VLAN) // tci = MBIM_IPS0_VID; break; case cpu_to_le32(USB_CDC_MBIM_NDP16_DSS_SIGN): c = (u8 *)&ndp16->dwSignature; tci = c[3] + 256; break; default: MSG_ERR("unsupported NDP signature <0x%08x>\n", le32_to_cpu(ndp16->dwSignature)); goto error; } #if 0 if (tci != 0) { MSG_ERR("unsupported tci %d by now\n", tci); goto error; } #endif dpe16 = ndp16->dpe16; for (x = 0; x < nframes; x++, dpe16++) { int offset = le16_to_cpu(dpe16->wDatagramIndex); int skb_len = le16_to_cpu(dpe16->wDatagramLength); struct sk_buff *skb; if (offset == 0 || skb_len == 0) { break; } /* sanity checking */ if (((offset + skb_len) > skb_in->len) || (skb_len > ctx->rx_max)) { MSG_ERR("invalid frame detected (ignored) x=%d, offset=%d, skb_len=%u\n", x, offset, skb_len); goto error; } skb = skb_clone(skb_in, GFP_ATOMIC); if (!skb) { MSG_ERR("skb_clone fail\n"); goto error; } skb_pull(skb, offset); skb_trim(skb, skb_len); switch (skb->data[0] & 0xf0) { case 0x40: skb->protocol = htons(ETH_P_IP); break; case 0x60: skb->protocol = htons(ETH_P_IPV6); break; default: MSG_ERR("unknow skb->protocol %02x\n", skb->data[0]); goto error; } skb_reset_mac_header(skb); /* map MBIM session to VLAN */ if (tci) __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), tci); netif_receive_skb(skb); } /* are there more NDPs to process? */ ndpoffset = le16_to_cpu(ndp16->wNextNdpIndex); } return 1; error: MSG_ERR("%s error\n", __func__); return 0; } #else static struct sk_buff *mhi_qmap_tx_fixup(struct mhi_netdev *mhi_netdev, struct sk_buff *skb, struct net_device *dev) { struct qmap_hdr *qhdr; u16 tci = 0; if (skb->len > VLAN_ETH_HLEN && __vlan_get_tag(skb, &tci) == 0) { skb_pull(skb, VLAN_ETH_HLEN); } else { skb_pull(skb, ETH_HLEN); } if (skb_headroom(skb) < sizeof(struct qmap_hdr)) { printk("skb_headroom small!\n"); return NULL; } qhdr = (struct qmap_hdr *)skb_push(skb, sizeof(struct qmap_hdr)); qhdr->cd_rsvd_pad = 0; qhdr->mux_id = FIBO_QMAP_MUX_ID + tci; qhdr->pkt_len = cpu_to_be16(skb->len - sizeof(struct qmap_hdr)); return skb; } static int mhi_qmap_rx_fixup(struct mhi_netdev *mhi_netdev, struct sk_buff *skb_in, struct net_device *dev) { while (skb_in->len > sizeof(struct qmap_hdr)) { struct qmap_hdr *qhdr = (struct qmap_hdr *)skb_in->data; struct sk_buff *skb = NULL; int pkt_len = be16_to_cpu(qhdr->pkt_len); u16 tci = qhdr->mux_id - FIBO_QMAP_MUX_ID; int skb_len; int ret; if (skb_in->len < (pkt_len + sizeof(struct qmap_hdr))) { MSG_ERR("drop qmap unknow pkt, len=%d, pkt_len=%d\n", skb_in->len, pkt_len); goto error; } if (qhdr->cd_rsvd_pad & 0x80) { MSG_ERR("drop qmap command packet %x\n", qhdr->cd_rsvd_pad); skb_pull(skb_in, pkt_len + sizeof(struct qmap_hdr)); continue; } skb_len = pkt_len - (qhdr->cd_rsvd_pad&0x3F); skb = netdev_alloc_skb_ip_align(dev, skb_len + ETH_HLEN); if (!skb) { MSG_ERR("netdev_alloc_skb_ip_align fail\n"); goto error; } switch (skb_in->data[sizeof(struct qmap_hdr)] & 0xf0) { case 0x40: { skb->protocol = htons(ETH_P_IP); break; } case 0x60: { skb->protocol = htons(ETH_P_IPV6); break; } default: { MSG_ERR("unknow skb->protocol %02x\n", skb->data[0]); kfree_skb(skb); goto error; } } /* add an ethernet header */ skb_put(skb, ETH_HLEN); skb_reset_mac_header(skb); eth_hdr(skb)->h_proto = skb->protocol;; eth_zero_addr(eth_hdr(skb)->h_source); memcpy(eth_hdr(skb)->h_dest, dev->dev_addr, ETH_ALEN); /* add datagram */ #if (LINUX_VERSION_CODE < KERNEL_VERSION( 4,15,0 )) fibo_skb_put_data(skb, skb_in->data + sizeof(struct qmap_hdr), skb_len); #else skb_put_data(skb, skb_in->data + sizeof(struct qmap_hdr), skb_len); #endif skb_pull(skb, ETH_HLEN); /* map MBIM session to VLAN */ if (tci) __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), tci); ret = netif_receive_skb(skb); skb_pull(skb_in, pkt_len + sizeof(struct qmap_hdr)); } return 1; error: MSG_ERR("%s error\n", __func__); return 0; } #endif static int mhi_netdev_xmit(struct sk_buff *skb, struct net_device *dev) { struct mhi_netdev_priv *mhi_netdev_priv = netdev_priv(dev); struct mhi_netdev *mhi_netdev = mhi_netdev_priv->mhi_netdev; struct mhi_device *mhi_dev = mhi_netdev->mhi_dev; int res = 0; struct mhi_skb_priv *tx_priv; MSG_VERB("Entered\n"); tx_priv = (struct mhi_skb_priv *)(skb->cb); tx_priv->mhi_netdev = mhi_netdev; read_lock_bh(&mhi_netdev->pm_lock); if (unlikely(!mhi_netdev->enabled)) { /* Only reason interface could be disabled and we get data * is due to an SSR. We do not want to stop the queue and * return error. Instead we will flush all the uplink packets * and return successful */ res = NETDEV_TX_OK; dev_kfree_skb_any(skb); goto mhi_xmit_exit; } #ifdef CONFIG_MHI_NETDEV_MBIM if (mhi_mbim_tx_fixup(mhi_netdev, skb, dev) == NULL) { res = NETDEV_TX_OK; dev_kfree_skb_any(skb); goto mhi_xmit_exit; } #else if (qmap_mode) { if (mhi_qmap_tx_fixup(mhi_netdev, skb, dev) == NULL) { res = NETDEV_TX_OK; dev_kfree_skb_any(skb); goto mhi_xmit_exit; } } #endif qmap_hex_dump(__func__, skb->data, skb->len); res = mhi_queue_transfer(mhi_dev, DMA_TO_DEVICE, skb, skb->len, MHI_EOT); if (res) { MSG_VERB("Failed to queue with reason:%d\n", res); netif_stop_queue(dev); mhi_netdev->stats.tx_full++; res = NETDEV_TX_BUSY; goto mhi_xmit_exit; } mhi_netdev->stats.tx_pkts++; mhi_xmit_exit: read_unlock_bh(&mhi_netdev->pm_lock); MSG_VERB("Exited\n"); return res; } static const struct net_device_ops mhi_netdev_ops_ip = { .ndo_open = mhi_netdev_open, .ndo_start_xmit = mhi_netdev_xmit, .ndo_change_mtu = mhi_netdev_change_mtu, .ndo_set_mac_address = eth_mac_addr, .ndo_validate_addr = eth_validate_addr, }; static void mhi_netdev_setup(struct net_device *dev) { dev->netdev_ops = &mhi_netdev_ops_ip; ether_setup(dev); /* set this after calling ether_setup */ dev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST; dev->watchdog_timeo = WATCHDOG_TIMEOUT; } /* enable mhi_netdev netdev, call only after grabbing mhi_netdev.mutex */ static int mhi_netdev_enable_iface(struct mhi_netdev *mhi_netdev) { int ret = 0; char ifname[IFNAMSIZ]; struct mhi_device *mhi_dev = mhi_netdev->mhi_dev; int no_tre; MSG_LOG("Prepare the channels for transfer\n"); ret = mhi_prepare_for_transfer(mhi_dev); if (ret) { MSG_ERR("Failed to start TX chan ret %d\n", ret); goto mhi_failed_to_start; } /* first time enabling the node */ if (!mhi_netdev->ndev) { struct mhi_netdev_priv *mhi_netdev_priv; snprintf(ifname, sizeof(ifname), "%s%%d", mhi_netdev->interface_name); rtnl_lock(); #ifdef NET_NAME_PREDICTABLE mhi_netdev->ndev = alloc_netdev(sizeof(*mhi_netdev_priv), ifname, NET_NAME_PREDICTABLE, mhi_netdev_setup); #else mhi_netdev->ndev = alloc_netdev(sizeof(*mhi_netdev_priv), ifname, mhi_netdev_setup); #endif if (!mhi_netdev->ndev) { ret = -ENOMEM; rtnl_unlock(); goto net_dev_alloc_fail; } //mhi_netdev->ndev->mtu = mhi_dev->mtu; SET_NETDEV_DEV(mhi_netdev->ndev, &mhi_dev->dev); mhi_netdev_priv = netdev_priv(mhi_netdev->ndev); mhi_netdev_priv->mhi_netdev = mhi_netdev; rtnl_unlock(); netif_napi_add(mhi_netdev->ndev, &mhi_netdev->napi, mhi_netdev_poll, NAPI_POLL_WEIGHT); ret = register_netdev(mhi_netdev->ndev); if (ret) { MSG_ERR("Network device registration failed\n"); goto net_dev_reg_fail; } skb_queue_head_init(&mhi_netdev->rx_allocated); } write_lock_irq(&mhi_netdev->pm_lock); mhi_netdev->enabled = true; write_unlock_irq(&mhi_netdev->pm_lock); /* queue buffer for rx path */ no_tre = mhi_get_no_free_descriptors(mhi_dev, DMA_FROM_DEVICE); ret = mhi_netdev_alloc_skb(mhi_netdev, GFP_KERNEL); if (ret) schedule_work(&mhi_netdev->alloc_work); /* if we recycle prepare one more set */ if (mhi_netdev->recycle_buf) for (; no_tre >= 0; no_tre--) { struct sk_buff *skb = alloc_skb(mhi_netdev->mru + ETH_HLEN, GFP_KERNEL); struct mhi_skb_priv *skb_priv; if (!skb) break; skb_reserve(skb, ETH_HLEN); skb_priv = (struct mhi_skb_priv *)skb->cb; skb_priv->buf = skb->data; skb_priv->size = mhi_netdev->mru; skb_priv->mhi_netdev = mhi_netdev; skb->dev = mhi_netdev->ndev; skb->destructor = mhi_netdev_skb_destructor; skb_queue_tail(&mhi_netdev->rx_allocated, skb); } napi_enable(&mhi_netdev->napi); MSG_LOG("Exited.\n"); return 0; net_dev_reg_fail: netif_napi_del(&mhi_netdev->napi); free_netdev(mhi_netdev->ndev); mhi_netdev->ndev = NULL; net_dev_alloc_fail: mhi_unprepare_from_transfer(mhi_dev); mhi_failed_to_start: MSG_ERR("Exited ret %d.\n", ret); return ret; } static void mhi_netdev_xfer_ul_cb(struct mhi_device *mhi_dev, struct mhi_result *mhi_result) { struct mhi_netdev *mhi_netdev = mhi_device_get_devdata(mhi_dev); struct sk_buff *skb = mhi_result->buf_addr; struct net_device *ndev = mhi_netdev->ndev; ndev->stats.tx_packets++; ndev->stats.tx_bytes += skb->len; dev_kfree_skb(skb); if (netif_queue_stopped(ndev)) netif_wake_queue(ndev); } static int mhi_netdev_process_fragment(struct mhi_netdev *mhi_netdev, struct sk_buff *skb) { struct sk_buff *temp_skb; if (mhi_netdev->frag_skb) { /* merge the new skb into the old fragment */ temp_skb = skb_copy_expand(mhi_netdev->frag_skb, ETH_HLEN, skb->len, GFP_ATOMIC); if (!temp_skb) { dev_kfree_skb(mhi_netdev->frag_skb); mhi_netdev->frag_skb = NULL; return -ENOMEM; } dev_kfree_skb_any(mhi_netdev->frag_skb); mhi_netdev->frag_skb = temp_skb; memcpy(skb_put(mhi_netdev->frag_skb, skb->len), skb->data, skb->len); } else { mhi_netdev->frag_skb = skb_copy(skb, GFP_ATOMIC); if (!mhi_netdev->frag_skb) return -ENOMEM; } mhi_netdev->stats.rx_frag++; return 0; } static void mhi_netdev_xfer_dl_cb(struct mhi_device *mhi_dev, struct mhi_result *mhi_result) { struct mhi_netdev *mhi_netdev = mhi_device_get_devdata(mhi_dev); struct sk_buff *skb = mhi_result->buf_addr; struct net_device *dev = mhi_netdev->ndev; int ret = 0; static size_t bytes_xferd = 0; if (mhi_result->transaction_status == -ENOTCONN) { dev_kfree_skb(skb); return; } if (mhi_result->bytes_xferd > bytes_xferd) { bytes_xferd = mhi_result->bytes_xferd; //printk("%s bytes_xferd=%zd\n", __func__, bytes_xferd); } skb_put(skb, mhi_result->bytes_xferd); dev->stats.rx_packets++; dev->stats.rx_bytes += mhi_result->bytes_xferd; /* merge skb's together, it's a chain transfer */ if (mhi_result->transaction_status == -EOVERFLOW || mhi_netdev->frag_skb) { ret = mhi_netdev_process_fragment(mhi_netdev, skb); /* recycle the skb */ if (mhi_netdev->recycle_buf) mhi_netdev_skb_destructor(skb); else dev_kfree_skb(skb); if (ret) return; } /* more data will come, don't submit the buffer */ if (mhi_result->transaction_status == -EOVERFLOW) return; if (mhi_netdev->frag_skb) { skb = mhi_netdev->frag_skb; skb->dev = dev; mhi_netdev->frag_skb = NULL; } qmap_hex_dump(__func__, skb->data, skb->len); #ifdef CONFIG_MHI_NETDEV_MBIM mhi_mbim_rx_fixup(mhi_netdev, skb, dev); dev_kfree_skb_any(skb); #else if (qmap_mode) { mhi_qmap_rx_fixup(mhi_netdev, skb, dev); dev_kfree_skb_any(skb); } else { switch (skb->data[0] & 0xf0) { case 0x40: skb->protocol = htons(ETH_P_IP); netif_receive_skb(skb); break; case 0x60: skb->protocol = htons(ETH_P_IPV6); netif_receive_skb(skb); break; default: break; } } #endif mhi_netdev->rx_queue(mhi_netdev, GFP_ATOMIC); } static void mhi_netdev_status_cb(struct mhi_device *mhi_dev, enum MHI_CB mhi_cb) { struct mhi_netdev *mhi_netdev = mhi_device_get_devdata(mhi_dev); if (mhi_cb != MHI_CB_PENDING_DATA) return; if (napi_schedule_prep(&mhi_netdev->napi)) { __napi_schedule(&mhi_netdev->napi); mhi_netdev->stats.rx_int++; return; } } #ifdef CONFIG_DEBUG_FS struct dentry *mhi_netdev_debugfs_dentry; static int mhi_netdev_debugfs_trigger_reset(void *data, u64 val) { struct mhi_netdev *mhi_netdev = data; struct mhi_device *mhi_dev = mhi_netdev->mhi_dev; int ret; MSG_LOG("Triggering channel reset\n"); /* disable the interface so no data processing */ write_lock_irq(&mhi_netdev->pm_lock); mhi_netdev->enabled = false; write_unlock_irq(&mhi_netdev->pm_lock); napi_disable(&mhi_netdev->napi); /* disable all hardware channels */ mhi_unprepare_from_transfer(mhi_dev); /* clean up all alocated buffers */ mhi_netdev_dealloc(mhi_netdev); MSG_LOG("Restarting iface\n"); ret = mhi_netdev_enable_iface(mhi_netdev); if (ret) return ret; return 0; } DEFINE_SIMPLE_ATTRIBUTE(mhi_netdev_debugfs_trigger_reset_fops, NULL, mhi_netdev_debugfs_trigger_reset, "%llu\n"); static void mhi_netdev_create_debugfs(struct mhi_netdev *mhi_netdev) { char node_name[32]; int i; const umode_t mode = 0600; struct dentry *file; struct mhi_device *mhi_dev = mhi_netdev->mhi_dev; struct dentry *dentry = mhi_netdev_debugfs_dentry; const struct { char *name; u32 *ptr; } debugfs_table[] = { { "rx_int", &mhi_netdev->stats.rx_int }, { "tx_full", &mhi_netdev->stats.tx_full }, { "tx_pkts", &mhi_netdev->stats.tx_pkts }, { "rx_budget_overflow", &mhi_netdev->stats.rx_budget_overflow }, { "rx_fragmentation", &mhi_netdev->stats.rx_frag }, { "alloc_failed", &mhi_netdev->stats.alloc_failed }, { NULL, NULL }, }; /* Both tx & rx client handle contain same device info */ snprintf(node_name, sizeof(node_name), "%s_%04x_%02u.%02u.%02u_%u", mhi_netdev->interface_name, mhi_dev->dev_id, mhi_dev->domain, mhi_dev->bus, mhi_dev->slot, mhi_netdev->alias); if (IS_ERR_OR_NULL(dentry)) return; mhi_netdev->dentry = debugfs_create_dir(node_name, dentry); if (IS_ERR_OR_NULL(mhi_netdev->dentry)) return; /*begin added by tony.du for mantis 0062018 on 2020-11-10*/ debugfs_create_u32("msg_lvl", mode, mhi_netdev->dentry, (u32 *)&mhi_netdev->msg_lvl); /*end added by tony.du for mantis 0062018 on 2020-11-10*/ /* Add debug stats table */ for (i = 0; debugfs_table[i].name; i++) { /*begin added by tony.du for mantis 0062018 on 2020-11-10*/ debugfs_create_u32(debugfs_table[i].name, mode, mhi_netdev->dentry, debugfs_table[i].ptr); /*end added by tony.du for mantis 0062018 on 2020-11-10*/ } debugfs_create_file("reset", mode, mhi_netdev->dentry, mhi_netdev, &mhi_netdev_debugfs_trigger_reset_fops); } static void mhi_netdev_create_debugfs_dir(struct dentry *parent) { mhi_netdev_debugfs_dentry = debugfs_create_dir(MHI_NETDEV_DRIVER_NAME, parent); } #else static void mhi_netdev_create_debugfs(struct mhi_netdev *mhi_netdev) { } static void mhi_netdev_create_debugfs_dir(struct dentry *parent) { } #endif static void mhi_netdev_remove(struct mhi_device *mhi_dev) { struct mhi_netdev *mhi_netdev = mhi_device_get_devdata(mhi_dev); MSG_LOG("Remove notification received\n"); write_lock_irq(&mhi_netdev->pm_lock); mhi_netdev->enabled = false; write_unlock_irq(&mhi_netdev->pm_lock); napi_disable(&mhi_netdev->napi); netif_napi_del(&mhi_netdev->napi); mhi_netdev_dealloc(mhi_netdev); unregister_netdev(mhi_netdev->ndev); free_netdev(mhi_netdev->ndev); flush_work(&mhi_netdev->alloc_work); if (!IS_ERR_OR_NULL(mhi_netdev->dentry)) debugfs_remove_recursive(mhi_netdev->dentry); } static int mhi_netdev_probe(struct mhi_device *mhi_dev, const struct mhi_device_id *id) { int ret; struct mhi_netdev *mhi_netdev; mhi_netdev = devm_kzalloc(&mhi_dev->dev, sizeof(*mhi_netdev), GFP_KERNEL); if (!mhi_netdev) return -ENOMEM; mhi_netdev->alias = 0; mhi_netdev->mhi_dev = mhi_dev; mhi_device_set_devdata(mhi_dev, mhi_netdev); mhi_netdev->mru = 0x4000; if (mhi_dev->dev_id == 0x0304) { //SDX24 mhi_netdev->mru = 0x8000; } #ifdef CONFIG_MHI_NETDEV_MBIM mhi_netdev->rx_max = 0x8000; #endif if (!strcmp(id->chan, "IP_HW0")) mhi_netdev->interface_name = "pcie_mhi"; else if (!strcmp(id->chan, "IP_SW0")) mhi_netdev->interface_name = "pcie_swip"; else mhi_netdev->interface_name = id->chan; mhi_netdev->recycle_buf = false; mhi_netdev->rx_queue = mhi_netdev->recycle_buf ? mhi_netdev_skb_recycle : mhi_netdev_alloc_skb; spin_lock_init(&mhi_netdev->rx_lock); rwlock_init(&mhi_netdev->pm_lock); INIT_WORK(&mhi_netdev->alloc_work, mhi_netdev_alloc_work); mhi_netdev->msg_lvl = MHI_MSG_LVL_INFO; /* setup network interface */ ret = mhi_netdev_enable_iface(mhi_netdev); if (ret) { pr_err("Error mhi_netdev_enable_iface ret:%d\n", ret); return ret; } mhi_netdev_create_debugfs(mhi_netdev); g_mhi_netdev = mhi_netdev; return 0; } static const struct mhi_device_id mhi_netdev_match_table[] = { { .chan = "IP_HW0" }, { .chan = "IP_SW0" }, { .chan = "IP_HW_ADPL" }, { }, }; static struct mhi_driver mhi_netdev_driver = { .id_table = mhi_netdev_match_table, .probe = mhi_netdev_probe, .remove = mhi_netdev_remove, .ul_xfer_cb = mhi_netdev_xfer_ul_cb, .dl_xfer_cb = mhi_netdev_xfer_dl_cb, .status_cb = mhi_netdev_status_cb, .driver = { .name = "mhi_netdev", .owner = THIS_MODULE, } }; int __init mhi_device_netdev_init(struct dentry *parent) { mhi_netdev_create_debugfs_dir(parent); return mhi_driver_register(&mhi_netdev_driver); } void mhi_device_netdev_exit(void) { #ifdef CONFIG_DEBUG_FS debugfs_remove_recursive(mhi_netdev_debugfs_dentry); #endif mhi_driver_unregister(&mhi_netdev_driver); }