~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
..	..	@@ -1482,6 +1584,8 @@
1482	1584	!hci_dev_test_flag(hdev, HCI_USER_CHANNEL) &&
1483	1585	hci_dev_test_flag(hdev, HCI_VENDOR_DIAG) && hdev->set_diag)
1484	1586	ret = hdev->set_diag(hdev, true);
	1587	+
	1588	+ msft_do_open(hdev);
1485	1589
1486	1590	clear_bit(HCI_INIT, &hdev->flags);
1487	1591
..	..	@@ -1628,14 +1732,6 @@
1628	1732	hci_request_cancel_all(hdev);
1629	1733	hci_req_sync_lock(hdev);
1630	1734
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	1735	if (!test_and_clear_bit(HCI_UP, &hdev->flags)) {
1640	1736	cancel_delayed_work_sync(&hdev->cmd_timer);
1641	1737	hci_req_sync_unlock(hdev);
..	..	@@ -1691,6 +1787,8 @@
1691	1787
1692	1788	hci_sock_dev_event(hdev, HCI_DEV_DOWN);
1693	1789
	1790	+ msft_do_close(hdev);
	1791	+
1694	1792	if (hdev->flush)
1695	1793	hdev->flush(hdev);
1696	1794
..	..	@@ -1721,6 +1819,9 @@
1721	1819
1722	1820	clear_bit(HCI_RUNNING, &hdev->flags);
1723	1821	hci_sock_dev_event(hdev, HCI_DEV_CLOSE);
	1822	+
	1823	+ if (test_and_clear_bit(SUSPEND_POWERING_DOWN, hdev->suspend_tasks))
	1824	+ wake_up(&hdev->suspend_wait_q);
1724	1825
1725	1826	/* After this point our queues are empty
1726	1827	* and no tasks are scheduled. */
..	..	@@ -2243,7 +2344,7 @@
2243	2344	{
2244	2345	struct link_key *key;
2245	2346
2246		- list_for_each_entry_rcu(key, &hdev->link_keys, list) {
	2347	+ list_for_each_entry(key, &hdev->link_keys, list) {
2247	2348	list_del_rcu(&key->list);
2248	2349	kfree_rcu(key, rcu);
2249	2350	}
..	..	@@ -2253,7 +2354,7 @@
2253	2354	{
2254	2355	struct smp_ltk *k;
2255	2356
2256		- list_for_each_entry_rcu(k, &hdev->long_term_keys, list) {
	2357	+ list_for_each_entry(k, &hdev->long_term_keys, list) {
2257	2358	list_del_rcu(&k->list);
2258	2359	kfree_rcu(k, rcu);
2259	2360	}
..	..	@@ -2263,10 +2364,37 @@
2263	2364	{
2264	2365	struct smp_irk *k;
2265	2366
2266		- list_for_each_entry_rcu(k, &hdev->identity_resolving_keys, list) {
	2367	+ list_for_each_entry(k, &hdev->identity_resolving_keys, list) {
2267	2368	list_del_rcu(&k->list);
2268	2369	kfree_rcu(k, rcu);
2269	2370	}
	2371	+}
	2372	+
	2373	+void hci_blocked_keys_clear(struct hci_dev *hdev)
	2374	+{
	2375	+ struct blocked_key *b;
	2376	+
	2377	+ list_for_each_entry(b, &hdev->blocked_keys, list) {
	2378	+ list_del_rcu(&b->list);
	2379	+ kfree_rcu(b, rcu);
	2380	+ }
	2381	+}
	2382	+
	2383	+bool hci_is_blocked_key(struct hci_dev *hdev, u8 type, u8 val[16])
	2384	+{
	2385	+ bool blocked = false;
	2386	+ struct blocked_key *b;
	2387	+
	2388	+ rcu_read_lock();
	2389	+ list_for_each_entry_rcu(b, &hdev->blocked_keys, list) {
	2390	+ if (b->type == type && !memcmp(b->val, val, sizeof(b->val))) {
	2391	+ blocked = true;
	2392	+ break;
	2393	+ }
	2394	+ }
	2395	+
	2396	+ rcu_read_unlock();
	2397	+ return blocked;
2270	2398	}
2271	2399
2272	2400	struct link_key hci_find_link_key(struct hci_dev hdev, bdaddr_t *bdaddr)
..	..	@@ -2277,6 +2405,16 @@
2277	2405	list_for_each_entry_rcu(k, &hdev->link_keys, list) {
2278	2406	if (bacmp(bdaddr, &k->bdaddr) == 0) {
2279	2407	rcu_read_unlock();
	2408	+
	2409	+ if (hci_is_blocked_key(hdev,
	2410	+ HCI_BLOCKED_KEY_TYPE_LINKKEY,
	2411	+ k->val)) {
	2412	+ bt_dev_warn_ratelimited(hdev,
	2413	+ "Link key blocked for %pMR",
	2414	+ &k->bdaddr);
	2415	+ return NULL;
	2416	+ }
	2417	+
2280	2418	return k;
2281	2419	}
2282	2420	}
..	..	@@ -2345,6 +2483,15 @@
2345	2483
2346	2484	if (smp_ltk_is_sc(k) \|\| ltk_role(k->type) == role) {
2347	2485	rcu_read_unlock();
	2486	+
	2487	+ if (hci_is_blocked_key(hdev, HCI_BLOCKED_KEY_TYPE_LTK,
	2488	+ k->val)) {
	2489	+ bt_dev_warn_ratelimited(hdev,
	2490	+ "LTK blocked for %pMR",
	2491	+ &k->bdaddr);
	2492	+ return NULL;
	2493	+ }
	2494	+
2348	2495	return k;
2349	2496	}
2350	2497	}
..	..	@@ -2355,31 +2502,42 @@
2355	2502
2356	2503	struct smp_irk hci_find_irk_by_rpa(struct hci_dev hdev, bdaddr_t *rpa)
2357	2504	{
	2505	+ struct smp_irk *irk_to_return = NULL;
2358	2506	struct smp_irk *irk;
2359	2507
2360	2508	rcu_read_lock();
2361	2509	list_for_each_entry_rcu(irk, &hdev->identity_resolving_keys, list) {
2362	2510	if (!bacmp(&irk->rpa, rpa)) {
2363		- rcu_read_unlock();
2364		- return irk;
	2511	+ irk_to_return = irk;
	2512	+ goto done;
2365	2513	}
2366	2514	}
2367	2515
2368	2516	list_for_each_entry_rcu(irk, &hdev->identity_resolving_keys, list) {
2369	2517	if (smp_irk_matches(hdev, irk->val, rpa)) {
2370	2518	bacpy(&irk->rpa, rpa);
2371		- rcu_read_unlock();
2372		- return irk;
	2519	+ irk_to_return = irk;
	2520	+ goto done;
2373	2521	}
2374	2522	}
	2523	+
	2524	+done:
	2525	+ if (irk_to_return && hci_is_blocked_key(hdev, HCI_BLOCKED_KEY_TYPE_IRK,
	2526	+ irk_to_return->val)) {
	2527	+ bt_dev_warn_ratelimited(hdev, "Identity key blocked for %pMR",
	2528	+ &irk_to_return->bdaddr);
	2529	+ irk_to_return = NULL;
	2530	+ }
	2531	+
2375	2532	rcu_read_unlock();
2376	2533
2377		- return NULL;
	2534	+ return irk_to_return;
2378	2535	}
2379	2536
2380	2537	struct smp_irk hci_find_irk_by_addr(struct hci_dev hdev, bdaddr_t *bdaddr,
2381	2538	u8 addr_type)
2382	2539	{
	2540	+ struct smp_irk *irk_to_return = NULL;
2383	2541	struct smp_irk *irk;
2384	2542
2385	2543	/* Identity Address must be public or static random */
..	..	@@ -2390,13 +2548,23 @@
2390	2548	list_for_each_entry_rcu(irk, &hdev->identity_resolving_keys, list) {
2391	2549	if (addr_type == irk->addr_type &&
2392	2550	bacmp(bdaddr, &irk->bdaddr) == 0) {
2393		- rcu_read_unlock();
2394		- return irk;
	2551	+ irk_to_return = irk;
	2552	+ goto done;
2395	2553	}
2396	2554	}
	2555	+
	2556	+done:
	2557	+
	2558	+ if (irk_to_return && hci_is_blocked_key(hdev, HCI_BLOCKED_KEY_TYPE_IRK,
	2559	+ irk_to_return->val)) {
	2560	+ bt_dev_warn_ratelimited(hdev, "Identity key blocked for %pMR",
	2561	+ &irk_to_return->bdaddr);
	2562	+ irk_to_return = NULL;
	2563	+ }
	2564	+
2397	2565	rcu_read_unlock();
2398	2566
2399		- return NULL;
	2567	+ return irk_to_return;
2400	2568	}
2401	2569
2402	2570	struct link_key hci_add_link_key(struct hci_dev hdev, struct hci_conn *conn,
..	..	@@ -2598,6 +2766,9 @@
2598	2766	} else {
2599	2767	bt_dev_err(hdev, "command tx timeout");
2600	2768	}
	2769	+
	2770	+ if (hdev->cmd_timeout)
	2771	+ hdev->cmd_timeout(hdev);
2601	2772
2602	2773	atomic_set(&hdev->cmd_cnt, 1);
2603	2774	queue_work(hdev->workqueue, &hdev->cmd_work);
..	..	@@ -2804,8 +2975,8 @@
2804	2975	memset(adv_instance->scan_rsp_data, 0,
2805	2976	sizeof(adv_instance->scan_rsp_data));
2806	2977	} else {
2807		- if (hdev->adv_instance_cnt >= HCI_MAX_ADV_INSTANCES \|\|
2808		- instance < 1 \|\| instance > HCI_MAX_ADV_INSTANCES)
	2978	+ if (hdev->adv_instance_cnt >= hdev->le_num_of_adv_sets \|\|
	2979	+ instance < 1 \|\| instance > hdev->le_num_of_adv_sets)
2809	2980	return -EOVERFLOW;
2810	2981
2811	2982	adv_instance = kzalloc(sizeof(*adv_instance), GFP_KERNEL);
..	..	@@ -2833,7 +3004,7 @@
2833	3004	adv_instance->remaining_time = timeout;
2834	3005
2835	3006	if (duration == 0)
2836		- adv_instance->duration = HCI_DEFAULT_ADV_DURATION;
	3007	+ adv_instance->duration = hdev->def_multi_adv_rotation_duration;
2837	3008	else
2838	3009	adv_instance->duration = duration;
2839	3010
..	..	@@ -2847,10 +3018,128 @@
2847	3018	return 0;
2848	3019	}
2849	3020
	3021	+/* This function requires the caller holds hdev->lock */
	3022	+void hci_adv_monitors_clear(struct hci_dev *hdev)
	3023	+{
	3024	+ struct adv_monitor *monitor;
	3025	+ int handle;
	3026	+
	3027	+ idr_for_each_entry(&hdev->adv_monitors_idr, monitor, handle)
	3028	+ hci_free_adv_monitor(monitor);
	3029	+
	3030	+ idr_destroy(&hdev->adv_monitors_idr);
	3031	+}
	3032	+
	3033	+void hci_free_adv_monitor(struct adv_monitor *monitor)
	3034	+{
	3035	+ struct adv_pattern *pattern;
	3036	+ struct adv_pattern *tmp;
	3037	+
	3038	+ if (!monitor)
	3039	+ return;
	3040	+
	3041	+ list_for_each_entry_safe(pattern, tmp, &monitor->patterns, list)
	3042	+ kfree(pattern);
	3043	+
	3044	+ kfree(monitor);
	3045	+}
	3046	+
	3047	+/* This function requires the caller holds hdev->lock */
	3048	+int hci_add_adv_monitor(struct hci_dev hdev, struct adv_monitor monitor)
	3049	+{
	3050	+ int min, max, handle;
	3051	+
	3052	+ if (!monitor)
	3053	+ return -EINVAL;
	3054	+
	3055	+ min = HCI_MIN_ADV_MONITOR_HANDLE;
	3056	+ max = HCI_MIN_ADV_MONITOR_HANDLE + HCI_MAX_ADV_MONITOR_NUM_HANDLES;
	3057	+ handle = idr_alloc(&hdev->adv_monitors_idr, monitor, min, max,
	3058	+ GFP_KERNEL);
	3059	+ if (handle < 0)
	3060	+ return handle;
	3061	+
	3062	+ hdev->adv_monitors_cnt++;
	3063	+ monitor->handle = handle;
	3064	+
	3065	+ hci_update_background_scan(hdev);
	3066	+
	3067	+ return 0;
	3068	+}
	3069	+
	3070	+static int free_adv_monitor(int id, void ptr, void data)
	3071	+{
	3072	+ struct hci_dev *hdev = data;
	3073	+ struct adv_monitor *monitor = ptr;
	3074	+
	3075	+ idr_remove(&hdev->adv_monitors_idr, monitor->handle);
	3076	+ hci_free_adv_monitor(monitor);
	3077	+ hdev->adv_monitors_cnt--;
	3078	+
	3079	+ return 0;
	3080	+}
	3081	+
	3082	+/* This function requires the caller holds hdev->lock */
	3083	+int hci_remove_adv_monitor(struct hci_dev *hdev, u16 handle)
	3084	+{
	3085	+ struct adv_monitor *monitor;
	3086	+
	3087	+ if (handle) {
	3088	+ monitor = idr_find(&hdev->adv_monitors_idr, handle);
	3089	+ if (!monitor)
	3090	+ return -ENOENT;
	3091	+
	3092	+ idr_remove(&hdev->adv_monitors_idr, monitor->handle);
	3093	+ hci_free_adv_monitor(monitor);
	3094	+ hdev->adv_monitors_cnt--;
	3095	+ } else {
	3096	+ /* Remove all monitors if handle is 0. */
	3097	+ idr_for_each(&hdev->adv_monitors_idr, &free_adv_monitor, hdev);
	3098	+ }
	3099	+
	3100	+ hci_update_background_scan(hdev);
	3101	+
	3102	+ return 0;
	3103	+}
	3104	+
	3105	+/* This function requires the caller holds hdev->lock */
	3106	+bool hci_is_adv_monitoring(struct hci_dev *hdev)
	3107	+{
	3108	+ return !idr_is_empty(&hdev->adv_monitors_idr);
	3109	+}
	3110	+
2850	3111	struct bdaddr_list hci_bdaddr_list_lookup(struct list_head bdaddr_list,
2851	3112	bdaddr_t *bdaddr, u8 type)
2852	3113	{
2853	3114	struct bdaddr_list *b;
	3115	+
	3116	+ list_for_each_entry(b, bdaddr_list, list) {
	3117	+ if (!bacmp(&b->bdaddr, bdaddr) && b->bdaddr_type == type)
	3118	+ return b;
	3119	+ }
	3120	+
	3121	+ return NULL;
	3122	+}
	3123	+
	3124	+struct bdaddr_list_with_irk *hci_bdaddr_list_lookup_with_irk(
	3125	+ struct list_head bdaddr_list, bdaddr_t bdaddr,
	3126	+ u8 type)
	3127	+{
	3128	+ struct bdaddr_list_with_irk *b;
	3129	+
	3130	+ list_for_each_entry(b, bdaddr_list, list) {
	3131	+ if (!bacmp(&b->bdaddr, bdaddr) && b->bdaddr_type == type)
	3132	+ return b;
	3133	+ }
	3134	+
	3135	+ return NULL;
	3136	+}
	3137	+
	3138	+struct bdaddr_list_with_flags *
	3139	+hci_bdaddr_list_lookup_with_flags(struct list_head *bdaddr_list,
	3140	+ bdaddr_t *bdaddr, u8 type)
	3141	+{
	3142	+ struct bdaddr_list_with_flags *b;
2854	3143
2855	3144	list_for_each_entry(b, bdaddr_list, list) {
2856	3145	if (!bacmp(&b->bdaddr, bdaddr) && b->bdaddr_type == type)
..	..	@@ -2892,6 +3181,59 @@
2892	3181	return 0;
2893	3182	}
2894	3183
	3184	+int hci_bdaddr_list_add_with_irk(struct list_head list, bdaddr_t bdaddr,
	3185	+ u8 type, u8 peer_irk, u8 local_irk)
	3186	+{
	3187	+ struct bdaddr_list_with_irk *entry;
	3188	+
	3189	+ if (!bacmp(bdaddr, BDADDR_ANY))
	3190	+ return -EBADF;
	3191	+
	3192	+ if (hci_bdaddr_list_lookup(list, bdaddr, type))
	3193	+ return -EEXIST;
	3194	+
	3195	+ entry = kzalloc(sizeof(*entry), GFP_KERNEL);
	3196	+ if (!entry)
	3197	+ return -ENOMEM;
	3198	+
	3199	+ bacpy(&entry->bdaddr, bdaddr);
	3200	+ entry->bdaddr_type = type;
	3201	+
	3202	+ if (peer_irk)
	3203	+ memcpy(entry->peer_irk, peer_irk, 16);
	3204	+
	3205	+ if (local_irk)
	3206	+ memcpy(entry->local_irk, local_irk, 16);
	3207	+
	3208	+ list_add(&entry->list, list);
	3209	+
	3210	+ return 0;
	3211	+}
	3212	+
	3213	+int hci_bdaddr_list_add_with_flags(struct list_head list, bdaddr_t bdaddr,
	3214	+ u8 type, u32 flags)
	3215	+{
	3216	+ struct bdaddr_list_with_flags *entry;
	3217	+
	3218	+ if (!bacmp(bdaddr, BDADDR_ANY))
	3219	+ return -EBADF;
	3220	+
	3221	+ if (hci_bdaddr_list_lookup(list, bdaddr, type))
	3222	+ return -EEXIST;
	3223	+
	3224	+ entry = kzalloc(sizeof(*entry), GFP_KERNEL);
	3225	+ if (!entry)
	3226	+ return -ENOMEM;
	3227	+
	3228	+ bacpy(&entry->bdaddr, bdaddr);
	3229	+ entry->bdaddr_type = type;
	3230	+ entry->current_flags = flags;
	3231	+
	3232	+ list_add(&entry->list, list);
	3233	+
	3234	+ return 0;
	3235	+}
	3236	+
2895	3237	int hci_bdaddr_list_del(struct list_head list, bdaddr_t bdaddr, u8 type)
2896	3238	{
2897	3239	struct bdaddr_list *entry;
..	..	@@ -2902,6 +3244,46 @@
2902	3244	}
2903	3245
2904	3246	entry = hci_bdaddr_list_lookup(list, bdaddr, type);
	3247	+ if (!entry)
	3248	+ return -ENOENT;
	3249	+
	3250	+ list_del(&entry->list);
	3251	+ kfree(entry);
	3252	+
	3253	+ return 0;
	3254	+}
	3255	+
	3256	+int hci_bdaddr_list_del_with_irk(struct list_head list, bdaddr_t bdaddr,
	3257	+ u8 type)
	3258	+{
	3259	+ struct bdaddr_list_with_irk *entry;
	3260	+
	3261	+ if (!bacmp(bdaddr, BDADDR_ANY)) {
	3262	+ hci_bdaddr_list_clear(list);
	3263	+ return 0;
	3264	+ }
	3265	+
	3266	+ entry = hci_bdaddr_list_lookup_with_irk(list, bdaddr, type);
	3267	+ if (!entry)
	3268	+ return -ENOENT;
	3269	+
	3270	+ list_del(&entry->list);
	3271	+ kfree(entry);
	3272	+
	3273	+ return 0;
	3274	+}
	3275	+
	3276	+int hci_bdaddr_list_del_with_flags(struct list_head list, bdaddr_t bdaddr,
	3277	+ u8 type)
	3278	+{
	3279	+ struct bdaddr_list_with_flags *entry;
	3280	+
	3281	+ if (!bacmp(bdaddr, BDADDR_ANY)) {
	3282	+ hci_bdaddr_list_clear(list);
	3283	+ return 0;
	3284	+ }
	3285	+
	3286	+ entry = hci_bdaddr_list_lookup_with_flags(list, bdaddr, type);
2905	3287	if (!entry)
2906	3288	return -ENOENT;
2907	3289
..	..	@@ -2932,6 +3314,15 @@
2932	3314	bdaddr_t *addr, u8 addr_type)
2933	3315	{
2934	3316	struct hci_conn_params *param;
	3317	+
	3318	+ switch (addr_type) {
	3319	+ case ADDR_LE_DEV_PUBLIC_RESOLVED:
	3320	+ addr_type = ADDR_LE_DEV_PUBLIC;
	3321	+ break;
	3322	+ case ADDR_LE_DEV_RANDOM_RESOLVED:
	3323	+ addr_type = ADDR_LE_DEV_RANDOM;
	3324	+ break;
	3325	+ }
2935	3326
2936	3327	list_for_each_entry(param, list, action) {
2937	3328	if (bacmp(&param->addr, addr) == 0 &&
..	..	@@ -3066,6 +3457,134 @@
3066	3457	}
3067	3458	}
3068	3459
	3460	+static void hci_suspend_clear_tasks(struct hci_dev *hdev)
	3461	+{
	3462	+ int i;
	3463	+
	3464	+ for (i = 0; i < __SUSPEND_NUM_TASKS; i++)
	3465	+ clear_bit(i, hdev->suspend_tasks);
	3466	+
	3467	+ wake_up(&hdev->suspend_wait_q);
	3468	+}
	3469	+
	3470	+static int hci_suspend_wait_event(struct hci_dev *hdev)
	3471	+{
	3472	+#define WAKE_COND \
	3473	+ (find_first_bit(hdev->suspend_tasks, __SUSPEND_NUM_TASKS) == \
	3474	+ __SUSPEND_NUM_TASKS)
	3475	+
	3476	+ int i;
	3477	+ int ret = wait_event_timeout(hdev->suspend_wait_q,
	3478	+ WAKE_COND, SUSPEND_NOTIFIER_TIMEOUT);
	3479	+
	3480	+ if (ret == 0) {
	3481	+ bt_dev_err(hdev, "Timed out waiting for suspend events");
	3482	+ for (i = 0; i < __SUSPEND_NUM_TASKS; ++i) {
	3483	+ if (test_bit(i, hdev->suspend_tasks))
	3484	+ bt_dev_err(hdev, "Suspend timeout bit: %d", i);
	3485	+ clear_bit(i, hdev->suspend_tasks);
	3486	+ }
	3487	+
	3488	+ ret = -ETIMEDOUT;
	3489	+ } else {
	3490	+ ret = 0;
	3491	+ }
	3492	+
	3493	+ return ret;
	3494	+}
	3495	+
	3496	+static void hci_prepare_suspend(struct work_struct *work)
	3497	+{
	3498	+ struct hci_dev *hdev =
	3499	+ container_of(work, struct hci_dev, suspend_prepare);
	3500	+
	3501	+ hci_dev_lock(hdev);
	3502	+ hci_req_prepare_suspend(hdev, hdev->suspend_state_next);
	3503	+ hci_dev_unlock(hdev);
	3504	+}
	3505	+
	3506	+static int hci_change_suspend_state(struct hci_dev *hdev,
	3507	+ enum suspended_state next)
	3508	+{
	3509	+ hdev->suspend_state_next = next;
	3510	+ set_bit(SUSPEND_PREPARE_NOTIFIER, hdev->suspend_tasks);
	3511	+ queue_work(hdev->req_workqueue, &hdev->suspend_prepare);
	3512	+ return hci_suspend_wait_event(hdev);
	3513	+}
	3514	+
	3515	+static void hci_clear_wake_reason(struct hci_dev *hdev)
	3516	+{
	3517	+ hci_dev_lock(hdev);
	3518	+
	3519	+ hdev->wake_reason = 0;
	3520	+ bacpy(&hdev->wake_addr, BDADDR_ANY);
	3521	+ hdev->wake_addr_type = 0;
	3522	+
	3523	+ hci_dev_unlock(hdev);
	3524	+}
	3525	+
	3526	+static int hci_suspend_notifier(struct notifier_block *nb, unsigned long action,
	3527	+ void *data)
	3528	+{
	3529	+ struct hci_dev *hdev =
	3530	+ container_of(nb, struct hci_dev, suspend_notifier);
	3531	+ int ret = 0;
	3532	+ u8 state = BT_RUNNING;
	3533	+
	3534	+ /* If powering down, wait for completion. */
	3535	+ if (mgmt_powering_down(hdev)) {
	3536	+ set_bit(SUSPEND_POWERING_DOWN, hdev->suspend_tasks);
	3537	+ ret = hci_suspend_wait_event(hdev);
	3538	+ if (ret)
	3539	+ goto done;
	3540	+ }
	3541	+
	3542	+ /* Suspend notifier should only act on events when powered. */
	3543	+ if (!hdev_is_powered(hdev) \|\|
	3544	+ hci_dev_test_flag(hdev, HCI_UNREGISTER))
	3545	+ goto done;
	3546	+
	3547	+ if (action == PM_SUSPEND_PREPARE) {
	3548	+ /* Suspend consists of two actions:
	3549	+ * - First, disconnect everything and make the controller not
	3550	+ * connectable (disabling scanning)
	3551	+ * - Second, program event filter/whitelist and enable scan
	3552	+ */
	3553	+ ret = hci_change_suspend_state(hdev, BT_SUSPEND_DISCONNECT);
	3554	+ if (!ret)
	3555	+ state = BT_SUSPEND_DISCONNECT;
	3556	+
	3557	+ /* Only configure whitelist if disconnect succeeded and wake
	3558	+ * isn't being prevented.
	3559	+ */
	3560	+ if (!ret && !(hdev->prevent_wake && hdev->prevent_wake(hdev))) {
	3561	+ ret = hci_change_suspend_state(hdev,
	3562	+ BT_SUSPEND_CONFIGURE_WAKE);
	3563	+ if (!ret)
	3564	+ state = BT_SUSPEND_CONFIGURE_WAKE;
	3565	+ }
	3566	+
	3567	+ hci_clear_wake_reason(hdev);
	3568	+ mgmt_suspending(hdev, state);
	3569	+
	3570	+ } else if (action == PM_POST_SUSPEND) {
	3571	+ ret = hci_change_suspend_state(hdev, BT_RUNNING);
	3572	+
	3573	+ mgmt_resuming(hdev, hdev->wake_reason, &hdev->wake_addr,
	3574	+ hdev->wake_addr_type);
	3575	+ }
	3576	+
	3577	+done:
	3578	+ /* We always allow suspend even if suspend preparation failed and
	3579	+ * attempt to recover in resume.
	3580	+ */
	3581	+ if (ret)
	3582	+ bt_dev_err(hdev, "Suspend notifier action (%lu) failed: %d",
	3583	+ action, ret);
	3584	+
	3585	+ return NOTIFY_DONE;
	3586	+}
	3587	+
3069	3588	/* Alloc HCI device */
3070	3589	struct hci_dev *hci_alloc_dev(void)
3071	3590	{
..	..	@@ -3095,6 +3614,12 @@
3095	3614	hdev->le_adv_max_interval = 0x0800;
3096	3615	hdev->le_scan_interval = 0x0060;
3097	3616	hdev->le_scan_window = 0x0030;
	3617	+ hdev->le_scan_int_suspend = 0x0400;
	3618	+ hdev->le_scan_window_suspend = 0x0012;
	3619	+ hdev->le_scan_int_discovery = DISCOV_LE_SCAN_INT;
	3620	+ hdev->le_scan_window_discovery = DISCOV_LE_SCAN_WIN;
	3621	+ hdev->le_scan_int_connect = 0x0060;
	3622	+ hdev->le_scan_window_connect = 0x0060;
3098	3623	hdev->le_conn_min_interval = 0x0018;
3099	3624	hdev->le_conn_max_interval = 0x0028;
3100	3625	hdev->le_conn_latency = 0x0000;
..	..	@@ -3109,11 +3634,21 @@
3109	3634	hdev->le_min_key_size = SMP_MIN_ENC_KEY_SIZE;
3110	3635	hdev->le_tx_def_phys = HCI_LE_SET_PHY_1M;
3111	3636	hdev->le_rx_def_phys = HCI_LE_SET_PHY_1M;
	3637	+ hdev->le_num_of_adv_sets = HCI_MAX_ADV_INSTANCES;
	3638	+ hdev->def_multi_adv_rotation_duration = HCI_DEFAULT_ADV_DURATION;
	3639	+ hdev->def_le_autoconnect_timeout = HCI_LE_AUTOCONN_TIMEOUT;
3112	3640
3113	3641	hdev->rpa_timeout = HCI_DEFAULT_RPA_TIMEOUT;
3114	3642	hdev->discov_interleaved_timeout = DISCOV_INTERLEAVED_TIMEOUT;
3115	3643	hdev->conn_info_min_age = DEFAULT_CONN_INFO_MIN_AGE;
3116	3644	hdev->conn_info_max_age = DEFAULT_CONN_INFO_MAX_AGE;
	3645	+ hdev->auth_payload_timeout = DEFAULT_AUTH_PAYLOAD_TIMEOUT;
	3646	+ hdev->min_enc_key_size = HCI_MIN_ENC_KEY_SIZE;
	3647	+
	3648	+ /* default 1.28 sec page scan */
	3649	+ hdev->def_page_scan_type = PAGE_SCAN_TYPE_STANDARD;
	3650	+ hdev->def_page_scan_int = 0x0800;
	3651	+ hdev->def_page_scan_window = 0x0012;
3117	3652
3118	3653	mutex_init(&hdev->lock);
3119	3654	mutex_init(&hdev->req_lock);
..	..	@@ -3133,12 +3668,14 @@
3133	3668	INIT_LIST_HEAD(&hdev->pend_le_reports);
3134	3669	INIT_LIST_HEAD(&hdev->conn_hash.list);
3135	3670	INIT_LIST_HEAD(&hdev->adv_instances);
	3671	+ INIT_LIST_HEAD(&hdev->blocked_keys);
3136	3672
3137	3673	INIT_WORK(&hdev->rx_work, hci_rx_work);
3138	3674	INIT_WORK(&hdev->cmd_work, hci_cmd_work);
3139	3675	INIT_WORK(&hdev->tx_work, hci_tx_work);
3140	3676	INIT_WORK(&hdev->power_on, hci_power_on);
3141	3677	INIT_WORK(&hdev->error_reset, hci_error_reset);
	3678	+ INIT_WORK(&hdev->suspend_prepare, hci_prepare_suspend);
3142	3679
3143	3680	INIT_DELAYED_WORK(&hdev->power_off, hci_power_off);
3144	3681
..	..	@@ -3147,6 +3684,7 @@
3147	3684	skb_queue_head_init(&hdev->raw_q);
3148	3685
3149	3686	init_waitqueue_head(&hdev->req_wait_q);
	3687	+ init_waitqueue_head(&hdev->suspend_wait_q);
3150	3688
3151	3689	INIT_DELAYED_WORK(&hdev->cmd_timer, hci_cmd_timeout);
3152	3690
..	..	@@ -3180,10 +3718,10 @@
3180	3718	*/
3181	3719	switch (hdev->dev_type) {
3182	3720	case HCI_PRIMARY:
3183		- id = ida_simple_get(&hci_index_ida, 0, 0, GFP_KERNEL);
	3721	+ id = ida_simple_get(&hci_index_ida, 0, HCI_MAX_ID, GFP_KERNEL);
3184	3722	break;
3185	3723	case HCI_AMP:
3186		- id = ida_simple_get(&hci_index_ida, 1, 0, GFP_KERNEL);
	3724	+ id = ida_simple_get(&hci_index_ida, 1, HCI_MAX_ID, GFP_KERNEL);
3187	3725	break;
3188	3726	default:
3189	3727	return -EINVAL;
..	..	@@ -3192,7 +3730,7 @@
3192	3730	if (id < 0)
3193	3731	return id;
3194	3732
3195		- sprintf(hdev->name, "hci%d", id);
	3733	+ snprintf(hdev->name, sizeof(hdev->name), "hci%d", id);
3196	3734	hdev->id = id;
3197	3735
3198	3736	BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus);
..	..	@@ -3258,7 +3796,17 @@
3258	3796	hci_sock_dev_event(hdev, HCI_DEV_REG);
3259	3797	hci_dev_hold(hdev);
3260	3798
	3799	+ if (!hdev->suspend_notifier.notifier_call &&
	3800	+ !test_bit(HCI_QUIRK_NO_SUSPEND_NOTIFIER, &hdev->quirks)) {
	3801	+ hdev->suspend_notifier.notifier_call = hci_suspend_notifier;
	3802	+ error = register_pm_notifier(&hdev->suspend_notifier);
	3803	+ if (error)
	3804	+ goto err_wqueue;
	3805	+ }
	3806	+
3261	3807	queue_work(hdev->req_workqueue, &hdev->power_on);
	3808	+
	3809	+ idr_init(&hdev->adv_monitors_idr);
3262	3810
3263	3811	return id;
3264	3812
..	..	@@ -3285,6 +3833,12 @@
3285	3833	write_unlock(&hci_dev_list_lock);
3286	3834
3287	3835	cancel_work_sync(&hdev->power_on);
	3836	+
	3837	+ if (!test_bit(HCI_QUIRK_NO_SUSPEND_NOTIFIER, &hdev->quirks)) {
	3838	+ hci_suspend_clear_tasks(hdev);
	3839	+ unregister_pm_notifier(&hdev->suspend_notifier);
	3840	+ cancel_work_sync(&hdev->suspend_prepare);
	3841	+ }
3288	3842
3289	3843	hci_dev_do_close(hdev);
3290	3844
..	..	@@ -3332,10 +3886,12 @@
3332	3886	hci_smp_irks_clear(hdev);
3333	3887	hci_remote_oob_data_clear(hdev);
3334	3888	hci_adv_instances_clear(hdev);
	3889	+ hci_adv_monitors_clear(hdev);
3335	3890	hci_bdaddr_list_clear(&hdev->le_white_list);
3336	3891	hci_bdaddr_list_clear(&hdev->le_resolv_list);
3337	3892	hci_conn_params_clear_all(hdev);
3338	3893	hci_discovery_filter_clear(hdev);
	3894	+ hci_blocked_keys_clear(hdev);
3339	3895	hci_dev_unlock(hdev);
3340	3896
3341	3897	ida_simple_remove(&hci_index_ida, hdev->id);
..	..	@@ -3360,7 +3916,7 @@
3360	3916	/* Reset HCI device */
3361	3917	int hci_reset_dev(struct hci_dev *hdev)
3362	3918	{
3363		- const u8 hw_err[] = { HCI_EV_HARDWARE_ERROR, 0x01, 0x00 };
	3919	+ static const u8 hw_err[] = { HCI_EV_HARDWARE_ERROR, 0x01, 0x00 };
3364	3920	struct sk_buff *skb;
3365	3921
3366	3922	skb = bt_skb_alloc(3, GFP_ATOMIC);
..	..	@@ -3386,7 +3942,8 @@
3386	3942
3387	3943	if (hci_skb_pkt_type(skb) != HCI_EVENT_PKT &&
3388	3944	hci_skb_pkt_type(skb) != HCI_ACLDATA_PKT &&
3389		- hci_skb_pkt_type(skb) != HCI_SCODATA_PKT) {
	3945	+ hci_skb_pkt_type(skb) != HCI_SCODATA_PKT &&
	3946	+ hci_skb_pkt_type(skb) != HCI_ISODATA_PKT) {
3390	3947	kfree_skb(skb);
3391	3948	return -EINVAL;
3392	3949	}
..	..	@@ -3923,14 +4480,74 @@
3923	4480	return DIV_ROUND_UP(skb->len - HCI_ACL_HDR_SIZE, hdev->block_len);
3924	4481	}
3925	4482
3926		-static void __check_timeout(struct hci_dev *hdev, unsigned int cnt)
	4483	+static void __check_timeout(struct hci_dev *hdev, unsigned int cnt, u8 type)
3927	4484	{
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);
	4485	+ unsigned long last_tx;
	4486	+
	4487	+ if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED))
	4488	+ return;
	4489	+
	4490	+ switch (type) {
	4491	+ case LE_LINK:
	4492	+ last_tx = hdev->le_last_tx;
	4493	+ break;
	4494	+ default:
	4495	+ last_tx = hdev->acl_last_tx;
	4496	+ break;
	4497	+ }
	4498	+
	4499	+ /* tx timeout must be longer than maximum link supervision timeout
	4500	+ * (40.9 seconds)
	4501	+ */
	4502	+ if (!cnt && time_after(jiffies, last_tx + HCI_ACL_TX_TIMEOUT))
	4503	+ hci_link_tx_to(hdev, type);
	4504	+}
	4505	+
	4506	+/* Schedule SCO */
	4507	+static void hci_sched_sco(struct hci_dev *hdev)
	4508	+{
	4509	+ struct hci_conn *conn;
	4510	+ struct sk_buff *skb;
	4511	+ int quote;
	4512	+
	4513	+ BT_DBG("%s", hdev->name);
	4514	+
	4515	+ if (!hci_conn_num(hdev, SCO_LINK))
	4516	+ return;
	4517	+
	4518	+ while (hdev->sco_cnt && (conn = hci_low_sent(hdev, SCO_LINK, &quote))) {
	4519	+ while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
	4520	+ BT_DBG("skb %p len %d", skb, skb->len);
	4521	+ hci_send_frame(hdev, skb);
	4522	+
	4523	+ conn->sent++;
	4524	+ if (conn->sent == ~0)
	4525	+ conn->sent = 0;
	4526	+ }
	4527	+ }
	4528	+}
	4529	+
	4530	+static void hci_sched_esco(struct hci_dev *hdev)
	4531	+{
	4532	+ struct hci_conn *conn;
	4533	+ struct sk_buff *skb;
	4534	+ int quote;
	4535	+
	4536	+ BT_DBG("%s", hdev->name);
	4537	+
	4538	+ if (!hci_conn_num(hdev, ESCO_LINK))
	4539	+ return;
	4540	+
	4541	+ while (hdev->sco_cnt && (conn = hci_low_sent(hdev, ESCO_LINK,
	4542	+ &quote))) {
	4543	+ while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
	4544	+ BT_DBG("skb %p len %d", skb, skb->len);
	4545	+ hci_send_frame(hdev, skb);
	4546	+
	4547	+ conn->sent++;
	4548	+ if (conn->sent == ~0)
	4549	+ conn->sent = 0;
	4550	+ }
3934	4551	}
3935	4552	}
3936	4553
..	..	@@ -3941,7 +4558,7 @@
3941	4558	struct sk_buff *skb;
3942	4559	int quote;
3943	4560
3944		- __check_timeout(hdev, cnt);
	4561	+ __check_timeout(hdev, cnt, ACL_LINK);
3945	4562
3946	4563	while (hdev->acl_cnt &&
3947	4564	(chan = hci_chan_sent(hdev, ACL_LINK, &quote))) {
..	..	@@ -3965,6 +4582,10 @@
3965	4582	hdev->acl_cnt--;
3966	4583	chan->sent++;
3967	4584	chan->conn->sent++;
	4585	+
	4586	+ /* Send pending SCO packets right away */
	4587	+ hci_sched_sco(hdev);
	4588	+ hci_sched_esco(hdev);
3968	4589	}
3969	4590	}
3970	4591
..	..	@@ -3980,14 +4601,14 @@
3980	4601	int quote;
3981	4602	u8 type;
3982	4603
3983		- __check_timeout(hdev, cnt);
3984		-
3985	4604	BT_DBG("%s", hdev->name);
3986	4605
3987	4606	if (hdev->dev_type == HCI_AMP)
3988	4607	type = AMP_LINK;
3989	4608	else
3990	4609	type = ACL_LINK;
	4610	+
	4611	+ __check_timeout(hdev, cnt, type);
3991	4612
3992	4613	while (hdev->block_cnt > 0 &&
3993	4614	(chan = hci_chan_sent(hdev, type, &quote))) {
..	..	@@ -4049,54 +4670,6 @@
4049	4670	}
4050	4671	}
4051	4672
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	4673	static void hci_sched_le(struct hci_dev *hdev)
4101	4674	{
4102	4675	struct hci_chan *chan;
..	..	@@ -4108,15 +4681,10 @@
4108	4681	if (!hci_conn_num(hdev, LE_LINK))
4109	4682	return;
4110	4683
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	4684	cnt = hdev->le_pkts ? hdev->le_cnt : hdev->acl_cnt;
	4685	+
	4686	+ __check_timeout(hdev, cnt, LE_LINK);
	4687	+
4120	4688	tmp = cnt;
4121	4689	while (cnt && (chan = hci_chan_sent(hdev, LE_LINK, &quote))) {
4122	4690	u32 priority = (skb_peek(&chan->data_q))->priority;
..	..	@@ -4136,6 +4704,10 @@
4136	4704	cnt--;
4137	4705	chan->sent++;
4138	4706	chan->conn->sent++;
	4707	+
	4708	+ /* Send pending SCO packets right away */
	4709	+ hci_sched_sco(hdev);
	4710	+ hci_sched_esco(hdev);
4139	4711	}
4140	4712	}
4141	4713
..	..	@@ -4158,9 +4730,9 @@
4158	4730
4159	4731	if (!hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) {
4160	4732	/* Schedule queues and send stuff to HCI driver */
4161		- hci_sched_acl(hdev);
4162	4733	hci_sched_sco(hdev);
4163	4734	hci_sched_esco(hdev);
	4735	+ hci_sched_acl(hdev);
4164	4736	hci_sched_le(hdev);
4165	4737	}
4166	4738
..	..	@@ -4212,13 +4784,16 @@
4212	4784	{
4213	4785	struct hci_sco_hdr hdr = (void ) skb->data;
4214	4786	struct hci_conn *conn;
4215		- __u16 handle;
	4787	+ __u16 handle, flags;
4216	4788
4217	4789	skb_pull(skb, HCI_SCO_HDR_SIZE);
4218	4790
4219	4791	handle = __le16_to_cpu(hdr->handle);
	4792	+ flags = hci_flags(handle);
	4793	+ handle = hci_handle(handle);
4220	4794
4221		- BT_DBG("%s len %d handle 0x%4.4x", hdev->name, skb->len, handle);
	4795	+ BT_DBG("%s len %d handle 0x%4.4x flags 0x%4.4x", hdev->name, skb->len,
	4796	+ handle, flags);
4222	4797
4223	4798	hdev->stat.sco_rx++;
4224	4799
..	..	@@ -4228,6 +4803,7 @@
4228	4803
4229	4804	if (conn) {
4230	4805	/* Send to upper protocol */
	4806	+ bt_cb(skb)->sco.pkt_status = flags & 0x03;
4231	4807	sco_recv_scodata(conn, skb);
4232	4808	return;
4233	4809	} else {
..	..	@@ -4369,6 +4945,7 @@
4369	4945	switch (hci_skb_pkt_type(skb)) {
4370	4946	case HCI_ACLDATA_PKT:
4371	4947	case HCI_SCODATA_PKT:
	4948	+ case HCI_ISODATA_PKT:
4372	4949	kfree_skb(skb);
4373	4950	continue;
4374	4951	}