0001
0002
0003
0004
0005
0006
0007
0008
0009
0010 #include <linux/module.h>
0011 #include <linux/netdevice.h>
0012 #include <linux/ethtool.h>
0013 #include <linux/if_vlan.h>
0014 #include <linux/ip.h>
0015 #include <linux/mii.h>
0016 #include <linux/usb.h>
0017 #include <linux/usb/cdc.h>
0018 #include <linux/usb/usbnet.h>
0019 #include <linux/usb/cdc-wdm.h>
0020 #include <linux/usb/cdc_ncm.h>
0021 #include <net/ipv6.h>
0022 #include <net/addrconf.h>
0023 #include <net/ipv6_stubs.h>
0024 #include <net/ndisc.h>
0025
0026
0027 #define MBIM_IPS0_VID 4094
0028
0029
0030 struct cdc_mbim_state {
0031 struct cdc_ncm_ctx *ctx;
0032 atomic_t pmcount;
0033 struct usb_driver *subdriver;
0034 unsigned long _unused;
0035 unsigned long flags;
0036 };
0037
0038
0039 enum cdc_mbim_flags {
0040 FLAG_IPS0_VLAN = 1 << 0,
0041 };
0042
0043
0044 static int cdc_mbim_manage_power(struct usbnet *dev, int on)
0045 {
0046 struct cdc_mbim_state *info = (void *)&dev->data;
0047 int rv = 0;
0048
0049 dev_dbg(&dev->intf->dev, "%s() pmcount=%d, on=%d\n", __func__, atomic_read(&info->pmcount), on);
0050
0051 if ((on && atomic_add_return(1, &info->pmcount) == 1) || (!on && atomic_dec_and_test(&info->pmcount))) {
0052
0053 rv = usb_autopm_get_interface(dev->intf);
0054 dev->intf->needs_remote_wakeup = on;
0055 if (!rv)
0056 usb_autopm_put_interface(dev->intf);
0057 }
0058 return 0;
0059 }
0060
0061 static int cdc_mbim_wdm_manage_power(struct usb_interface *intf, int status)
0062 {
0063 struct usbnet *dev = usb_get_intfdata(intf);
0064
0065
0066 if (!dev)
0067 return 0;
0068
0069 return cdc_mbim_manage_power(dev, status);
0070 }
0071
0072 static int cdc_mbim_rx_add_vid(struct net_device *netdev, __be16 proto, u16 vid)
0073 {
0074 struct usbnet *dev = netdev_priv(netdev);
0075 struct cdc_mbim_state *info = (void *)&dev->data;
0076
0077
0078 if (vid == MBIM_IPS0_VID)
0079 info->flags |= FLAG_IPS0_VLAN;
0080 else
0081 if (vid >= 512)
0082 return -EINVAL;
0083 return 0;
0084 }
0085
0086 static int cdc_mbim_rx_kill_vid(struct net_device *netdev, __be16 proto, u16 vid)
0087 {
0088 struct usbnet *dev = netdev_priv(netdev);
0089 struct cdc_mbim_state *info = (void *)&dev->data;
0090
0091
0092 if (vid == MBIM_IPS0_VID)
0093 info->flags &= ~FLAG_IPS0_VLAN;
0094 return 0;
0095 }
0096
0097 static const struct net_device_ops cdc_mbim_netdev_ops = {
0098 .ndo_open = usbnet_open,
0099 .ndo_stop = usbnet_stop,
0100 .ndo_start_xmit = usbnet_start_xmit,
0101 .ndo_tx_timeout = usbnet_tx_timeout,
0102 .ndo_get_stats64 = dev_get_tstats64,
0103 .ndo_change_mtu = cdc_ncm_change_mtu,
0104 .ndo_set_mac_address = eth_mac_addr,
0105 .ndo_validate_addr = eth_validate_addr,
0106 .ndo_vlan_rx_add_vid = cdc_mbim_rx_add_vid,
0107 .ndo_vlan_rx_kill_vid = cdc_mbim_rx_kill_vid,
0108 };
0109
0110
0111
0112
0113
0114 static int cdc_mbim_set_ctrlalt(struct usbnet *dev, struct usb_interface *intf, u8 alt)
0115 {
0116 struct usb_driver *driver = to_usb_driver(intf->dev.driver);
0117 const struct usb_device_id *id;
0118 struct driver_info *info;
0119 int ret;
0120
0121 ret = usb_set_interface(dev->udev,
0122 intf->cur_altsetting->desc.bInterfaceNumber,
0123 alt);
0124 if (ret)
0125 return ret;
0126
0127 id = usb_match_id(intf, driver->id_table);
0128 if (!id)
0129 return -ENODEV;
0130
0131 info = (struct driver_info *)id->driver_info;
0132 if (info != dev->driver_info) {
0133 dev_dbg(&intf->dev, "driver_info updated to '%s'\n",
0134 info->description);
0135 dev->driver_info = info;
0136 }
0137 return 0;
0138 }
0139
0140 static int cdc_mbim_bind(struct usbnet *dev, struct usb_interface *intf)
0141 {
0142 struct cdc_ncm_ctx *ctx;
0143 struct usb_driver *subdriver = ERR_PTR(-ENODEV);
0144 int ret = -ENODEV;
0145 u8 data_altsetting = 1;
0146 struct cdc_mbim_state *info = (void *)&dev->data;
0147
0148
0149 if (cdc_ncm_select_altsetting(intf) == CDC_NCM_COMM_ALTSETTING_MBIM) {
0150 data_altsetting = CDC_NCM_DATA_ALTSETTING_MBIM;
0151 ret = cdc_mbim_set_ctrlalt(dev, intf, CDC_NCM_COMM_ALTSETTING_MBIM);
0152 if (ret)
0153 goto err;
0154 ret = -ENODEV;
0155 }
0156
0157
0158 if (!cdc_ncm_comm_intf_is_mbim(intf->cur_altsetting))
0159 goto err;
0160
0161 ret = cdc_ncm_bind_common(dev, intf, data_altsetting, dev->driver_info->data);
0162 if (ret)
0163 goto err;
0164
0165 ctx = info->ctx;
0166
0167
0168 if (ctx->mbim_desc && dev->status)
0169 subdriver = usb_cdc_wdm_register(ctx->control,
0170 &dev->status->desc,
0171 le16_to_cpu(ctx->mbim_desc->wMaxControlMessage),
0172 WWAN_PORT_MBIM,
0173 cdc_mbim_wdm_manage_power);
0174 if (IS_ERR(subdriver)) {
0175 ret = PTR_ERR(subdriver);
0176 cdc_ncm_unbind(dev, intf);
0177 goto err;
0178 }
0179
0180
0181 dev->status = NULL;
0182 info->subdriver = subdriver;
0183
0184
0185 dev->net->flags |= IFF_NOARP;
0186
0187
0188 dev->net->features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_FILTER;
0189
0190
0191 dev->net->netdev_ops = &cdc_mbim_netdev_ops;
0192 err:
0193 return ret;
0194 }
0195
0196 static void cdc_mbim_unbind(struct usbnet *dev, struct usb_interface *intf)
0197 {
0198 struct cdc_mbim_state *info = (void *)&dev->data;
0199 struct cdc_ncm_ctx *ctx = info->ctx;
0200
0201
0202 if (info->subdriver && info->subdriver->disconnect)
0203 info->subdriver->disconnect(ctx->control);
0204 info->subdriver = NULL;
0205
0206
0207 cdc_ncm_unbind(dev, intf);
0208 }
0209
0210
0211 static bool is_ip_proto(__be16 proto)
0212 {
0213 switch (proto) {
0214 case htons(ETH_P_IP):
0215 case htons(ETH_P_IPV6):
0216 return true;
0217 }
0218 return false;
0219 }
0220
0221 static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags)
0222 {
0223 struct sk_buff *skb_out;
0224 struct cdc_mbim_state *info = (void *)&dev->data;
0225 struct cdc_ncm_ctx *ctx = info->ctx;
0226 __le32 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
0227 u16 tci = 0;
0228 bool is_ip;
0229 u8 *c;
0230
0231 if (!ctx)
0232 goto error;
0233
0234 if (skb) {
0235 if (skb->len <= ETH_HLEN)
0236 goto error;
0237
0238
0239
0240
0241
0242
0243
0244 skb_reset_mac_header(skb);
0245 if (vlan_get_tag(skb, &tci) < 0 && skb->len > VLAN_ETH_HLEN &&
0246 __vlan_get_tag(skb, &tci) == 0) {
0247 is_ip = is_ip_proto(vlan_eth_hdr(skb)->h_vlan_encapsulated_proto);
0248 skb_pull(skb, VLAN_ETH_HLEN);
0249 } else {
0250 is_ip = is_ip_proto(eth_hdr(skb)->h_proto);
0251 skb_pull(skb, ETH_HLEN);
0252 }
0253
0254
0255 if (info->flags & FLAG_IPS0_VLAN) {
0256
0257 if (!tci)
0258 goto error;
0259
0260 if (tci == MBIM_IPS0_VID)
0261 tci = 0;
0262 }
0263
0264
0265
0266
0267
0268
0269
0270
0271
0272 switch (tci & 0x0f00) {
0273 case 0x0000:
0274 if (!is_ip)
0275 goto error;
0276 c = (u8 *)&sign;
0277 c[3] = tci;
0278 break;
0279 case 0x0100:
0280 if (is_ip)
0281 goto error;
0282 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_DSS_SIGN);
0283 c = (u8 *)&sign;
0284 c[3] = tci;
0285 break;
0286 default:
0287 netif_err(dev, tx_err, dev->net,
0288 "unsupported tci=0x%04x\n", tci);
0289 goto error;
0290 }
0291 }
0292
0293 spin_lock_bh(&ctx->mtx);
0294 skb_out = cdc_ncm_fill_tx_frame(dev, skb, sign);
0295 spin_unlock_bh(&ctx->mtx);
0296 return skb_out;
0297
0298 error:
0299 if (skb)
0300 dev_kfree_skb_any(skb);
0301
0302 return NULL;
0303 }
0304
0305
0306
0307
0308
0309 static void do_neigh_solicit(struct usbnet *dev, u8 *buf, u16 tci)
0310 {
0311 struct ipv6hdr *iph = (void *)buf;
0312 struct nd_msg *msg = (void *)(iph + 1);
0313 struct net_device *netdev;
0314 struct inet6_dev *in6_dev;
0315 bool is_router;
0316
0317
0318
0319
0320 if (!ipv6_addr_is_solict_mult(&iph->daddr) ||
0321 !(ipv6_addr_type(&iph->saddr) & IPV6_ADDR_UNICAST))
0322 return;
0323
0324
0325 rcu_read_lock();
0326 if (tci) {
0327 netdev = __vlan_find_dev_deep_rcu(dev->net, htons(ETH_P_8021Q),
0328 tci);
0329 if (!netdev) {
0330 rcu_read_unlock();
0331 return;
0332 }
0333 } else {
0334 netdev = dev->net;
0335 }
0336 dev_hold(netdev);
0337 rcu_read_unlock();
0338
0339 in6_dev = in6_dev_get(netdev);
0340 if (!in6_dev)
0341 goto out;
0342 is_router = !!in6_dev->cnf.forwarding;
0343 in6_dev_put(in6_dev);
0344
0345
0346 ipv6_stub->ndisc_send_na(netdev, &iph->saddr, &msg->target,
0347 is_router ,
0348 true ,
0349 false ,
0350 true );
0351 out:
0352 dev_put(netdev);
0353 }
0354
0355 static bool is_neigh_solicit(u8 *buf, size_t len)
0356 {
0357 struct ipv6hdr *iph = (void *)buf;
0358 struct nd_msg *msg = (void *)(iph + 1);
0359
0360 return (len >= sizeof(struct ipv6hdr) + sizeof(struct nd_msg) &&
0361 iph->nexthdr == IPPROTO_ICMPV6 &&
0362 msg->icmph.icmp6_code == 0 &&
0363 msg->icmph.icmp6_type == NDISC_NEIGHBOUR_SOLICITATION);
0364 }
0365
0366
0367 static struct sk_buff *cdc_mbim_process_dgram(struct usbnet *dev, u8 *buf, size_t len, u16 tci)
0368 {
0369 __be16 proto = htons(ETH_P_802_3);
0370 struct sk_buff *skb = NULL;
0371
0372 if (tci < 256 || tci == MBIM_IPS0_VID) {
0373 if (len < sizeof(struct iphdr))
0374 goto err;
0375
0376 switch (*buf & 0xf0) {
0377 case 0x40:
0378 proto = htons(ETH_P_IP);
0379 break;
0380 case 0x60:
0381 if (is_neigh_solicit(buf, len))
0382 do_neigh_solicit(dev, buf, tci);
0383 proto = htons(ETH_P_IPV6);
0384 break;
0385 default:
0386 goto err;
0387 }
0388 }
0389
0390 skb = netdev_alloc_skb_ip_align(dev->net, len + ETH_HLEN);
0391 if (!skb)
0392 goto err;
0393
0394
0395 skb_put(skb, ETH_HLEN);
0396 skb_reset_mac_header(skb);
0397 eth_hdr(skb)->h_proto = proto;
0398 eth_zero_addr(eth_hdr(skb)->h_source);
0399 memcpy(eth_hdr(skb)->h_dest, dev->net->dev_addr, ETH_ALEN);
0400
0401
0402 skb_put_data(skb, buf, len);
0403
0404
0405 if (tci)
0406 __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), tci);
0407 err:
0408 return skb;
0409 }
0410
0411 static int cdc_mbim_rx_fixup(struct usbnet *dev, struct sk_buff *skb_in)
0412 {
0413 struct sk_buff *skb;
0414 struct cdc_mbim_state *info = (void *)&dev->data;
0415 struct cdc_ncm_ctx *ctx = info->ctx;
0416 int len;
0417 int nframes;
0418 int x;
0419 int offset;
0420 struct usb_cdc_ncm_ndp16 *ndp16;
0421 struct usb_cdc_ncm_dpe16 *dpe16;
0422 int ndpoffset;
0423 int loopcount = 50;
0424 u32 payload = 0;
0425 u8 *c;
0426 u16 tci;
0427
0428 ndpoffset = cdc_ncm_rx_verify_nth16(ctx, skb_in);
0429 if (ndpoffset < 0)
0430 goto error;
0431
0432 next_ndp:
0433 nframes = cdc_ncm_rx_verify_ndp16(skb_in, ndpoffset);
0434 if (nframes < 0)
0435 goto error;
0436
0437 ndp16 = (struct usb_cdc_ncm_ndp16 *)(skb_in->data + ndpoffset);
0438
0439 switch (ndp16->dwSignature & cpu_to_le32(0x00ffffff)) {
0440 case cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN):
0441 c = (u8 *)&ndp16->dwSignature;
0442 tci = c[3];
0443
0444 if (!tci && info->flags & FLAG_IPS0_VLAN)
0445 tci = MBIM_IPS0_VID;
0446 break;
0447 case cpu_to_le32(USB_CDC_MBIM_NDP16_DSS_SIGN):
0448 c = (u8 *)&ndp16->dwSignature;
0449 tci = c[3] + 256;
0450 break;
0451 default:
0452 netif_dbg(dev, rx_err, dev->net,
0453 "unsupported NDP signature <0x%08x>\n",
0454 le32_to_cpu(ndp16->dwSignature));
0455 goto err_ndp;
0456
0457 }
0458
0459 dpe16 = ndp16->dpe16;
0460 for (x = 0; x < nframes; x++, dpe16++) {
0461 offset = le16_to_cpu(dpe16->wDatagramIndex);
0462 len = le16_to_cpu(dpe16->wDatagramLength);
0463
0464
0465
0466
0467
0468 if ((offset == 0) || (len == 0)) {
0469 if (!x)
0470 goto err_ndp;
0471 break;
0472 }
0473
0474
0475 if (((offset + len) > skb_in->len) || (len > ctx->rx_max)) {
0476 netif_dbg(dev, rx_err, dev->net,
0477 "invalid frame detected (ignored) offset[%u]=%u, length=%u, skb=%p\n",
0478 x, offset, len, skb_in);
0479 if (!x)
0480 goto err_ndp;
0481 break;
0482 } else {
0483 skb = cdc_mbim_process_dgram(dev, skb_in->data + offset, len, tci);
0484 if (!skb)
0485 goto error;
0486 usbnet_skb_return(dev, skb);
0487 payload += len;
0488 }
0489 }
0490 err_ndp:
0491
0492 ndpoffset = le16_to_cpu(ndp16->wNextNdpIndex);
0493 if (ndpoffset && loopcount--)
0494 goto next_ndp;
0495
0496
0497 ctx->rx_overhead += skb_in->len - payload;
0498 ctx->rx_ntbs++;
0499
0500 return 1;
0501 error:
0502 return 0;
0503 }
0504
0505 static int cdc_mbim_suspend(struct usb_interface *intf, pm_message_t message)
0506 {
0507 int ret = -ENODEV;
0508 struct usbnet *dev = usb_get_intfdata(intf);
0509 struct cdc_mbim_state *info = (void *)&dev->data;
0510 struct cdc_ncm_ctx *ctx = info->ctx;
0511
0512 if (!ctx)
0513 goto error;
0514
0515
0516
0517
0518
0519
0520 ret = usbnet_suspend(intf, message);
0521 if (ret < 0)
0522 goto error;
0523
0524 if (intf == ctx->control && info->subdriver && info->subdriver->suspend)
0525 ret = info->subdriver->suspend(intf, message);
0526 if (ret < 0)
0527 usbnet_resume(intf);
0528
0529 error:
0530 return ret;
0531 }
0532
0533 static int cdc_mbim_resume(struct usb_interface *intf)
0534 {
0535 int ret = 0;
0536 struct usbnet *dev = usb_get_intfdata(intf);
0537 struct cdc_mbim_state *info = (void *)&dev->data;
0538 struct cdc_ncm_ctx *ctx = info->ctx;
0539 bool callsub = (intf == ctx->control && info->subdriver && info->subdriver->resume);
0540
0541 if (callsub)
0542 ret = info->subdriver->resume(intf);
0543 if (ret < 0)
0544 goto err;
0545 ret = usbnet_resume(intf);
0546 if (ret < 0 && callsub)
0547 info->subdriver->suspend(intf, PMSG_SUSPEND);
0548 err:
0549 return ret;
0550 }
0551
0552 static const struct driver_info cdc_mbim_info = {
0553 .description = "CDC MBIM",
0554 .flags = FLAG_NO_SETINT | FLAG_MULTI_PACKET | FLAG_WWAN,
0555 .bind = cdc_mbim_bind,
0556 .unbind = cdc_mbim_unbind,
0557 .manage_power = cdc_mbim_manage_power,
0558 .rx_fixup = cdc_mbim_rx_fixup,
0559 .tx_fixup = cdc_mbim_tx_fixup,
0560 };
0561
0562
0563
0564
0565
0566
0567
0568
0569
0570
0571
0572
0573
0574
0575
0576 static const struct driver_info cdc_mbim_info_zlp = {
0577 .description = "CDC MBIM",
0578 .flags = FLAG_NO_SETINT | FLAG_MULTI_PACKET | FLAG_WWAN | FLAG_SEND_ZLP,
0579 .bind = cdc_mbim_bind,
0580 .unbind = cdc_mbim_unbind,
0581 .manage_power = cdc_mbim_manage_power,
0582 .rx_fixup = cdc_mbim_rx_fixup,
0583 .tx_fixup = cdc_mbim_tx_fixup,
0584 };
0585
0586
0587
0588
0589
0590
0591
0592
0593
0594
0595 static const struct driver_info cdc_mbim_info_ndp_to_end = {
0596 .description = "CDC MBIM",
0597 .flags = FLAG_NO_SETINT | FLAG_MULTI_PACKET | FLAG_WWAN,
0598 .bind = cdc_mbim_bind,
0599 .unbind = cdc_mbim_unbind,
0600 .manage_power = cdc_mbim_manage_power,
0601 .rx_fixup = cdc_mbim_rx_fixup,
0602 .tx_fixup = cdc_mbim_tx_fixup,
0603 .data = CDC_NCM_FLAG_NDP_TO_END,
0604 };
0605
0606
0607
0608
0609
0610 static const struct driver_info cdc_mbim_info_avoid_altsetting_toggle = {
0611 .description = "CDC MBIM",
0612 .flags = FLAG_NO_SETINT | FLAG_MULTI_PACKET | FLAG_WWAN | FLAG_SEND_ZLP,
0613 .bind = cdc_mbim_bind,
0614 .unbind = cdc_mbim_unbind,
0615 .manage_power = cdc_mbim_manage_power,
0616 .rx_fixup = cdc_mbim_rx_fixup,
0617 .tx_fixup = cdc_mbim_tx_fixup,
0618 .data = CDC_MBIM_FLAG_AVOID_ALTSETTING_TOGGLE,
0619 };
0620
0621 static const struct usb_device_id mbim_devs[] = {
0622
0623
0624
0625
0626
0627
0628
0629 { USB_INTERFACE_INFO(USB_CLASS_COMM, USB_CDC_SUBCLASS_NCM, USB_CDC_PROTO_NONE),
0630 .driver_info = (unsigned long)&cdc_mbim_info,
0631 },
0632
0633 { USB_VENDOR_AND_INTERFACE_INFO(0x0bdb, USB_CLASS_COMM, USB_CDC_SUBCLASS_MBIM, USB_CDC_PROTO_NONE),
0634 .driver_info = (unsigned long)&cdc_mbim_info,
0635 },
0636
0637
0638
0639
0640
0641
0642 { USB_VENDOR_AND_INTERFACE_INFO(0x12d1, USB_CLASS_COMM, USB_CDC_SUBCLASS_MBIM, USB_CDC_PROTO_NONE),
0643 .driver_info = (unsigned long)&cdc_mbim_info_ndp_to_end,
0644 },
0645
0646
0647
0648
0649 { USB_DEVICE_AND_INTERFACE_INFO(0x03f0, 0xa31d, USB_CLASS_COMM, USB_CDC_SUBCLASS_MBIM, USB_CDC_PROTO_NONE),
0650 .driver_info = (unsigned long)&cdc_mbim_info_ndp_to_end,
0651 },
0652
0653
0654 { USB_DEVICE_AND_INTERFACE_INFO(0x1bc7, 0x1041, USB_CLASS_COMM, USB_CDC_SUBCLASS_MBIM, USB_CDC_PROTO_NONE),
0655 .driver_info = (unsigned long)&cdc_mbim_info_avoid_altsetting_toggle,
0656 },
0657
0658
0659 { USB_DEVICE_AND_INTERFACE_INFO(0x1bc7, 0x1061, USB_CLASS_COMM, USB_CDC_SUBCLASS_MBIM, USB_CDC_PROTO_NONE),
0660 .driver_info = (unsigned long)&cdc_mbim_info_avoid_altsetting_toggle,
0661 },
0662
0663
0664 { USB_DEVICE_AND_INTERFACE_INFO(0x1bc7, 0x1071, USB_CLASS_COMM, USB_CDC_SUBCLASS_MBIM, USB_CDC_PROTO_NONE),
0665 .driver_info = (unsigned long)&cdc_mbim_info_avoid_altsetting_toggle,
0666 },
0667
0668
0669 { USB_INTERFACE_INFO(USB_CLASS_COMM, USB_CDC_SUBCLASS_MBIM, USB_CDC_PROTO_NONE),
0670 .driver_info = (unsigned long)&cdc_mbim_info_zlp,
0671 },
0672 {
0673 },
0674 };
0675 MODULE_DEVICE_TABLE(usb, mbim_devs);
0676
0677 static struct usb_driver cdc_mbim_driver = {
0678 .name = "cdc_mbim",
0679 .id_table = mbim_devs,
0680 .probe = usbnet_probe,
0681 .disconnect = usbnet_disconnect,
0682 .suspend = cdc_mbim_suspend,
0683 .resume = cdc_mbim_resume,
0684 .reset_resume = cdc_mbim_resume,
0685 .supports_autosuspend = 1,
0686 .disable_hub_initiated_lpm = 1,
0687 };
0688 module_usb_driver(cdc_mbim_driver);
0689
0690 MODULE_AUTHOR("Greg Suarez <gsuarez@smithmicro.com>");
0691 MODULE_AUTHOR("Bjørn Mork <bjorn@mork.no>");
0692 MODULE_DESCRIPTION("USB CDC MBIM host driver");
0693 MODULE_LICENSE("GPL");