1*aa730a99SLoic Poulain // SPDX-License-Identifier: GPL-2.0-or-later 2*aa730a99SLoic Poulain /* MHI MBIM Network driver - Network/MBIM over MHI bus 3*aa730a99SLoic Poulain * 4*aa730a99SLoic Poulain * Copyright (C) 2021 Linaro Ltd <loic.poulain@linaro.org> 5*aa730a99SLoic Poulain * 6*aa730a99SLoic Poulain * This driver copy some code from cdc_ncm, which is: 7*aa730a99SLoic Poulain * Copyright (C) ST-Ericsson 2010-2012 8*aa730a99SLoic Poulain * and cdc_mbim, which is: 9*aa730a99SLoic Poulain * Copyright (c) 2012 Smith Micro Software, Inc. 10*aa730a99SLoic Poulain * Copyright (c) 2012 Bjørn Mork <bjorn@mork.no> 11*aa730a99SLoic Poulain * 12*aa730a99SLoic Poulain */ 13*aa730a99SLoic Poulain 14*aa730a99SLoic Poulain #include <linux/ethtool.h> 15*aa730a99SLoic Poulain #include <linux/if_arp.h> 16*aa730a99SLoic Poulain #include <linux/if_vlan.h> 17*aa730a99SLoic Poulain #include <linux/ip.h> 18*aa730a99SLoic Poulain #include <linux/mhi.h> 19*aa730a99SLoic Poulain #include <linux/mii.h> 20*aa730a99SLoic Poulain #include <linux/mod_devicetable.h> 21*aa730a99SLoic Poulain #include <linux/module.h> 22*aa730a99SLoic Poulain #include <linux/netdevice.h> 23*aa730a99SLoic Poulain #include <linux/skbuff.h> 24*aa730a99SLoic Poulain #include <linux/u64_stats_sync.h> 25*aa730a99SLoic Poulain #include <linux/usb.h> 26*aa730a99SLoic Poulain #include <linux/usb/cdc.h> 27*aa730a99SLoic Poulain #include <linux/usb/usbnet.h> 28*aa730a99SLoic Poulain #include <linux/usb/cdc_ncm.h> 29*aa730a99SLoic Poulain #include <linux/wwan.h> 30*aa730a99SLoic Poulain 31*aa730a99SLoic Poulain /* 3500 allows to optimize skb allocation, the skbs will basically fit in 32*aa730a99SLoic Poulain * one 4K page. Large MBIM packets will simply be split over several MHI 33*aa730a99SLoic Poulain * transfers and chained by the MHI net layer (zerocopy). 34*aa730a99SLoic Poulain */ 35*aa730a99SLoic Poulain #define MHI_DEFAULT_MRU 3500 36*aa730a99SLoic Poulain 37*aa730a99SLoic Poulain #define MHI_MBIM_DEFAULT_MTU 1500 38*aa730a99SLoic Poulain #define MHI_MAX_BUF_SZ 0xffff 39*aa730a99SLoic Poulain 40*aa730a99SLoic Poulain #define MBIM_NDP16_SIGN_MASK 0x00ffffff 41*aa730a99SLoic Poulain 42*aa730a99SLoic Poulain #define MHI_MBIM_LINK_HASH_SIZE 8 43*aa730a99SLoic Poulain #define LINK_HASH(session) ((session) % MHI_MBIM_LINK_HASH_SIZE) 44*aa730a99SLoic Poulain 45*aa730a99SLoic Poulain struct mhi_mbim_link { 46*aa730a99SLoic Poulain struct mhi_mbim_context *mbim; 47*aa730a99SLoic Poulain struct net_device *ndev; 48*aa730a99SLoic Poulain unsigned int session; 49*aa730a99SLoic Poulain 50*aa730a99SLoic Poulain /* stats */ 51*aa730a99SLoic Poulain u64_stats_t rx_packets; 52*aa730a99SLoic Poulain u64_stats_t rx_bytes; 53*aa730a99SLoic Poulain u64_stats_t rx_errors; 54*aa730a99SLoic Poulain u64_stats_t tx_packets; 55*aa730a99SLoic Poulain u64_stats_t tx_bytes; 56*aa730a99SLoic Poulain u64_stats_t tx_errors; 57*aa730a99SLoic Poulain u64_stats_t tx_dropped; 58*aa730a99SLoic Poulain struct u64_stats_sync tx_syncp; 59*aa730a99SLoic Poulain struct u64_stats_sync rx_syncp; 60*aa730a99SLoic Poulain 61*aa730a99SLoic Poulain struct hlist_node hlnode; 62*aa730a99SLoic Poulain }; 63*aa730a99SLoic Poulain 64*aa730a99SLoic Poulain struct mhi_mbim_context { 65*aa730a99SLoic Poulain struct mhi_device *mdev; 66*aa730a99SLoic Poulain struct sk_buff *skbagg_head; 67*aa730a99SLoic Poulain struct sk_buff *skbagg_tail; 68*aa730a99SLoic Poulain unsigned int mru; 69*aa730a99SLoic Poulain u32 rx_queue_sz; 70*aa730a99SLoic Poulain u16 rx_seq; 71*aa730a99SLoic Poulain u16 tx_seq; 72*aa730a99SLoic Poulain struct delayed_work rx_refill; 73*aa730a99SLoic Poulain spinlock_t tx_lock; 74*aa730a99SLoic Poulain struct hlist_head link_list[MHI_MBIM_LINK_HASH_SIZE]; 75*aa730a99SLoic Poulain }; 76*aa730a99SLoic Poulain 77*aa730a99SLoic Poulain struct mbim_tx_hdr { 78*aa730a99SLoic Poulain struct usb_cdc_ncm_nth16 nth16; 79*aa730a99SLoic Poulain struct usb_cdc_ncm_ndp16 ndp16; 80*aa730a99SLoic Poulain struct usb_cdc_ncm_dpe16 dpe16[2]; 81*aa730a99SLoic Poulain } __packed; 82*aa730a99SLoic Poulain 83*aa730a99SLoic Poulain static struct mhi_mbim_link *mhi_mbim_get_link_rcu(struct mhi_mbim_context *mbim, 84*aa730a99SLoic Poulain unsigned int session) 85*aa730a99SLoic Poulain { 86*aa730a99SLoic Poulain struct mhi_mbim_link *link; 87*aa730a99SLoic Poulain 88*aa730a99SLoic Poulain hlist_for_each_entry_rcu(link, &mbim->link_list[LINK_HASH(session)], hlnode) { 89*aa730a99SLoic Poulain if (link->session == session) 90*aa730a99SLoic Poulain return link; 91*aa730a99SLoic Poulain } 92*aa730a99SLoic Poulain 93*aa730a99SLoic Poulain return NULL; 94*aa730a99SLoic Poulain } 95*aa730a99SLoic Poulain 96*aa730a99SLoic Poulain static struct sk_buff *mbim_tx_fixup(struct sk_buff *skb, unsigned int session, 97*aa730a99SLoic Poulain u16 tx_seq) 98*aa730a99SLoic Poulain { 99*aa730a99SLoic Poulain unsigned int dgram_size = skb->len; 100*aa730a99SLoic Poulain struct usb_cdc_ncm_nth16 *nth16; 101*aa730a99SLoic Poulain struct usb_cdc_ncm_ndp16 *ndp16; 102*aa730a99SLoic Poulain struct mbim_tx_hdr *mbim_hdr; 103*aa730a99SLoic Poulain 104*aa730a99SLoic Poulain /* Only one NDP is sent, containing the IP packet (no aggregation) */ 105*aa730a99SLoic Poulain 106*aa730a99SLoic Poulain /* Ensure we have enough headroom for crafting MBIM header */ 107*aa730a99SLoic Poulain if (skb_cow_head(skb, sizeof(struct mbim_tx_hdr))) { 108*aa730a99SLoic Poulain dev_kfree_skb_any(skb); 109*aa730a99SLoic Poulain return NULL; 110*aa730a99SLoic Poulain } 111*aa730a99SLoic Poulain 112*aa730a99SLoic Poulain mbim_hdr = skb_push(skb, sizeof(struct mbim_tx_hdr)); 113*aa730a99SLoic Poulain 114*aa730a99SLoic Poulain /* Fill NTB header */ 115*aa730a99SLoic Poulain nth16 = &mbim_hdr->nth16; 116*aa730a99SLoic Poulain nth16->dwSignature = cpu_to_le32(USB_CDC_NCM_NTH16_SIGN); 117*aa730a99SLoic Poulain nth16->wHeaderLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16)); 118*aa730a99SLoic Poulain nth16->wSequence = cpu_to_le16(tx_seq); 119*aa730a99SLoic Poulain nth16->wBlockLength = cpu_to_le16(skb->len); 120*aa730a99SLoic Poulain nth16->wNdpIndex = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16)); 121*aa730a99SLoic Poulain 122*aa730a99SLoic Poulain /* Fill the unique NDP */ 123*aa730a99SLoic Poulain ndp16 = &mbim_hdr->ndp16; 124*aa730a99SLoic Poulain ndp16->dwSignature = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN | (session << 24)); 125*aa730a99SLoic Poulain ndp16->wLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_ndp16) 126*aa730a99SLoic Poulain + sizeof(struct usb_cdc_ncm_dpe16) * 2); 127*aa730a99SLoic Poulain ndp16->wNextNdpIndex = 0; 128*aa730a99SLoic Poulain 129*aa730a99SLoic Poulain /* Datagram follows the mbim header */ 130*aa730a99SLoic Poulain ndp16->dpe16[0].wDatagramIndex = cpu_to_le16(sizeof(struct mbim_tx_hdr)); 131*aa730a99SLoic Poulain ndp16->dpe16[0].wDatagramLength = cpu_to_le16(dgram_size); 132*aa730a99SLoic Poulain 133*aa730a99SLoic Poulain /* null termination */ 134*aa730a99SLoic Poulain ndp16->dpe16[1].wDatagramIndex = 0; 135*aa730a99SLoic Poulain ndp16->dpe16[1].wDatagramLength = 0; 136*aa730a99SLoic Poulain 137*aa730a99SLoic Poulain return skb; 138*aa730a99SLoic Poulain } 139*aa730a99SLoic Poulain 140*aa730a99SLoic Poulain static netdev_tx_t mhi_mbim_ndo_xmit(struct sk_buff *skb, struct net_device *ndev) 141*aa730a99SLoic Poulain { 142*aa730a99SLoic Poulain struct mhi_mbim_link *link = wwan_netdev_drvpriv(ndev); 143*aa730a99SLoic Poulain struct mhi_mbim_context *mbim = link->mbim; 144*aa730a99SLoic Poulain unsigned long flags; 145*aa730a99SLoic Poulain int err = -ENOMEM; 146*aa730a99SLoic Poulain 147*aa730a99SLoic Poulain /* Serialize MHI channel queuing and MBIM seq */ 148*aa730a99SLoic Poulain spin_lock_irqsave(&mbim->tx_lock, flags); 149*aa730a99SLoic Poulain 150*aa730a99SLoic Poulain skb = mbim_tx_fixup(skb, link->session, mbim->tx_seq); 151*aa730a99SLoic Poulain if (unlikely(!skb)) 152*aa730a99SLoic Poulain goto exit_unlock; 153*aa730a99SLoic Poulain 154*aa730a99SLoic Poulain err = mhi_queue_skb(mbim->mdev, DMA_TO_DEVICE, skb, skb->len, MHI_EOT); 155*aa730a99SLoic Poulain 156*aa730a99SLoic Poulain if (mhi_queue_is_full(mbim->mdev, DMA_TO_DEVICE)) 157*aa730a99SLoic Poulain netif_stop_queue(ndev); 158*aa730a99SLoic Poulain 159*aa730a99SLoic Poulain if (!err) 160*aa730a99SLoic Poulain mbim->tx_seq++; 161*aa730a99SLoic Poulain 162*aa730a99SLoic Poulain exit_unlock: 163*aa730a99SLoic Poulain spin_unlock_irqrestore(&mbim->tx_lock, flags); 164*aa730a99SLoic Poulain 165*aa730a99SLoic Poulain if (unlikely(err)) { 166*aa730a99SLoic Poulain net_err_ratelimited("%s: Failed to queue TX buf (%d)\n", 167*aa730a99SLoic Poulain ndev->name, err); 168*aa730a99SLoic Poulain dev_kfree_skb_any(skb); 169*aa730a99SLoic Poulain goto exit_drop; 170*aa730a99SLoic Poulain } 171*aa730a99SLoic Poulain 172*aa730a99SLoic Poulain return NETDEV_TX_OK; 173*aa730a99SLoic Poulain 174*aa730a99SLoic Poulain exit_drop: 175*aa730a99SLoic Poulain u64_stats_update_begin(&link->tx_syncp); 176*aa730a99SLoic Poulain u64_stats_inc(&link->tx_dropped); 177*aa730a99SLoic Poulain u64_stats_update_end(&link->tx_syncp); 178*aa730a99SLoic Poulain 179*aa730a99SLoic Poulain return NETDEV_TX_OK; 180*aa730a99SLoic Poulain } 181*aa730a99SLoic Poulain 182*aa730a99SLoic Poulain static int mbim_rx_verify_nth16(struct mhi_mbim_context *mbim, struct sk_buff *skb) 183*aa730a99SLoic Poulain { 184*aa730a99SLoic Poulain struct usb_cdc_ncm_nth16 *nth16; 185*aa730a99SLoic Poulain int len; 186*aa730a99SLoic Poulain 187*aa730a99SLoic Poulain if (skb->len < sizeof(struct usb_cdc_ncm_nth16) + 188*aa730a99SLoic Poulain sizeof(struct usb_cdc_ncm_ndp16)) { 189*aa730a99SLoic Poulain net_err_ratelimited("frame too short\n"); 190*aa730a99SLoic Poulain return -EINVAL; 191*aa730a99SLoic Poulain } 192*aa730a99SLoic Poulain 193*aa730a99SLoic Poulain nth16 = (struct usb_cdc_ncm_nth16 *)skb->data; 194*aa730a99SLoic Poulain 195*aa730a99SLoic Poulain if (nth16->dwSignature != cpu_to_le32(USB_CDC_NCM_NTH16_SIGN)) { 196*aa730a99SLoic Poulain net_err_ratelimited("invalid NTH16 signature <%#010x>\n", 197*aa730a99SLoic Poulain le32_to_cpu(nth16->dwSignature)); 198*aa730a99SLoic Poulain return -EINVAL; 199*aa730a99SLoic Poulain } 200*aa730a99SLoic Poulain 201*aa730a99SLoic Poulain /* No limit on the block length, except the size of the data pkt */ 202*aa730a99SLoic Poulain len = le16_to_cpu(nth16->wBlockLength); 203*aa730a99SLoic Poulain if (len > skb->len) { 204*aa730a99SLoic Poulain net_err_ratelimited("NTB does not fit into the skb %u/%u\n", 205*aa730a99SLoic Poulain len, skb->len); 206*aa730a99SLoic Poulain return -EINVAL; 207*aa730a99SLoic Poulain } 208*aa730a99SLoic Poulain 209*aa730a99SLoic Poulain if (mbim->rx_seq + 1 != le16_to_cpu(nth16->wSequence) && 210*aa730a99SLoic Poulain (mbim->rx_seq || le16_to_cpu(nth16->wSequence)) && 211*aa730a99SLoic Poulain !(mbim->rx_seq == 0xffff && !le16_to_cpu(nth16->wSequence))) { 212*aa730a99SLoic Poulain net_err_ratelimited("sequence number glitch prev=%d curr=%d\n", 213*aa730a99SLoic Poulain mbim->rx_seq, le16_to_cpu(nth16->wSequence)); 214*aa730a99SLoic Poulain } 215*aa730a99SLoic Poulain mbim->rx_seq = le16_to_cpu(nth16->wSequence); 216*aa730a99SLoic Poulain 217*aa730a99SLoic Poulain return le16_to_cpu(nth16->wNdpIndex); 218*aa730a99SLoic Poulain } 219*aa730a99SLoic Poulain 220*aa730a99SLoic Poulain static int mbim_rx_verify_ndp16(struct sk_buff *skb, struct usb_cdc_ncm_ndp16 *ndp16) 221*aa730a99SLoic Poulain { 222*aa730a99SLoic Poulain int ret; 223*aa730a99SLoic Poulain 224*aa730a99SLoic Poulain if (le16_to_cpu(ndp16->wLength) < USB_CDC_NCM_NDP16_LENGTH_MIN) { 225*aa730a99SLoic Poulain net_err_ratelimited("invalid DPT16 length <%u>\n", 226*aa730a99SLoic Poulain le16_to_cpu(ndp16->wLength)); 227*aa730a99SLoic Poulain return -EINVAL; 228*aa730a99SLoic Poulain } 229*aa730a99SLoic Poulain 230*aa730a99SLoic Poulain ret = ((le16_to_cpu(ndp16->wLength) - sizeof(struct usb_cdc_ncm_ndp16)) 231*aa730a99SLoic Poulain / sizeof(struct usb_cdc_ncm_dpe16)); 232*aa730a99SLoic Poulain ret--; /* Last entry is always a NULL terminator */ 233*aa730a99SLoic Poulain 234*aa730a99SLoic Poulain if (sizeof(struct usb_cdc_ncm_ndp16) + 235*aa730a99SLoic Poulain ret * sizeof(struct usb_cdc_ncm_dpe16) > skb->len) { 236*aa730a99SLoic Poulain net_err_ratelimited("Invalid nframes = %d\n", ret); 237*aa730a99SLoic Poulain return -EINVAL; 238*aa730a99SLoic Poulain } 239*aa730a99SLoic Poulain 240*aa730a99SLoic Poulain return ret; 241*aa730a99SLoic Poulain } 242*aa730a99SLoic Poulain 243*aa730a99SLoic Poulain static void mhi_mbim_rx(struct mhi_mbim_context *mbim, struct sk_buff *skb) 244*aa730a99SLoic Poulain { 245*aa730a99SLoic Poulain int ndpoffset; 246*aa730a99SLoic Poulain 247*aa730a99SLoic Poulain /* Check NTB header and retrieve first NDP offset */ 248*aa730a99SLoic Poulain ndpoffset = mbim_rx_verify_nth16(mbim, skb); 249*aa730a99SLoic Poulain if (ndpoffset < 0) { 250*aa730a99SLoic Poulain net_err_ratelimited("mbim: Incorrect NTB header\n"); 251*aa730a99SLoic Poulain goto error; 252*aa730a99SLoic Poulain } 253*aa730a99SLoic Poulain 254*aa730a99SLoic Poulain /* Process each NDP */ 255*aa730a99SLoic Poulain while (1) { 256*aa730a99SLoic Poulain struct usb_cdc_ncm_ndp16 ndp16; 257*aa730a99SLoic Poulain struct usb_cdc_ncm_dpe16 dpe16; 258*aa730a99SLoic Poulain struct mhi_mbim_link *link; 259*aa730a99SLoic Poulain int nframes, n, dpeoffset; 260*aa730a99SLoic Poulain unsigned int session; 261*aa730a99SLoic Poulain 262*aa730a99SLoic Poulain if (skb_copy_bits(skb, ndpoffset, &ndp16, sizeof(ndp16))) { 263*aa730a99SLoic Poulain net_err_ratelimited("mbim: Incorrect NDP offset (%u)\n", 264*aa730a99SLoic Poulain ndpoffset); 265*aa730a99SLoic Poulain goto error; 266*aa730a99SLoic Poulain } 267*aa730a99SLoic Poulain 268*aa730a99SLoic Poulain /* Check NDP header and retrieve number of datagrams */ 269*aa730a99SLoic Poulain nframes = mbim_rx_verify_ndp16(skb, &ndp16); 270*aa730a99SLoic Poulain if (nframes < 0) { 271*aa730a99SLoic Poulain net_err_ratelimited("mbim: Incorrect NDP16\n"); 272*aa730a99SLoic Poulain goto error; 273*aa730a99SLoic Poulain } 274*aa730a99SLoic Poulain 275*aa730a99SLoic Poulain /* Only IP data type supported, no DSS in MHI context */ 276*aa730a99SLoic Poulain if ((ndp16.dwSignature & cpu_to_le32(MBIM_NDP16_SIGN_MASK)) 277*aa730a99SLoic Poulain != cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN)) { 278*aa730a99SLoic Poulain net_err_ratelimited("mbim: Unsupported NDP type\n"); 279*aa730a99SLoic Poulain goto next_ndp; 280*aa730a99SLoic Poulain } 281*aa730a99SLoic Poulain 282*aa730a99SLoic Poulain session = (le32_to_cpu(ndp16.dwSignature) & ~MBIM_NDP16_SIGN_MASK) >> 24; 283*aa730a99SLoic Poulain 284*aa730a99SLoic Poulain rcu_read_lock(); 285*aa730a99SLoic Poulain 286*aa730a99SLoic Poulain link = mhi_mbim_get_link_rcu(mbim, session); 287*aa730a99SLoic Poulain if (!link) { 288*aa730a99SLoic Poulain net_err_ratelimited("mbim: bad packet session (%u)\n", session); 289*aa730a99SLoic Poulain goto unlock; 290*aa730a99SLoic Poulain } 291*aa730a99SLoic Poulain 292*aa730a99SLoic Poulain /* de-aggregate and deliver IP packets */ 293*aa730a99SLoic Poulain dpeoffset = ndpoffset + sizeof(struct usb_cdc_ncm_ndp16); 294*aa730a99SLoic Poulain for (n = 0; n < nframes; n++, dpeoffset += sizeof(dpe16)) { 295*aa730a99SLoic Poulain u16 dgram_offset, dgram_len; 296*aa730a99SLoic Poulain struct sk_buff *skbn; 297*aa730a99SLoic Poulain 298*aa730a99SLoic Poulain if (skb_copy_bits(skb, dpeoffset, &dpe16, sizeof(dpe16))) 299*aa730a99SLoic Poulain break; 300*aa730a99SLoic Poulain 301*aa730a99SLoic Poulain dgram_offset = le16_to_cpu(dpe16.wDatagramIndex); 302*aa730a99SLoic Poulain dgram_len = le16_to_cpu(dpe16.wDatagramLength); 303*aa730a99SLoic Poulain 304*aa730a99SLoic Poulain if (!dgram_offset || !dgram_len) 305*aa730a99SLoic Poulain break; /* null terminator */ 306*aa730a99SLoic Poulain 307*aa730a99SLoic Poulain skbn = netdev_alloc_skb(link->ndev, dgram_len); 308*aa730a99SLoic Poulain if (!skbn) 309*aa730a99SLoic Poulain continue; 310*aa730a99SLoic Poulain 311*aa730a99SLoic Poulain skb_put(skbn, dgram_len); 312*aa730a99SLoic Poulain skb_copy_bits(skb, dgram_offset, skbn->data, dgram_len); 313*aa730a99SLoic Poulain 314*aa730a99SLoic Poulain switch (skbn->data[0] & 0xf0) { 315*aa730a99SLoic Poulain case 0x40: 316*aa730a99SLoic Poulain skbn->protocol = htons(ETH_P_IP); 317*aa730a99SLoic Poulain break; 318*aa730a99SLoic Poulain case 0x60: 319*aa730a99SLoic Poulain skbn->protocol = htons(ETH_P_IPV6); 320*aa730a99SLoic Poulain break; 321*aa730a99SLoic Poulain default: 322*aa730a99SLoic Poulain net_err_ratelimited("%s: unknown protocol\n", 323*aa730a99SLoic Poulain link->ndev->name); 324*aa730a99SLoic Poulain dev_kfree_skb_any(skbn); 325*aa730a99SLoic Poulain u64_stats_update_begin(&link->rx_syncp); 326*aa730a99SLoic Poulain u64_stats_inc(&link->rx_errors); 327*aa730a99SLoic Poulain u64_stats_update_end(&link->rx_syncp); 328*aa730a99SLoic Poulain continue; 329*aa730a99SLoic Poulain } 330*aa730a99SLoic Poulain 331*aa730a99SLoic Poulain u64_stats_update_begin(&link->rx_syncp); 332*aa730a99SLoic Poulain u64_stats_inc(&link->rx_packets); 333*aa730a99SLoic Poulain u64_stats_add(&link->rx_bytes, skbn->len); 334*aa730a99SLoic Poulain u64_stats_update_end(&link->rx_syncp); 335*aa730a99SLoic Poulain 336*aa730a99SLoic Poulain netif_rx(skbn); 337*aa730a99SLoic Poulain } 338*aa730a99SLoic Poulain unlock: 339*aa730a99SLoic Poulain rcu_read_unlock(); 340*aa730a99SLoic Poulain next_ndp: 341*aa730a99SLoic Poulain /* Other NDP to process? */ 342*aa730a99SLoic Poulain ndpoffset = (int)le16_to_cpu(ndp16.wNextNdpIndex); 343*aa730a99SLoic Poulain if (!ndpoffset) 344*aa730a99SLoic Poulain break; 345*aa730a99SLoic Poulain } 346*aa730a99SLoic Poulain 347*aa730a99SLoic Poulain /* free skb */ 348*aa730a99SLoic Poulain dev_consume_skb_any(skb); 349*aa730a99SLoic Poulain return; 350*aa730a99SLoic Poulain error: 351*aa730a99SLoic Poulain dev_kfree_skb_any(skb); 352*aa730a99SLoic Poulain } 353*aa730a99SLoic Poulain 354*aa730a99SLoic Poulain static struct sk_buff *mhi_net_skb_agg(struct mhi_mbim_context *mbim, 355*aa730a99SLoic Poulain struct sk_buff *skb) 356*aa730a99SLoic Poulain { 357*aa730a99SLoic Poulain struct sk_buff *head = mbim->skbagg_head; 358*aa730a99SLoic Poulain struct sk_buff *tail = mbim->skbagg_tail; 359*aa730a99SLoic Poulain 360*aa730a99SLoic Poulain /* This is non-paged skb chaining using frag_list */ 361*aa730a99SLoic Poulain if (!head) { 362*aa730a99SLoic Poulain mbim->skbagg_head = skb; 363*aa730a99SLoic Poulain return skb; 364*aa730a99SLoic Poulain } 365*aa730a99SLoic Poulain 366*aa730a99SLoic Poulain if (!skb_shinfo(head)->frag_list) 367*aa730a99SLoic Poulain skb_shinfo(head)->frag_list = skb; 368*aa730a99SLoic Poulain else 369*aa730a99SLoic Poulain tail->next = skb; 370*aa730a99SLoic Poulain 371*aa730a99SLoic Poulain head->len += skb->len; 372*aa730a99SLoic Poulain head->data_len += skb->len; 373*aa730a99SLoic Poulain head->truesize += skb->truesize; 374*aa730a99SLoic Poulain 375*aa730a99SLoic Poulain mbim->skbagg_tail = skb; 376*aa730a99SLoic Poulain 377*aa730a99SLoic Poulain return mbim->skbagg_head; 378*aa730a99SLoic Poulain } 379*aa730a99SLoic Poulain 380*aa730a99SLoic Poulain static void mhi_net_rx_refill_work(struct work_struct *work) 381*aa730a99SLoic Poulain { 382*aa730a99SLoic Poulain struct mhi_mbim_context *mbim = container_of(work, struct mhi_mbim_context, 383*aa730a99SLoic Poulain rx_refill.work); 384*aa730a99SLoic Poulain struct mhi_device *mdev = mbim->mdev; 385*aa730a99SLoic Poulain int err; 386*aa730a99SLoic Poulain 387*aa730a99SLoic Poulain while (!mhi_queue_is_full(mdev, DMA_FROM_DEVICE)) { 388*aa730a99SLoic Poulain struct sk_buff *skb = alloc_skb(MHI_DEFAULT_MRU, GFP_KERNEL); 389*aa730a99SLoic Poulain 390*aa730a99SLoic Poulain if (unlikely(!skb)) 391*aa730a99SLoic Poulain break; 392*aa730a99SLoic Poulain 393*aa730a99SLoic Poulain err = mhi_queue_skb(mdev, DMA_FROM_DEVICE, skb, 394*aa730a99SLoic Poulain MHI_DEFAULT_MRU, MHI_EOT); 395*aa730a99SLoic Poulain if (unlikely(err)) { 396*aa730a99SLoic Poulain kfree_skb(skb); 397*aa730a99SLoic Poulain break; 398*aa730a99SLoic Poulain } 399*aa730a99SLoic Poulain 400*aa730a99SLoic Poulain /* Do not hog the CPU if rx buffers are consumed faster than 401*aa730a99SLoic Poulain * queued (unlikely). 402*aa730a99SLoic Poulain */ 403*aa730a99SLoic Poulain cond_resched(); 404*aa730a99SLoic Poulain } 405*aa730a99SLoic Poulain 406*aa730a99SLoic Poulain /* If we're still starved of rx buffers, reschedule later */ 407*aa730a99SLoic Poulain if (mhi_get_free_desc_count(mdev, DMA_FROM_DEVICE) == mbim->rx_queue_sz) 408*aa730a99SLoic Poulain schedule_delayed_work(&mbim->rx_refill, HZ / 2); 409*aa730a99SLoic Poulain } 410*aa730a99SLoic Poulain 411*aa730a99SLoic Poulain static void mhi_mbim_dl_callback(struct mhi_device *mhi_dev, 412*aa730a99SLoic Poulain struct mhi_result *mhi_res) 413*aa730a99SLoic Poulain { 414*aa730a99SLoic Poulain struct mhi_mbim_context *mbim = dev_get_drvdata(&mhi_dev->dev); 415*aa730a99SLoic Poulain struct sk_buff *skb = mhi_res->buf_addr; 416*aa730a99SLoic Poulain int free_desc_count; 417*aa730a99SLoic Poulain 418*aa730a99SLoic Poulain free_desc_count = mhi_get_free_desc_count(mhi_dev, DMA_FROM_DEVICE); 419*aa730a99SLoic Poulain 420*aa730a99SLoic Poulain if (unlikely(mhi_res->transaction_status)) { 421*aa730a99SLoic Poulain switch (mhi_res->transaction_status) { 422*aa730a99SLoic Poulain case -EOVERFLOW: 423*aa730a99SLoic Poulain /* Packet has been split over multiple transfers */ 424*aa730a99SLoic Poulain skb_put(skb, mhi_res->bytes_xferd); 425*aa730a99SLoic Poulain mhi_net_skb_agg(mbim, skb); 426*aa730a99SLoic Poulain break; 427*aa730a99SLoic Poulain case -ENOTCONN: 428*aa730a99SLoic Poulain /* MHI layer stopping/resetting the DL channel */ 429*aa730a99SLoic Poulain dev_kfree_skb_any(skb); 430*aa730a99SLoic Poulain return; 431*aa730a99SLoic Poulain default: 432*aa730a99SLoic Poulain /* Unknown error, simply drop */ 433*aa730a99SLoic Poulain dev_kfree_skb_any(skb); 434*aa730a99SLoic Poulain } 435*aa730a99SLoic Poulain } else { 436*aa730a99SLoic Poulain skb_put(skb, mhi_res->bytes_xferd); 437*aa730a99SLoic Poulain 438*aa730a99SLoic Poulain if (mbim->skbagg_head) { 439*aa730a99SLoic Poulain /* Aggregate the final fragment */ 440*aa730a99SLoic Poulain skb = mhi_net_skb_agg(mbim, skb); 441*aa730a99SLoic Poulain mbim->skbagg_head = NULL; 442*aa730a99SLoic Poulain } 443*aa730a99SLoic Poulain 444*aa730a99SLoic Poulain mhi_mbim_rx(mbim, skb); 445*aa730a99SLoic Poulain } 446*aa730a99SLoic Poulain 447*aa730a99SLoic Poulain /* Refill if RX buffers queue becomes low */ 448*aa730a99SLoic Poulain if (free_desc_count >= mbim->rx_queue_sz / 2) 449*aa730a99SLoic Poulain schedule_delayed_work(&mbim->rx_refill, 0); 450*aa730a99SLoic Poulain } 451*aa730a99SLoic Poulain 452*aa730a99SLoic Poulain static void mhi_mbim_ndo_get_stats64(struct net_device *ndev, 453*aa730a99SLoic Poulain struct rtnl_link_stats64 *stats) 454*aa730a99SLoic Poulain { 455*aa730a99SLoic Poulain struct mhi_mbim_link *link = wwan_netdev_drvpriv(ndev); 456*aa730a99SLoic Poulain unsigned int start; 457*aa730a99SLoic Poulain 458*aa730a99SLoic Poulain do { 459*aa730a99SLoic Poulain start = u64_stats_fetch_begin_irq(&link->rx_syncp); 460*aa730a99SLoic Poulain stats->rx_packets = u64_stats_read(&link->rx_packets); 461*aa730a99SLoic Poulain stats->rx_bytes = u64_stats_read(&link->rx_bytes); 462*aa730a99SLoic Poulain stats->rx_errors = u64_stats_read(&link->rx_errors); 463*aa730a99SLoic Poulain } while (u64_stats_fetch_retry_irq(&link->rx_syncp, start)); 464*aa730a99SLoic Poulain 465*aa730a99SLoic Poulain do { 466*aa730a99SLoic Poulain start = u64_stats_fetch_begin_irq(&link->tx_syncp); 467*aa730a99SLoic Poulain stats->tx_packets = u64_stats_read(&link->tx_packets); 468*aa730a99SLoic Poulain stats->tx_bytes = u64_stats_read(&link->tx_bytes); 469*aa730a99SLoic Poulain stats->tx_errors = u64_stats_read(&link->tx_errors); 470*aa730a99SLoic Poulain stats->tx_dropped = u64_stats_read(&link->tx_dropped); 471*aa730a99SLoic Poulain } while (u64_stats_fetch_retry_irq(&link->tx_syncp, start)); 472*aa730a99SLoic Poulain } 473*aa730a99SLoic Poulain 474*aa730a99SLoic Poulain static void mhi_mbim_ul_callback(struct mhi_device *mhi_dev, 475*aa730a99SLoic Poulain struct mhi_result *mhi_res) 476*aa730a99SLoic Poulain { 477*aa730a99SLoic Poulain struct mhi_mbim_context *mbim = dev_get_drvdata(&mhi_dev->dev); 478*aa730a99SLoic Poulain struct sk_buff *skb = mhi_res->buf_addr; 479*aa730a99SLoic Poulain struct net_device *ndev = skb->dev; 480*aa730a99SLoic Poulain struct mhi_mbim_link *link = wwan_netdev_drvpriv(ndev); 481*aa730a99SLoic Poulain 482*aa730a99SLoic Poulain /* Hardware has consumed the buffer, so free the skb (which is not 483*aa730a99SLoic Poulain * freed by the MHI stack) and perform accounting. 484*aa730a99SLoic Poulain */ 485*aa730a99SLoic Poulain dev_consume_skb_any(skb); 486*aa730a99SLoic Poulain 487*aa730a99SLoic Poulain u64_stats_update_begin(&link->tx_syncp); 488*aa730a99SLoic Poulain if (unlikely(mhi_res->transaction_status)) { 489*aa730a99SLoic Poulain /* MHI layer stopping/resetting the UL channel */ 490*aa730a99SLoic Poulain if (mhi_res->transaction_status == -ENOTCONN) { 491*aa730a99SLoic Poulain u64_stats_update_end(&link->tx_syncp); 492*aa730a99SLoic Poulain return; 493*aa730a99SLoic Poulain } 494*aa730a99SLoic Poulain 495*aa730a99SLoic Poulain u64_stats_inc(&link->tx_errors); 496*aa730a99SLoic Poulain } else { 497*aa730a99SLoic Poulain u64_stats_inc(&link->tx_packets); 498*aa730a99SLoic Poulain u64_stats_add(&link->tx_bytes, mhi_res->bytes_xferd); 499*aa730a99SLoic Poulain } 500*aa730a99SLoic Poulain u64_stats_update_end(&link->tx_syncp); 501*aa730a99SLoic Poulain 502*aa730a99SLoic Poulain if (netif_queue_stopped(ndev) && !mhi_queue_is_full(mbim->mdev, DMA_TO_DEVICE)) 503*aa730a99SLoic Poulain netif_wake_queue(ndev); 504*aa730a99SLoic Poulain } 505*aa730a99SLoic Poulain 506*aa730a99SLoic Poulain static int mhi_mbim_ndo_open(struct net_device *ndev) 507*aa730a99SLoic Poulain { 508*aa730a99SLoic Poulain struct mhi_mbim_link *link = wwan_netdev_drvpriv(ndev); 509*aa730a99SLoic Poulain 510*aa730a99SLoic Poulain /* Feed the MHI rx buffer pool */ 511*aa730a99SLoic Poulain schedule_delayed_work(&link->mbim->rx_refill, 0); 512*aa730a99SLoic Poulain 513*aa730a99SLoic Poulain /* Carrier is established via out-of-band channel (e.g. qmi) */ 514*aa730a99SLoic Poulain netif_carrier_on(ndev); 515*aa730a99SLoic Poulain 516*aa730a99SLoic Poulain netif_start_queue(ndev); 517*aa730a99SLoic Poulain 518*aa730a99SLoic Poulain return 0; 519*aa730a99SLoic Poulain } 520*aa730a99SLoic Poulain 521*aa730a99SLoic Poulain static int mhi_mbim_ndo_stop(struct net_device *ndev) 522*aa730a99SLoic Poulain { 523*aa730a99SLoic Poulain netif_stop_queue(ndev); 524*aa730a99SLoic Poulain netif_carrier_off(ndev); 525*aa730a99SLoic Poulain 526*aa730a99SLoic Poulain return 0; 527*aa730a99SLoic Poulain } 528*aa730a99SLoic Poulain 529*aa730a99SLoic Poulain static const struct net_device_ops mhi_mbim_ndo = { 530*aa730a99SLoic Poulain .ndo_open = mhi_mbim_ndo_open, 531*aa730a99SLoic Poulain .ndo_stop = mhi_mbim_ndo_stop, 532*aa730a99SLoic Poulain .ndo_start_xmit = mhi_mbim_ndo_xmit, 533*aa730a99SLoic Poulain .ndo_get_stats64 = mhi_mbim_ndo_get_stats64, 534*aa730a99SLoic Poulain }; 535*aa730a99SLoic Poulain 536*aa730a99SLoic Poulain static int mhi_mbim_newlink(void *ctxt, struct net_device *ndev, u32 if_id, 537*aa730a99SLoic Poulain struct netlink_ext_ack *extack) 538*aa730a99SLoic Poulain { 539*aa730a99SLoic Poulain struct mhi_mbim_link *link = wwan_netdev_drvpriv(ndev); 540*aa730a99SLoic Poulain struct mhi_mbim_context *mbim = ctxt; 541*aa730a99SLoic Poulain 542*aa730a99SLoic Poulain link->session = if_id; 543*aa730a99SLoic Poulain link->mbim = mbim; 544*aa730a99SLoic Poulain link->ndev = ndev; 545*aa730a99SLoic Poulain u64_stats_init(&link->rx_syncp); 546*aa730a99SLoic Poulain u64_stats_init(&link->tx_syncp); 547*aa730a99SLoic Poulain 548*aa730a99SLoic Poulain rcu_read_lock(); 549*aa730a99SLoic Poulain if (mhi_mbim_get_link_rcu(mbim, if_id)) { 550*aa730a99SLoic Poulain rcu_read_unlock(); 551*aa730a99SLoic Poulain return -EEXIST; 552*aa730a99SLoic Poulain } 553*aa730a99SLoic Poulain rcu_read_unlock(); 554*aa730a99SLoic Poulain 555*aa730a99SLoic Poulain /* Already protected by RTNL lock */ 556*aa730a99SLoic Poulain hlist_add_head_rcu(&link->hlnode, &mbim->link_list[LINK_HASH(if_id)]); 557*aa730a99SLoic Poulain 558*aa730a99SLoic Poulain return register_netdevice(ndev); 559*aa730a99SLoic Poulain } 560*aa730a99SLoic Poulain 561*aa730a99SLoic Poulain static void mhi_mbim_dellink(void *ctxt, struct net_device *ndev, 562*aa730a99SLoic Poulain struct list_head *head) 563*aa730a99SLoic Poulain { 564*aa730a99SLoic Poulain struct mhi_mbim_link *link = wwan_netdev_drvpriv(ndev); 565*aa730a99SLoic Poulain 566*aa730a99SLoic Poulain hlist_del_init_rcu(&link->hlnode); 567*aa730a99SLoic Poulain synchronize_rcu(); 568*aa730a99SLoic Poulain 569*aa730a99SLoic Poulain unregister_netdevice_queue(ndev, head); 570*aa730a99SLoic Poulain } 571*aa730a99SLoic Poulain 572*aa730a99SLoic Poulain static void mhi_mbim_setup(struct net_device *ndev) 573*aa730a99SLoic Poulain { 574*aa730a99SLoic Poulain ndev->header_ops = NULL; /* No header */ 575*aa730a99SLoic Poulain ndev->type = ARPHRD_RAWIP; 576*aa730a99SLoic Poulain ndev->needed_headroom = sizeof(struct mbim_tx_hdr); 577*aa730a99SLoic Poulain ndev->hard_header_len = 0; 578*aa730a99SLoic Poulain ndev->addr_len = 0; 579*aa730a99SLoic Poulain ndev->flags = IFF_POINTOPOINT | IFF_NOARP; 580*aa730a99SLoic Poulain ndev->netdev_ops = &mhi_mbim_ndo; 581*aa730a99SLoic Poulain ndev->mtu = MHI_MBIM_DEFAULT_MTU; 582*aa730a99SLoic Poulain ndev->min_mtu = ETH_MIN_MTU; 583*aa730a99SLoic Poulain ndev->max_mtu = MHI_MAX_BUF_SZ - ndev->needed_headroom; 584*aa730a99SLoic Poulain ndev->tx_queue_len = 1000; 585*aa730a99SLoic Poulain } 586*aa730a99SLoic Poulain 587*aa730a99SLoic Poulain static const struct wwan_ops mhi_mbim_wwan_ops = { 588*aa730a99SLoic Poulain .priv_size = sizeof(struct mhi_mbim_link), 589*aa730a99SLoic Poulain .setup = mhi_mbim_setup, 590*aa730a99SLoic Poulain .newlink = mhi_mbim_newlink, 591*aa730a99SLoic Poulain .dellink = mhi_mbim_dellink, 592*aa730a99SLoic Poulain }; 593*aa730a99SLoic Poulain 594*aa730a99SLoic Poulain static int mhi_mbim_probe(struct mhi_device *mhi_dev, const struct mhi_device_id *id) 595*aa730a99SLoic Poulain { 596*aa730a99SLoic Poulain struct mhi_controller *cntrl = mhi_dev->mhi_cntrl; 597*aa730a99SLoic Poulain struct mhi_mbim_context *mbim; 598*aa730a99SLoic Poulain int err; 599*aa730a99SLoic Poulain 600*aa730a99SLoic Poulain mbim = devm_kzalloc(&mhi_dev->dev, sizeof(*mbim), GFP_KERNEL); 601*aa730a99SLoic Poulain if (!mbim) 602*aa730a99SLoic Poulain return -ENOMEM; 603*aa730a99SLoic Poulain 604*aa730a99SLoic Poulain dev_set_drvdata(&mhi_dev->dev, mbim); 605*aa730a99SLoic Poulain mbim->mdev = mhi_dev; 606*aa730a99SLoic Poulain mbim->mru = mhi_dev->mhi_cntrl->mru ? mhi_dev->mhi_cntrl->mru : MHI_DEFAULT_MRU; 607*aa730a99SLoic Poulain 608*aa730a99SLoic Poulain INIT_DELAYED_WORK(&mbim->rx_refill, mhi_net_rx_refill_work); 609*aa730a99SLoic Poulain 610*aa730a99SLoic Poulain /* Start MHI channels */ 611*aa730a99SLoic Poulain err = mhi_prepare_for_transfer(mhi_dev); 612*aa730a99SLoic Poulain if (err) 613*aa730a99SLoic Poulain return err; 614*aa730a99SLoic Poulain 615*aa730a99SLoic Poulain /* Number of transfer descriptors determines size of the queue */ 616*aa730a99SLoic Poulain mbim->rx_queue_sz = mhi_get_free_desc_count(mhi_dev, DMA_FROM_DEVICE); 617*aa730a99SLoic Poulain 618*aa730a99SLoic Poulain /* Register wwan link ops with MHI controller representing WWAN instance */ 619*aa730a99SLoic Poulain return wwan_register_ops(&cntrl->mhi_dev->dev, &mhi_mbim_wwan_ops, mbim, 0); 620*aa730a99SLoic Poulain } 621*aa730a99SLoic Poulain 622*aa730a99SLoic Poulain static void mhi_mbim_remove(struct mhi_device *mhi_dev) 623*aa730a99SLoic Poulain { 624*aa730a99SLoic Poulain struct mhi_mbim_context *mbim = dev_get_drvdata(&mhi_dev->dev); 625*aa730a99SLoic Poulain struct mhi_controller *cntrl = mhi_dev->mhi_cntrl; 626*aa730a99SLoic Poulain 627*aa730a99SLoic Poulain mhi_unprepare_from_transfer(mhi_dev); 628*aa730a99SLoic Poulain cancel_delayed_work_sync(&mbim->rx_refill); 629*aa730a99SLoic Poulain wwan_unregister_ops(&cntrl->mhi_dev->dev); 630*aa730a99SLoic Poulain kfree_skb(mbim->skbagg_head); 631*aa730a99SLoic Poulain dev_set_drvdata(&mhi_dev->dev, NULL); 632*aa730a99SLoic Poulain } 633*aa730a99SLoic Poulain 634*aa730a99SLoic Poulain static const struct mhi_device_id mhi_mbim_id_table[] = { 635*aa730a99SLoic Poulain /* Hardware accelerated data PATH (to modem IPA), MBIM protocol */ 636*aa730a99SLoic Poulain { .chan = "IP_HW0_MBIM", .driver_data = 0 }, 637*aa730a99SLoic Poulain {} 638*aa730a99SLoic Poulain }; 639*aa730a99SLoic Poulain MODULE_DEVICE_TABLE(mhi, mhi_mbim_id_table); 640*aa730a99SLoic Poulain 641*aa730a99SLoic Poulain static struct mhi_driver mhi_mbim_driver = { 642*aa730a99SLoic Poulain .probe = mhi_mbim_probe, 643*aa730a99SLoic Poulain .remove = mhi_mbim_remove, 644*aa730a99SLoic Poulain .dl_xfer_cb = mhi_mbim_dl_callback, 645*aa730a99SLoic Poulain .ul_xfer_cb = mhi_mbim_ul_callback, 646*aa730a99SLoic Poulain .id_table = mhi_mbim_id_table, 647*aa730a99SLoic Poulain .driver = { 648*aa730a99SLoic Poulain .name = "mhi_wwan_mbim", 649*aa730a99SLoic Poulain .owner = THIS_MODULE, 650*aa730a99SLoic Poulain }, 651*aa730a99SLoic Poulain }; 652*aa730a99SLoic Poulain 653*aa730a99SLoic Poulain module_mhi_driver(mhi_mbim_driver); 654*aa730a99SLoic Poulain 655*aa730a99SLoic Poulain MODULE_AUTHOR("Loic Poulain <loic.poulain@linaro.org>"); 656*aa730a99SLoic Poulain MODULE_DESCRIPTION("Network/MBIM over MHI"); 657*aa730a99SLoic Poulain MODULE_LICENSE("GPL v2"); 658