forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2024-10-09 244b2c5ca8b14627e4a17755e5922221e121c771 
 kernel/drivers/net/usb/cdc_ncm.c
....@@ -175,10 +175,17 @@
175175 	u32 val, max, min;
176176 
177177 	/* clamp new_tx to sane values */
178
-	min = ctx->max_datagram_size + ctx->max_ndp_size + sizeof(struct usb_cdc_ncm_nth16);
179
-	max = min_t(u32, CDC_NCM_NTB_MAX_SIZE_TX, le32_to_cpu(ctx->ncm_parm.dwNtbOutMaxSize));
180
-	if (max == 0)
178
+	if (ctx->is_ndp16)
179
+		min = ctx->max_datagram_size + ctx->max_ndp_size + sizeof(struct usb_cdc_ncm_nth16);
180
+	else
181
+		min = ctx->max_datagram_size + ctx->max_ndp_size + sizeof(struct usb_cdc_ncm_nth32);
182
+
183
+	if (le32_to_cpu(ctx->ncm_parm.dwNtbOutMaxSize) == 0)
181184 		max = CDC_NCM_NTB_MAX_SIZE_TX; /* dwNtbOutMaxSize not set */
185
+	else
186
+		max = clamp_t(u32, le32_to_cpu(ctx->ncm_parm.dwNtbOutMaxSize),
187
+			      USB_CDC_NCM_NTB_MIN_OUT_SIZE,
188
+			      CDC_NCM_NTB_MAX_SIZE_TX);
182189 
183190 	/* some devices set dwNtbOutMaxSize too low for the above default */
184191 	min = min(min, max);
....@@ -309,10 +316,17 @@
309316 	if (enable == (ctx->drvflags & CDC_NCM_FLAG_NDP_TO_END))
310317 		return len;
311318 
312
-	if (enable && !ctx->delayed_ndp16) {
313
-		ctx->delayed_ndp16 = kzalloc(ctx->max_ndp_size, GFP_KERNEL);
314
-		if (!ctx->delayed_ndp16)
315
-			return -ENOMEM;
319
+	if (enable) {
320
+		if (ctx->is_ndp16 && !ctx->delayed_ndp16) {
321
+			ctx->delayed_ndp16 = kzalloc(ctx->max_ndp_size, GFP_KERNEL);
322
+			if (!ctx->delayed_ndp16)
323
+				return -ENOMEM;
324
+		}
325
+		if (!ctx->is_ndp16 && !ctx->delayed_ndp32) {
326
+			ctx->delayed_ndp32 = kzalloc(ctx->max_ndp_size, GFP_KERNEL);
327
+			if (!ctx->delayed_ndp32)
328
+				return -ENOMEM;
329
+		}
316330 	}
317331 
318332 	/* flush pending data before changing flag */
....@@ -514,6 +528,9 @@
514528 			dev_err(&dev->intf->dev, "SET_CRC_MODE failed\n");
515529 	}
516530 
531
+	/* use ndp16 by default */
532
+	ctx->is_ndp16 = 1;
533
+
517534 	/* set NTB format, if both formats are supported.
518535 	 *
519536 	 * "The host shall only send this command while the NCM Data
....@@ -521,14 +538,27 @@
521538 	 */
522539 	if (le16_to_cpu(ctx->ncm_parm.bmNtbFormatsSupported) &
523540 						USB_CDC_NCM_NTB32_SUPPORTED) {
524
-		dev_dbg(&dev->intf->dev, "Setting NTB format to 16-bit\n");
525
-		err = usbnet_write_cmd(dev, USB_CDC_SET_NTB_FORMAT,
526
-				       USB_TYPE_CLASS | USB_DIR_OUT
527
-				       | USB_RECIP_INTERFACE,
528
-				       USB_CDC_NCM_NTB16_FORMAT,
529
-				       iface_no, NULL, 0);
530
-		if (err < 0)
541
+		if (ctx->drvflags & CDC_NCM_FLAG_PREFER_NTB32) {
542
+			ctx->is_ndp16 = 0;
543
+			dev_dbg(&dev->intf->dev, "Setting NTB format to 32-bit\n");
544
+			err = usbnet_write_cmd(dev, USB_CDC_SET_NTB_FORMAT,
545
+					       USB_TYPE_CLASS | USB_DIR_OUT
546
+					       | USB_RECIP_INTERFACE,
547
+					       USB_CDC_NCM_NTB32_FORMAT,
548
+					       iface_no, NULL, 0);
549
+		} else {
550
+			ctx->is_ndp16 = 1;
551
+			dev_dbg(&dev->intf->dev, "Setting NTB format to 16-bit\n");
552
+			err = usbnet_write_cmd(dev, USB_CDC_SET_NTB_FORMAT,
553
+					       USB_TYPE_CLASS | USB_DIR_OUT
554
+					       | USB_RECIP_INTERFACE,
555
+					       USB_CDC_NCM_NTB16_FORMAT,
556
+					       iface_no, NULL, 0);
557
+		}
558
+		if (err < 0) {
559
+			ctx->is_ndp16 = 1;
531560 			dev_err(&dev->intf->dev, "SET_NTB_FORMAT failed\n");
561
+		}
532562 	}
533563 
534564 	/* set initial device values */
....@@ -551,7 +581,10 @@
551581 		ctx->tx_max_datagrams = CDC_NCM_DPT_DATAGRAMS_MAX;
552582 
553583 	/* set up maximum NDP size */
554
-	ctx->max_ndp_size = sizeof(struct usb_cdc_ncm_ndp16) + (ctx->tx_max_datagrams + 1) * sizeof(struct usb_cdc_ncm_dpe16);
584
+	if (ctx->is_ndp16)
585
+		ctx->max_ndp_size = sizeof(struct usb_cdc_ncm_ndp16) + (ctx->tx_max_datagrams + 1) * sizeof(struct usb_cdc_ncm_dpe16);
586
+	else
587
+		ctx->max_ndp_size = sizeof(struct usb_cdc_ncm_ndp32) + (ctx->tx_max_datagrams + 1) * sizeof(struct usb_cdc_ncm_dpe32);
555588 
556589 	/* initial coalescing timer interval */
557590 	ctx->timer_interval = CDC_NCM_TIMER_INTERVAL_USEC * NSEC_PER_USEC;
....@@ -736,7 +769,10 @@
736769 		ctx->tx_curr_skb = NULL;
737770 	}
738771 
739
-	kfree(ctx->delayed_ndp16);
772
+	if (ctx->is_ndp16)
773
+		kfree(ctx->delayed_ndp16);
774
+	else
775
+		kfree(ctx->delayed_ndp32);
740776 
741777 	kfree(ctx);
742778 }
....@@ -761,6 +797,7 @@
761797 	.ndo_stop	     = usbnet_stop,
762798 	.ndo_start_xmit	     = usbnet_start_xmit,
763799 	.ndo_tx_timeout	     = usbnet_tx_timeout,
800
+	.ndo_set_rx_mode     = usbnet_set_rx_mode,
764801 	.ndo_get_stats64     = usbnet_get_stats64,
765802 	.ndo_change_mtu	     = cdc_ncm_change_mtu,
766803 	.ndo_set_mac_address = eth_mac_addr,
....@@ -774,10 +811,8 @@
774811 	u8 *buf;
775812 	int len;
776813 	int temp;
777
-	int err;
778814 	u8 iface_no;
779815 	struct usb_cdc_parsed_header hdr;
780
-	__le16 curr_ntb_format;
781816 
782817 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
783818 	if (!ctx)
....@@ -785,8 +820,7 @@
785820 
786821 	hrtimer_init(&ctx->tx_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
787822 	ctx->tx_timer.function = &cdc_ncm_tx_timer_cb;
788
-	ctx->bh.data = (unsigned long)dev;
789
-	ctx->bh.func = cdc_ncm_txpath_bh;
823
+	tasklet_init(&ctx->bh, cdc_ncm_txpath_bh, (unsigned long)dev);
790824 	atomic_set(&ctx->stop, 0);
791825 	spin_lock_init(&ctx->mtx);
792826 
....@@ -882,32 +916,6 @@
882916 		goto error2;
883917 	}
884918 
885
-	/*
886
-	 * Some Huawei devices have been observed to come out of reset in NDP32 mode.
887
-	 * Let's check if this is the case, and set the device to NDP16 mode again if
888
-	 * needed.
889
-	*/
890
-	if (ctx->drvflags & CDC_NCM_FLAG_RESET_NTB16) {
891
-		err = usbnet_read_cmd(dev, USB_CDC_GET_NTB_FORMAT,
892
-				      USB_TYPE_CLASS | USB_DIR_IN | USB_RECIP_INTERFACE,
893
-				      0, iface_no, &curr_ntb_format, 2);
894
-		if (err < 0) {
895
-			goto error2;
896
-		}
897
-
898
-		if (curr_ntb_format == cpu_to_le16(USB_CDC_NCM_NTB32_FORMAT)) {
899
-			dev_info(&intf->dev, "resetting NTB format to 16-bit");
900
-			err = usbnet_write_cmd(dev, USB_CDC_SET_NTB_FORMAT,
901
-					       USB_TYPE_CLASS | USB_DIR_OUT
902
-					       | USB_RECIP_INTERFACE,
903
-					       USB_CDC_NCM_NTB16_FORMAT,
904
-					       iface_no, NULL, 0);
905
-
906
-			if (err < 0)
907
-				goto error2;
908
-		}
909
-	}
910
-
911919 	cdc_ncm_find_endpoints(dev, ctx->data);
912920 	cdc_ncm_find_endpoints(dev, ctx->control);
913921 	if (!dev->in || !dev->out || !dev->status) {
....@@ -932,9 +940,15 @@
932940 
933941 	/* Allocate the delayed NDP if needed. */
934942 	if (ctx->drvflags & CDC_NCM_FLAG_NDP_TO_END) {
935
-		ctx->delayed_ndp16 = kzalloc(ctx->max_ndp_size, GFP_KERNEL);
936
-		if (!ctx->delayed_ndp16)
937
-			goto error2;
943
+		if (ctx->is_ndp16) {
944
+			ctx->delayed_ndp16 = kzalloc(ctx->max_ndp_size, GFP_KERNEL);
945
+			if (!ctx->delayed_ndp16)
946
+				goto error2;
947
+		} else {
948
+			ctx->delayed_ndp32 = kzalloc(ctx->max_ndp_size, GFP_KERNEL);
949
+			if (!ctx->delayed_ndp32)
950
+				goto error2;
951
+		}
938952 		dev_info(&intf->dev, "NDP will be placed at end of frame for this device.");
939953 	}
940954 
....@@ -1058,7 +1072,7 @@
10581072 /* return a pointer to a valid struct usb_cdc_ncm_ndp16 of type sign, possibly
10591073  * allocating a new one within skb
10601074  */
1061
-static struct usb_cdc_ncm_ndp16 *cdc_ncm_ndp(struct cdc_ncm_ctx *ctx, struct sk_buff *skb, __le32 sign, size_t reserve)
1075
+static struct usb_cdc_ncm_ndp16 *cdc_ncm_ndp16(struct cdc_ncm_ctx *ctx, struct sk_buff *skb, __le32 sign, size_t reserve)
10621076 {
10631077 	struct usb_cdc_ncm_ndp16 *ndp16 = NULL;
10641078 	struct usb_cdc_ncm_nth16 *nth16 = (void *)skb->data;
....@@ -1113,12 +1127,73 @@
11131127 	return ndp16;
11141128 }
11151129 
1130
+static struct usb_cdc_ncm_ndp32 *cdc_ncm_ndp32(struct cdc_ncm_ctx *ctx, struct sk_buff *skb, __le32 sign, size_t reserve)
1131
+{
1132
+	struct usb_cdc_ncm_ndp32 *ndp32 = NULL;
1133
+	struct usb_cdc_ncm_nth32 *nth32 = (void *)skb->data;
1134
+	size_t ndpoffset = le32_to_cpu(nth32->dwNdpIndex);
1135
+
1136
+	/* If NDP should be moved to the end of the NCM package, we can't follow the
1137
+	 * NTH32 header as we would normally do. NDP isn't written to the SKB yet, and
1138
+	 * the wNdpIndex field in the header is actually not consistent with reality. It will be later.
1139
+	 */
1140
+	if (ctx->drvflags & CDC_NCM_FLAG_NDP_TO_END) {
1141
+		if (ctx->delayed_ndp32->dwSignature == sign)
1142
+			return ctx->delayed_ndp32;
1143
+
1144
+		/* We can only push a single NDP to the end. Return
1145
+		 * NULL to send what we've already got and queue this
1146
+		 * skb for later.
1147
+		 */
1148
+		else if (ctx->delayed_ndp32->dwSignature)
1149
+			return NULL;
1150
+	}
1151
+
1152
+	/* follow the chain of NDPs, looking for a match */
1153
+	while (ndpoffset) {
1154
+		ndp32 = (struct usb_cdc_ncm_ndp32 *)(skb->data + ndpoffset);
1155
+		if  (ndp32->dwSignature == sign)
1156
+			return ndp32;
1157
+		ndpoffset = le32_to_cpu(ndp32->dwNextNdpIndex);
1158
+	}
1159
+
1160
+	/* align new NDP */
1161
+	if (!(ctx->drvflags & CDC_NCM_FLAG_NDP_TO_END))
1162
+		cdc_ncm_align_tail(skb, ctx->tx_ndp_modulus, 0, ctx->tx_curr_size);
1163
+
1164
+	/* verify that there is room for the NDP and the datagram (reserve) */
1165
+	if ((ctx->tx_curr_size - skb->len - reserve) < ctx->max_ndp_size)
1166
+		return NULL;
1167
+
1168
+	/* link to it */
1169
+	if (ndp32)
1170
+		ndp32->dwNextNdpIndex = cpu_to_le32(skb->len);
1171
+	else
1172
+		nth32->dwNdpIndex = cpu_to_le32(skb->len);
1173
+
1174
+	/* push a new empty NDP */
1175
+	if (!(ctx->drvflags & CDC_NCM_FLAG_NDP_TO_END))
1176
+		ndp32 = skb_put_zero(skb, ctx->max_ndp_size);
1177
+	else
1178
+		ndp32 = ctx->delayed_ndp32;
1179
+
1180
+	ndp32->dwSignature = sign;
1181
+	ndp32->wLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_ndp32) + sizeof(struct usb_cdc_ncm_dpe32));
1182
+	return ndp32;
1183
+}
1184
+
11161185 struct sk_buff *
11171186 cdc_ncm_fill_tx_frame(struct usbnet *dev, struct sk_buff *skb, __le32 sign)
11181187 {
11191188 	struct cdc_ncm_ctx *ctx = (struct cdc_ncm_ctx *)dev->data[0];
1120
-	struct usb_cdc_ncm_nth16 *nth16;
1121
-	struct usb_cdc_ncm_ndp16 *ndp16;
1189
+	union {
1190
+		struct usb_cdc_ncm_nth16 *nth16;
1191
+		struct usb_cdc_ncm_nth32 *nth32;
1192
+	} nth;
1193
+	union {
1194
+		struct usb_cdc_ncm_ndp16 *ndp16;
1195
+		struct usb_cdc_ncm_ndp32 *ndp32;
1196
+	} ndp;
11221197 	struct sk_buff *skb_out;
11231198 	u16 n = 0, index, ndplen;
11241199 	u8 ready2send = 0;
....@@ -1158,6 +1233,9 @@
11581233 			 * further.
11591234 			 */
11601235 			if (skb_out == NULL) {
1236
+				/* If even the smallest allocation fails, abort. */
1237
+				if (ctx->tx_curr_size == USB_CDC_NCM_NTB_MIN_OUT_SIZE)
1238
+					goto alloc_failed;
11611239 				ctx->tx_low_mem_max_cnt = min(ctx->tx_low_mem_max_cnt + 1,
11621240 							      (unsigned)CDC_NCM_LOW_MEM_MAX_CNT);
11631241 				ctx->tx_low_mem_val = ctx->tx_low_mem_max_cnt;
....@@ -1176,20 +1254,23 @@
11761254 			skb_out = alloc_skb(ctx->tx_curr_size, GFP_ATOMIC);
11771255 
11781256 			/* No allocation possible so we will abort */
1179
-			if (skb_out == NULL) {
1180
-				if (skb != NULL) {
1181
-					dev_kfree_skb_any(skb);
1182
-					dev->net->stats.tx_dropped++;
1183
-				}
1184
-				goto exit_no_skb;
1185
-			}
1257
+			if (!skb_out)
1258
+				goto alloc_failed;
11861259 			ctx->tx_low_mem_val--;
11871260 		}
1188
-		/* fill out the initial 16-bit NTB header */
1189
-		nth16 = skb_put_zero(skb_out, sizeof(struct usb_cdc_ncm_nth16));
1190
-		nth16->dwSignature = cpu_to_le32(USB_CDC_NCM_NTH16_SIGN);
1191
-		nth16->wHeaderLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16));
1192
-		nth16->wSequence = cpu_to_le16(ctx->tx_seq++);
1261
+		if (ctx->is_ndp16) {
1262
+			/* fill out the initial 16-bit NTB header */
1263
+			nth.nth16 = skb_put_zero(skb_out, sizeof(struct usb_cdc_ncm_nth16));
1264
+			nth.nth16->dwSignature = cpu_to_le32(USB_CDC_NCM_NTH16_SIGN);
1265
+			nth.nth16->wHeaderLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16));
1266
+			nth.nth16->wSequence = cpu_to_le16(ctx->tx_seq++);
1267
+		} else {
1268
+			/* fill out the initial 32-bit NTB header */
1269
+			nth.nth32 = skb_put_zero(skb_out, sizeof(struct usb_cdc_ncm_nth32));
1270
+			nth.nth32->dwSignature = cpu_to_le32(USB_CDC_NCM_NTH32_SIGN);
1271
+			nth.nth32->wHeaderLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth32));
1272
+			nth.nth32->wSequence = cpu_to_le16(ctx->tx_seq++);
1273
+		}
11931274 
11941275 		/* count total number of frames in this NTB */
11951276 		ctx->tx_curr_frame_num = 0;
....@@ -1211,13 +1292,17 @@
12111292 		}
12121293 
12131294 		/* get the appropriate NDP for this skb */
1214
-		ndp16 = cdc_ncm_ndp(ctx, skb_out, sign, skb->len + ctx->tx_modulus + ctx->tx_remainder);
1295
+		if (ctx->is_ndp16)
1296
+			ndp.ndp16 = cdc_ncm_ndp16(ctx, skb_out, sign, skb->len + ctx->tx_modulus + ctx->tx_remainder);
1297
+		else
1298
+			ndp.ndp32 = cdc_ncm_ndp32(ctx, skb_out, sign, skb->len + ctx->tx_modulus + ctx->tx_remainder);
12151299 
12161300 		/* align beginning of next frame */
12171301 		cdc_ncm_align_tail(skb_out,  ctx->tx_modulus, ctx->tx_remainder, ctx->tx_curr_size);
12181302 
12191303 		/* check if we had enough room left for both NDP and frame */
1220
-		if (!ndp16 || skb_out->len + skb->len + delayed_ndp_size > ctx->tx_curr_size) {
1304
+		if ((ctx->is_ndp16 && !ndp.ndp16) || (!ctx->is_ndp16 && !ndp.ndp32) ||
1305
+		    skb_out->len + skb->len + delayed_ndp_size > ctx->tx_curr_size) {
12211306 			if (n == 0) {
12221307 				/* won't fit, MTU problem? */
12231308 				dev_kfree_skb_any(skb);
....@@ -1239,13 +1324,22 @@
12391324 		}
12401325 
12411326 		/* calculate frame number withing this NDP */
1242
-		ndplen = le16_to_cpu(ndp16->wLength);
1243
-		index = (ndplen - sizeof(struct usb_cdc_ncm_ndp16)) / sizeof(struct usb_cdc_ncm_dpe16) - 1;
1327
+		if (ctx->is_ndp16) {
1328
+			ndplen = le16_to_cpu(ndp.ndp16->wLength);
1329
+			index = (ndplen - sizeof(struct usb_cdc_ncm_ndp16)) / sizeof(struct usb_cdc_ncm_dpe16) - 1;
12441330 
1245
-		/* OK, add this skb */
1246
-		ndp16->dpe16[index].wDatagramLength = cpu_to_le16(skb->len);
1247
-		ndp16->dpe16[index].wDatagramIndex = cpu_to_le16(skb_out->len);
1248
-		ndp16->wLength = cpu_to_le16(ndplen + sizeof(struct usb_cdc_ncm_dpe16));
1331
+			/* OK, add this skb */
1332
+			ndp.ndp16->dpe16[index].wDatagramLength = cpu_to_le16(skb->len);
1333
+			ndp.ndp16->dpe16[index].wDatagramIndex = cpu_to_le16(skb_out->len);
1334
+			ndp.ndp16->wLength = cpu_to_le16(ndplen + sizeof(struct usb_cdc_ncm_dpe16));
1335
+		} else {
1336
+			ndplen = le16_to_cpu(ndp.ndp32->wLength);
1337
+			index = (ndplen - sizeof(struct usb_cdc_ncm_ndp32)) / sizeof(struct usb_cdc_ncm_dpe32) - 1;
1338
+
1339
+			ndp.ndp32->dpe32[index].dwDatagramLength = cpu_to_le32(skb->len);
1340
+			ndp.ndp32->dpe32[index].dwDatagramIndex = cpu_to_le32(skb_out->len);
1341
+			ndp.ndp32->wLength = cpu_to_le16(ndplen + sizeof(struct usb_cdc_ncm_dpe32));
1342
+		}
12491343 		skb_put_data(skb_out, skb->data, skb->len);
12501344 		ctx->tx_curr_frame_payload += skb->len;	/* count real tx payload data */
12511345 		dev_kfree_skb_any(skb);
....@@ -1292,13 +1386,22 @@
12921386 
12931387 	/* If requested, put NDP at end of frame. */
12941388 	if (ctx->drvflags & CDC_NCM_FLAG_NDP_TO_END) {
1295
-		nth16 = (struct usb_cdc_ncm_nth16 *)skb_out->data;
1296
-		cdc_ncm_align_tail(skb_out, ctx->tx_ndp_modulus, 0, ctx->tx_curr_size - ctx->max_ndp_size);
1297
-		nth16->wNdpIndex = cpu_to_le16(skb_out->len);
1298
-		skb_put_data(skb_out, ctx->delayed_ndp16, ctx->max_ndp_size);
1389
+		if (ctx->is_ndp16) {
1390
+			nth.nth16 = (struct usb_cdc_ncm_nth16 *)skb_out->data;
1391
+			cdc_ncm_align_tail(skb_out, ctx->tx_ndp_modulus, 0, ctx->tx_curr_size - ctx->max_ndp_size);
1392
+			nth.nth16->wNdpIndex = cpu_to_le16(skb_out->len);
1393
+			skb_put_data(skb_out, ctx->delayed_ndp16, ctx->max_ndp_size);
12991394 
1300
-		/* Zero out delayed NDP - signature checking will naturally fail. */
1301
-		ndp16 = memset(ctx->delayed_ndp16, 0, ctx->max_ndp_size);
1395
+			/* Zero out delayed NDP - signature checking will naturally fail. */
1396
+			ndp.ndp16 = memset(ctx->delayed_ndp16, 0, ctx->max_ndp_size);
1397
+		} else {
1398
+			nth.nth32 = (struct usb_cdc_ncm_nth32 *)skb_out->data;
1399
+			cdc_ncm_align_tail(skb_out, ctx->tx_ndp_modulus, 0, ctx->tx_curr_size - ctx->max_ndp_size);
1400
+			nth.nth32->dwNdpIndex = cpu_to_le32(skb_out->len);
1401
+			skb_put_data(skb_out, ctx->delayed_ndp32, ctx->max_ndp_size);
1402
+
1403
+			ndp.ndp32 = memset(ctx->delayed_ndp32, 0, ctx->max_ndp_size);
1404
+		}
13021405 	}
13031406 
13041407 	/* If collected data size is less or equal ctx->min_tx_pkt
....@@ -1321,8 +1424,13 @@
13211424 	}
13221425 
13231426 	/* set final frame length */
1324
-	nth16 = (struct usb_cdc_ncm_nth16 *)skb_out->data;
1325
-	nth16->wBlockLength = cpu_to_le16(skb_out->len);
1427
+	if (ctx->is_ndp16) {
1428
+		nth.nth16 = (struct usb_cdc_ncm_nth16 *)skb_out->data;
1429
+		nth.nth16->wBlockLength = cpu_to_le16(skb_out->len);
1430
+	} else {
1431
+		nth.nth32 = (struct usb_cdc_ncm_nth32 *)skb_out->data;
1432
+		nth.nth32->dwBlockLength = cpu_to_le32(skb_out->len);
1433
+	}
13261434 
13271435 	/* return skb */
13281436 	ctx->tx_curr_skb = NULL;
....@@ -1340,6 +1448,11 @@
13401448 
13411449 	return skb_out;
13421450 
1451
+alloc_failed:
1452
+	if (skb) {
1453
+		dev_kfree_skb_any(skb);
1454
+		dev->net->stats.tx_dropped++;
1455
+	}
13431456 exit_no_skb:
13441457 	/* Start timer, if there is a remaining non-empty skb */
13451458 	if (ctx->tx_curr_skb != NULL && n > 0)
....@@ -1405,7 +1518,12 @@
14051518 		goto error;
14061519 
14071520 	spin_lock_bh(&ctx->mtx);
1408
-	skb_out = cdc_ncm_fill_tx_frame(dev, skb, cpu_to_le32(USB_CDC_NCM_NDP16_NOCRC_SIGN));
1521
+
1522
+	if (ctx->is_ndp16)
1523
+		skb_out = cdc_ncm_fill_tx_frame(dev, skb, cpu_to_le32(USB_CDC_NCM_NDP16_NOCRC_SIGN));
1524
+	else
1525
+		skb_out = cdc_ncm_fill_tx_frame(dev, skb, cpu_to_le32(USB_CDC_NCM_NDP32_NOCRC_SIGN));
1526
+
14091527 	spin_unlock_bh(&ctx->mtx);
14101528 	return skb_out;
14111529 
....@@ -1466,6 +1584,54 @@
14661584 }
14671585 EXPORT_SYMBOL_GPL(cdc_ncm_rx_verify_nth16);
14681586 
1587
+int cdc_ncm_rx_verify_nth32(struct cdc_ncm_ctx *ctx, struct sk_buff *skb_in)
1588
+{
1589
+	struct usbnet *dev = netdev_priv(skb_in->dev);
1590
+	struct usb_cdc_ncm_nth32 *nth32;
1591
+	int len;
1592
+	int ret = -EINVAL;
1593
+
1594
+	if (ctx == NULL)
1595
+		goto error;
1596
+
1597
+	if (skb_in->len < (sizeof(struct usb_cdc_ncm_nth32) +
1598
+					sizeof(struct usb_cdc_ncm_ndp32))) {
1599
+		netif_dbg(dev, rx_err, dev->net, "frame too short\n");
1600
+		goto error;
1601
+	}
1602
+
1603
+	nth32 = (struct usb_cdc_ncm_nth32 *)skb_in->data;
1604
+
1605
+	if (nth32->dwSignature != cpu_to_le32(USB_CDC_NCM_NTH32_SIGN)) {
1606
+		netif_dbg(dev, rx_err, dev->net,
1607
+			  "invalid NTH32 signature <%#010x>\n",
1608
+			  le32_to_cpu(nth32->dwSignature));
1609
+		goto error;
1610
+	}
1611
+
1612
+	len = le32_to_cpu(nth32->dwBlockLength);
1613
+	if (len > ctx->rx_max) {
1614
+		netif_dbg(dev, rx_err, dev->net,
1615
+			  "unsupported NTB block length %u/%u\n", len,
1616
+			  ctx->rx_max);
1617
+		goto error;
1618
+	}
1619
+
1620
+	if ((ctx->rx_seq + 1) != le16_to_cpu(nth32->wSequence) &&
1621
+	    (ctx->rx_seq || le16_to_cpu(nth32->wSequence)) &&
1622
+	    !((ctx->rx_seq == 0xffff) && !le16_to_cpu(nth32->wSequence))) {
1623
+		netif_dbg(dev, rx_err, dev->net,
1624
+			  "sequence number glitch prev=%d curr=%d\n",
1625
+			  ctx->rx_seq, le16_to_cpu(nth32->wSequence));
1626
+	}
1627
+	ctx->rx_seq = le16_to_cpu(nth32->wSequence);
1628
+
1629
+	ret = le32_to_cpu(nth32->dwNdpIndex);
1630
+error:
1631
+	return ret;
1632
+}
1633
+EXPORT_SYMBOL_GPL(cdc_ncm_rx_verify_nth32);
1634
+
14691635 /* verify NDP header and return number of datagrams, or negative error */
14701636 int cdc_ncm_rx_verify_ndp16(struct sk_buff *skb_in, int ndpoffset)
14711637 {
....@@ -1502,42 +1668,110 @@
15021668 }
15031669 EXPORT_SYMBOL_GPL(cdc_ncm_rx_verify_ndp16);
15041670 
1671
+/* verify NDP header and return number of datagrams, or negative error */
1672
+int cdc_ncm_rx_verify_ndp32(struct sk_buff *skb_in, int ndpoffset)
1673
+{
1674
+	struct usbnet *dev = netdev_priv(skb_in->dev);
1675
+	struct usb_cdc_ncm_ndp32 *ndp32;
1676
+	int ret = -EINVAL;
1677
+
1678
+	if ((ndpoffset + sizeof(struct usb_cdc_ncm_ndp32)) > skb_in->len) {
1679
+		netif_dbg(dev, rx_err, dev->net, "invalid NDP offset  <%u>\n",
1680
+			  ndpoffset);
1681
+		goto error;
1682
+	}
1683
+	ndp32 = (struct usb_cdc_ncm_ndp32 *)(skb_in->data + ndpoffset);
1684
+
1685
+	if (le16_to_cpu(ndp32->wLength) < USB_CDC_NCM_NDP32_LENGTH_MIN) {
1686
+		netif_dbg(dev, rx_err, dev->net, "invalid DPT32 length <%u>\n",
1687
+			  le16_to_cpu(ndp32->wLength));
1688
+		goto error;
1689
+	}
1690
+
1691
+	ret = ((le16_to_cpu(ndp32->wLength) -
1692
+					sizeof(struct usb_cdc_ncm_ndp32)) /
1693
+					sizeof(struct usb_cdc_ncm_dpe32));
1694
+	ret--; /* we process NDP entries except for the last one */
1695
+
1696
+	if ((sizeof(struct usb_cdc_ncm_ndp32) +
1697
+	     ret * (sizeof(struct usb_cdc_ncm_dpe32))) > skb_in->len) {
1698
+		netif_dbg(dev, rx_err, dev->net, "Invalid nframes = %d\n", ret);
1699
+		ret = -EINVAL;
1700
+	}
1701
+
1702
+error:
1703
+	return ret;
1704
+}
1705
+EXPORT_SYMBOL_GPL(cdc_ncm_rx_verify_ndp32);
1706
+
15051707 int cdc_ncm_rx_fixup(struct usbnet *dev, struct sk_buff *skb_in)
15061708 {
15071709 	struct sk_buff *skb;
15081710 	struct cdc_ncm_ctx *ctx = (struct cdc_ncm_ctx *)dev->data[0];
1509
-	int len;
1711
+	unsigned int len;
15101712 	int nframes;
15111713 	int x;
1512
-	int offset;
1513
-	struct usb_cdc_ncm_ndp16 *ndp16;
1514
-	struct usb_cdc_ncm_dpe16 *dpe16;
1714
+	unsigned int offset;
1715
+	union {
1716
+		struct usb_cdc_ncm_ndp16 *ndp16;
1717
+		struct usb_cdc_ncm_ndp32 *ndp32;
1718
+	} ndp;
1719
+	union {
1720
+		struct usb_cdc_ncm_dpe16 *dpe16;
1721
+		struct usb_cdc_ncm_dpe32 *dpe32;
1722
+	} dpe;
1723
+
15151724 	int ndpoffset;
15161725 	int loopcount = 50; /* arbitrary max preventing infinite loop */
15171726 	u32 payload = 0;
15181727 
1519
-	ndpoffset = cdc_ncm_rx_verify_nth16(ctx, skb_in);
1728
+	if (ctx->is_ndp16)
1729
+		ndpoffset = cdc_ncm_rx_verify_nth16(ctx, skb_in);
1730
+	else
1731
+		ndpoffset = cdc_ncm_rx_verify_nth32(ctx, skb_in);
1732
+
15201733 	if (ndpoffset < 0)
15211734 		goto error;
15221735 
15231736 next_ndp:
1524
-	nframes = cdc_ncm_rx_verify_ndp16(skb_in, ndpoffset);
1525
-	if (nframes < 0)
1526
-		goto error;
1737
+	if (ctx->is_ndp16) {
1738
+		nframes = cdc_ncm_rx_verify_ndp16(skb_in, ndpoffset);
1739
+		if (nframes < 0)
1740
+			goto error;
15271741 
1528
-	ndp16 = (struct usb_cdc_ncm_ndp16 *)(skb_in->data + ndpoffset);
1742
+		ndp.ndp16 = (struct usb_cdc_ncm_ndp16 *)(skb_in->data + ndpoffset);
15291743 
1530
-	if (ndp16->dwSignature != cpu_to_le32(USB_CDC_NCM_NDP16_NOCRC_SIGN)) {
1531
-		netif_dbg(dev, rx_err, dev->net,
1532
-			  "invalid DPT16 signature <%#010x>\n",
1533
-			  le32_to_cpu(ndp16->dwSignature));
1534
-		goto err_ndp;
1744
+		if (ndp.ndp16->dwSignature != cpu_to_le32(USB_CDC_NCM_NDP16_NOCRC_SIGN)) {
1745
+			netif_dbg(dev, rx_err, dev->net,
1746
+				  "invalid DPT16 signature <%#010x>\n",
1747
+				  le32_to_cpu(ndp.ndp16->dwSignature));
1748
+			goto err_ndp;
1749
+		}
1750
+		dpe.dpe16 = ndp.ndp16->dpe16;
1751
+	} else {
1752
+		nframes = cdc_ncm_rx_verify_ndp32(skb_in, ndpoffset);
1753
+		if (nframes < 0)
1754
+			goto error;
1755
+
1756
+		ndp.ndp32 = (struct usb_cdc_ncm_ndp32 *)(skb_in->data + ndpoffset);
1757
+
1758
+		if (ndp.ndp32->dwSignature != cpu_to_le32(USB_CDC_NCM_NDP32_NOCRC_SIGN)) {
1759
+			netif_dbg(dev, rx_err, dev->net,
1760
+				  "invalid DPT32 signature <%#010x>\n",
1761
+				  le32_to_cpu(ndp.ndp32->dwSignature));
1762
+			goto err_ndp;
1763
+		}
1764
+		dpe.dpe32 = ndp.ndp32->dpe32;
15351765 	}
1536
-	dpe16 = ndp16->dpe16;
15371766 
1538
-	for (x = 0; x < nframes; x++, dpe16++) {
1539
-		offset = le16_to_cpu(dpe16->wDatagramIndex);
1540
-		len = le16_to_cpu(dpe16->wDatagramLength);
1767
+	for (x = 0; x < nframes; x++) {
1768
+		if (ctx->is_ndp16) {
1769
+			offset = le16_to_cpu(dpe.dpe16->wDatagramIndex);
1770
+			len = le16_to_cpu(dpe.dpe16->wDatagramLength);
1771
+		} else {
1772
+			offset = le32_to_cpu(dpe.dpe32->dwDatagramIndex);
1773
+			len = le32_to_cpu(dpe.dpe32->dwDatagramLength);
1774
+		}
15411775 
15421776 		/*
15431777 		 * CDC NCM ch. 3.7
....@@ -1549,8 +1783,8 @@
15491783 			break;
15501784 		}
15511785 
1552
-		/* sanity checking */
1553
-		if (((offset + len) > skb_in->len) ||
1786
+		/* sanity checking - watch out for integer wrap*/
1787
+		if ((offset > skb_in->len) || (len > skb_in->len - offset) ||
15541788 				(len > ctx->rx_max) || (len < ETH_HLEN)) {
15551789 			netif_dbg(dev, rx_err, dev->net,
15561790 				  "invalid frame detected (ignored) offset[%u]=%u, length=%u, skb=%p\n",
....@@ -1568,10 +1802,19 @@
15681802 			usbnet_skb_return(dev, skb);
15691803 			payload += len;	/* count payload bytes in this NTB */
15701804 		}
1805
+
1806
+		if (ctx->is_ndp16)
1807
+			dpe.dpe16++;
1808
+		else
1809
+			dpe.dpe32++;
15711810 	}
15721811 err_ndp:
15731812 	/* are there more NDPs to process? */
1574
-	ndpoffset = le16_to_cpu(ndp16->wNextNdpIndex);
1813
+	if (ctx->is_ndp16)
1814
+		ndpoffset = le16_to_cpu(ndp.ndp16->wNextNdpIndex);
1815
+	else
1816
+		ndpoffset = le32_to_cpu(ndp.ndp32->dwNextNdpIndex);
1817
+
15751818 	if (ndpoffset && loopcount--)
15761819 		goto next_ndp;
15771820 
....@@ -1592,15 +1835,6 @@
15921835 	uint32_t rx_speed = le32_to_cpu(data->DLBitRRate);
15931836 	uint32_t tx_speed = le32_to_cpu(data->ULBitRate);
15941837 
1595
-	/* if the speed hasn't changed, don't report it.
1596
-	 * RTL8156 shipped before 2021 sends notification about every 32ms.
1597
-	 */
1598
-	if (dev->rx_speed == rx_speed && dev->tx_speed == tx_speed)
1599
-		return;
1600
-
1601
-	dev->rx_speed = rx_speed;
1602
-	dev->tx_speed = tx_speed;
1603
-
16041838 	/*
16051839 	 * Currently the USB-NET API does not support reporting the actual
16061840 	 * device speed. Do print it instead.
....@@ -1620,10 +1854,7 @@
16201854 
16211855 static void cdc_ncm_status(struct usbnet *dev, struct urb *urb)
16221856 {
1623
-	struct cdc_ncm_ctx *ctx;
16241857 	struct usb_cdc_notification *event;
1625
-
1626
-	ctx = (struct cdc_ncm_ctx *)dev->data[0];
16271858 
16281859 	if (urb->actual_length < sizeof(*event))
16291860 		return;
....@@ -1644,8 +1875,7 @@
16441875 		 * USB_CDC_NOTIFY_NETWORK_CONNECTION notification shall be
16451876 		 * sent by device after USB_CDC_NOTIFY_SPEED_CHANGE.
16461877 		 */
1647
-		if (netif_carrier_ok(dev->net) != !!event->wValue)
1648
-			usbnet_link_change(dev, !!event->wValue, 0);
1878
+		usbnet_link_change(dev, !!event->wValue, 0);
16491879 		break;
16501880 
16511881 	case USB_CDC_NOTIFY_SPEED_CHANGE:
....@@ -1675,6 +1905,7 @@
16751905 	.status = cdc_ncm_status,
16761906 	.rx_fixup = cdc_ncm_rx_fixup,
16771907 	.tx_fixup = cdc_ncm_tx_fixup,
1908
+	.set_rx_mode = usbnet_cdc_update_filter,
16781909 };
16791910 
16801911 /* Same as cdc_ncm_info, but with FLAG_WWAN */
....@@ -1688,6 +1919,7 @@
16881919 	.status = cdc_ncm_status,
16891920 	.rx_fixup = cdc_ncm_rx_fixup,
16901921 	.tx_fixup = cdc_ncm_tx_fixup,
1922
+	.set_rx_mode = usbnet_cdc_update_filter,
16911923 };
16921924 
16931925 /* Same as wwan_info, but with FLAG_NOARP  */
....@@ -1701,6 +1933,7 @@
17011933 	.status = cdc_ncm_status,
17021934 	.rx_fixup = cdc_ncm_rx_fixup,
17031935 	.tx_fixup = cdc_ncm_tx_fixup,
1936
+	.set_rx_mode = usbnet_cdc_update_filter,
17041937 };
17051938 
17061939 static const struct usb_device_id cdc_devs[] = {