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>

1 files changed, 78 insertions, 68 deletions

diff --git a/drivers/usb/gadget/f_ncm.c b/drivers/usb/gadget/f_ncm.c
index a9499fd30792..d0ebbac8845f 100644
--- 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);