--- //ktx/aimus-remote-5.40/pkgs/linux_2.6/drivers/net/usb/qmi_wwan.c 2014-10-09 14:23:09.000000000 -0400 +++ /tmphome/jeffh/dev/remote/aimus/modules/Linux-2.6/pkgs/linux-GPL/drivers/net/usb/qmi_wwan.c 2014-10-09 14:23:09.000000000 -0400 @@ -14,12 +14,16 @@ #include #include #include +#include +#include #include #include #include #include #include +#define MAC48_IPV6_MULTICAST_ID 33 + /* This driver supports wwan (3G/LTE/?) devices using a vendor * specific management protocol called Qualcomm MSM Interface (QMI) - * in addition to the more common AT commands over serial interface @@ -56,7 +60,72 @@ /* default ethernet address used by the modem */ static const u8 default_modem_addr[ETH_ALEN] = {0x02, 0x50, 0xf3}; -/* Make up an ethernet header if the packet doesn't have one. + +/* Reset the ethernet header in the packet to point to this device with the + * given protocol. */ +static void qmi_wwan_reset_eth_hdr(struct usbnet *dev, + struct sk_buff *skb, + u16 proto) +{ + skb_reset_mac_header(skb); + memset(eth_hdr(skb)->h_source, 0, ETH_ALEN); + memcpy(eth_hdr(skb)->h_dest, dev->net->dev_addr, ETH_ALEN); + eth_hdr(skb)->h_proto = cpu_to_be16(proto); +} + +/* Fix the ethernet header in the packet if necessary. If the destination MAC + * address is not expected, the header is assumed to be invalid and is + * rewritten. + * + * true is returned if the packet is known to be valid; false otherwise. + */ +static bool qmi_wwan_rx_lte_fixup(struct usbnet *dev, + struct sk_buff *skb, + u16 proto) +{ + /* All MAC-48 multicast identifiers prefixed "33-33", do not overwrite + * the MAC address if it is not corrupted */ + if ((skb->data[0] == MAC48_IPV6_MULTICAST_ID) && + (skb->data[1] == MAC48_IPV6_MULTICAST_ID)) { + __be16 hdr_proto = eth_hdr(skb)->h_proto; + /* check the IP type field, if it is correct, we can consider + * this is not a corrupted packet */ + if (hdr_proto != skb->protocol) + qmi_wwan_reset_eth_hdr(dev, skb, proto); + return true; + } else if (memcmp(eth_hdr(skb)->h_dest, + dev->net->dev_addr, ETH_ALEN) == 0) { + /* MAC address is correct, no need to overwrite, pass + * through */ + return true; + } else if ((proto == ETH_P_IP) && + (skb->len >= (ETH_HLEN + sizeof(struct iphdr)))) { + struct iphdr* hdr = (struct iphdr*)&skb->data[ETH_HLEN]; + u16 len = be16_to_cpu(hdr->tot_len); + /* Length includes IP header and payload */ + if (skb->len >= (len + ETH_HLEN)) { + qmi_wwan_reset_eth_hdr(dev, skb, proto); + return true; + } + } else if ((proto == ETH_P_IPV6) && + (skb->len >= (ETH_HLEN + sizeof(struct ipv6hdr)))) { + /* IPv6 */ + struct ipv6hdr* hdr = (struct ipv6hdr*)&skb->data[ETH_HLEN]; + u16 len = be16_to_cpu(hdr->payload_len); + /* Length is payload only */ + if (skb->len >= (len + ETH_HLEN + sizeof(struct ipv6hdr))) { + qmi_wwan_reset_eth_hdr(dev, skb, proto); + return true; + } + } + return false; +} + + +/* Make up or fix the ethernet header in the packet. + * + * A firmware bug causes some LTE devices produce an invalid ethernet header + * in dual-stack mode. The header is corrected. * * A firmware bug common among several devices cause them to send raw * IP packets under some circumstances. There is no way for the @@ -78,25 +147,43 @@ */ static int qmi_wwan_rx_fixup(struct usbnet *dev, struct sk_buff *skb) { - __be16 proto; + u16 proto; /* This check is no longer done by usbnet */ if (skb->len < dev->net->hard_header_len) return 0; + /* Check for corrupt MAC headers and fix */ + if (skb->len < (ETH_HLEN + 1)) + return 0; + + switch (skb->data[ETH_HLEN] & 0xf0) { + case 0x40: + if (qmi_wwan_rx_lte_fixup(dev, skb, ETH_P_IP)) + return 1; + break; + case 0x60: + if (qmi_wwan_rx_lte_fixup(dev, skb, ETH_P_IPV6)) + return 1; + break; + } + + /* Check for raw IP packets and fix */ switch (skb->data[0] & 0xf0) { case 0x40: - proto = htons(ETH_P_IP); + proto = ETH_P_IP; break; case 0x60: - proto = htons(ETH_P_IPV6); + proto = ETH_P_IPV6; break; case 0x00: - if (is_multicast_ether_addr(skb->data)) - return 1; - /* possibly bogus destination - rewrite just in case */ - skb_reset_mac_header(skb); - goto fix_dest; + if (!is_multicast_ether_addr(skb->data)) { + /* possibly bogus destination - rewrite just in case */ + skb_reset_mac_header(skb); + memcpy(eth_hdr(skb)->h_dest, + dev->net->dev_addr, ETH_ALEN); + } + return 1; default: /* pass along other packets without modifications */ return 1; @@ -104,11 +191,7 @@ if (skb_headroom(skb) < ETH_HLEN) return 0; skb_push(skb, ETH_HLEN); - skb_reset_mac_header(skb); - eth_hdr(skb)->h_proto = proto; - memset(eth_hdr(skb)->h_source, 0, ETH_ALEN); -fix_dest: - memcpy(eth_hdr(skb)->h_dest, dev->net->dev_addr, ETH_ALEN); + qmi_wwan_reset_eth_hdr(dev, skb, proto); return 1; }