nrf_ble_gq.c 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611
  1. /**
  2. * Copyright (c) 2018 - 2020, Nordic Semiconductor ASA
  3. *
  4. * All rights reserved.
  5. *
  6. * Redistribution and use in source and binary forms, with or without modification,
  7. * are permitted provided that the following conditions are met:
  8. *
  9. * 1. Redistributions of source code must retain the above copyright notice, this
  10. * list of conditions and the following disclaimer.
  11. *
  12. * 2. Redistributions in binary form, except as embedded into a Nordic
  13. * Semiconductor ASA integrated circuit in a product or a software update for
  14. * such product, must reproduce the above copyright notice, this list of
  15. * conditions and the following disclaimer in the documentation and/or other
  16. * materials provided with the distribution.
  17. *
  18. * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
  19. * contributors may be used to endorse or promote products derived from this
  20. * software without specific prior written permission.
  21. *
  22. * 4. This software, with or without modification, must only be used with a
  23. * Nordic Semiconductor ASA integrated circuit.
  24. *
  25. * 5. Any software provided in binary form under this license must not be reverse
  26. * engineered, decompiled, modified and/or disassembled.
  27. *
  28. * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
  29. * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  30. * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
  31. * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
  32. * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  33. * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
  34. * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  35. * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  36. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
  37. * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  38. *
  39. */
  40. #include "sdk_common.h"
  41. #if NRF_MODULE_ENABLED(NRF_BLE_GQ)
  42. #include "nrf_ble_gq.h"
  43. #define NRF_LOG_MODULE_NAME nrf_ble_gq
  44. #include "nrf_log.h"
  45. NRF_LOG_MODULE_REGISTER();
  46. /**@brief Pointer used to describe memory allocator for GATT request. */
  47. typedef ret_code_t (* req_data_alloc_t) (nrf_memobj_pool_t const * p_data_pool,
  48. nrf_ble_gq_req_t * const p_req);
  49. /**@brief Function allocates memory for data associated with @ref NRF_BLE_GQ_REQ_GATTC_WRITE
  50. * request.
  51. *
  52. * @param[in] p_data_pool Pointer to general memory pool.
  53. * @param[in] p_req Pointer to GATTC write request.
  54. *
  55. * @retval NRF_SUCCESS If the write data was allocated successfully.
  56. * @retval NRF_ERROR_INVALID_LENGTH If data to be written is too long.
  57. * @retval NRF_ERROR_NO_MEM If there was no room either in the data pool for new allocation.
  58. */
  59. static ret_code_t gattc_write_alloc(nrf_memobj_pool_t const * p_data_pool,
  60. nrf_ble_gq_req_t * const p_req)
  61. {
  62. nrf_ble_gq_gattc_write_t * p_gattc_write = &p_req->params.gattc_write;
  63. // Check if the payload data is not too long.
  64. if (p_gattc_write->len > NRF_BLE_GQ_GATTC_WRITE_MAX_DATA_LEN)
  65. {
  66. return NRF_ERROR_INVALID_LENGTH;
  67. }
  68. // Allocate memory for GATTC write request.
  69. p_req->p_mem_obj = nrf_memobj_alloc(p_data_pool,
  70. p_gattc_write->len);
  71. if (p_req->p_mem_obj == NULL)
  72. {
  73. return NRF_ERROR_NO_MEM;
  74. }
  75. // Copy relevant data to the pool.
  76. nrf_memobj_write(p_req->p_mem_obj, (void *) p_gattc_write->p_value, p_gattc_write->len, 0);
  77. NRF_LOG_DEBUG("Pointer to allocated memory block: %p.", p_req->p_mem_obj);
  78. return NRF_SUCCESS;
  79. }
  80. /**@brief Function allocates memory for data associated with @ref NRF_BLE_GQ_REQ_GATTS_HVX
  81. * request.
  82. *
  83. * @param[in] p_data_pool Pointer to general memory pool.
  84. * @param[in] p_req Pointer to GATTS hvx request.
  85. *
  86. * @retval NRF_SUCCESS If the notification or indication data was allocated successfully.
  87. * @retval NRF_ERROR_INVALID_LENGTH If data to be written is too long.
  88. * @retval NRF_ERROR_NO_MEM If there was no room either in the data pool for new allocation.
  89. */
  90. static ret_code_t gatts_hvx_alloc(nrf_memobj_pool_t const * p_data_pool,
  91. nrf_ble_gq_req_t * const p_req)
  92. {
  93. nrf_ble_gq_gatts_hvx_t * p_gatts_hvx = &p_req->params.gatts_hvx;
  94. // Check if the payload data is not too long.
  95. if (*p_gatts_hvx->p_len > NRF_BLE_GQ_GATTS_HVX_MAX_DATA_LEN)
  96. {
  97. return NRF_ERROR_INVALID_LENGTH;
  98. }
  99. // Allocate memory for GATTS notification or indication request.
  100. p_req->p_mem_obj = nrf_memobj_alloc(p_data_pool,
  101. *p_gatts_hvx->p_len + sizeof(uint16_t));
  102. if (p_req->p_mem_obj == NULL)
  103. {
  104. return NRF_ERROR_NO_MEM;
  105. }
  106. // Copy relevant data to the pool.
  107. nrf_memobj_write(p_req->p_mem_obj, (void *)p_gatts_hvx->p_len, sizeof(uint16_t), 0);
  108. nrf_memobj_write(p_req->p_mem_obj,
  109. (void *)p_gatts_hvx->p_data,
  110. *p_gatts_hvx->p_len,
  111. sizeof(uint16_t));
  112. NRF_LOG_DEBUG("Pointer to allocated memory block: %p.", p_req->p_mem_obj);
  113. return NRF_SUCCESS;
  114. }
  115. /**@brief Array of memory allocators for different types of @ref nrf_ble_gq_req_t. */
  116. static const req_data_alloc_t m_req_data_alloc[NRF_BLE_GQ_REQ_NUM] =
  117. {
  118. [NRF_BLE_GQ_REQ_GATTC_READ] = NULL,
  119. [NRF_BLE_GQ_REQ_GATTC_WRITE] = gattc_write_alloc,
  120. [NRF_BLE_GQ_REQ_SRV_DISCOVERY] = NULL,
  121. [NRF_BLE_GQ_REQ_CHAR_DISCOVERY] = NULL,
  122. [NRF_BLE_GQ_REQ_DESC_DISCOVERY] = NULL,
  123. [NRF_BLE_GQ_REQ_GATTS_HVX] = gatts_hvx_alloc
  124. };
  125. /**@brief Function handles error codes returned by GATT requests.
  126. *
  127. * @param[in] p_req Pointer to GATT request.
  128. * @param[in] err_code Error code returned by SoftDevice.
  129. * @param[in] conn_handle Connection handle.
  130. */
  131. __STATIC_INLINE void request_err_code_handle(nrf_ble_gq_req_t const * const p_req,
  132. uint16_t conn_handle,
  133. ret_code_t err_code)
  134. {
  135. if (err_code == NRF_SUCCESS)
  136. {
  137. NRF_LOG_DEBUG("SD GATT procedure (%d) succeeded on connection handle: %d.",
  138. p_req->type,
  139. conn_handle);
  140. }
  141. else
  142. {
  143. NRF_LOG_ERROR("SD GATT procedure (%d) failed on connection handle %d with error: 0x%08X.",
  144. p_req->type, conn_handle, err_code);
  145. if (p_req->error_handler.cb != NULL)
  146. {
  147. p_req->error_handler.cb(err_code, p_req->error_handler.p_ctx, conn_handle);
  148. }
  149. }
  150. }
  151. /**@brief Function processes subsequent requests from the BGQ instance queue.
  152. *
  153. * @param[in] p_queue Pointer to the queue instance.
  154. * @param[in] conn_handle Connection handle.
  155. */
  156. static void queue_process(nrf_queue_t const * const p_queue, uint16_t conn_handle)
  157. {
  158. ret_code_t err_code;
  159. nrf_ble_gq_req_t ble_req;
  160. NRF_LOG_DEBUG("Processing the request queue...");
  161. err_code = nrf_queue_peek(p_queue, &ble_req);
  162. if (err_code == NRF_SUCCESS) // Queue is not empty
  163. {
  164. switch (ble_req.type)
  165. {
  166. case NRF_BLE_GQ_REQ_GATTC_READ:
  167. NRF_LOG_DEBUG("GATTC Read Request");
  168. err_code = sd_ble_gattc_read(conn_handle,
  169. ble_req.params.gattc_read.handle,
  170. ble_req.params.gattc_read.offset);
  171. break;
  172. case NRF_BLE_GQ_REQ_GATTC_WRITE:
  173. {
  174. uint8_t write_data[NRF_BLE_GQ_GATTC_WRITE_MAX_DATA_LEN];
  175. // Retrieve allocated data.
  176. ble_req.params.gattc_write.p_value = write_data;
  177. nrf_memobj_read(ble_req.p_mem_obj,
  178. (void *) ble_req.params.gattc_write.p_value,
  179. ble_req.params.gattc_write.len, 0);
  180. NRF_LOG_DEBUG("GATTC Write Request");
  181. err_code = sd_ble_gattc_write(conn_handle,
  182. &ble_req.params.gattc_write);
  183. } break;
  184. case NRF_BLE_GQ_REQ_SRV_DISCOVERY:
  185. {
  186. NRF_LOG_DEBUG("GATTC Primary Service Discovery Request");
  187. err_code = sd_ble_gattc_primary_services_discover(conn_handle,
  188. ble_req.params.gattc_srv_disc.start_handle,
  189. &ble_req.params.gattc_srv_disc.srvc_uuid);
  190. } break;
  191. case NRF_BLE_GQ_REQ_CHAR_DISCOVERY:
  192. {
  193. NRF_LOG_DEBUG("GATTC Characteristic Discovery Request");
  194. err_code = sd_ble_gattc_characteristics_discover(conn_handle,
  195. &ble_req.params.gattc_char_disc);
  196. } break;
  197. case NRF_BLE_GQ_REQ_DESC_DISCOVERY:
  198. {
  199. NRF_LOG_DEBUG("GATTC Characteristic Descriptor Discovery Request")
  200. err_code = sd_ble_gattc_descriptors_discover(conn_handle,
  201. &ble_req.params.gattc_desc_disc);
  202. } break;
  203. case NRF_BLE_GQ_REQ_GATTS_HVX:
  204. {
  205. uint8_t hvx_data[NRF_BLE_GQ_GATTS_HVX_MAX_DATA_LEN];
  206. uint16_t len;
  207. uint16_t hvx_len;
  208. // Retrieve allocated data.
  209. ble_req.params.gatts_hvx.p_data = hvx_data;
  210. nrf_memobj_read(ble_req.p_mem_obj,
  211. (void *) &hvx_len,
  212. sizeof(uint16_t),
  213. 0);
  214. ble_req.params.gatts_hvx.p_len = &hvx_len;
  215. nrf_memobj_read(ble_req.p_mem_obj,
  216. (void *) ble_req.params.gatts_hvx.p_data,
  217. *ble_req.params.gatts_hvx.p_len,
  218. sizeof(uint16_t));
  219. len = hvx_len;
  220. NRF_LOG_DEBUG("GATTS HVX");
  221. err_code = sd_ble_gatts_hvx(conn_handle,
  222. &ble_req.params.gatts_hvx);
  223. if ((err_code == NRF_SUCCESS) &&
  224. (len != hvx_len))
  225. {
  226. err_code = NRF_ERROR_DATA_SIZE;
  227. }
  228. } break;
  229. default:
  230. NRF_LOG_WARNING("Unimplemented GATT Request");
  231. break;
  232. }
  233. if (err_code == NRF_ERROR_BUSY) // Softdevice is processing another GATT request.
  234. {
  235. NRF_LOG_DEBUG("SD is currently busy. The GATT request procedure will be attempted \
  236. again later.");
  237. }
  238. else
  239. {
  240. // Remove last request descriptor from the queue and free data associated with it.
  241. if (m_req_data_alloc[ble_req.type] != NULL)
  242. {
  243. nrf_memobj_free(ble_req.p_mem_obj);
  244. NRF_LOG_DEBUG("Pointer to freed memory block: %p.", ble_req.p_mem_obj);
  245. }
  246. UNUSED_RETURN_VALUE(nrf_queue_pop(p_queue, &ble_req));
  247. request_err_code_handle(&ble_req, conn_handle, err_code);
  248. }
  249. }
  250. }
  251. /**@brief Function purges all requests from BGQ instance queues that are
  252. * no longer used by any connection.
  253. *
  254. * @param[in] p_gatt_queue Pointer to the BGQ instance.
  255. */
  256. static void queues_purge(nrf_ble_gq_t const * const p_gatt_queue)
  257. {
  258. ret_code_t err_code;
  259. uint16_t conn_id;
  260. err_code = nrf_queue_pop(p_gatt_queue->p_purge_queue, &conn_id);
  261. while (err_code == NRF_SUCCESS)
  262. {
  263. nrf_ble_gq_req_t ble_req;
  264. nrf_queue_t const * p_queue;
  265. NRF_LOG_DEBUG("Purging request queue with id: %d", conn_id);
  266. p_queue = &p_gatt_queue->p_req_queue[conn_id];
  267. err_code = nrf_queue_pop(p_queue, &ble_req);
  268. while (err_code == NRF_SUCCESS)
  269. {
  270. // Free data associated with this request if there is any.
  271. if (m_req_data_alloc[ble_req.type] != NULL)
  272. {
  273. nrf_memobj_free(ble_req.p_mem_obj);
  274. NRF_LOG_DEBUG("Pointer to freed memory block: %p.", ble_req.p_mem_obj);
  275. }
  276. err_code = nrf_queue_pop(p_queue, &ble_req);
  277. }
  278. err_code = nrf_queue_pop(p_gatt_queue->p_purge_queue, &conn_id);
  279. }
  280. }
  281. /**@brief Function processes single GATT request without queue.
  282. *
  283. * @param[in] p_req Pointer to GATT request.
  284. * @param[in] conn_handle Connection handle.
  285. *
  286. * @retval true If request is accepted by Softdevice.
  287. * @retval false If Softdevice is busy and the request should be queued.
  288. */
  289. static bool request_process(nrf_ble_gq_req_t const * const p_req, uint16_t conn_handle)
  290. {
  291. ret_code_t err_code = NRF_SUCCESS;
  292. switch (p_req->type)
  293. {
  294. case NRF_BLE_GQ_REQ_GATTC_READ:
  295. NRF_LOG_DEBUG("GATTC Read Request");
  296. err_code = sd_ble_gattc_read(conn_handle,
  297. p_req->params.gattc_read.handle,
  298. p_req->params.gattc_read.offset);
  299. break;
  300. case NRF_BLE_GQ_REQ_GATTC_WRITE:
  301. NRF_LOG_DEBUG("GATTC Write Request");
  302. err_code = sd_ble_gattc_write(conn_handle,
  303. &p_req->params.gattc_write);
  304. break;
  305. case NRF_BLE_GQ_REQ_SRV_DISCOVERY:
  306. NRF_LOG_DEBUG("GATTC Primary Services Discovery Request");
  307. err_code = sd_ble_gattc_primary_services_discover(conn_handle,
  308. p_req->params.gattc_srv_disc.start_handle,
  309. &p_req->params.gattc_srv_disc.srvc_uuid);
  310. break;
  311. case NRF_BLE_GQ_REQ_CHAR_DISCOVERY:
  312. NRF_LOG_DEBUG("GATTC Characteristic Discovery Request");
  313. err_code = sd_ble_gattc_characteristics_discover(conn_handle,
  314. &p_req->params.gattc_char_disc);
  315. break;
  316. case NRF_BLE_GQ_REQ_DESC_DISCOVERY:
  317. NRF_LOG_DEBUG("GATTC Characteristic Descriptor Request");
  318. err_code = sd_ble_gattc_descriptors_discover(conn_handle,
  319. &p_req->params.gattc_desc_disc);
  320. break;
  321. case NRF_BLE_GQ_REQ_GATTS_HVX:
  322. {
  323. uint16_t len = *p_req->params.gatts_hvx.p_len;
  324. NRF_LOG_DEBUG("GATTS Notification or Indication");
  325. err_code = sd_ble_gatts_hvx(conn_handle,
  326. &p_req->params.gatts_hvx);
  327. if ((err_code == NRF_SUCCESS) &&
  328. (len != *p_req->params.gatts_hvx.p_len))
  329. {
  330. err_code = NRF_ERROR_DATA_SIZE;
  331. }
  332. } break;
  333. default:
  334. NRF_LOG_WARNING("Unimplemented GATT Request");
  335. break;
  336. }
  337. if (err_code == NRF_ERROR_BUSY) // Softdevice is processing another GATT request.
  338. {
  339. NRF_LOG_DEBUG("SD is currently busy. The GATT request procedure will be attempted \
  340. again later.");
  341. return false;
  342. }
  343. else
  344. {
  345. request_err_code_handle(p_req, conn_handle, err_code);
  346. return true;
  347. }
  348. }
  349. /**@brief Function finds ID for the provided connection handle within nrf_ble_gq_t instance registry.
  350. *
  351. * @param[in] p_gatt_queue Pointer to the nrf_ble_gq_t instance.
  352. * @param[in] conn_handle Connection handle.
  353. *
  354. * @return Connection ID.
  355. */
  356. static uint16_t conn_handle_id_find(nrf_ble_gq_t const * const p_gatt_queue, uint16_t conn_handle)
  357. {
  358. uint16_t id;
  359. for (id = 0; id < p_gatt_queue->max_conns; id++)
  360. {
  361. if (conn_handle == p_gatt_queue->p_conn_handles[id])
  362. {
  363. return id;
  364. }
  365. }
  366. return id;
  367. }
  368. /**@brief Function registers provided connection handle within nrf_ble_gq_t instance registry.
  369. *
  370. * @param[in] p_gatt_queue Pointer to the nrf_ble_gq_t instance.
  371. * @param[in] conn_handle Connection handle.
  372. *
  373. * @retval NRF_SUCCESS If the registration was successful.
  374. * @retval NRF_ERROR_NO_MEM If there was no space for another connection handle.
  375. */
  376. static ret_code_t conn_handle_register(nrf_ble_gq_t const * const p_gatt_queue, uint16_t conn_handle)
  377. {
  378. for (uint16_t id = 0; id < p_gatt_queue->max_conns; id++)
  379. {
  380. if (p_gatt_queue->p_conn_handles[id] == BLE_CONN_HANDLE_INVALID)
  381. {
  382. p_gatt_queue->p_conn_handles[id] = conn_handle;
  383. return NRF_SUCCESS;
  384. }
  385. }
  386. return NRF_ERROR_NO_MEM;
  387. }
  388. /**@brief Function checks if any connection handle is registered in nrf_ble_gq_t instance.
  389. *
  390. * @param[in] p_gatt_queue Pointer to the nrf_ble_gq_t instance.
  391. *
  392. * @retval true There is at least one registered connection handle.
  393. * @retval false Connection handle registry is empty.
  394. */
  395. static bool is_any_conn_handle_registered(nrf_ble_gq_t const * const p_gatt_queue)
  396. {
  397. for (uint16_t id = 0; id < p_gatt_queue->max_conns; id++)
  398. {
  399. if (p_gatt_queue->p_conn_handles[id] != BLE_CONN_HANDLE_INVALID)
  400. {
  401. return true;
  402. }
  403. }
  404. return false;
  405. }
  406. ret_code_t nrf_ble_gq_item_add(nrf_ble_gq_t const * const p_gatt_queue,
  407. nrf_ble_gq_req_t * const p_req,
  408. uint16_t conn_handle)
  409. {
  410. ret_code_t err_code = NRF_SUCCESS;
  411. uint16_t conn_id;
  412. NRF_LOG_DEBUG("Adding item to the request queue");
  413. VERIFY_PARAM_NOT_NULL(p_gatt_queue);
  414. VERIFY_PARAM_NOT_NULL(p_req);
  415. // Purge queues that are no longer used by any connection.
  416. queues_purge(p_gatt_queue);
  417. // Check if connection handle is registered and if GATT request is valid.
  418. conn_id = conn_handle_id_find(p_gatt_queue, conn_handle);
  419. if ((p_req->type >= NRF_BLE_GQ_REQ_NUM) || (conn_id == p_gatt_queue->max_conns))
  420. {
  421. return NRF_ERROR_INVALID_PARAM;
  422. }
  423. // Try processing a request without buffering.
  424. if (nrf_queue_is_empty(&p_gatt_queue->p_req_queue[conn_id]))
  425. {
  426. bool req_processed = request_process(p_req, conn_handle);
  427. if (req_processed)
  428. {
  429. return err_code;
  430. }
  431. }
  432. // Prepare request for buffering and add it to the queue.
  433. if (m_req_data_alloc[p_req->type] != NULL)
  434. {
  435. VERIFY_PARAM_NOT_NULL(p_gatt_queue->p_data_pool);
  436. err_code = m_req_data_alloc[p_req->type](p_gatt_queue->p_data_pool, p_req);
  437. VERIFY_SUCCESS(err_code);
  438. }
  439. err_code = nrf_queue_push(&p_gatt_queue->p_req_queue[conn_id], p_req);
  440. if ((err_code != NRF_SUCCESS) && (m_req_data_alloc[p_req->type] != NULL))
  441. {
  442. nrf_memobj_free(p_req->p_mem_obj);
  443. NRF_LOG_DEBUG("Pointer to freed memory block: %p.", p_req->p_mem_obj);
  444. }
  445. // Check if Softdevice is still busy.
  446. queue_process(&p_gatt_queue->p_req_queue[conn_id], conn_handle);
  447. return err_code;
  448. }
  449. ret_code_t nrf_ble_gq_conn_handle_register(nrf_ble_gq_t * const p_gatt_queue, uint16_t conn_handle)
  450. {
  451. ret_code_t err_code = NRF_SUCCESS;
  452. uint16_t conn_id;
  453. VERIFY_PARAM_NOT_NULL(p_gatt_queue);
  454. // Purge queues that are no longer used by any connection.
  455. queues_purge(p_gatt_queue);
  456. // Allow instance to claim connection handle only if it has not been claimed already.
  457. conn_id = conn_handle_id_find(p_gatt_queue, conn_handle);
  458. if (conn_id == p_gatt_queue->max_conns)
  459. {
  460. NRF_LOG_DEBUG("Registering connection handle: 0x%04X", conn_handle);
  461. // Initialize/reset data pool if possible.
  462. if (!is_any_conn_handle_registered(p_gatt_queue))
  463. {
  464. err_code = nrf_memobj_pool_init(p_gatt_queue->p_data_pool);
  465. }
  466. err_code = conn_handle_register(p_gatt_queue, conn_handle);
  467. VERIFY_SUCCESS(err_code);
  468. }
  469. return err_code;
  470. }
  471. void nrf_ble_gq_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context)
  472. {
  473. nrf_ble_gq_t * p_gatt_queue = (nrf_ble_gq_t *) p_context;
  474. uint16_t conn_handle;
  475. uint16_t conn_id;
  476. if ((p_ble_evt == NULL) || (p_gatt_queue == NULL))
  477. {
  478. return;
  479. }
  480. // Obtain connection handle and filter out the events that do not trigger queue processing.
  481. if (p_ble_evt->header.evt_id == BLE_GAP_EVT_DISCONNECTED)
  482. {
  483. conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
  484. }
  485. else if ((p_ble_evt->header.evt_id >= BLE_GATTC_EVT_BASE) &&
  486. (p_ble_evt->header.evt_id <= BLE_GATTC_EVT_LAST))
  487. {
  488. conn_handle = p_ble_evt->evt.gattc_evt.conn_handle;
  489. }
  490. else if ((p_ble_evt->header.evt_id >= BLE_GATTS_EVT_BASE) &&
  491. (p_ble_evt->header.evt_id <= BLE_GATTS_EVT_LAST))
  492. {
  493. conn_handle = p_ble_evt->evt.gatts_evt.conn_handle;
  494. }
  495. else
  496. {
  497. // These events are irrelevant for this module.
  498. return;
  499. }
  500. // Check if connection handle is registered.
  501. conn_id = conn_handle_id_find(p_gatt_queue, conn_handle);
  502. if (conn_id == p_gatt_queue->max_conns)
  503. {
  504. return;
  505. }
  506. // Perform operations on the queue.
  507. if (p_ble_evt->header.evt_id == BLE_GAP_EVT_DISCONNECTED)
  508. {
  509. p_gatt_queue->p_conn_handles[conn_id] = BLE_CONN_HANDLE_INVALID;
  510. UNUSED_RETURN_VALUE(nrf_queue_push(p_gatt_queue->p_purge_queue, &conn_id));
  511. }
  512. else
  513. {
  514. queue_process(&p_gatt_queue->p_req_queue[conn_id], conn_handle);
  515. }
  516. }
  517. #endif // NRF_MODULE_ENABLED(NRF_BLE_GQ)