drivers/net/mhi/proto_mbim.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /* MHI Network driver - Network over MHI bus
   3  *
   4  * Copyright (C) 2021 Linaro Ltd <loic.poulain@linaro.org>
   5  *
   6  * This driver copy some code from cdc_ncm, which is:
   7  * Copyright (C) ST-Ericsson 2010-2012
   8  * and cdc_mbim, which is:
   9  * Copyright (c) 2012  Smith Micro Software, Inc.
  10  * Copyright (c) 2012  Bjørn Mork <bjorn@mork.no>
  11  *
  12  */
  13
  14 #include <linux/ethtool.h>
  15 #include <linux/if_vlan.h>
  16 #include <linux/ip.h>
  17 #include <linux/mii.h>
  18 #include <linux/netdevice.h>
  19 #include <linux/wwan.h>
  20 #include <linux/skbuff.h>
  21 #include <linux/usb.h>
  22 #include <linux/usb/cdc.h>
  23 #include <linux/usb/usbnet.h>
  24 #include <linux/usb/cdc_ncm.h>
  25
  26 #include "mhi.h"
  27
  28 #define MBIM_NDP16_SIGN_MASK 0x00ffffff
  29
  30 /* Usual WWAN MTU */
  31 #define MHI_MBIM_DEFAULT_MTU 1500
  32
  33 /* 3500 allows to optimize skb allocation, the skbs will basically fit in
  34  * one 4K page. Large MBIM packets will simply be split over several MHI
  35  * transfers and chained by the MHI net layer (zerocopy).
  36  */
  37 #define MHI_MBIM_DEFAULT_MRU 3500
  38
  39 struct mbim_context {
  40         u16 rx_seq;
  41         u16 tx_seq;
  42 };
  43
  44 static void __mbim_length_errors_inc(struct mhi_net_dev *dev)
  45 {
  46         u64_stats_update_begin(&dev->stats.rx_syncp);
  47         u64_stats_inc(&dev->stats.rx_length_errors);
  48         u64_stats_update_end(&dev->stats.rx_syncp);
  49 }
  50
  51 static void __mbim_errors_inc(struct mhi_net_dev *dev)
  52 {
  53         u64_stats_update_begin(&dev->stats.rx_syncp);
  54         u64_stats_inc(&dev->stats.rx_errors);
  55         u64_stats_update_end(&dev->stats.rx_syncp);
  56 }
  57
  58 static int mbim_rx_verify_nth16(struct sk_buff *skb)
  59 {
  60         struct mhi_net_dev *dev = wwan_netdev_drvpriv(skb->dev);
  61         struct mbim_context *ctx = dev->proto_data;
  62         struct usb_cdc_ncm_nth16 *nth16;
  63         int len;
  64
  65         if (skb->len < sizeof(struct usb_cdc_ncm_nth16) +
  66                         sizeof(struct usb_cdc_ncm_ndp16)) {
  67                 netif_dbg(dev, rx_err, dev->ndev, "frame too short\n");
  68                 __mbim_length_errors_inc(dev);
  69                 return -EINVAL;
  70         }
  71
  72         nth16 = (struct usb_cdc_ncm_nth16 *)skb->data;
  73
  74         if (nth16->dwSignature != cpu_to_le32(USB_CDC_NCM_NTH16_SIGN)) {
  75                 netif_dbg(dev, rx_err, dev->ndev,
  76                           "invalid NTH16 signature <%#010x>\n",
  77                           le32_to_cpu(nth16->dwSignature));
  78                 __mbim_errors_inc(dev);
  79                 return -EINVAL;
  80         }
  81
  82         /* No limit on the block length, except the size of the data pkt */
  83         len = le16_to_cpu(nth16->wBlockLength);
  84         if (len > skb->len) {
  85                 netif_dbg(dev, rx_err, dev->ndev,
  86                           "NTB does not fit into the skb %u/%u\n", len,
  87                           skb->len);
  88                 __mbim_length_errors_inc(dev);
  89                 return -EINVAL;
  90         }
  91
  92         if (ctx->rx_seq + 1 != le16_to_cpu(nth16->wSequence) &&
  93             (ctx->rx_seq || le16_to_cpu(nth16->wSequence)) &&
  94             !(ctx->rx_seq == 0xffff && !le16_to_cpu(nth16->wSequence))) {
  95                 netif_dbg(dev, rx_err, dev->ndev,
  96                           "sequence number glitch prev=%d curr=%d\n",
  97                           ctx->rx_seq, le16_to_cpu(nth16->wSequence));
  98         }
  99         ctx->rx_seq = le16_to_cpu(nth16->wSequence);
 100
 101         return le16_to_cpu(nth16->wNdpIndex);
 102 }
 103
 104 static int mbim_rx_verify_ndp16(struct sk_buff *skb, struct usb_cdc_ncm_ndp16 *ndp16)
 105 {
 106         struct mhi_net_dev *dev = wwan_netdev_drvpriv(skb->dev);
 107         int ret;
 108
 109         if (le16_to_cpu(ndp16->wLength) < USB_CDC_NCM_NDP16_LENGTH_MIN) {
 110                 netif_dbg(dev, rx_err, dev->ndev, "invalid DPT16 length <%u>\n",
 111                           le16_to_cpu(ndp16->wLength));
 112                 return -EINVAL;
 113         }
 114
 115         ret = ((le16_to_cpu(ndp16->wLength) - sizeof(struct usb_cdc_ncm_ndp16))
 116                         / sizeof(struct usb_cdc_ncm_dpe16));
 117         ret--; /* Last entry is always a NULL terminator */
 118
 119         if (sizeof(struct usb_cdc_ncm_ndp16) +
 120              ret * sizeof(struct usb_cdc_ncm_dpe16) > skb->len) {
 121                 netif_dbg(dev, rx_err, dev->ndev,
 122                           "Invalid nframes = %d\n", ret);
 123                 return -EINVAL;
 124         }
 125
 126         return ret;
 127 }
 128
 129 static void mbim_rx(struct mhi_net_dev *mhi_netdev, struct sk_buff *skb)
 130 {
 131         struct net_device *ndev = mhi_netdev->ndev;
 132         int ndpoffset;
 133
 134         /* Check NTB header and retrieve first NDP offset */
 135         ndpoffset = mbim_rx_verify_nth16(skb);
 136         if (ndpoffset < 0) {
 137                 net_err_ratelimited("%s: Incorrect NTB header\n", ndev->name);
 138                 goto error;
 139         }
 140
 141         /* Process each NDP */
 142         while (1) {
 143                 struct usb_cdc_ncm_ndp16 ndp16;
 144                 struct usb_cdc_ncm_dpe16 dpe16;
 145                 int nframes, n, dpeoffset;
 146
 147                 if (skb_copy_bits(skb, ndpoffset, &ndp16, sizeof(ndp16))) {
 148                         net_err_ratelimited("%s: Incorrect NDP offset (%u)\n",
 149                                             ndev->name, ndpoffset);
 150                         __mbim_length_errors_inc(mhi_netdev);
 151                         goto error;
 152                 }
 153
 154                 /* Check NDP header and retrieve number of datagrams */
 155                 nframes = mbim_rx_verify_ndp16(skb, &ndp16);
 156                 if (nframes < 0) {
 157                         net_err_ratelimited("%s: Incorrect NDP16\n", ndev->name);
 158                         __mbim_length_errors_inc(mhi_netdev);
 159                         goto error;
 160                 }
 161
 162                  /* Only IP data type supported, no DSS in MHI context */
 163                 if ((ndp16.dwSignature & cpu_to_le32(MBIM_NDP16_SIGN_MASK))
 164                                 != cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN)) {
 165                         net_err_ratelimited("%s: Unsupported NDP type\n", ndev->name);
 166                         __mbim_errors_inc(mhi_netdev);
 167                         goto next_ndp;
 168                 }
 169
 170                 /* Only primary IP session 0 (0x00) supported for now */
 171                 if (ndp16.dwSignature & ~cpu_to_le32(MBIM_NDP16_SIGN_MASK)) {
 172                         net_err_ratelimited("%s: bad packet session\n", ndev->name);
 173                         __mbim_errors_inc(mhi_netdev);
 174                         goto next_ndp;
 175                 }
 176
 177                 /* de-aggregate and deliver IP packets */
 178                 dpeoffset = ndpoffset + sizeof(struct usb_cdc_ncm_ndp16);
 179                 for (n = 0; n < nframes; n++, dpeoffset += sizeof(dpe16)) {
 180                         u16 dgram_offset, dgram_len;
 181                         struct sk_buff *skbn;
 182
 183                         if (skb_copy_bits(skb, dpeoffset, &dpe16, sizeof(dpe16)))
 184                                 break;
 185
 186                         dgram_offset = le16_to_cpu(dpe16.wDatagramIndex);
 187                         dgram_len = le16_to_cpu(dpe16.wDatagramLength);
 188
 189                         if (!dgram_offset || !dgram_len)
 190                                 break; /* null terminator */
 191
 192                         skbn = netdev_alloc_skb(ndev, dgram_len);
 193                         if (!skbn)
 194                                 continue;
 195
 196                         skb_put(skbn, dgram_len);
 197                         skb_copy_bits(skb, dgram_offset, skbn->data, dgram_len);
 198
 199                         switch (skbn->data[0] & 0xf0) {
 200                         case 0x40:
 201                                 skbn->protocol = htons(ETH_P_IP);
 202                                 break;
 203                         case 0x60:
 204                                 skbn->protocol = htons(ETH_P_IPV6);
 205                                 break;
 206                         default:
 207                                 net_err_ratelimited("%s: unknown protocol\n",
 208                                                     ndev->name);
 209                                 __mbim_errors_inc(mhi_netdev);
 210                                 dev_kfree_skb_any(skbn);
 211                                 continue;
 212                         }
 213
 214                         u64_stats_update_begin(&mhi_netdev->stats.rx_syncp);
 215                         u64_stats_inc(&mhi_netdev->stats.rx_packets);
 216                         u64_stats_add(&mhi_netdev->stats.rx_bytes, skbn->len);
 217                         u64_stats_update_end(&mhi_netdev->stats.rx_syncp);
 218                         netif_rx(skbn);
 219                 }
 220 next_ndp:
 221                 /* Other NDP to process? */
 222                 ndpoffset = (int)le16_to_cpu(ndp16.wNextNdpIndex);
 223                 if (!ndpoffset)
 224                         break;
 225         }
 226
 227         /* free skb */
 228         dev_consume_skb_any(skb);
 229         return;
 230 error:
 231         dev_kfree_skb_any(skb);
 232 }
 233
 234 struct mbim_tx_hdr {
 235         struct usb_cdc_ncm_nth16 nth16;
 236         struct usb_cdc_ncm_ndp16 ndp16;
 237         struct usb_cdc_ncm_dpe16 dpe16[2];
 238 } __packed;
 239
 240 static struct sk_buff *mbim_tx_fixup(struct mhi_net_dev *mhi_netdev,
 241                                      struct sk_buff *skb)
 242 {
 243         struct mbim_context *ctx = mhi_netdev->proto_data;
 244         unsigned int dgram_size = skb->len;
 245         struct usb_cdc_ncm_nth16 *nth16;
 246         struct usb_cdc_ncm_ndp16 *ndp16;
 247         struct mbim_tx_hdr *mbim_hdr;
 248
 249         /* For now, this is a partial implementation of CDC MBIM, only one NDP
 250          * is sent, containing the IP packet (no aggregation).
 251          */
 252
 253         /* Ensure we have enough headroom for crafting MBIM header */
 254         if (skb_cow_head(skb, sizeof(struct mbim_tx_hdr))) {
 255                 dev_kfree_skb_any(skb);
 256                 return NULL;
 257         }
 258
 259         mbim_hdr = skb_push(skb, sizeof(struct mbim_tx_hdr));
 260
 261         /* Fill NTB header */
 262         nth16 = &mbim_hdr->nth16;
 263         nth16->dwSignature = cpu_to_le32(USB_CDC_NCM_NTH16_SIGN);
 264         nth16->wHeaderLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16));
 265         nth16->wSequence = cpu_to_le16(ctx->tx_seq++);
 266         nth16->wBlockLength = cpu_to_le16(skb->len);
 267         nth16->wNdpIndex = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16));
 268
 269         /* Fill the unique NDP */
 270         ndp16 = &mbim_hdr->ndp16;
 271         ndp16->dwSignature = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
 272         ndp16->wLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_ndp16)
 273                                         + sizeof(struct usb_cdc_ncm_dpe16) * 2);
 274         ndp16->wNextNdpIndex = 0;
 275
 276         /* Datagram follows the mbim header */
 277         ndp16->dpe16[0].wDatagramIndex = cpu_to_le16(sizeof(struct mbim_tx_hdr));
 278         ndp16->dpe16[0].wDatagramLength = cpu_to_le16(dgram_size);
 279
 280         /* null termination */
 281         ndp16->dpe16[1].wDatagramIndex = 0;
 282         ndp16->dpe16[1].wDatagramLength = 0;
 283
 284         return skb;
 285 }
 286
 287 static int mbim_init(struct mhi_net_dev *mhi_netdev)
 288 {
 289         struct net_device *ndev = mhi_netdev->ndev;
 290
 291         mhi_netdev->proto_data = devm_kzalloc(&ndev->dev,
 292                                               sizeof(struct mbim_context),
 293                                               GFP_KERNEL);
 294         if (!mhi_netdev->proto_data)
 295                 return -ENOMEM;
 296
 297         ndev->needed_headroom = sizeof(struct mbim_tx_hdr);
 298         ndev->mtu = MHI_MBIM_DEFAULT_MTU;
 299
 300         if (!mhi_netdev->mru)
 301                 mhi_netdev->mru = MHI_MBIM_DEFAULT_MRU;
 302
 303         return 0;
 304 }
 305
 306 const struct mhi_net_proto proto_mbim = {
 307         .init = mbim_init,
 308         .rx = mbim_rx,
 309         .tx_fixup = mbim_tx_fixup,
 310 };