forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2024-05-13 9d77db3c730780c8ef5ccd4b66403ff5675cfe4e 
 kernel/drivers/usb/gadget/function/f_ncm.c
....@@ -23,6 +23,7 @@
2323 #include "u_ether.h"
2424 #include "u_ether_configfs.h"
2525 #include "u_ncm.h"
26
+#include "configfs.h"
2627 
2728 /*
2829  * This function is a "CDC Network Control Model" (CDC NCM) Ethernet link.
....@@ -35,9 +36,7 @@
3536 
3637 /* to trigger crc/non-crc ndp signature */
3738 
38
-#define NCM_NDP_HDR_CRC_MASK	0x01000000
3939 #define NCM_NDP_HDR_CRC		0x01000000
40
-#define NCM_NDP_HDR_NOCRC	0x00000000
4140 
4241 enum ncm_notify_state {
4342 	NCM_NOTIFY_NONE,		/* don't notify */
....@@ -86,7 +85,9 @@
8685 /* peak (theoretical) bulk transfer rate in bits-per-second */
8786 static inline unsigned ncm_bitrate(struct usb_gadget *g)
8887 {
89
-	if (gadget_is_superspeed(g) && g->speed >= USB_SPEED_SUPER_PLUS)
88
+	if (!g)
89
+		return 0;
90
+	else if (gadget_is_superspeed(g) && g->speed >= USB_SPEED_SUPER_PLUS)
9091 		return 4250000000U;
9192 	else if (gadget_is_superspeed(g) && g->speed == USB_SPEED_SUPER)
9293 		return 3750000000U;
....@@ -379,7 +380,7 @@
379380 	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
380381 
381382 	/* the following 2 values can be tweaked if necessary */
382
-	/* .bMaxBurst =		0, */
383
+	.bMaxBurst =		15,
383384 	/* .bmAttributes =	0, */
384385 };
385386 
....@@ -529,6 +530,7 @@
529530 {
530531 	ncm->parser_opts = &ndp16_opts;
531532 	ncm->is_crc = false;
533
+	ncm->ndp_sign = ncm->parser_opts->ndp_sign;
532534 	ncm->port.cdc_filter = DEFAULT_FILTER;
533535 
534536 	/* doesn't make sense for ncm, fixed size used */
....@@ -811,25 +813,20 @@
811813 	case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
812814 		| USB_CDC_SET_CRC_MODE:
813815 	{
814
-		int ndp_hdr_crc = 0;
815
-
816816 		if (w_length != 0 || w_index != ncm->ctrl_id)
817817 			goto invalid;
818818 		switch (w_value) {
819819 		case 0x0000:
820820 			ncm->is_crc = false;
821
-			ndp_hdr_crc = NCM_NDP_HDR_NOCRC;
822821 			DBG(cdev, "non-CRC mode selected\n");
823822 			break;
824823 		case 0x0001:
825824 			ncm->is_crc = true;
826
-			ndp_hdr_crc = NCM_NDP_HDR_CRC;
827825 			DBG(cdev, "CRC mode selected\n");
828826 			break;
829827 		default:
830828 			goto invalid;
831829 		}
832
-		ncm->ndp_sign = ncm->parser_opts->ndp_sign | ndp_hdr_crc;
833830 		value = 0;
834831 		break;
835832 	}
....@@ -846,6 +843,8 @@
846843 			ctrl->bRequestType, ctrl->bRequest,
847844 			w_value, w_index, w_length);
848845 	}
846
+	ncm->ndp_sign = ncm->parser_opts->ndp_sign |
847
+		(ncm->is_crc ? NCM_NDP_HDR_CRC : 0);
849848 
850849 	/* respond with data transfer or status phase? */
851850 	if (value >= 0) {
....@@ -1181,7 +1180,8 @@
11811180 			  struct sk_buff_head *list)
11821181 {
11831182 	struct f_ncm	*ncm = func_to_ncm(&port->func);
1184
-	__le16		*tmp = (void *) skb->data;
1183
+	unsigned char	*ntb_ptr = skb->data;
1184
+	__le16		*tmp;
11851185 	unsigned	index, index2;
11861186 	int		ndp_index;
11871187 	unsigned	dg_len, dg_len2;
....@@ -1194,6 +1194,10 @@
11941194 	const struct ndp_parser_opts *opts = ncm->parser_opts;
11951195 	unsigned	crc_len = ncm->is_crc ? sizeof(uint32_t) : 0;
11961196 	int		dgram_counter;
1197
+	int		to_process = skb->len;
1198
+
1199
+parse_ntb:
1200
+	tmp = (__le16 *)ntb_ptr;
11971201 
11981202 	/* dwSignature */
11991203 	if (get_unaligned_le32(tmp) != opts->nth_sign) {
....@@ -1240,7 +1244,7 @@
12401244 		 * walk through NDP
12411245 		 * dwSignature
12421246 		 */
1243
-		tmp = (void *)(skb->data + ndp_index);
1247
+		tmp = (__le16 *)(ntb_ptr + ndp_index);
12441248 		if (get_unaligned_le32(tmp) != ncm->ndp_sign) {
12451249 			INFO(port->func.config->cdev, "Wrong NDP SIGN\n");
12461250 			goto err;
....@@ -1297,11 +1301,11 @@
12971301 			if (ncm->is_crc) {
12981302 				uint32_t crc, crc2;
12991303 
1300
-				crc = get_unaligned_le32(skb->data +
1304
+				crc = get_unaligned_le32(ntb_ptr +
13011305 							 index + dg_len -
13021306 							 crc_len);
13031307 				crc2 = ~crc32_le(~0,
1304
-						 skb->data + index,
1308
+						 ntb_ptr + index,
13051309 						 dg_len - crc_len);
13061310 				if (crc != crc2) {
13071311 					INFO(port->func.config->cdev,
....@@ -1328,7 +1332,7 @@
13281332 							 dg_len - crc_len);
13291333 			if (skb2 == NULL)
13301334 				goto err;
1331
-			skb_put_data(skb2, skb->data + index,
1335
+			skb_put_data(skb2, ntb_ptr + index,
13321336 				     dg_len - crc_len);
13331337 
13341338 			skb_queue_tail(list, skb2);
....@@ -1341,10 +1345,17 @@
13411345 		} while (ndp_len > 2 * (opts->dgram_item_len * 2));
13421346 	} while (ndp_index);
13431347 
1344
-	dev_consume_skb_any(skb);
1345
-
13461348 	VDBG(port->func.config->cdev,
13471349 	     "Parsed NTB with %d frames\n", dgram_counter);
1350
+
1351
+	to_process -= block_len;
1352
+	if (to_process != 0) {
1353
+		ntb_ptr = (unsigned char *)(ntb_ptr + block_len);
1354
+		goto parse_ntb;
1355
+	}
1356
+
1357
+	dev_consume_skb_any(skb);
1358
+
13481359 	return 0;
13491360 err:
13501361 	skb_queue_purge(list);
....@@ -1432,6 +1443,16 @@
14321443 		return -EINVAL;
14331444 
14341445 	ncm_opts = container_of(f->fi, struct f_ncm_opts, func_inst);
1446
+
1447
+	if (cdev->use_os_string) {
1448
+		f->os_desc_table = kzalloc(sizeof(*f->os_desc_table),
1449
+					   GFP_KERNEL);
1450
+		if (!f->os_desc_table)
1451
+			return -ENOMEM;
1452
+		f->os_desc_n = 1;
1453
+		f->os_desc_table[0].os_desc = &ncm_opts->ncm_os_desc;
1454
+	}
1455
+
14351456 	/*
14361457 	 * in drivers/usb/gadget/configfs.c:configfs_composite_bind()
14371458 	 * configurations are bound in sequence with list_for_each_entry,
....@@ -1445,13 +1466,15 @@
14451466 		status = gether_register_netdev(ncm_opts->net);
14461467 		mutex_unlock(&ncm_opts->lock);
14471468 		if (status)
1448
-			return status;
1469
+			goto fail;
14491470 		ncm_opts->bound = true;
14501471 	}
14511472 	us = usb_gstrings_attach(cdev, ncm_strings,
14521473 				 ARRAY_SIZE(ncm_string_defs));
1453
-	if (IS_ERR(us))
1454
-		return PTR_ERR(us);
1474
+	if (IS_ERR(us)) {
1475
+		status = PTR_ERR(us);
1476
+		goto fail;
1477
+	}
14551478 	ncm_control_intf.iInterface = us[STRING_CTRL_IDX].id;
14561479 	ncm_data_nop_intf.iInterface = us[STRING_DATA_IDX].id;
14571480 	ncm_data_intf.iInterface = us[STRING_DATA_IDX].id;
....@@ -1467,6 +1490,10 @@
14671490 
14681491 	ncm_control_intf.bInterfaceNumber = status;
14691492 	ncm_union_desc.bMasterInterface0 = status;
1493
+
1494
+	if (cdev->use_os_string)
1495
+		f->os_desc_table[0].if_id =
1496
+			ncm_iad_desc.bFirstInterface;
14701497 
14711498 	status = usb_interface_id(c, f);
14721499 	if (status < 0)
....@@ -1547,6 +1574,9 @@
15471574 	return 0;
15481575 
15491576 fail:
1577
+	kfree(f->os_desc_table);
1578
+	f->os_desc_n = 0;
1579
+
15501580 	if (ncm->notify_req) {
15511581 		kfree(ncm->notify_req->buf);
15521582 		usb_ep_free_request(ncm->notify, ncm->notify_req);
....@@ -1601,16 +1631,22 @@
16011631 		gether_cleanup(netdev_priv(opts->net));
16021632 	else
16031633 		free_netdev(opts->net);
1634
+	kfree(opts->ncm_interf_group);
16041635 	kfree(opts);
16051636 }
16061637 
16071638 static struct usb_function_instance *ncm_alloc_inst(void)
16081639 {
16091640 	struct f_ncm_opts *opts;
1641
+	struct usb_os_desc *descs[1];
1642
+	char *names[1];
1643
+	struct config_group *ncm_interf_group;
16101644 
16111645 	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
16121646 	if (!opts)
16131647 		return ERR_PTR(-ENOMEM);
1648
+	opts->ncm_os_desc.ext_compat_id = opts->ncm_ext_compat_id;
1649
+
16141650 	mutex_init(&opts->lock);
16151651 	opts->func_inst.free_func_inst = ncm_free_inst;
16161652 	opts->net = gether_setup_default();
....@@ -1619,8 +1655,20 @@
16191655 		kfree(opts);
16201656 		return ERR_CAST(net);
16211657 	}
1658
+	INIT_LIST_HEAD(&opts->ncm_os_desc.ext_prop);
1659
+
1660
+	descs[0] = &opts->ncm_os_desc;
1661
+	names[0] = "ncm";
16221662 
16231663 	config_group_init_type_name(&opts->func_inst.group, "", &ncm_func_type);
1664
+	ncm_interf_group =
1665
+		usb_os_desc_prepare_interf_dir(&opts->func_inst.group, 1, descs,
1666
+					       names, THIS_MODULE);
1667
+	if (IS_ERR(ncm_interf_group)) {
1668
+		ncm_free_inst(&opts->func_inst);
1669
+		return ERR_CAST(ncm_interf_group);
1670
+	}
1671
+	opts->ncm_interf_group = ncm_interf_group;
16241672 
16251673 	return &opts->func_inst;
16261674 }
....@@ -1646,6 +1694,9 @@
16461694 
16471695 	hrtimer_cancel(&ncm->task_timer);
16481696 
1697
+	kfree(f->os_desc_table);
1698
+	f->os_desc_n = 0;
1699
+
16491700 	ncm_string_defs[0].id = 0;
16501701 	usb_free_all_descriptors(f);
16511702