bt_ioctl.c 27 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959
  1. /****************************************************************************
  2. * wireless/bluetooth/bt_ioctl.c
  3. * Bluetooth network IOCTL handler
  4. *
  5. * Licensed to the Apache Software Foundation (ASF) under one or more
  6. * contributor license agreements. See the NOTICE file distributed with
  7. * this work for additional information regarding copyright ownership. The
  8. * ASF licenses this file to you under the Apache License, Version 2.0 (the
  9. * "License"); you may not use this file except in compliance with the
  10. * License. You may obtain a copy of the License at
  11. *
  12. * http://www.apache.org/licenses/LICENSE-2.0
  13. *
  14. * Unless required by applicable law or agreed to in writing, software
  15. * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
  16. * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
  17. * License for the specific language governing permissions and limitations
  18. * under the License.
  19. *
  20. ****************************************************************************/
  21. /****************************************************************************
  22. * Included Files
  23. ****************************************************************************/
  24. #include <nuttx/config.h>
  25. #include <assert.h>
  26. #include <errno.h>
  27. #include <debug.h>
  28. #include <nuttx/wqueue.h>
  29. #include <nuttx/semaphore.h>
  30. #include <nuttx/net/netdev.h>
  31. #include <nuttx/net/bluetooth.h>
  32. #include <nuttx/wireless/bluetooth/bt_core.h>
  33. #include <nuttx/wireless/bluetooth/bt_hci.h>
  34. #include <nuttx/wireless/bluetooth/bt_ioctl.h>
  35. #include "bt_hcicore.h"
  36. #include "bt_conn.h"
  37. #include "bt_ioctl.h"
  38. #ifdef CONFIG_NETDEV_IOCTL /* Not optional! */
  39. /****************************************************************************
  40. * Private Types
  41. ****************************************************************************/
  42. /* These structures encapsulate all globals used by the IOCTL logic. */
  43. /* Scan state variables */
  44. struct btnet_scanstate_s
  45. {
  46. sem_t bs_exclsem; /* Manages exclusive access */
  47. volatile bool bs_scanning; /* True: Scanning in progress */
  48. volatile uint8_t bs_head; /* Head of circular list (for removal) */
  49. uint8_t bs_tail; /* Tail of circular list (for addition) */
  50. struct bt_scanresponse_s bs_rsp[CONFIG_BLUETOOTH_MAXSCANRESULT];
  51. };
  52. /* Discovery state variables. NOTE: This function must be cast-compatible
  53. * with struct bt_gatt_discover_params_s.
  54. */
  55. struct btnet_discoverstate_s
  56. {
  57. struct bt_gatt_discover_params_s bd_params;
  58. struct bt_uuid_s bd_uuid; /* Discovery UUID */
  59. sem_t bd_donesem; /* Manages exclusive access */
  60. };
  61. /* GATT read state variables. */
  62. struct btnet_rdstate_s
  63. {
  64. struct btreq_s *rd_btreq;
  65. uint8_t rd_result; /* The result of the read */
  66. sem_t rd_donesem; /* Manages exclusive access */
  67. };
  68. /* GATT write state variables. */
  69. struct btnet_wrstate_s
  70. {
  71. struct btreq_s *wr_btreq;
  72. uint8_t wr_result; /* The result of the read */
  73. sem_t wr_donesem; /* Manages exclusive access */
  74. };
  75. /****************************************************************************
  76. * Private Data
  77. ****************************************************************************/
  78. /* At present only a single Bluetooth device is supported. So we can simply
  79. * maintain the pending scan, discovery, MTU exchange, read and write states
  80. * as globals.
  81. *
  82. * NOTE: This limits to a single Bluetooth device with one concurrent scan
  83. * action, one concurrent MTU exchange, one concurrent discovery action,
  84. * etc.
  85. *
  86. * REVISIT: A fix might be to (1) allocate instances on each IOCTL command
  87. * the starts an operation, keeping the allocated structures in a list. (2)
  88. * Return a reference number with each such command that starts an
  89. * operation. (3) That reference number would then be used in each IOCTL
  90. * command that gets the result of the requested operation. (4) The
  91. * allocated instance would be freed when either: (a) the result has been
  92. * returned or (b) it has expired without being harvested. This implies
  93. * a timer that runs while there are pending operations in order to expire
  94. * the unharvested results.
  95. */
  96. static struct btnet_scanstate_s g_scanstate;
  97. /****************************************************************************
  98. * Private Functions
  99. ****************************************************************************/
  100. /****************************************************************************
  101. * Name: btnet_scan_callback
  102. *
  103. * Description:
  104. * This is an HCI callback function. HCI provides scan result data via
  105. * this callback function. The scan result data will be added to the
  106. * cached scan results.
  107. *
  108. * Input Parameters:
  109. * Scan result data
  110. *
  111. * Returned Value:
  112. * None
  113. *
  114. ****************************************************************************/
  115. static void btnet_scan_callback(FAR const bt_addr_le_t *addr,
  116. int8_t rssi, uint8_t adv_type,
  117. FAR const uint8_t *adv_data, uint8_t len)
  118. {
  119. FAR struct bt_scanresponse_s *rsp;
  120. uint8_t nexttail;
  121. uint8_t head;
  122. uint8_t tail;
  123. int ret;
  124. if (!g_scanstate.bs_scanning)
  125. {
  126. wlerr("ERROR: Results received while not scanning\n");
  127. return;
  128. }
  129. if (len > CONFIG_BLUETOOTH_MAXSCANDATA)
  130. {
  131. wlerr("ERROR: Scan result is too big: %u\n", len);
  132. return;
  133. }
  134. /* Get exclusive access to the scan data */
  135. ret = nxsem_wait_uninterruptible(&g_scanstate.bs_exclsem);
  136. if (ret < 0)
  137. {
  138. wlerr("nxsem_wait_uninterruptible() failed: %d\n", ret);
  139. return;
  140. }
  141. /* Add the scan data to the cache */
  142. tail = g_scanstate.bs_tail;
  143. nexttail = tail + 1;
  144. if (nexttail >= CONFIG_BLUETOOTH_MAXSCANRESULT)
  145. {
  146. nexttail = 0;
  147. }
  148. /* Is the circular buffer full? */
  149. head = g_scanstate.bs_head;
  150. if (nexttail == head)
  151. {
  152. wlerr("ERROR: Too many scan results\n");
  153. if (++head >= CONFIG_BLUETOOTH_MAXSCANRESULT)
  154. {
  155. head = 0;
  156. }
  157. g_scanstate.bs_head = head;
  158. }
  159. /* Save the new scan result */
  160. rsp = &g_scanstate.bs_rsp[tail];
  161. memcpy(&rsp->sr_addr, addr, sizeof(bt_addr_le_t));
  162. rsp->sr_rssi = rssi;
  163. rsp->sr_type = adv_type;
  164. rsp->sr_len = len;
  165. memcpy(&rsp->sr_data, adv_data, len);
  166. g_scanstate.bs_tail = nexttail;
  167. nxsem_post(&g_scanstate.bs_exclsem);
  168. }
  169. /****************************************************************************
  170. * Name: btnet_scan_result
  171. *
  172. * Description:
  173. * This function implements the SIOCBTSCANGET IOCTL command. It returns
  174. * the current, buffered discovered handles.
  175. *
  176. * Input Parameters:
  177. * result - Location to return the scan result data
  178. * maxrsp - The maximum number of responses that can be returned.
  179. *
  180. * Returned Value:
  181. * On success, the actual number of scan results obtain is returned. A
  182. * negated errno value is returned on any failure.
  183. *
  184. ****************************************************************************/
  185. static int btnet_scan_result(FAR struct bt_scanresponse_s *result,
  186. uint8_t maxrsp)
  187. {
  188. uint8_t head;
  189. uint8_t tail;
  190. uint8_t nrsp;
  191. bool scanning;
  192. int ret;
  193. scanning = g_scanstate.bs_scanning;
  194. wlinfo("Scanning? %s\n", scanning ? "YES" : "NO");
  195. /* Get exclusive access to the scan data while we are actively scanning.
  196. * The semaphore is uninitialized in other cases.
  197. */
  198. if (scanning)
  199. {
  200. /* Get exclusive access to the scan data */
  201. ret = nxsem_wait(&g_scanstate.bs_exclsem);
  202. if (ret < 0)
  203. {
  204. return ret;
  205. }
  206. }
  207. /* Copy all available results */
  208. head = g_scanstate.bs_head;
  209. tail = g_scanstate.bs_tail;
  210. for (nrsp = 0; nrsp < maxrsp && head != tail; nrsp++)
  211. {
  212. FAR const uint8_t *src;
  213. FAR uint8_t *dest;
  214. /* Copy data from the head index into the user buffer */
  215. src = (FAR const uint8_t *)&g_scanstate.bs_rsp[head];
  216. dest = (FAR uint8_t *)&result[nrsp];
  217. memcpy(dest, src, sizeof(struct bt_scanresponse_s));
  218. /* Increment the head index */
  219. if (++head >= CONFIG_BLUETOOTH_MAXSCANRESULT)
  220. {
  221. head = 0;
  222. }
  223. }
  224. g_scanstate.bs_head = head;
  225. if (scanning)
  226. {
  227. nxsem_post(&g_scanstate.bs_exclsem);
  228. }
  229. return nrsp;
  230. }
  231. /****************************************************************************
  232. * Name: btnet_exchange_rsp
  233. *
  234. * Description:
  235. * Result of MTU exchange.
  236. *
  237. * Input Parameters:
  238. * conn - The address of the peer in the MTU exchange
  239. * result - The result of the MTU exchange
  240. *
  241. * Returned Value:
  242. * None
  243. *
  244. ****************************************************************************/
  245. static void btnet_exchange_rsp(FAR struct bt_conn_s *conn, uint8_t result)
  246. {
  247. FAR struct btnet_wrstate_s *pstate = conn->p_iostate;
  248. FAR struct btreq_s *btreq = pstate->wr_btreq;
  249. wlinfo("%s\n", result == 0 ? "succeeded" : "failed");
  250. btreq->btr_exresult = result;
  251. nxsem_post(&pstate->wr_donesem);
  252. }
  253. /****************************************************************************
  254. * Name: btnet_discover_func
  255. *
  256. * Description:
  257. * GATT discovery callback. This function is called when a new handle is
  258. * discovered
  259. *
  260. * Input Parameters:
  261. * attr - The discovered attributes
  262. * arg - The original discovery parameters
  263. *
  264. * Returned Value:
  265. * BT_GATT_ITER_CONTINUE meaning to continue the iteration.
  266. *
  267. ****************************************************************************/
  268. static uint8_t btnet_discover_func(FAR const struct bt_gatt_attr_s *attr,
  269. FAR void *arg)
  270. {
  271. FAR struct bt_gatt_discover_params_s *params = arg;
  272. FAR struct btreq_s *btreq = (FAR struct btreq_s *)(params->p_data);
  273. wlinfo("Discovered handle %x\n", attr->handle);
  274. if (btreq->btr_indx >= btreq->btr_gnrsp)
  275. {
  276. wlerr("ERROR: No space for results\n");
  277. return BT_GATT_ITER_STOP;
  278. }
  279. btreq->btr_grsp[btreq->btr_indx].dr_handle = attr->handle;
  280. btreq->btr_grsp[btreq->btr_indx].dr_perm = attr->perm;
  281. btreq->btr_indx++;
  282. return BT_GATT_ITER_CONTINUE;
  283. }
  284. /****************************************************************************
  285. * Name: btnet_discover_destroy
  286. *
  287. * Description:
  288. * GATT destroy callback. This function is called when a discovery
  289. * completes.
  290. *
  291. * Input Parameters:
  292. * arg - The original discovery parameters
  293. *
  294. * Returned Value:
  295. * None
  296. *
  297. ****************************************************************************/
  298. static void btnet_discover_destroy(FAR void *arg)
  299. {
  300. struct btnet_discoverstate_s *pstate = arg;
  301. wlinfo("Discover destroy\n");
  302. nxsem_post(&pstate->bd_donesem);
  303. }
  304. /****************************************************************************
  305. * Name: btnet_read_callback
  306. *
  307. * Description:
  308. * Handle network IOCTL commands directed to this device.
  309. *
  310. * Input Parameters:
  311. * conn - Connection of read
  312. * result - The result status of the read.
  313. * data - The data read
  314. * length - The Number of bytes read
  315. *
  316. * Returned Value:
  317. * None
  318. *
  319. ****************************************************************************/
  320. static void btnet_read_callback(FAR struct bt_conn_s *conn, int result,
  321. FAR const void *data, uint16_t length)
  322. {
  323. FAR struct btnet_rdstate_s *pstate = conn->p_iostate;
  324. FAR struct btreq_s *btreq = pstate->rd_btreq;
  325. wlinfo("Read complete: result %d length %u\n", result, length);
  326. if (length > HCI_GATTRD_DATA)
  327. {
  328. wlerr("ERROR: Unexpected length %u, result = %d\n", length, result);
  329. btreq->btr_rdresult = ENFILE;
  330. }
  331. else
  332. {
  333. DEBUGASSERT((unsigned int)result < UINT8_MAX);
  334. btreq->btr_rdresult = result;
  335. btreq->btr_rdsize = length;
  336. memcpy(btreq->btr_rddata, data, length);
  337. }
  338. nxsem_post(&pstate->rd_donesem);
  339. }
  340. /****************************************************************************
  341. * Name: bnet_write_callback
  342. *
  343. * Description:
  344. * Result of write operation.
  345. *
  346. * Input Parameters:
  347. * conn - The address of the peer in the MTU exchange
  348. * result - The result of the MTU exchange
  349. *
  350. * Returned Value:
  351. * None
  352. *
  353. ****************************************************************************/
  354. static void bnet_write_callback(FAR struct bt_conn_s *conn, uint8_t result)
  355. {
  356. FAR struct btnet_wrstate_s *pstate = conn->p_iostate;
  357. FAR struct btreq_s *btreq = pstate->wr_btreq;
  358. wlinfo("%s\n", result == 0 ? "succeeded" : "failed");
  359. btreq->btr_wrresult = result;
  360. nxsem_post(&pstate->wr_donesem);
  361. }
  362. /****************************************************************************
  363. * Public Functions
  364. ****************************************************************************/
  365. /****************************************************************************
  366. * Name: btnet_ioctl
  367. *
  368. * Description:
  369. * Handle network IOCTL commands directed to this device.
  370. *
  371. * Input Parameters:
  372. * netdev - Reference to the NuttX driver state structure
  373. * cmd - The IOCTL command
  374. * arg - The argument for the IOCTL command
  375. *
  376. * Returned Value:
  377. * OK on success; Negated errno on failure.
  378. *
  379. ****************************************************************************/
  380. int btnet_ioctl(FAR struct net_driver_s *netdev, int cmd, unsigned long arg)
  381. {
  382. FAR struct btreq_s *btreq = (FAR struct btreq_s *)((uintptr_t)arg);
  383. int ret;
  384. wlinfo("cmd=%04x arg=%lu\n", cmd, arg);
  385. DEBUGASSERT(netdev != NULL && netdev->d_private != NULL);
  386. if (btreq == NULL)
  387. {
  388. return -EINVAL;
  389. }
  390. wlinfo("ifname: %s\n", btreq->btr_name);
  391. /* Process the command */
  392. switch (cmd)
  393. {
  394. case SIOCBTCONNECT:
  395. {
  396. FAR struct bt_conn_s *conn;
  397. conn = bt_conn_create_le(&btreq->btr_rmtpeer);
  398. if (!conn)
  399. {
  400. wlerr("Connection failed\n");
  401. ret = -ENOTCONN;
  402. }
  403. else
  404. {
  405. wlinfo("Connection pending\n");
  406. ret = OK;
  407. }
  408. }
  409. break;
  410. case SIOCBTDISCONNECT:
  411. {
  412. FAR struct bt_conn_s *conn;
  413. conn = bt_conn_lookup_addr_le(&btreq->btr_rmtpeer);
  414. if (!conn)
  415. {
  416. wlerr("Peer not connected\n");
  417. ret = -ENOTCONN;
  418. }
  419. else
  420. {
  421. ret = bt_conn_disconnect(conn,
  422. BT_HCI_ERR_REMOTE_USER_TERM_CONN);
  423. if (ret == -ENOTCONN)
  424. {
  425. wlerr("Already disconnected\n");
  426. }
  427. else
  428. {
  429. /* Release reference taken in bt_conn_create_le */
  430. bt_conn_release(conn);
  431. ret = OK;
  432. }
  433. /* Release reference taken in bt_conn_lookup_addr_le */
  434. bt_conn_release(conn);
  435. }
  436. }
  437. break;
  438. /* SIOCGBTINFO: Get Bluetooth device Info. Given the device name,
  439. * fill in the btreq_s structure.
  440. *
  441. * REVISIT: Little more than a stub at present. It does return the
  442. * device address associated with the device name which in itself is
  443. * important.
  444. */
  445. case SIOCGBTINFO:
  446. {
  447. memset(&btreq->btru.btri, 0, sizeof(btreq->btru.btri));
  448. BLUETOOTH_ADDRCOPY(btreq->btr_bdaddr.val, g_btdev.bdaddr.val);
  449. btreq->btr_num_cmd = CONFIG_BLUETOOTH_BUFFER_PREALLOC;
  450. btreq->btr_num_acl = CONFIG_BLUETOOTH_BUFFER_PREALLOC;
  451. btreq->btr_acl_mtu = BLUETOOTH_MAX_MTU;
  452. btreq->btr_sco_mtu = BLUETOOTH_MAX_MTU;
  453. btreq->btr_max_acl = CONFIG_IOB_NBUFFERS;
  454. ret = OK;
  455. }
  456. break;
  457. /* SIOCGBTFEAT
  458. * Get Bluetooth BR/BDR device Features. This returns the cached
  459. * basic (page 0) and extended (page 1 & 2) features. Only page 0
  460. * is valid.
  461. * SIOCGBTLEFEAT
  462. * Get Bluetooth LE device Features. This returns the cached page
  463. * 0-2 features. Only page 0 is value.
  464. */
  465. case SIOCGBTFEAT:
  466. case SIOCGBTLEFEAT:
  467. {
  468. FAR const uint8_t *src;
  469. memset(&btreq->btru.btrf, 0, sizeof(btreq->btru.btrf));
  470. if (cmd == SIOCGBTFEAT)
  471. {
  472. src = g_btdev.features;
  473. }
  474. else
  475. {
  476. src = g_btdev.le_features;
  477. }
  478. memcpy(btreq->btr_features0, src, 8);
  479. ret = OK;
  480. }
  481. break;
  482. /* SIOCBTADVSTART: Set advertisement data, scan response data,
  483. * advertisement parameters and start advertising.
  484. */
  485. case SIOCBTADVSTART:
  486. {
  487. ret = bt_start_advertising(btreq->btr_advtype,
  488. btreq->btr_advad,
  489. btreq->btr_advsd);
  490. wlinfo("Start advertising: %d\n", ret);
  491. }
  492. break;
  493. /* SIOCBTADVSTOP: Stop advertising. */
  494. case SIOCBTADVSTOP:
  495. {
  496. wlinfo("Stop advertising\n");
  497. bt_stop_advertising();
  498. ret = OK;
  499. }
  500. break;
  501. /* SIOCBTSCANSTART: Start LE scanning. Buffered scan results may be
  502. * obtained via SIOCBTSCANGET
  503. */
  504. case SIOCBTSCANSTART:
  505. {
  506. /* Are we already scanning? */
  507. if (g_scanstate.bs_scanning)
  508. {
  509. wlwarn("WARNING: Already scanning\n");
  510. ret = -EBUSY;
  511. }
  512. else
  513. {
  514. /* Initialize scan state */
  515. nxsem_init(&g_scanstate.bs_exclsem, 0, 1);
  516. g_scanstate.bs_scanning = true;
  517. g_scanstate.bs_head = 0;
  518. g_scanstate.bs_tail = 0;
  519. ret = bt_start_scanning(btreq->btr_dupenable,
  520. btnet_scan_callback);
  521. wlinfo("Start scan: %d\n", ret);
  522. if (ret < 0)
  523. {
  524. nxsem_destroy(&g_scanstate.bs_exclsem);
  525. g_scanstate.bs_scanning = false;
  526. }
  527. }
  528. }
  529. break;
  530. /* SIOCBTSCANGET: Return scan results buffered since the call time
  531. * that the SIOCBTSCANGET command was invoked.
  532. */
  533. case SIOCBTSCANGET:
  534. {
  535. ret = btnet_scan_result(btreq->btr_rsp, btreq->btr_nrsp);
  536. wlinfo("Get scan results: %d\n", ret);
  537. if (ret >= 0)
  538. {
  539. btreq->btr_nrsp = ret;
  540. ret = OK;
  541. }
  542. }
  543. break;
  544. /* SIOCBTSCANSTOP:
  545. * Stop LE scanning and discard any buffered results.
  546. */
  547. case SIOCBTSCANSTOP:
  548. {
  549. /* Stop scanning */
  550. ret = bt_stop_scanning();
  551. wlinfo("Stop scanning: %d\n", ret);
  552. nxsem_destroy(&g_scanstate.bs_exclsem);
  553. g_scanstate.bs_scanning = false;
  554. }
  555. break;
  556. /* SIOCBTSECURITY: Enable security for a connection. */
  557. case SIOCBTSECURITY:
  558. {
  559. FAR struct bt_conn_s *conn;
  560. /* Get the connection associated with the provided LE address */
  561. conn = bt_conn_lookup_addr_le(&btreq->btr_secaddr);
  562. if (conn == NULL)
  563. {
  564. wlwarn("WARNING: Peer not connected\n");
  565. ret = -ENOTCONN;
  566. }
  567. else
  568. {
  569. ret = bt_conn_security(conn, btreq->btr_seclevel);
  570. if (ret < 0)
  571. {
  572. wlerr("ERROR: Security setting failed: %d\n", ret);
  573. }
  574. bt_conn_release(conn);
  575. }
  576. }
  577. break;
  578. /* SIOCBTEXCHANGE: Exchange MTUs */
  579. case SIOCBTEXCHANGE:
  580. {
  581. FAR struct bt_conn_s *conn;
  582. /* Get the connection associated with the provided LE address */
  583. conn = bt_conn_lookup_addr_le(&btreq->btr_expeer);
  584. if (conn == NULL)
  585. {
  586. wlwarn("WARNING: Peer not connected\n");
  587. ret = -ENOTCONN;
  588. }
  589. else
  590. {
  591. struct btnet_wrstate_s wrstate;
  592. memset(&wrstate, 0, sizeof(wrstate));
  593. wrstate.wr_btreq = btreq;
  594. conn->p_iostate = &wrstate;
  595. nxsem_init(&wrstate.wr_donesem, 0, 0);
  596. btreq->btr_wrresult = EBUSY;
  597. ret = bt_gatt_exchange_mtu(conn, btnet_exchange_rsp);
  598. if (ret < 0)
  599. {
  600. wlerr("ERROR: Exchange operation failed: %d\n", ret);
  601. }
  602. else
  603. {
  604. /* Wait for callback to complete the exchange */
  605. ret = nxsem_wait(&wrstate.wr_donesem);
  606. }
  607. nxsem_destroy(&wrstate.wr_donesem);
  608. bt_conn_release(conn);
  609. }
  610. }
  611. break;
  612. /* SIOCBTDISCOVER: Starts GATT discovery */
  613. case SIOCBTDISCOVER:
  614. {
  615. FAR struct bt_conn_s *conn;
  616. /* Get the connection associated with the provided LE address */
  617. conn = bt_conn_lookup_addr_le(&btreq->btr_dpeer);
  618. if (conn == NULL)
  619. {
  620. wlwarn("WARNING: Peer not connected\n");
  621. ret = -ENOTCONN;
  622. }
  623. else
  624. {
  625. struct btnet_discoverstate_s dstate;
  626. FAR struct bt_gatt_discover_params_s *params;
  627. memset(&dstate, 0, sizeof(dstate));
  628. /* Set up the query */
  629. dstate.bd_uuid.type = BT_UUID_16;
  630. dstate.bd_uuid.u.u16 = btreq->btr_duuid16;
  631. params = &dstate.bd_params;
  632. params->func = btnet_discover_func;
  633. params->destroy = btnet_discover_destroy;
  634. params->start_handle = btreq->btr_dstart;
  635. params->end_handle = btreq->btr_dend;
  636. params->p_data = (void *)arg;
  637. btreq->btr_indx = 0;
  638. if (btreq->btr_duuid16 == 0)
  639. {
  640. params->uuid = NULL;
  641. }
  642. else
  643. {
  644. params->uuid = &dstate.bd_uuid;
  645. }
  646. nxsem_init(&dstate.bd_donesem, 0, 0);
  647. /* Start the query */
  648. switch (btreq->btr_dtype)
  649. {
  650. case GATT_DISCOVER:
  651. ret = bt_gatt_discover(conn, params);
  652. break;
  653. case GATT_DISCOVER_DESC:
  654. ret = bt_gatt_discover_descriptor(conn, params);
  655. break;
  656. case GATT_DISCOVER_CHAR:
  657. ret = bt_gatt_discover_characteristic(conn, params);
  658. break;
  659. default:
  660. wlerr("ERROR: Unrecognized GATT discover type: %u\n",
  661. btreq->btr_dtype);
  662. ret = -EINVAL;
  663. }
  664. if (ret < 0)
  665. {
  666. wlerr("ERROR: Failed to start discovery: %d\n", ret);
  667. }
  668. else
  669. {
  670. ret = nxsem_wait(&dstate.bd_donesem);
  671. if (ret == 0)
  672. {
  673. btreq->btr_gnrsp = btreq->btr_indx;
  674. }
  675. }
  676. nxsem_destroy(&dstate.bd_donesem);
  677. bt_conn_release(conn);
  678. }
  679. }
  680. break;
  681. /* SIOCBTGATTRD: Read GATT data */
  682. case SIOCBTGATTRD:
  683. {
  684. FAR struct bt_conn_s *conn;
  685. /* Get the connection associated with the provided LE address */
  686. conn = bt_conn_lookup_addr_le(&btreq->btr_rdpeer);
  687. if (conn == NULL)
  688. {
  689. wlwarn("WARNING: Peer not connected\n");
  690. ret = -ENOTCONN;
  691. }
  692. else
  693. {
  694. /* Set up for the read */
  695. struct btnet_rdstate_s rdstate;
  696. memset(&rdstate, 0, sizeof(rdstate));
  697. rdstate.rd_btreq = btreq;
  698. conn->p_iostate = &rdstate;
  699. nxsem_init(&rdstate.rd_donesem, 0, 0);
  700. /* Initiate read.. single or multiple? */
  701. if (btreq->btr_rdnhandles == 1)
  702. {
  703. /* Read single */
  704. DEBUGASSERT(conn->p_iostate != NULL);
  705. ret = bt_gatt_read(conn, btreq->btr_rdhandles[0],
  706. btreq->btr_rdoffset,
  707. btnet_read_callback);
  708. }
  709. else if (btreq->btr_rdnhandles < HCI_GATT_MAXHANDLES)
  710. {
  711. /* Read multiple */
  712. DEBUGASSERT(conn->p_iostate != NULL);
  713. DEBUGASSERT(btreq->btr_rdnhandles > 0);
  714. ret = bt_gatt_read_multiple(conn, btreq->btr_rdhandles,
  715. btreq->btr_rdnhandles,
  716. btnet_read_callback);
  717. }
  718. else
  719. {
  720. ret = -ENFILE;
  721. }
  722. if (ret < 0)
  723. {
  724. wlerr("ERROR: Read operation failed: %d\n", ret);
  725. }
  726. else
  727. {
  728. /* Wait for callback to complete the transfer */
  729. ret = nxsem_wait(&rdstate.rd_donesem);
  730. }
  731. nxsem_destroy(&rdstate.rd_donesem);
  732. bt_conn_release(conn);
  733. }
  734. }
  735. break;
  736. /* SIOCBTGATTWR: Write GATT data */
  737. case SIOCBTGATTWR:
  738. {
  739. DEBUGASSERT(btreq->btr_wrdata != NULL);
  740. FAR struct bt_conn_s *conn;
  741. /* Get the connection associated with the provided LE address */
  742. conn = bt_conn_lookup_addr_le(&btreq->btr_wrpeer);
  743. if (conn == NULL)
  744. {
  745. wlwarn("WARNING: Peer not connected\n");
  746. ret = -ENOTCONN;
  747. }
  748. else
  749. {
  750. /* Set up for the write */
  751. struct btnet_wrstate_s wrstate;
  752. memset(&wrstate, 0, sizeof(wrstate));
  753. wrstate.wr_btreq = btreq;
  754. conn->p_iostate = &wrstate;
  755. nxsem_init(&wrstate.wr_donesem, 0, 0);
  756. btreq->btr_wrresult = EBUSY;
  757. /* Initiate write */
  758. ret = bt_gatt_write(conn, btreq->btr_wrhandle,
  759. btreq->btr_wrdata,
  760. btreq->btr_wrnbytes,
  761. bnet_write_callback);
  762. if (ret < 0)
  763. {
  764. wlerr("ERROR: Write operation failed: %d\n", ret);
  765. }
  766. else
  767. {
  768. /* Wait for callback to complete the transfer */
  769. ret = nxsem_wait(&wrstate.wr_donesem);
  770. }
  771. nxsem_destroy(&wrstate.wr_donesem);
  772. bt_conn_release(conn);
  773. }
  774. }
  775. break;
  776. default:
  777. wlwarn("WARNING: Unrecognized IOCTL command: %02x\n", cmd);
  778. ret = -ENOTTY;
  779. break;
  780. }
  781. return ret;
  782. }
  783. #endif /* CONFIG_NETDEV_IOCTL */