From a82c7ce5bc5be4d0cf8004d1f2ee362f9748c922 Mon Sep 17 00:00:00 2001 From: =?utf8?q?Bj=C3=B8rn=20Mork?= Date: Mon, 22 Oct 2012 10:56:39 +0000 Subject: [PATCH] net: cdc_ncm: map MBIM IPS SessionID to VLAN ID MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit MBIM devices can support up to 256 independent IP Streams. The main network device will only handle SessionID 0. Mapping SessionIDs 1 to 255 to VLANs using the SessionID as VLAN ID allow userspace to use these streams with traditional tools like vconfig. Signed-off-by: Bjørn Mork Signed-off-by: David S. Miller --- drivers/net/usb/cdc_mbim.c | 51 ++++++++++++++++++++++++++++++++------ 1 file changed, 43 insertions(+), 8 deletions(-) diff --git a/drivers/net/usb/cdc_mbim.c b/drivers/net/usb/cdc_mbim.c index 2f21139466ec..45f5f50f8f26 100644 --- a/drivers/net/usb/cdc_mbim.c +++ b/drivers/net/usb/cdc_mbim.c @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -107,6 +108,9 @@ static int cdc_mbim_bind(struct usbnet *dev, struct usb_interface *intf) /* MBIM cannot do ARP */ dev->net->flags |= IFF_NOARP; + + /* no need to put the VLAN tci in the packet headers */ + dev->net->features |= NETIF_F_HW_VLAN_TX; err: return ret; } @@ -131,6 +135,9 @@ static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb struct sk_buff *skb_out; struct cdc_mbim_state *info = (void *)&dev->data; struct cdc_ncm_ctx *ctx = info->ctx; + __le32 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN); + u16 tci = 0; + u8 *c; if (!ctx) goto error; @@ -139,6 +146,24 @@ static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb if (skb->len <= sizeof(ETH_HLEN)) goto error; + /* mapping VLANs to MBIM sessions: + * no tag => IPS session <0> + * 1 - 255 => IPS session + * 256 - 4095 => unsupported, drop + */ + vlan_get_tag(skb, &tci); + + switch (tci & 0x0f00) { + case 0x0000: /* VLAN ID 0 - 255 */ + c = (u8 *)&sign; + c[3] = tci; + break; + default: + netif_err(dev, tx_err, dev->net, + "unsupported tci=0x%04x\n", tci); + goto error; + } + skb_reset_mac_header(skb); switch (eth_hdr(skb)->h_proto) { case htons(ETH_P_IP): @@ -151,7 +176,7 @@ static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb } spin_lock_bh(&ctx->mtx); - skb_out = cdc_ncm_fill_tx_frame(ctx, skb, cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN)); + skb_out = cdc_ncm_fill_tx_frame(ctx, skb, sign); spin_unlock_bh(&ctx->mtx); return skb_out; @@ -162,11 +187,14 @@ error: return NULL; } -static struct sk_buff *cdc_mbim_process_dgram(struct usbnet *dev, u8 *buf, size_t len) +static struct sk_buff *cdc_mbim_process_dgram(struct usbnet *dev, u8 *buf, size_t len, u16 tci) { __be16 proto; struct sk_buff *skb = NULL; + if (len < sizeof(struct iphdr)) + goto err; + switch (*buf & 0xf0) { case 0x40: proto = htons(ETH_P_IP); @@ -191,6 +219,10 @@ static struct sk_buff *cdc_mbim_process_dgram(struct usbnet *dev, u8 *buf, size_ /* add datagram */ memcpy(skb_put(skb, len), buf, len); + + /* map MBIM session to VLAN */ + if (tci) + vlan_put_tag(skb, tci); err: return skb; } @@ -198,7 +230,8 @@ err: static int cdc_mbim_rx_fixup(struct usbnet *dev, struct sk_buff *skb_in) { struct sk_buff *skb; - struct cdc_ncm_ctx *ctx = (struct cdc_ncm_ctx *)dev->data[0]; + struct cdc_mbim_state *info = (void *)&dev->data; + struct cdc_ncm_ctx *ctx = info->ctx; int len; int nframes; int x; @@ -207,6 +240,8 @@ static int cdc_mbim_rx_fixup(struct usbnet *dev, struct sk_buff *skb_in) struct usb_cdc_ncm_dpe16 *dpe16; int ndpoffset; int loopcount = 50; /* arbitrary max preventing infinite loop */ + u8 *c; + u16 tci; ndpoffset = cdc_ncm_rx_verify_nth16(ctx, skb_in); if (ndpoffset < 0) @@ -219,9 +254,10 @@ next_ndp: ndp16 = (struct usb_cdc_ncm_ndp16 *)(skb_in->data + ndpoffset); - /* only supporting IPS Session #0 for now */ - switch (ndp16->dwSignature) { + switch (ndp16->dwSignature & cpu_to_le32(0x00ffffff)) { case cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN): + c = (u8 *)&ndp16->dwSignature; + tci = c[3]; break; default: netif_dbg(dev, rx_err, dev->net, @@ -247,8 +283,7 @@ next_ndp: } /* sanity checking */ - if (((offset + len) > skb_in->len) || (len > ctx->rx_max) || - (len < sizeof(struct iphdr))) { + if (((offset + len) > skb_in->len) || (len > ctx->rx_max)) { netif_dbg(dev, rx_err, dev->net, "invalid frame detected (ignored) offset[%u]=%u, length=%u, skb=%p\n", x, offset, len, skb_in); @@ -256,7 +291,7 @@ next_ndp: goto err_ndp; break; } else { - skb = cdc_mbim_process_dgram(dev, skb_in->data + offset, len); + skb = cdc_mbim_process_dgram(dev, skb_in->data + offset, len, tci); if (!skb) goto error; usbnet_skb_return(dev, skb); -- 2.20.1