@@ -1205,9 +1205,126 @@ static int hci_set_adv_set_random_addr_sync(struct hci_dev *hdev, u8 instance,
12051205 sizeof (cp ), & cp , HCI_CMD_TIMEOUT );
12061206}
12071207
1208+ static int
1209+ hci_set_ext_adv_params_sync (struct hci_dev * hdev , struct adv_info * adv ,
1210+ const struct hci_cp_le_set_ext_adv_params * cp ,
1211+ struct hci_rp_le_set_ext_adv_params * rp )
1212+ {
1213+ struct sk_buff * skb ;
1214+
1215+ skb = __hci_cmd_sync (hdev , HCI_OP_LE_SET_EXT_ADV_PARAMS , sizeof (* cp ),
1216+ cp , HCI_CMD_TIMEOUT );
1217+
1218+ /* If command return a status event, skb will be set to -ENODATA */
1219+ if (skb == ERR_PTR (- ENODATA ))
1220+ return 0 ;
1221+
1222+ if (IS_ERR (skb )) {
1223+ bt_dev_err (hdev , "Opcode 0x%4.4x failed: %ld" ,
1224+ HCI_OP_LE_SET_EXT_ADV_PARAMS , PTR_ERR (skb ));
1225+ return PTR_ERR (skb );
1226+ }
1227+
1228+ if (skb -> len != sizeof (* rp )) {
1229+ bt_dev_err (hdev , "Invalid response length for 0x%4.4x: %u" ,
1230+ HCI_OP_LE_SET_EXT_ADV_PARAMS , skb -> len );
1231+ kfree_skb (skb );
1232+ return - EIO ;
1233+ }
1234+
1235+ memcpy (rp , skb -> data , sizeof (* rp ));
1236+ kfree_skb (skb );
1237+
1238+ if (!rp -> status ) {
1239+ hdev -> adv_addr_type = cp -> own_addr_type ;
1240+ if (!cp -> handle ) {
1241+ /* Store in hdev for instance 0 */
1242+ hdev -> adv_tx_power = rp -> tx_power ;
1243+ } else if (adv ) {
1244+ adv -> tx_power = rp -> tx_power ;
1245+ }
1246+ }
1247+
1248+ return rp -> status ;
1249+ }
1250+
1251+ static int hci_set_ext_adv_data_sync (struct hci_dev * hdev , u8 instance )
1252+ {
1253+ DEFINE_FLEX (struct hci_cp_le_set_ext_adv_data , pdu , data , length ,
1254+ HCI_MAX_EXT_AD_LENGTH );
1255+ u8 len ;
1256+ struct adv_info * adv = NULL ;
1257+ int err ;
1258+
1259+ if (instance ) {
1260+ adv = hci_find_adv_instance (hdev , instance );
1261+ if (!adv || !adv -> adv_data_changed )
1262+ return 0 ;
1263+ }
1264+
1265+ len = eir_create_adv_data (hdev , instance , pdu -> data ,
1266+ HCI_MAX_EXT_AD_LENGTH );
1267+
1268+ pdu -> length = len ;
1269+ pdu -> handle = adv ? adv -> handle : instance ;
1270+ pdu -> operation = LE_SET_ADV_DATA_OP_COMPLETE ;
1271+ pdu -> frag_pref = LE_SET_ADV_DATA_NO_FRAG ;
1272+
1273+ err = __hci_cmd_sync_status (hdev , HCI_OP_LE_SET_EXT_ADV_DATA ,
1274+ struct_size (pdu , data , len ), pdu ,
1275+ HCI_CMD_TIMEOUT );
1276+ if (err )
1277+ return err ;
1278+
1279+ /* Update data if the command succeed */
1280+ if (adv ) {
1281+ adv -> adv_data_changed = false;
1282+ } else {
1283+ memcpy (hdev -> adv_data , pdu -> data , len );
1284+ hdev -> adv_data_len = len ;
1285+ }
1286+
1287+ return 0 ;
1288+ }
1289+
1290+ static int hci_set_adv_data_sync (struct hci_dev * hdev , u8 instance )
1291+ {
1292+ struct hci_cp_le_set_adv_data cp ;
1293+ u8 len ;
1294+
1295+ memset (& cp , 0 , sizeof (cp ));
1296+
1297+ len = eir_create_adv_data (hdev , instance , cp .data , sizeof (cp .data ));
1298+
1299+ /* There's nothing to do if the data hasn't changed */
1300+ if (hdev -> adv_data_len == len &&
1301+ memcmp (cp .data , hdev -> adv_data , len ) == 0 )
1302+ return 0 ;
1303+
1304+ memcpy (hdev -> adv_data , cp .data , sizeof (cp .data ));
1305+ hdev -> adv_data_len = len ;
1306+
1307+ cp .length = len ;
1308+
1309+ return __hci_cmd_sync_status (hdev , HCI_OP_LE_SET_ADV_DATA ,
1310+ sizeof (cp ), & cp , HCI_CMD_TIMEOUT );
1311+ }
1312+
1313+ int hci_update_adv_data_sync (struct hci_dev * hdev , u8 instance )
1314+ {
1315+ if (!hci_dev_test_flag (hdev , HCI_LE_ENABLED ))
1316+ return 0 ;
1317+
1318+ if (ext_adv_capable (hdev ))
1319+ return hci_set_ext_adv_data_sync (hdev , instance );
1320+
1321+ return hci_set_adv_data_sync (hdev , instance );
1322+ }
1323+
12081324int hci_setup_ext_adv_instance_sync (struct hci_dev * hdev , u8 instance )
12091325{
12101326 struct hci_cp_le_set_ext_adv_params cp ;
1327+ struct hci_rp_le_set_ext_adv_params rp ;
12111328 bool connectable ;
12121329 u32 flags ;
12131330 bdaddr_t random_addr ;
@@ -1316,8 +1433,12 @@ int hci_setup_ext_adv_instance_sync(struct hci_dev *hdev, u8 instance)
13161433 cp .secondary_phy = HCI_ADV_PHY_1M ;
13171434 }
13181435
1319- err = __hci_cmd_sync_status (hdev , HCI_OP_LE_SET_EXT_ADV_PARAMS ,
1320- sizeof (cp ), & cp , HCI_CMD_TIMEOUT );
1436+ err = hci_set_ext_adv_params_sync (hdev , adv , & cp , & rp );
1437+ if (err )
1438+ return err ;
1439+
1440+ /* Update adv data as tx power is known now */
1441+ err = hci_set_ext_adv_data_sync (hdev , cp .handle );
13211442 if (err )
13221443 return err ;
13231444
@@ -1822,79 +1943,6 @@ int hci_le_terminate_big_sync(struct hci_dev *hdev, u8 handle, u8 reason)
18221943 sizeof (cp ), & cp , HCI_CMD_TIMEOUT );
18231944}
18241945
1825- static int hci_set_ext_adv_data_sync (struct hci_dev * hdev , u8 instance )
1826- {
1827- DEFINE_FLEX (struct hci_cp_le_set_ext_adv_data , pdu , data , length ,
1828- HCI_MAX_EXT_AD_LENGTH );
1829- u8 len ;
1830- struct adv_info * adv = NULL ;
1831- int err ;
1832-
1833- if (instance ) {
1834- adv = hci_find_adv_instance (hdev , instance );
1835- if (!adv || !adv -> adv_data_changed )
1836- return 0 ;
1837- }
1838-
1839- len = eir_create_adv_data (hdev , instance , pdu -> data ,
1840- HCI_MAX_EXT_AD_LENGTH );
1841-
1842- pdu -> length = len ;
1843- pdu -> handle = adv ? adv -> handle : instance ;
1844- pdu -> operation = LE_SET_ADV_DATA_OP_COMPLETE ;
1845- pdu -> frag_pref = LE_SET_ADV_DATA_NO_FRAG ;
1846-
1847- err = __hci_cmd_sync_status (hdev , HCI_OP_LE_SET_EXT_ADV_DATA ,
1848- struct_size (pdu , data , len ), pdu ,
1849- HCI_CMD_TIMEOUT );
1850- if (err )
1851- return err ;
1852-
1853- /* Update data if the command succeed */
1854- if (adv ) {
1855- adv -> adv_data_changed = false;
1856- } else {
1857- memcpy (hdev -> adv_data , pdu -> data , len );
1858- hdev -> adv_data_len = len ;
1859- }
1860-
1861- return 0 ;
1862- }
1863-
1864- static int hci_set_adv_data_sync (struct hci_dev * hdev , u8 instance )
1865- {
1866- struct hci_cp_le_set_adv_data cp ;
1867- u8 len ;
1868-
1869- memset (& cp , 0 , sizeof (cp ));
1870-
1871- len = eir_create_adv_data (hdev , instance , cp .data , sizeof (cp .data ));
1872-
1873- /* There's nothing to do if the data hasn't changed */
1874- if (hdev -> adv_data_len == len &&
1875- memcmp (cp .data , hdev -> adv_data , len ) == 0 )
1876- return 0 ;
1877-
1878- memcpy (hdev -> adv_data , cp .data , sizeof (cp .data ));
1879- hdev -> adv_data_len = len ;
1880-
1881- cp .length = len ;
1882-
1883- return __hci_cmd_sync_status (hdev , HCI_OP_LE_SET_ADV_DATA ,
1884- sizeof (cp ), & cp , HCI_CMD_TIMEOUT );
1885- }
1886-
1887- int hci_update_adv_data_sync (struct hci_dev * hdev , u8 instance )
1888- {
1889- if (!hci_dev_test_flag (hdev , HCI_LE_ENABLED ))
1890- return 0 ;
1891-
1892- if (ext_adv_capable (hdev ))
1893- return hci_set_ext_adv_data_sync (hdev , instance );
1894-
1895- return hci_set_adv_data_sync (hdev , instance );
1896- }
1897-
18981946int hci_schedule_adv_instance_sync (struct hci_dev * hdev , u8 instance ,
18991947 bool force )
19001948{
@@ -1970,13 +2018,10 @@ static int hci_clear_adv_sets_sync(struct hci_dev *hdev, struct sock *sk)
19702018static int hci_clear_adv_sync (struct hci_dev * hdev , struct sock * sk , bool force )
19712019{
19722020 struct adv_info * adv , * n ;
1973- int err = 0 ;
19742021
19752022 if (ext_adv_capable (hdev ))
19762023 /* Remove all existing sets */
1977- err = hci_clear_adv_sets_sync (hdev , sk );
1978- if (ext_adv_capable (hdev ))
1979- return err ;
2024+ return hci_clear_adv_sets_sync (hdev , sk );
19802025
19812026 /* This is safe as long as there is no command send while the lock is
19822027 * held.
@@ -2004,13 +2049,11 @@ static int hci_clear_adv_sync(struct hci_dev *hdev, struct sock *sk, bool force)
20042049static int hci_remove_adv_sync (struct hci_dev * hdev , u8 instance ,
20052050 struct sock * sk )
20062051{
2007- int err = 0 ;
2052+ int err ;
20082053
20092054 /* If we use extended advertising, instance has to be removed first. */
20102055 if (ext_adv_capable (hdev ))
2011- err = hci_remove_ext_adv_instance_sync (hdev , instance , sk );
2012- if (ext_adv_capable (hdev ))
2013- return err ;
2056+ return hci_remove_ext_adv_instance_sync (hdev , instance , sk );
20142057
20152058 /* This is safe as long as there is no command send while the lock is
20162059 * held.
@@ -2109,16 +2152,13 @@ int hci_read_tx_power_sync(struct hci_dev *hdev, __le16 handle, u8 type)
21092152int hci_disable_advertising_sync (struct hci_dev * hdev )
21102153{
21112154 u8 enable = 0x00 ;
2112- int err = 0 ;
21132155
21142156 /* If controller is not advertising we are done. */
21152157 if (!hci_dev_test_flag (hdev , HCI_LE_ADV ))
21162158 return 0 ;
21172159
21182160 if (ext_adv_capable (hdev ))
2119- err = hci_disable_ext_adv_instance_sync (hdev , 0x00 );
2120- if (ext_adv_capable (hdev ))
2121- return err ;
2161+ return hci_disable_ext_adv_instance_sync (hdev , 0x00 );
21222162
21232163 return __hci_cmd_sync_status (hdev , HCI_OP_LE_SET_ADV_ENABLE ,
21242164 sizeof (enable ), & enable , HCI_CMD_TIMEOUT );
@@ -2481,6 +2521,10 @@ static int hci_pause_advertising_sync(struct hci_dev *hdev)
24812521 int err ;
24822522 int old_state ;
24832523
2524+ /* If controller is not advertising we are done. */
2525+ if (!hci_dev_test_flag (hdev , HCI_LE_ADV ))
2526+ return 0 ;
2527+
24842528 /* If already been paused there is nothing to do. */
24852529 if (hdev -> advertising_paused )
24862530 return 0 ;
@@ -6277,6 +6321,7 @@ static int hci_le_ext_directed_advertising_sync(struct hci_dev *hdev,
62776321 struct hci_conn * conn )
62786322{
62796323 struct hci_cp_le_set_ext_adv_params cp ;
6324+ struct hci_rp_le_set_ext_adv_params rp ;
62806325 int err ;
62816326 bdaddr_t random_addr ;
62826327 u8 own_addr_type ;
@@ -6318,8 +6363,12 @@ static int hci_le_ext_directed_advertising_sync(struct hci_dev *hdev,
63186363 if (err )
63196364 return err ;
63206365
6321- err = __hci_cmd_sync_status (hdev , HCI_OP_LE_SET_EXT_ADV_PARAMS ,
6322- sizeof (cp ), & cp , HCI_CMD_TIMEOUT );
6366+ err = hci_set_ext_adv_params_sync (hdev , NULL , & cp , & rp );
6367+ if (err )
6368+ return err ;
6369+
6370+ /* Update adv data as tx power is known now */
6371+ err = hci_set_ext_adv_data_sync (hdev , cp .handle );
63236372 if (err )
63246373 return err ;
63256374
0 commit comments