usb: gadget: NCM: RX function support multiple NDPs

The NDP was ignoring the wNextNdpIndex in the NDP which
means that NTBs containing multiple NDPs would have missed
frames.

Signed-off-by: Jim Baxter <jim_baxter@mentor.com>
Signed-off-by: Felipe Balbi <balbi@ti.com>

diff --git a/drivers/usb/gadget/f_ncm.c b/drivers/usb/gadget/f_ncm.c
index a9499fd3079273ed2629b424fa11a3a4e5da2e86..d0ebbac8845f5ac8b37c724393daaa91a0164eee 100644 (file)
--- a/drivers/usb/gadget/f_ncm.c
+++ b/drivers/usb/gadget/f_ncm.c
@@ -963,6 +963,7 @@ static int ncm_unwrap_ntb(struct gether *port,
        struct f_ncm    *ncm = func_to_ncm(&port->func);
        __le16          *tmp = (void *) skb->data;
        unsigned        index, index2;
+       int             ndp_index;
        unsigned        dg_len, dg_len2;
        unsigned        ndp_len;
        struct sk_buff  *skb2;
@@ -995,91 +996,100 @@ static int ncm_unwrap_ntb(struct gether *port,
                goto err;
        }
 
-       index = get_ncm(&tmp, opts->fp_index);
-       /* NCM 3.2 */
-       if (((index % 4) != 0) && (index < opts->nth_size)) {
-               INFO(port->func.config->cdev, "Bad index: %x\n",
-                       index);
-               goto err;
-       }
-
-       /* walk through NDP */
-       tmp = ((void *)skb->data) + index;
-       if (get_unaligned_le32(tmp) != ncm->ndp_sign) {
-               INFO(port->func.config->cdev, "Wrong NDP SIGN\n");
-               goto err;
-       }
-       tmp += 2;
-
-       ndp_len = get_unaligned_le16(tmp++);
-       /*
-        * NCM 3.3.1
-        * entry is 2 items
-        * item size is 16/32 bits, opts->dgram_item_len * 2 bytes
-        * minimal: struct usb_cdc_ncm_ndpX + normal entry + zero entry
-        */
-       if ((ndp_len < opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2))
-           || (ndp_len % opts->ndplen_align != 0)) {
-               INFO(port->func.config->cdev, "Bad NDP length: %x\n", ndp_len);
-               goto err;
-       }
-       tmp += opts->reserved1;
-       tmp += opts->next_fp_index; /* skip reserved (d)wNextFpIndex */
-       tmp += opts->reserved2;
-
-       ndp_len -= opts->ndp_size;
-       index2 = get_ncm(&tmp, opts->dgram_item_len);
-       dg_len2 = get_ncm(&tmp, opts->dgram_item_len);
-       dgram_counter = 0;
+       ndp_index = get_ncm(&tmp, opts->fp_index);
 
+       /* Run through all the NDP's in the NTB */
        do {
-               index = index2;
-               dg_len = dg_len2;
-               if (dg_len < 14 + crc_len) { /* ethernet header + crc */
-                       INFO(port->func.config->cdev, "Bad dgram length: %x\n",
-                            dg_len);
+               /* NCM 3.2 */
+               if (((ndp_index % 4) != 0) &&
+                               (ndp_index < opts->nth_size)) {
+                       INFO(port->func.config->cdev, "Bad index: %#X\n",
+                            ndp_index);
                        goto err;
                }
-               if (ncm->is_crc) {
-                       uint32_t crc, crc2;
-
-                       crc = get_unaligned_le32(skb->data +
-                                                index + dg_len - crc_len);
-                       crc2 = ~crc32_le(~0,
-                                        skb->data + index,
-                                        dg_len - crc_len);
-                       if (crc != crc2) {
-                               INFO(port->func.config->cdev, "Bad CRC\n");
-                               goto err;
-                       }
+
+               /* walk through NDP */
+               tmp = (void *)(skb->data + ndp_index);
+               if (get_unaligned_le32(tmp) != ncm->ndp_sign) {
+                       INFO(port->func.config->cdev, "Wrong NDP SIGN\n");
+                       goto err;
                }
+               tmp += 2;
 
+               ndp_len = get_unaligned_le16(tmp++);
+               /*
+                * NCM 3.3.1
+                * entry is 2 items
+                * item size is 16/32 bits, opts->dgram_item_len * 2 bytes
+                * minimal: struct usb_cdc_ncm_ndpX + normal entry + zero entry
+                * Each entry is a dgram index and a dgram length.
+                */
+               if ((ndp_len < opts->ndp_size
+                               + 2 * 2 * (opts->dgram_item_len * 2))
+                               || (ndp_len % opts->ndplen_align != 0)) {
+                       INFO(port->func.config->cdev, "Bad NDP length: %#X\n",
+                            ndp_len);
+                       goto err;
+               }
+               tmp += opts->reserved1;
+               /* Check for another NDP (d)wNextNdpIndex */
+               ndp_index = get_ncm(&tmp, opts->next_fp_index);
+               tmp += opts->reserved2;
+
+               ndp_len -= opts->ndp_size;
                index2 = get_ncm(&tmp, opts->dgram_item_len);
                dg_len2 = get_ncm(&tmp, opts->dgram_item_len);
+               dgram_counter = 0;
+
+               do {
+                       index = index2;
+                       dg_len = dg_len2;
+                       if (dg_len < 14 + crc_len) { /* ethernet hdr + crc */
+                               INFO(port->func.config->cdev,
+                                    "Bad dgram length: %#X\n", dg_len);
+                               goto err;
+                       }
+                       if (ncm->is_crc) {
+                               uint32_t crc, crc2;
+
+                               crc = get_unaligned_le32(skb->data +
+                                                        index + dg_len -
+                                                        crc_len);
+                               crc2 = ~crc32_le(~0,
+                                                skb->data + index,
+                                                dg_len - crc_len);
+                               if (crc != crc2) {
+                                       INFO(port->func.config->cdev,
+                                            "Bad CRC\n");
+                                       goto err;
+                               }
+                       }
+
+                       index2 = get_ncm(&tmp, opts->dgram_item_len);
+                       dg_len2 = get_ncm(&tmp, opts->dgram_item_len);
 
-               if (index2 == 0 || dg_len2 == 0) {
-                       skb2 = skb;
-               } else {
                        skb2 = skb_clone(skb, GFP_ATOMIC);
                        if (skb2 == NULL)
                                goto err;
-               }
 
-               if (!skb_pull(skb2, index)) {
-                       ret = -EOVERFLOW;
-                       goto err;
-               }
+                       if (!skb_pull(skb2, index)) {
+                               ret = -EOVERFLOW;
+                               goto err;
+                       }
 
-               skb_trim(skb2, dg_len - crc_len);
-               skb_queue_tail(list, skb2);
+                       skb_trim(skb2, dg_len - crc_len);
+                       skb_queue_tail(list, skb2);
 
-               ndp_len -= 2 * (opts->dgram_item_len * 2);
+                       ndp_len -= 2 * (opts->dgram_item_len * 2);
 
-               dgram_counter++;
+                       dgram_counter++;
 
-               if (index2 == 0 || dg_len2 == 0)
-                       break;
-       } while (ndp_len > 2 * (opts->dgram_item_len * 2)); /* zero entry */
+                       if (index2 == 0 || dg_len2 == 0)
+                               break;
+               } while (ndp_len > 2 * (opts->dgram_item_len * 2));
+       } while (ndp_index);
+
+       dev_kfree_skb_any(skb);
 
        VDBG(port->func.config->cdev,
             "Parsed NTB with %d frames\n", dgram_counter);