~hc/RK356X_SDK_RELEASE.git

..	..	@@ -26,10 +26,12 @@
26	26	/* Bluetooth HCI core. */
27	27
28	28	#include <linux/export.h>
29		-#include <linux/idr.h>
30	29	#include <linux/rfkill.h>
31	30	#include <linux/debugfs.h>
32	31	#include <linux/crypto.h>
	32	+#include <linux/property.h>
	33	+#include <linux/suspend.h>
	34	+#include <linux/wait.h>
33	35	#include <asm/unaligned.h>
34	36
35	37	#include <net/bluetooth/bluetooth.h>
..	..	@@ -41,6 +43,7 @@
41	43	#include "hci_debugfs.h"
42	44	#include "smp.h"
43	45	#include "leds.h"
	46	+#include "msft.h"
44	47
45	48	static void hci_rx_work(struct work_struct *work);
46	49	static void hci_cmd_work(struct work_struct *work);
..	..	@@ -602,6 +605,10 @@
602	605	if (hdev->commands[8] & 0x01)
603	606	hci_req_add(req, HCI_OP_READ_PAGE_SCAN_ACTIVITY, 0, NULL);
604	607
	608	+ if (hdev->commands[18] & 0x04 &&
	609	+ !test_bit(HCI_QUIRK_BROKEN_ERR_DATA_REPORTING, &hdev->quirks))
	610	+ hci_req_add(req, HCI_OP_READ_DEF_ERR_DATA_REPORTING, 0, NULL);
	611	+
605	612	/* Some older Broadcom based Bluetooth 1.2 controllers do not
606	613	* support the Read Page Scan Type command. Check support for
607	614	* this command in the bit mask of supported commands.
..	..	@@ -630,6 +637,14 @@
630	637	*/
631	638	if (hdev->le_features[0] & HCI_LE_DATA_LEN_EXT)
632	639	events[0] \|= 0x40; /* LE Data Length Change */
	640	+
	641	+ /* If the controller supports LL Privacy feature, enable
	642	+ * the corresponding event.
	643	+ */
	644	+ if (hdev->le_features[0] & HCI_LE_LL_PRIVACY)
	645	+ events[1] \|= 0x02; /* LE Enhanced Connection
	646	+ * Complete
	647	+ */
633	648
634	649	/* If the controller supports Extended Scanner Filter
635	650	* Policies, enable the correspondig event.
..	..	@@ -704,14 +719,6 @@
704	719	* Report
705	720	*/
706	721
707		- /* If the controller supports the LE Extended Create Connection
708		- * command, enable the corresponding event.
709		- */
710		- if (use_ext_conn(hdev))
711		- events[1] \|= 0x02; /* LE Enhanced Connection
712		- * Complete
713		- */
714		-
715	722	/* If the controller supports the LE Extended Advertising
716	723	* command, enable the corresponding event.
717	724	*/
..	..	@@ -756,6 +763,14 @@
756	763	hci_req_add(req, HCI_OP_LE_CLEAR_RESOLV_LIST, 0, NULL);
757	764	}
758	765
	766	+ if (hdev->commands[35] & 0x04) {
	767	+ __le16 rpa_timeout = cpu_to_le16(hdev->rpa_timeout);
	768	+
	769	+ /* Set RPA timeout */
	770	+ hci_req_add(req, HCI_OP_LE_SET_RPA_TIMEOUT, 2,
	771	+ &rpa_timeout);
	772	+ }
	773	+
759	774	if (hdev->le_features[0] & HCI_LE_DATA_LEN_EXT) {
760	775	/* Read LE Maximum Data Length */
761	776	hci_req_add(req, HCI_OP_LE_READ_MAX_DATA_LEN, 0, NULL);
..	..	@@ -793,7 +808,7 @@
793	808	* Delete Stored Link Key command. They are clearly indicating its
794	809	* absence in the bit mask of supported commands.
795	810	*
796		- * Check the supported commands and only if the the command is marked
	811	+ * Check the supported commands and only if the command is marked
797	812	* as supported send it. If not supported assume that the controller
798	813	* does not have actual support for stored link keys which makes this
799	814	* command redundant anyway.
..	..	@@ -820,6 +835,10 @@
820	835	if (hdev->commands[29] & 0x20)
821	836	hci_req_add(req, HCI_OP_READ_LOCAL_CODECS, 0, NULL);
822	837
	838	+ /* Read local pairing options if the HCI command is supported */
	839	+ if (hdev->commands[41] & 0x08)
	840	+ hci_req_add(req, HCI_OP_READ_LOCAL_PAIRING_OPTS, 0, NULL);
	841	+
823	842	/* Get MWS transport configuration if the HCI command is supported */
824	843	if (hdev->commands[30] & 0x08)
825	844	hci_req_add(req, HCI_OP_GET_MWS_TRANSPORT_CONFIG, 0, NULL);
..	..	@@ -835,6 +854,27 @@
835	854
836	855	hci_req_add(req, HCI_OP_WRITE_SC_SUPPORT,
837	856	sizeof(support), &support);
	857	+ }
	858	+
	859	+ /* Set erroneous data reporting if supported to the wideband speech
	860	+ * setting value
	861	+ */
	862	+ if (hdev->commands[18] & 0x08 &&
	863	+ !test_bit(HCI_QUIRK_BROKEN_ERR_DATA_REPORTING, &hdev->quirks)) {
	864	+ bool enabled = hci_dev_test_flag(hdev,
	865	+ HCI_WIDEBAND_SPEECH_ENABLED);
	866	+
	867	+ if (enabled !=
	868	+ (hdev->err_data_reporting == ERR_DATA_REPORTING_ENABLED)) {
	869	+ struct hci_cp_write_def_err_data_reporting cp;
	870	+
	871	+ cp.err_data_reporting = enabled ?
	872	+ ERR_DATA_REPORTING_ENABLED :
	873	+ ERR_DATA_REPORTING_DISABLED;
	874	+
	875	+ hci_req_add(req, HCI_OP_WRITE_DEF_ERR_DATA_REPORTING,
	876	+ sizeof(cp), &cp);
	877	+ }
838	878	}
839	879
840	880	/* Set Suggested Default Data Length to maximum if supported */
..	..	@@ -1363,6 +1403,32 @@
1363	1403	return err;
1364	1404	}
1365	1405
	1406	+/**
	1407	+ * hci_dev_get_bd_addr_from_property - Get the Bluetooth Device Address
	1408	+ * (BD_ADDR) for a HCI device from
	1409	+ * a firmware node property.
	1410	+ * @hdev: The HCI device
	1411	+ *
	1412	+ * Search the firmware node for 'local-bd-address'.
	1413	+ *
	1414	+ * All-zero BD addresses are rejected, because those could be properties
	1415	+ * that exist in the firmware tables, but were not updated by the firmware. For
	1416	+ * example, the DTS could define 'local-bd-address', with zero BD addresses.
	1417	+ */
	1418	+static void hci_dev_get_bd_addr_from_property(struct hci_dev *hdev)
	1419	+{
	1420	+ struct fwnode_handle *fwnode = dev_fwnode(hdev->dev.parent);
	1421	+ bdaddr_t ba;
	1422	+ int ret;
	1423	+
	1424	+ ret = fwnode_property_read_u8_array(fwnode, "local-bd-address",
	1425	+ (u8 *)&ba, sizeof(ba));
	1426	+ if (ret < 0 \|\| !bacmp(&ba, BDADDR_ANY))
	1427	+ return;
	1428	+
	1429	+ bacpy(&hdev->public_addr, &ba);
	1430	+}
	1431	+
1366	1432	static int hci_dev_do_open(struct hci_dev *hdev)
1367	1433	{
1368	1434	int ret = 0;
..	..	@@ -1425,19 +1491,55 @@
1425	1491
1426	1492	if (hci_dev_test_flag(hdev, HCI_SETUP) \|\|
1427	1493	test_bit(HCI_QUIRK_NON_PERSISTENT_SETUP, &hdev->quirks)) {
	1494	+ bool invalid_bdaddr;
	1495	+
1428	1496	hci_sock_dev_event(hdev, HCI_DEV_SETUP);
1429	1497
1430	1498	if (hdev->setup)
1431	1499	ret = hdev->setup(hdev);
1432	1500
	1501	+ /* The transport driver can set the quirk to mark the
	1502	+ * BD_ADDR invalid before creating the HCI device or in
	1503	+ * its setup callback.
	1504	+ */
	1505	+ invalid_bdaddr = test_bit(HCI_QUIRK_INVALID_BDADDR,
	1506	+ &hdev->quirks);
	1507	+
	1508	+ if (ret)
	1509	+ goto setup_failed;
	1510	+
	1511	+ if (test_bit(HCI_QUIRK_USE_BDADDR_PROPERTY, &hdev->quirks)) {
	1512	+ if (!bacmp(&hdev->public_addr, BDADDR_ANY))
	1513	+ hci_dev_get_bd_addr_from_property(hdev);
	1514	+
	1515	+ if (bacmp(&hdev->public_addr, BDADDR_ANY) &&
	1516	+ hdev->set_bdaddr) {
	1517	+ ret = hdev->set_bdaddr(hdev,
	1518	+ &hdev->public_addr);
	1519	+
	1520	+ /* If setting of the BD_ADDR from the device
	1521	+ * property succeeds, then treat the address
	1522	+ * as valid even if the invalid BD_ADDR
	1523	+ * quirk indicates otherwise.
	1524	+ */
	1525	+ if (!ret)
	1526	+ invalid_bdaddr = false;
	1527	+ }
	1528	+ }
	1529	+
	1530	+setup_failed:
1433	1531	/* The transport driver can set these quirks before
1434	1532	* creating the HCI device or in its setup callback.
	1533	+ *
	1534	+ * For the invalid BD_ADDR quirk it is possible that
	1535	+ * it becomes a valid address if the bootloader does
	1536	+ * provide it (see above).
1435	1537	*
1436	1538	* In case any of them is set, the controller has to
1437	1539	* start up as unconfigured.
1438	1540	*/
1439	1541	if (test_bit(HCI_QUIRK_EXTERNAL_CONFIG, &hdev->quirks) \|\|
1440		- test_bit(HCI_QUIRK_INVALID_BDADDR, &hdev->quirks))
	1542	+ invalid_bdaddr)
1441	1543	hci_dev_set_flag(hdev, HCI_UNCONFIGURED);
1442	1544
1443	1545	/* For an unconfigured controller it is required to
..	..	@@ -1483,6 +1585,8 @@
1483	1585	hci_dev_test_flag(hdev, HCI_VENDOR_DIAG) && hdev->set_diag)
1484	1586	ret = hdev->set_diag(hdev, true);
1485	1587
	1588	+ msft_do_open(hdev);
	1589	+
1486	1590	clear_bit(HCI_INIT, &hdev->flags);
1487	1591
1488	1592	if (!ret) {
..	..	@@ -1519,6 +1623,7 @@
1519	1623	hdev->flush(hdev);
1520	1624
1521	1625	if (hdev->sent_cmd) {
	1626	+ cancel_delayed_work_sync(&hdev->cmd_timer);
1522	1627	kfree_skb(hdev->sent_cmd);
1523	1628	hdev->sent_cmd = NULL;
1524	1629	}
..	..	@@ -1628,14 +1733,6 @@
1628	1733	hci_request_cancel_all(hdev);
1629	1734	hci_req_sync_lock(hdev);
1630	1735
1631		- if (!hci_dev_test_flag(hdev, HCI_UNREGISTER) &&
1632		- !hci_dev_test_flag(hdev, HCI_USER_CHANNEL) &&
1633		- test_bit(HCI_UP, &hdev->flags)) {
1634		- /* Execute vendor specific shutdown routine */
1635		- if (hdev->shutdown)
1636		- hdev->shutdown(hdev);
1637		- }
1638		-
1639	1736	if (!test_and_clear_bit(HCI_UP, &hdev->flags)) {
1640	1737	cancel_delayed_work_sync(&hdev->cmd_timer);
1641	1738	hci_req_sync_unlock(hdev);
..	..	@@ -1691,6 +1788,8 @@
1691	1788
1692	1789	hci_sock_dev_event(hdev, HCI_DEV_DOWN);
1693	1790
	1791	+ msft_do_close(hdev);
	1792	+
1694	1793	if (hdev->flush)
1695	1794	hdev->flush(hdev);
1696	1795
..	..	@@ -1721,6 +1820,9 @@
1721	1820
1722	1821	clear_bit(HCI_RUNNING, &hdev->flags);
1723	1822	hci_sock_dev_event(hdev, HCI_DEV_CLOSE);
	1823	+
	1824	+ if (test_and_clear_bit(SUSPEND_POWERING_DOWN, hdev->suspend_tasks))
	1825	+ wake_up(&hdev->suspend_wait_q);
1724	1826
1725	1827	/* After this point our queues are empty
1726	1828	* and no tasks are scheduled. */
..	..	@@ -2241,9 +2343,9 @@
2241	2343
2242	2344	void hci_link_keys_clear(struct hci_dev *hdev)
2243	2345	{
2244		- struct link_key *key;
	2346	+ struct link_key key, tmp;
2245	2347
2246		- list_for_each_entry_rcu(key, &hdev->link_keys, list) {
	2348	+ list_for_each_entry_safe(key, tmp, &hdev->link_keys, list) {
2247	2349	list_del_rcu(&key->list);
2248	2350	kfree_rcu(key, rcu);
2249	2351	}
..	..	@@ -2251,9 +2353,9 @@
2251	2353
2252	2354	void hci_smp_ltks_clear(struct hci_dev *hdev)
2253	2355	{
2254		- struct smp_ltk *k;
	2356	+ struct smp_ltk k, tmp;
2255	2357
2256		- list_for_each_entry_rcu(k, &hdev->long_term_keys, list) {
	2358	+ list_for_each_entry_safe(k, tmp, &hdev->long_term_keys, list) {
2257	2359	list_del_rcu(&k->list);
2258	2360	kfree_rcu(k, rcu);
2259	2361	}
..	..	@@ -2261,12 +2363,39 @@
2261	2363
2262	2364	void hci_smp_irks_clear(struct hci_dev *hdev)
2263	2365	{
2264		- struct smp_irk *k;
	2366	+ struct smp_irk k, tmp;
2265	2367
2266		- list_for_each_entry_rcu(k, &hdev->identity_resolving_keys, list) {
	2368	+ list_for_each_entry_safe(k, tmp, &hdev->identity_resolving_keys, list) {
2267	2369	list_del_rcu(&k->list);
2268	2370	kfree_rcu(k, rcu);
2269	2371	}
	2372	+}
	2373	+
	2374	+void hci_blocked_keys_clear(struct hci_dev *hdev)
	2375	+{
	2376	+ struct blocked_key b, tmp;
	2377	+
	2378	+ list_for_each_entry_safe(b, tmp, &hdev->blocked_keys, list) {
	2379	+ list_del_rcu(&b->list);
	2380	+ kfree_rcu(b, rcu);
	2381	+ }
	2382	+}
	2383	+
	2384	+bool hci_is_blocked_key(struct hci_dev *hdev, u8 type, u8 val[16])
	2385	+{
	2386	+ bool blocked = false;
	2387	+ struct blocked_key *b;
	2388	+
	2389	+ rcu_read_lock();
	2390	+ list_for_each_entry_rcu(b, &hdev->blocked_keys, list) {
	2391	+ if (b->type == type && !memcmp(b->val, val, sizeof(b->val))) {
	2392	+ blocked = true;
	2393	+ break;
	2394	+ }
	2395	+ }
	2396	+
	2397	+ rcu_read_unlock();
	2398	+ return blocked;
2270	2399	}
2271	2400
2272	2401	struct link_key hci_find_link_key(struct hci_dev hdev, bdaddr_t *bdaddr)
..	..	@@ -2277,6 +2406,16 @@
2277	2406	list_for_each_entry_rcu(k, &hdev->link_keys, list) {
2278	2407	if (bacmp(bdaddr, &k->bdaddr) == 0) {
2279	2408	rcu_read_unlock();
	2409	+
	2410	+ if (hci_is_blocked_key(hdev,
	2411	+ HCI_BLOCKED_KEY_TYPE_LINKKEY,
	2412	+ k->val)) {
	2413	+ bt_dev_warn_ratelimited(hdev,
	2414	+ "Link key blocked for %pMR",
	2415	+ &k->bdaddr);
	2416	+ return NULL;
	2417	+ }
	2418	+
2280	2419	return k;
2281	2420	}
2282	2421	}
..	..	@@ -2345,6 +2484,15 @@
2345	2484
2346	2485	if (smp_ltk_is_sc(k) \|\| ltk_role(k->type) == role) {
2347	2486	rcu_read_unlock();
	2487	+
	2488	+ if (hci_is_blocked_key(hdev, HCI_BLOCKED_KEY_TYPE_LTK,
	2489	+ k->val)) {
	2490	+ bt_dev_warn_ratelimited(hdev,
	2491	+ "LTK blocked for %pMR",
	2492	+ &k->bdaddr);
	2493	+ return NULL;
	2494	+ }
	2495	+
2348	2496	return k;
2349	2497	}
2350	2498	}
..	..	@@ -2355,31 +2503,42 @@
2355	2503
2356	2504	struct smp_irk hci_find_irk_by_rpa(struct hci_dev hdev, bdaddr_t *rpa)
2357	2505	{
	2506	+ struct smp_irk *irk_to_return = NULL;
2358	2507	struct smp_irk *irk;
2359	2508
2360	2509	rcu_read_lock();
2361	2510	list_for_each_entry_rcu(irk, &hdev->identity_resolving_keys, list) {
2362	2511	if (!bacmp(&irk->rpa, rpa)) {
2363		- rcu_read_unlock();
2364		- return irk;
	2512	+ irk_to_return = irk;
	2513	+ goto done;
2365	2514	}
2366	2515	}
2367	2516
2368	2517	list_for_each_entry_rcu(irk, &hdev->identity_resolving_keys, list) {
2369	2518	if (smp_irk_matches(hdev, irk->val, rpa)) {
2370	2519	bacpy(&irk->rpa, rpa);
2371		- rcu_read_unlock();
2372		- return irk;
	2520	+ irk_to_return = irk;
	2521	+ goto done;
2373	2522	}
2374	2523	}
	2524	+
	2525	+done:
	2526	+ if (irk_to_return && hci_is_blocked_key(hdev, HCI_BLOCKED_KEY_TYPE_IRK,
	2527	+ irk_to_return->val)) {
	2528	+ bt_dev_warn_ratelimited(hdev, "Identity key blocked for %pMR",
	2529	+ &irk_to_return->bdaddr);
	2530	+ irk_to_return = NULL;
	2531	+ }
	2532	+
2375	2533	rcu_read_unlock();
2376	2534
2377		- return NULL;
	2535	+ return irk_to_return;
2378	2536	}
2379	2537
2380	2538	struct smp_irk hci_find_irk_by_addr(struct hci_dev hdev, bdaddr_t *bdaddr,
2381	2539	u8 addr_type)
2382	2540	{
	2541	+ struct smp_irk *irk_to_return = NULL;
2383	2542	struct smp_irk *irk;
2384	2543
2385	2544	/* Identity Address must be public or static random */
..	..	@@ -2390,13 +2549,23 @@
2390	2549	list_for_each_entry_rcu(irk, &hdev->identity_resolving_keys, list) {
2391	2550	if (addr_type == irk->addr_type &&
2392	2551	bacmp(bdaddr, &irk->bdaddr) == 0) {
2393		- rcu_read_unlock();
2394		- return irk;
	2552	+ irk_to_return = irk;
	2553	+ goto done;
2395	2554	}
2396	2555	}
	2556	+
	2557	+done:
	2558	+
	2559	+ if (irk_to_return && hci_is_blocked_key(hdev, HCI_BLOCKED_KEY_TYPE_IRK,
	2560	+ irk_to_return->val)) {
	2561	+ bt_dev_warn_ratelimited(hdev, "Identity key blocked for %pMR",
	2562	+ &irk_to_return->bdaddr);
	2563	+ irk_to_return = NULL;
	2564	+ }
	2565	+
2397	2566	rcu_read_unlock();
2398	2567
2399		- return NULL;
	2568	+ return irk_to_return;
2400	2569	}
2401	2570
2402	2571	struct link_key hci_add_link_key(struct hci_dev hdev, struct hci_conn *conn,
..	..	@@ -2516,10 +2685,10 @@
2516	2685
2517	2686	int hci_remove_ltk(struct hci_dev hdev, bdaddr_t bdaddr, u8 bdaddr_type)
2518	2687	{
2519		- struct smp_ltk *k;
	2688	+ struct smp_ltk k, tmp;
2520	2689	int removed = 0;
2521	2690
2522		- list_for_each_entry_rcu(k, &hdev->long_term_keys, list) {
	2691	+ list_for_each_entry_safe(k, tmp, &hdev->long_term_keys, list) {
2523	2692	if (bacmp(bdaddr, &k->bdaddr) \|\| k->bdaddr_type != bdaddr_type)
2524	2693	continue;
2525	2694
..	..	@@ -2535,9 +2704,9 @@
2535	2704
2536	2705	void hci_remove_irk(struct hci_dev hdev, bdaddr_t bdaddr, u8 addr_type)
2537	2706	{
2538		- struct smp_irk *k;
	2707	+ struct smp_irk k, tmp;
2539	2708
2540		- list_for_each_entry_rcu(k, &hdev->identity_resolving_keys, list) {
	2709	+ list_for_each_entry_safe(k, tmp, &hdev->identity_resolving_keys, list) {
2541	2710	if (bacmp(bdaddr, &k->bdaddr) \|\| k->addr_type != addr_type)
2542	2711	continue;
2543	2712
..	..	@@ -2598,6 +2767,9 @@
2598	2767	} else {
2599	2768	bt_dev_err(hdev, "command tx timeout");
2600	2769	}
	2770	+
	2771	+ if (hdev->cmd_timeout)
	2772	+ hdev->cmd_timeout(hdev);
2601	2773
2602	2774	atomic_set(&hdev->cmd_cnt, 1);
2603	2775	queue_work(hdev->workqueue, &hdev->cmd_work);
..	..	@@ -2804,8 +2976,8 @@
2804	2976	memset(adv_instance->scan_rsp_data, 0,
2805	2977	sizeof(adv_instance->scan_rsp_data));
2806	2978	} else {
2807		- if (hdev->adv_instance_cnt >= HCI_MAX_ADV_INSTANCES \|\|
2808		- instance < 1 \|\| instance > HCI_MAX_ADV_INSTANCES)
	2979	+ if (hdev->adv_instance_cnt >= hdev->le_num_of_adv_sets \|\|
	2980	+ instance < 1 \|\| instance > hdev->le_num_of_adv_sets)
2809	2981	return -EOVERFLOW;
2810	2982
2811	2983	adv_instance = kzalloc(sizeof(*adv_instance), GFP_KERNEL);
..	..	@@ -2833,7 +3005,7 @@
2833	3005	adv_instance->remaining_time = timeout;
2834	3006
2835	3007	if (duration == 0)
2836		- adv_instance->duration = HCI_DEFAULT_ADV_DURATION;
	3008	+ adv_instance->duration = hdev->def_multi_adv_rotation_duration;
2837	3009	else
2838	3010	adv_instance->duration = duration;
2839	3011
..	..	@@ -2847,10 +3019,128 @@
2847	3019	return 0;
2848	3020	}
2849	3021
	3022	+/* This function requires the caller holds hdev->lock */
	3023	+void hci_adv_monitors_clear(struct hci_dev *hdev)
	3024	+{
	3025	+ struct adv_monitor *monitor;
	3026	+ int handle;
	3027	+
	3028	+ idr_for_each_entry(&hdev->adv_monitors_idr, monitor, handle)
	3029	+ hci_free_adv_monitor(monitor);
	3030	+
	3031	+ idr_destroy(&hdev->adv_monitors_idr);
	3032	+}
	3033	+
	3034	+void hci_free_adv_monitor(struct adv_monitor *monitor)
	3035	+{
	3036	+ struct adv_pattern *pattern;
	3037	+ struct adv_pattern *tmp;
	3038	+
	3039	+ if (!monitor)
	3040	+ return;
	3041	+
	3042	+ list_for_each_entry_safe(pattern, tmp, &monitor->patterns, list)
	3043	+ kfree(pattern);
	3044	+
	3045	+ kfree(monitor);
	3046	+}
	3047	+
	3048	+/* This function requires the caller holds hdev->lock */
	3049	+int hci_add_adv_monitor(struct hci_dev hdev, struct adv_monitor monitor)
	3050	+{
	3051	+ int min, max, handle;
	3052	+
	3053	+ if (!monitor)
	3054	+ return -EINVAL;
	3055	+
	3056	+ min = HCI_MIN_ADV_MONITOR_HANDLE;
	3057	+ max = HCI_MIN_ADV_MONITOR_HANDLE + HCI_MAX_ADV_MONITOR_NUM_HANDLES;
	3058	+ handle = idr_alloc(&hdev->adv_monitors_idr, monitor, min, max,
	3059	+ GFP_KERNEL);
	3060	+ if (handle < 0)
	3061	+ return handle;
	3062	+
	3063	+ hdev->adv_monitors_cnt++;
	3064	+ monitor->handle = handle;
	3065	+
	3066	+ hci_update_background_scan(hdev);
	3067	+
	3068	+ return 0;
	3069	+}
	3070	+
	3071	+static int free_adv_monitor(int id, void ptr, void data)
	3072	+{
	3073	+ struct hci_dev *hdev = data;
	3074	+ struct adv_monitor *monitor = ptr;
	3075	+
	3076	+ idr_remove(&hdev->adv_monitors_idr, monitor->handle);
	3077	+ hci_free_adv_monitor(monitor);
	3078	+ hdev->adv_monitors_cnt--;
	3079	+
	3080	+ return 0;
	3081	+}
	3082	+
	3083	+/* This function requires the caller holds hdev->lock */
	3084	+int hci_remove_adv_monitor(struct hci_dev *hdev, u16 handle)
	3085	+{
	3086	+ struct adv_monitor *monitor;
	3087	+
	3088	+ if (handle) {
	3089	+ monitor = idr_find(&hdev->adv_monitors_idr, handle);
	3090	+ if (!monitor)
	3091	+ return -ENOENT;
	3092	+
	3093	+ idr_remove(&hdev->adv_monitors_idr, monitor->handle);
	3094	+ hci_free_adv_monitor(monitor);
	3095	+ hdev->adv_monitors_cnt--;
	3096	+ } else {
	3097	+ /* Remove all monitors if handle is 0. */
	3098	+ idr_for_each(&hdev->adv_monitors_idr, &free_adv_monitor, hdev);
	3099	+ }
	3100	+
	3101	+ hci_update_background_scan(hdev);
	3102	+
	3103	+ return 0;
	3104	+}
	3105	+
	3106	+/* This function requires the caller holds hdev->lock */
	3107	+bool hci_is_adv_monitoring(struct hci_dev *hdev)
	3108	+{
	3109	+ return !idr_is_empty(&hdev->adv_monitors_idr);
	3110	+}
	3111	+
2850	3112	struct bdaddr_list hci_bdaddr_list_lookup(struct list_head bdaddr_list,
2851	3113	bdaddr_t *bdaddr, u8 type)
2852	3114	{
2853	3115	struct bdaddr_list *b;
	3116	+
	3117	+ list_for_each_entry(b, bdaddr_list, list) {
	3118	+ if (!bacmp(&b->bdaddr, bdaddr) && b->bdaddr_type == type)
	3119	+ return b;
	3120	+ }
	3121	+
	3122	+ return NULL;
	3123	+}
	3124	+
	3125	+struct bdaddr_list_with_irk *hci_bdaddr_list_lookup_with_irk(
	3126	+ struct list_head bdaddr_list, bdaddr_t bdaddr,
	3127	+ u8 type)
	3128	+{
	3129	+ struct bdaddr_list_with_irk *b;
	3130	+
	3131	+ list_for_each_entry(b, bdaddr_list, list) {
	3132	+ if (!bacmp(&b->bdaddr, bdaddr) && b->bdaddr_type == type)
	3133	+ return b;
	3134	+ }
	3135	+
	3136	+ return NULL;
	3137	+}
	3138	+
	3139	+struct bdaddr_list_with_flags *
	3140	+hci_bdaddr_list_lookup_with_flags(struct list_head *bdaddr_list,
	3141	+ bdaddr_t *bdaddr, u8 type)
	3142	+{
	3143	+ struct bdaddr_list_with_flags *b;
2854	3144
2855	3145	list_for_each_entry(b, bdaddr_list, list) {
2856	3146	if (!bacmp(&b->bdaddr, bdaddr) && b->bdaddr_type == type)
..	..	@@ -2892,6 +3182,59 @@
2892	3182	return 0;
2893	3183	}
2894	3184
	3185	+int hci_bdaddr_list_add_with_irk(struct list_head list, bdaddr_t bdaddr,
	3186	+ u8 type, u8 peer_irk, u8 local_irk)
	3187	+{
	3188	+ struct bdaddr_list_with_irk *entry;
	3189	+
	3190	+ if (!bacmp(bdaddr, BDADDR_ANY))
	3191	+ return -EBADF;
	3192	+
	3193	+ if (hci_bdaddr_list_lookup(list, bdaddr, type))
	3194	+ return -EEXIST;
	3195	+
	3196	+ entry = kzalloc(sizeof(*entry), GFP_KERNEL);
	3197	+ if (!entry)
	3198	+ return -ENOMEM;
	3199	+
	3200	+ bacpy(&entry->bdaddr, bdaddr);
	3201	+ entry->bdaddr_type = type;
	3202	+
	3203	+ if (peer_irk)
	3204	+ memcpy(entry->peer_irk, peer_irk, 16);
	3205	+
	3206	+ if (local_irk)
	3207	+ memcpy(entry->local_irk, local_irk, 16);
	3208	+
	3209	+ list_add(&entry->list, list);
	3210	+
	3211	+ return 0;
	3212	+}
	3213	+
	3214	+int hci_bdaddr_list_add_with_flags(struct list_head list, bdaddr_t bdaddr,
	3215	+ u8 type, u32 flags)
	3216	+{
	3217	+ struct bdaddr_list_with_flags *entry;
	3218	+
	3219	+ if (!bacmp(bdaddr, BDADDR_ANY))
	3220	+ return -EBADF;
	3221	+
	3222	+ if (hci_bdaddr_list_lookup(list, bdaddr, type))
	3223	+ return -EEXIST;
	3224	+
	3225	+ entry = kzalloc(sizeof(*entry), GFP_KERNEL);
	3226	+ if (!entry)
	3227	+ return -ENOMEM;
	3228	+
	3229	+ bacpy(&entry->bdaddr, bdaddr);
	3230	+ entry->bdaddr_type = type;
	3231	+ entry->current_flags = flags;
	3232	+
	3233	+ list_add(&entry->list, list);
	3234	+
	3235	+ return 0;
	3236	+}
	3237	+
2895	3238	int hci_bdaddr_list_del(struct list_head list, bdaddr_t bdaddr, u8 type)
2896	3239	{
2897	3240	struct bdaddr_list *entry;
..	..	@@ -2902,6 +3245,46 @@
2902	3245	}
2903	3246
2904	3247	entry = hci_bdaddr_list_lookup(list, bdaddr, type);
	3248	+ if (!entry)
	3249	+ return -ENOENT;
	3250	+
	3251	+ list_del(&entry->list);
	3252	+ kfree(entry);
	3253	+
	3254	+ return 0;
	3255	+}
	3256	+
	3257	+int hci_bdaddr_list_del_with_irk(struct list_head list, bdaddr_t bdaddr,
	3258	+ u8 type)
	3259	+{
	3260	+ struct bdaddr_list_with_irk *entry;
	3261	+
	3262	+ if (!bacmp(bdaddr, BDADDR_ANY)) {
	3263	+ hci_bdaddr_list_clear(list);
	3264	+ return 0;
	3265	+ }
	3266	+
	3267	+ entry = hci_bdaddr_list_lookup_with_irk(list, bdaddr, type);
	3268	+ if (!entry)
	3269	+ return -ENOENT;
	3270	+
	3271	+ list_del(&entry->list);
	3272	+ kfree(entry);
	3273	+
	3274	+ return 0;
	3275	+}
	3276	+
	3277	+int hci_bdaddr_list_del_with_flags(struct list_head list, bdaddr_t bdaddr,
	3278	+ u8 type)
	3279	+{
	3280	+ struct bdaddr_list_with_flags *entry;
	3281	+
	3282	+ if (!bacmp(bdaddr, BDADDR_ANY)) {
	3283	+ hci_bdaddr_list_clear(list);
	3284	+ return 0;
	3285	+ }
	3286	+
	3287	+ entry = hci_bdaddr_list_lookup_with_flags(list, bdaddr, type);
2905	3288	if (!entry)
2906	3289	return -ENOENT;
2907	3290
..	..	@@ -2932,6 +3315,15 @@
2932	3315	bdaddr_t *addr, u8 addr_type)
2933	3316	{
2934	3317	struct hci_conn_params *param;
	3318	+
	3319	+ switch (addr_type) {
	3320	+ case ADDR_LE_DEV_PUBLIC_RESOLVED:
	3321	+ addr_type = ADDR_LE_DEV_PUBLIC;
	3322	+ break;
	3323	+ case ADDR_LE_DEV_RANDOM_RESOLVED:
	3324	+ addr_type = ADDR_LE_DEV_RANDOM;
	3325	+ break;
	3326	+ }
2935	3327
2936	3328	list_for_each_entry(param, list, action) {
2937	3329	if (bacmp(&param->addr, addr) == 0 &&
..	..	@@ -3066,6 +3458,134 @@
3066	3458	}
3067	3459	}
3068	3460
	3461	+static void hci_suspend_clear_tasks(struct hci_dev *hdev)
	3462	+{
	3463	+ int i;
	3464	+
	3465	+ for (i = 0; i < __SUSPEND_NUM_TASKS; i++)
	3466	+ clear_bit(i, hdev->suspend_tasks);
	3467	+
	3468	+ wake_up(&hdev->suspend_wait_q);
	3469	+}
	3470	+
	3471	+static int hci_suspend_wait_event(struct hci_dev *hdev)
	3472	+{
	3473	+#define WAKE_COND \
	3474	+ (find_first_bit(hdev->suspend_tasks, __SUSPEND_NUM_TASKS) == \
	3475	+ __SUSPEND_NUM_TASKS)
	3476	+
	3477	+ int i;
	3478	+ int ret = wait_event_timeout(hdev->suspend_wait_q,
	3479	+ WAKE_COND, SUSPEND_NOTIFIER_TIMEOUT);
	3480	+
	3481	+ if (ret == 0) {
	3482	+ bt_dev_err(hdev, "Timed out waiting for suspend events");
	3483	+ for (i = 0; i < __SUSPEND_NUM_TASKS; ++i) {
	3484	+ if (test_bit(i, hdev->suspend_tasks))
	3485	+ bt_dev_err(hdev, "Suspend timeout bit: %d", i);
	3486	+ clear_bit(i, hdev->suspend_tasks);
	3487	+ }
	3488	+
	3489	+ ret = -ETIMEDOUT;
	3490	+ } else {
	3491	+ ret = 0;
	3492	+ }
	3493	+
	3494	+ return ret;
	3495	+}
	3496	+
	3497	+static void hci_prepare_suspend(struct work_struct *work)
	3498	+{
	3499	+ struct hci_dev *hdev =
	3500	+ container_of(work, struct hci_dev, suspend_prepare);
	3501	+
	3502	+ hci_dev_lock(hdev);
	3503	+ hci_req_prepare_suspend(hdev, hdev->suspend_state_next);
	3504	+ hci_dev_unlock(hdev);
	3505	+}
	3506	+
	3507	+static int hci_change_suspend_state(struct hci_dev *hdev,
	3508	+ enum suspended_state next)
	3509	+{
	3510	+ hdev->suspend_state_next = next;
	3511	+ set_bit(SUSPEND_PREPARE_NOTIFIER, hdev->suspend_tasks);
	3512	+ queue_work(hdev->req_workqueue, &hdev->suspend_prepare);
	3513	+ return hci_suspend_wait_event(hdev);
	3514	+}
	3515	+
	3516	+static void hci_clear_wake_reason(struct hci_dev *hdev)
	3517	+{
	3518	+ hci_dev_lock(hdev);
	3519	+
	3520	+ hdev->wake_reason = 0;
	3521	+ bacpy(&hdev->wake_addr, BDADDR_ANY);
	3522	+ hdev->wake_addr_type = 0;
	3523	+
	3524	+ hci_dev_unlock(hdev);
	3525	+}
	3526	+
	3527	+static int hci_suspend_notifier(struct notifier_block *nb, unsigned long action,
	3528	+ void *data)
	3529	+{
	3530	+ struct hci_dev *hdev =
	3531	+ container_of(nb, struct hci_dev, suspend_notifier);
	3532	+ int ret = 0;
	3533	+ u8 state = BT_RUNNING;
	3534	+
	3535	+ /* If powering down, wait for completion. */
	3536	+ if (mgmt_powering_down(hdev)) {
	3537	+ set_bit(SUSPEND_POWERING_DOWN, hdev->suspend_tasks);
	3538	+ ret = hci_suspend_wait_event(hdev);
	3539	+ if (ret)
	3540	+ goto done;
	3541	+ }
	3542	+
	3543	+ /* Suspend notifier should only act on events when powered. */
	3544	+ if (!hdev_is_powered(hdev) \|\|
	3545	+ hci_dev_test_flag(hdev, HCI_UNREGISTER))
	3546	+ goto done;
	3547	+
	3548	+ if (action == PM_SUSPEND_PREPARE) {
	3549	+ /* Suspend consists of two actions:
	3550	+ * - First, disconnect everything and make the controller not
	3551	+ * connectable (disabling scanning)
	3552	+ * - Second, program event filter/whitelist and enable scan
	3553	+ */
	3554	+ ret = hci_change_suspend_state(hdev, BT_SUSPEND_DISCONNECT);
	3555	+ if (!ret)
	3556	+ state = BT_SUSPEND_DISCONNECT;
	3557	+
	3558	+ /* Only configure whitelist if disconnect succeeded and wake
	3559	+ * isn't being prevented.
	3560	+ */
	3561	+ if (!ret && !(hdev->prevent_wake && hdev->prevent_wake(hdev))) {
	3562	+ ret = hci_change_suspend_state(hdev,
	3563	+ BT_SUSPEND_CONFIGURE_WAKE);
	3564	+ if (!ret)
	3565	+ state = BT_SUSPEND_CONFIGURE_WAKE;
	3566	+ }
	3567	+
	3568	+ hci_clear_wake_reason(hdev);
	3569	+ mgmt_suspending(hdev, state);
	3570	+
	3571	+ } else if (action == PM_POST_SUSPEND) {
	3572	+ ret = hci_change_suspend_state(hdev, BT_RUNNING);
	3573	+
	3574	+ mgmt_resuming(hdev, hdev->wake_reason, &hdev->wake_addr,
	3575	+ hdev->wake_addr_type);
	3576	+ }
	3577	+
	3578	+done:
	3579	+ /* We always allow suspend even if suspend preparation failed and
	3580	+ * attempt to recover in resume.
	3581	+ */
	3582	+ if (ret)
	3583	+ bt_dev_err(hdev, "Suspend notifier action (%lu) failed: %d",
	3584	+ action, ret);
	3585	+
	3586	+ return NOTIFY_DONE;
	3587	+}
	3588	+
3069	3589	/* Alloc HCI device */
3070	3590	struct hci_dev *hci_alloc_dev(void)
3071	3591	{
..	..	@@ -3095,6 +3615,12 @@
3095	3615	hdev->le_adv_max_interval = 0x0800;
3096	3616	hdev->le_scan_interval = 0x0060;
3097	3617	hdev->le_scan_window = 0x0030;
	3618	+ hdev->le_scan_int_suspend = 0x0400;
	3619	+ hdev->le_scan_window_suspend = 0x0012;
	3620	+ hdev->le_scan_int_discovery = DISCOV_LE_SCAN_INT;
	3621	+ hdev->le_scan_window_discovery = DISCOV_LE_SCAN_WIN;
	3622	+ hdev->le_scan_int_connect = 0x0060;
	3623	+ hdev->le_scan_window_connect = 0x0060;
3098	3624	hdev->le_conn_min_interval = 0x0018;
3099	3625	hdev->le_conn_max_interval = 0x0028;
3100	3626	hdev->le_conn_latency = 0x0000;
..	..	@@ -3109,11 +3635,21 @@
3109	3635	hdev->le_min_key_size = SMP_MIN_ENC_KEY_SIZE;
3110	3636	hdev->le_tx_def_phys = HCI_LE_SET_PHY_1M;
3111	3637	hdev->le_rx_def_phys = HCI_LE_SET_PHY_1M;
	3638	+ hdev->le_num_of_adv_sets = HCI_MAX_ADV_INSTANCES;
	3639	+ hdev->def_multi_adv_rotation_duration = HCI_DEFAULT_ADV_DURATION;
	3640	+ hdev->def_le_autoconnect_timeout = HCI_LE_AUTOCONN_TIMEOUT;
3112	3641
3113	3642	hdev->rpa_timeout = HCI_DEFAULT_RPA_TIMEOUT;
3114	3643	hdev->discov_interleaved_timeout = DISCOV_INTERLEAVED_TIMEOUT;
3115	3644	hdev->conn_info_min_age = DEFAULT_CONN_INFO_MIN_AGE;
3116	3645	hdev->conn_info_max_age = DEFAULT_CONN_INFO_MAX_AGE;
	3646	+ hdev->auth_payload_timeout = DEFAULT_AUTH_PAYLOAD_TIMEOUT;
	3647	+ hdev->min_enc_key_size = HCI_MIN_ENC_KEY_SIZE;
	3648	+
	3649	+ /* default 1.28 sec page scan */
	3650	+ hdev->def_page_scan_type = PAGE_SCAN_TYPE_STANDARD;
	3651	+ hdev->def_page_scan_int = 0x0800;
	3652	+ hdev->def_page_scan_window = 0x0012;
3117	3653
3118	3654	mutex_init(&hdev->lock);
3119	3655	mutex_init(&hdev->req_lock);
..	..	@@ -3133,12 +3669,14 @@
3133	3669	INIT_LIST_HEAD(&hdev->pend_le_reports);
3134	3670	INIT_LIST_HEAD(&hdev->conn_hash.list);
3135	3671	INIT_LIST_HEAD(&hdev->adv_instances);
	3672	+ INIT_LIST_HEAD(&hdev->blocked_keys);
3136	3673
3137	3674	INIT_WORK(&hdev->rx_work, hci_rx_work);
3138	3675	INIT_WORK(&hdev->cmd_work, hci_cmd_work);
3139	3676	INIT_WORK(&hdev->tx_work, hci_tx_work);
3140	3677	INIT_WORK(&hdev->power_on, hci_power_on);
3141	3678	INIT_WORK(&hdev->error_reset, hci_error_reset);
	3679	+ INIT_WORK(&hdev->suspend_prepare, hci_prepare_suspend);
3142	3680
3143	3681	INIT_DELAYED_WORK(&hdev->power_off, hci_power_off);
3144	3682
..	..	@@ -3147,6 +3685,7 @@
3147	3685	skb_queue_head_init(&hdev->raw_q);
3148	3686
3149	3687	init_waitqueue_head(&hdev->req_wait_q);
	3688	+ init_waitqueue_head(&hdev->suspend_wait_q);
3150	3689
3151	3690	INIT_DELAYED_WORK(&hdev->cmd_timer, hci_cmd_timeout);
3152	3691
..	..	@@ -3180,10 +3719,10 @@
3180	3719	*/
3181	3720	switch (hdev->dev_type) {
3182	3721	case HCI_PRIMARY:
3183		- id = ida_simple_get(&hci_index_ida, 0, 0, GFP_KERNEL);
	3722	+ id = ida_simple_get(&hci_index_ida, 0, HCI_MAX_ID, GFP_KERNEL);
3184	3723	break;
3185	3724	case HCI_AMP:
3186		- id = ida_simple_get(&hci_index_ida, 1, 0, GFP_KERNEL);
	3725	+ id = ida_simple_get(&hci_index_ida, 1, HCI_MAX_ID, GFP_KERNEL);
3187	3726	break;
3188	3727	default:
3189	3728	return -EINVAL;
..	..	@@ -3192,7 +3731,7 @@
3192	3731	if (id < 0)
3193	3732	return id;
3194	3733
3195		- sprintf(hdev->name, "hci%d", id);
	3734	+ snprintf(hdev->name, sizeof(hdev->name), "hci%d", id);
3196	3735	hdev->id = id;
3197	3736
3198	3737	BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus);
..	..	@@ -3258,7 +3797,17 @@
3258	3797	hci_sock_dev_event(hdev, HCI_DEV_REG);
3259	3798	hci_dev_hold(hdev);
3260	3799
	3800	+ if (!hdev->suspend_notifier.notifier_call &&
	3801	+ !test_bit(HCI_QUIRK_NO_SUSPEND_NOTIFIER, &hdev->quirks)) {
	3802	+ hdev->suspend_notifier.notifier_call = hci_suspend_notifier;
	3803	+ error = register_pm_notifier(&hdev->suspend_notifier);
	3804	+ if (error)
	3805	+ goto err_wqueue;
	3806	+ }
	3807	+
3261	3808	queue_work(hdev->req_workqueue, &hdev->power_on);
	3809	+
	3810	+ idr_init(&hdev->adv_monitors_idr);
3262	3811
3263	3812	return id;
3264	3813
..	..	@@ -3285,6 +3834,12 @@
3285	3834	write_unlock(&hci_dev_list_lock);
3286	3835
3287	3836	cancel_work_sync(&hdev->power_on);
	3837	+
	3838	+ if (!test_bit(HCI_QUIRK_NO_SUSPEND_NOTIFIER, &hdev->quirks)) {
	3839	+ hci_suspend_clear_tasks(hdev);
	3840	+ unregister_pm_notifier(&hdev->suspend_notifier);
	3841	+ cancel_work_sync(&hdev->suspend_prepare);
	3842	+ }
3288	3843
3289	3844	hci_dev_do_close(hdev);
3290	3845
..	..	@@ -3332,10 +3887,12 @@
3332	3887	hci_smp_irks_clear(hdev);
3333	3888	hci_remote_oob_data_clear(hdev);
3334	3889	hci_adv_instances_clear(hdev);
	3890	+ hci_adv_monitors_clear(hdev);
3335	3891	hci_bdaddr_list_clear(&hdev->le_white_list);
3336	3892	hci_bdaddr_list_clear(&hdev->le_resolv_list);
3337	3893	hci_conn_params_clear_all(hdev);
3338	3894	hci_discovery_filter_clear(hdev);
	3895	+ hci_blocked_keys_clear(hdev);
3339	3896	hci_dev_unlock(hdev);
3340	3897
3341	3898	ida_simple_remove(&hci_index_ida, hdev->id);
..	..	@@ -3360,7 +3917,7 @@
3360	3917	/* Reset HCI device */
3361	3918	int hci_reset_dev(struct hci_dev *hdev)
3362	3919	{
3363		- const u8 hw_err[] = { HCI_EV_HARDWARE_ERROR, 0x01, 0x00 };
	3920	+ static const u8 hw_err[] = { HCI_EV_HARDWARE_ERROR, 0x01, 0x00 };
3364	3921	struct sk_buff *skb;
3365	3922
3366	3923	skb = bt_skb_alloc(3, GFP_ATOMIC);
..	..	@@ -3386,7 +3943,8 @@
3386	3943
3387	3944	if (hci_skb_pkt_type(skb) != HCI_EVENT_PKT &&
3388	3945	hci_skb_pkt_type(skb) != HCI_ACLDATA_PKT &&
3389		- hci_skb_pkt_type(skb) != HCI_SCODATA_PKT) {
	3946	+ hci_skb_pkt_type(skb) != HCI_SCODATA_PKT &&
	3947	+ hci_skb_pkt_type(skb) != HCI_ISODATA_PKT) {
3390	3948	kfree_skb(skb);
3391	3949	return -EINVAL;
3392	3950	}
..	..	@@ -3923,14 +4481,74 @@
3923	4481	return DIV_ROUND_UP(skb->len - HCI_ACL_HDR_SIZE, hdev->block_len);
3924	4482	}
3925	4483
3926		-static void __check_timeout(struct hci_dev *hdev, unsigned int cnt)
	4484	+static void __check_timeout(struct hci_dev *hdev, unsigned int cnt, u8 type)
3927	4485	{
3928		- if (!hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) {
3929		- /* ACL tx timeout must be longer than maximum
3930		- * link supervision timeout (40.9 seconds) */
3931		- if (!cnt && time_after(jiffies, hdev->acl_last_tx +
3932		- HCI_ACL_TX_TIMEOUT))
3933		- hci_link_tx_to(hdev, ACL_LINK);
	4486	+ unsigned long last_tx;
	4487	+
	4488	+ if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED))
	4489	+ return;
	4490	+
	4491	+ switch (type) {
	4492	+ case LE_LINK:
	4493	+ last_tx = hdev->le_last_tx;
	4494	+ break;
	4495	+ default:
	4496	+ last_tx = hdev->acl_last_tx;
	4497	+ break;
	4498	+ }
	4499	+
	4500	+ /* tx timeout must be longer than maximum link supervision timeout
	4501	+ * (40.9 seconds)
	4502	+ */
	4503	+ if (!cnt && time_after(jiffies, last_tx + HCI_ACL_TX_TIMEOUT))
	4504	+ hci_link_tx_to(hdev, type);
	4505	+}
	4506	+
	4507	+/* Schedule SCO */
	4508	+static void hci_sched_sco(struct hci_dev *hdev)
	4509	+{
	4510	+ struct hci_conn *conn;
	4511	+ struct sk_buff *skb;
	4512	+ int quote;
	4513	+
	4514	+ BT_DBG("%s", hdev->name);
	4515	+
	4516	+ if (!hci_conn_num(hdev, SCO_LINK))
	4517	+ return;
	4518	+
	4519	+ while (hdev->sco_cnt && (conn = hci_low_sent(hdev, SCO_LINK, &quote))) {
	4520	+ while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
	4521	+ BT_DBG("skb %p len %d", skb, skb->len);
	4522	+ hci_send_frame(hdev, skb);
	4523	+
	4524	+ conn->sent++;
	4525	+ if (conn->sent == ~0)
	4526	+ conn->sent = 0;
	4527	+ }
	4528	+ }
	4529	+}
	4530	+
	4531	+static void hci_sched_esco(struct hci_dev *hdev)
	4532	+{
	4533	+ struct hci_conn *conn;
	4534	+ struct sk_buff *skb;
	4535	+ int quote;
	4536	+
	4537	+ BT_DBG("%s", hdev->name);
	4538	+
	4539	+ if (!hci_conn_num(hdev, ESCO_LINK))
	4540	+ return;
	4541	+
	4542	+ while (hdev->sco_cnt && (conn = hci_low_sent(hdev, ESCO_LINK,
	4543	+ &quote))) {
	4544	+ while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
	4545	+ BT_DBG("skb %p len %d", skb, skb->len);
	4546	+ hci_send_frame(hdev, skb);
	4547	+
	4548	+ conn->sent++;
	4549	+ if (conn->sent == ~0)
	4550	+ conn->sent = 0;
	4551	+ }
3934	4552	}
3935	4553	}
3936	4554
..	..	@@ -3941,7 +4559,7 @@
3941	4559	struct sk_buff *skb;
3942	4560	int quote;
3943	4561
3944		- __check_timeout(hdev, cnt);
	4562	+ __check_timeout(hdev, cnt, ACL_LINK);
3945	4563
3946	4564	while (hdev->acl_cnt &&
3947	4565	(chan = hci_chan_sent(hdev, ACL_LINK, &quote))) {
..	..	@@ -3965,6 +4583,10 @@
3965	4583	hdev->acl_cnt--;
3966	4584	chan->sent++;
3967	4585	chan->conn->sent++;
	4586	+
	4587	+ /* Send pending SCO packets right away */
	4588	+ hci_sched_sco(hdev);
	4589	+ hci_sched_esco(hdev);
3968	4590	}
3969	4591	}
3970	4592
..	..	@@ -3980,14 +4602,14 @@
3980	4602	int quote;
3981	4603	u8 type;
3982	4604
3983		- __check_timeout(hdev, cnt);
3984		-
3985	4605	BT_DBG("%s", hdev->name);
3986	4606
3987	4607	if (hdev->dev_type == HCI_AMP)
3988	4608	type = AMP_LINK;
3989	4609	else
3990	4610	type = ACL_LINK;
	4611	+
	4612	+ __check_timeout(hdev, cnt, type);
3991	4613
3992	4614	while (hdev->block_cnt > 0 &&
3993	4615	(chan = hci_chan_sent(hdev, type, &quote))) {
..	..	@@ -4049,54 +4671,6 @@
4049	4671	}
4050	4672	}
4051	4673
4052		-/* Schedule SCO */
4053		-static void hci_sched_sco(struct hci_dev *hdev)
4054		-{
4055		- struct hci_conn *conn;
4056		- struct sk_buff *skb;
4057		- int quote;
4058		-
4059		- BT_DBG("%s", hdev->name);
4060		-
4061		- if (!hci_conn_num(hdev, SCO_LINK))
4062		- return;
4063		-
4064		- while (hdev->sco_cnt && (conn = hci_low_sent(hdev, SCO_LINK, &quote))) {
4065		- while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
4066		- BT_DBG("skb %p len %d", skb, skb->len);
4067		- hci_send_frame(hdev, skb);
4068		-
4069		- conn->sent++;
4070		- if (conn->sent == ~0)
4071		- conn->sent = 0;
4072		- }
4073		- }
4074		-}
4075		-
4076		-static void hci_sched_esco(struct hci_dev *hdev)
4077		-{
4078		- struct hci_conn *conn;
4079		- struct sk_buff *skb;
4080		- int quote;
4081		-
4082		- BT_DBG("%s", hdev->name);
4083		-
4084		- if (!hci_conn_num(hdev, ESCO_LINK))
4085		- return;
4086		-
4087		- while (hdev->sco_cnt && (conn = hci_low_sent(hdev, ESCO_LINK,
4088		- &quote))) {
4089		- while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
4090		- BT_DBG("skb %p len %d", skb, skb->len);
4091		- hci_send_frame(hdev, skb);
4092		-
4093		- conn->sent++;
4094		- if (conn->sent == ~0)
4095		- conn->sent = 0;
4096		- }
4097		- }
4098		-}
4099		-
4100	4674	static void hci_sched_le(struct hci_dev *hdev)
4101	4675	{
4102	4676	struct hci_chan *chan;
..	..	@@ -4108,15 +4682,10 @@
4108	4682	if (!hci_conn_num(hdev, LE_LINK))
4109	4683	return;
4110	4684
4111		- if (!hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) {
4112		- /* LE tx timeout must be longer than maximum
4113		- * link supervision timeout (40.9 seconds) */
4114		- if (!hdev->le_cnt && hdev->le_pkts &&
4115		- time_after(jiffies, hdev->le_last_tx + HZ * 45))
4116		- hci_link_tx_to(hdev, LE_LINK);
4117		- }
4118		-
4119	4685	cnt = hdev->le_pkts ? hdev->le_cnt : hdev->acl_cnt;
	4686	+
	4687	+ __check_timeout(hdev, cnt, LE_LINK);
	4688	+
4120	4689	tmp = cnt;
4121	4690	while (cnt && (chan = hci_chan_sent(hdev, LE_LINK, &quote))) {
4122	4691	u32 priority = (skb_peek(&chan->data_q))->priority;
..	..	@@ -4136,6 +4705,10 @@
4136	4705	cnt--;
4137	4706	chan->sent++;
4138	4707	chan->conn->sent++;
	4708	+
	4709	+ /* Send pending SCO packets right away */
	4710	+ hci_sched_sco(hdev);
	4711	+ hci_sched_esco(hdev);
4139	4712	}
4140	4713	}
4141	4714
..	..	@@ -4158,9 +4731,9 @@
4158	4731
4159	4732	if (!hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) {
4160	4733	/* Schedule queues and send stuff to HCI driver */
4161		- hci_sched_acl(hdev);
4162	4734	hci_sched_sco(hdev);
4163	4735	hci_sched_esco(hdev);
	4736	+ hci_sched_acl(hdev);
4164	4737	hci_sched_le(hdev);
4165	4738	}
4166	4739
..	..	@@ -4212,13 +4785,16 @@
4212	4785	{
4213	4786	struct hci_sco_hdr hdr = (void ) skb->data;
4214	4787	struct hci_conn *conn;
4215		- __u16 handle;
	4788	+ __u16 handle, flags;
4216	4789
4217	4790	skb_pull(skb, HCI_SCO_HDR_SIZE);
4218	4791
4219	4792	handle = __le16_to_cpu(hdr->handle);
	4793	+ flags = hci_flags(handle);
	4794	+ handle = hci_handle(handle);
4220	4795
4221		- BT_DBG("%s len %d handle 0x%4.4x", hdev->name, skb->len, handle);
	4796	+ BT_DBG("%s len %d handle 0x%4.4x flags 0x%4.4x", hdev->name, skb->len,
	4797	+ handle, flags);
4222	4798
4223	4799	hdev->stat.sco_rx++;
4224	4800
..	..	@@ -4228,6 +4804,7 @@
4228	4804
4229	4805	if (conn) {
4230	4806	/* Send to upper protocol */
	4807	+ bt_cb(skb)->sco.pkt_status = flags & 0x03;
4231	4808	sco_recv_scodata(conn, skb);
4232	4809	return;
4233	4810	} else {
..	..	@@ -4331,7 +4908,7 @@
4331	4908	*req_complete_skb = bt_cb(skb)->hci.req_complete_skb;
4332	4909	else
4333	4910	*req_complete = bt_cb(skb)->hci.req_complete;
4334		- kfree_skb(skb);
	4911	+ dev_kfree_skb_irq(skb);
4335	4912	}
4336	4913	spin_unlock_irqrestore(&hdev->cmd_q.lock, flags);
4337	4914	}
..	..	@@ -4369,6 +4946,7 @@
4369	4946	switch (hci_skb_pkt_type(skb)) {
4370	4947	case HCI_ACLDATA_PKT:
4371	4948	case HCI_SCODATA_PKT:
	4949	+ case HCI_ISODATA_PKT:
4372	4950	kfree_skb(skb);
4373	4951	continue;
4374	4952	}