ble_ln_cp.c 25 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806
  1. /**
  2. * Copyright (c) 2015 - 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 "ble_ln_cp.h"
  41. #include "ble_ln_db.h"
  42. #include "ble_ln_common.h"
  43. #include "sdk_common.h"
  44. #define NRF_LOG_MODULE_NAME ble_ln_cp
  45. #include "nrf_log.h"
  46. NRF_LOG_MODULE_REGISTER();
  47. // Feature Mask bits
  48. #define FEATURE_MASK_INSTANTANEOUS_SPEED (0x01 << 0) /**< Instantaneous Speed mask bit. */
  49. #define FEATURE_MASK_TOTAL_DISTANCE (0x01 << 1) /**< Total Distance mask bit. */
  50. #define FEATURE_MASK_LOCATION (0x01 << 2) /**< Location mask bit. */
  51. #define FEATURE_MASK_ELEVATION (0x01 << 3) /**< Elevation mask bit. */
  52. #define FEATURE_MASK_HEADING (0x01 << 4) /**< Heading mask bit. */
  53. #define FEATURE_MASK_ROLLING_TIME (0x01 << 5) /**< Rolling Time mask bit. */
  54. #define FEATURE_MASK_UTC_TIME (0x01 << 6) /**< UTC Time mask bit. */
  55. // Data Control point parameter type lengths.
  56. #define INT8_LEN 1
  57. #define INT16_LEN 2
  58. #define INT24_LEN 3
  59. #define INT32_LEN 4
  60. #define OPCODE_LENGTH 1 /**< Length of opcode inside Location and Navigation Measurement packet. */
  61. #define HANDLE_LENGTH 2 /**< Length of handle inside Location and Navigation Measurement packet. */
  62. /**@brief Function for interception of GATT errors and @ref nrf_ble_gq errors.
  63. *
  64. * @param[in] nrf_error Error code.
  65. * @param[in] p_ctx Parameter from the event handler.
  66. * @param[in] conn_handle Connection handle.
  67. */
  68. static void gatt_error_handler(uint32_t nrf_error,
  69. void * p_ctx,
  70. uint16_t conn_handle)
  71. {
  72. ble_lncp_t * p_lncp = (ble_lncp_t *)p_ctx;
  73. if (p_lncp->error_handler != NULL)
  74. {
  75. p_lncp->error_handler(nrf_error);
  76. }
  77. }
  78. static ble_lncp_rsp_code_t notify_app(ble_lncp_t const * p_lncp, ble_lncp_evt_t const * p_evt)
  79. {
  80. ble_lncp_rsp_code_t rsp = LNCP_RSP_SUCCESS;
  81. if (p_lncp->evt_handler != NULL)
  82. {
  83. rsp = p_lncp->evt_handler(p_lncp, p_evt);
  84. }
  85. return rsp;
  86. }
  87. static void resp_send(ble_lncp_t * p_lncp)
  88. {
  89. // Send indication
  90. uint16_t hvx_len;
  91. uint8_t hvx_data[MAX_CTRL_POINT_RESP_PARAM_LEN];
  92. uint32_t err_code;
  93. nrf_ble_gq_req_t lncp_req;
  94. memset(&lncp_req, 0, sizeof(nrf_ble_gq_req_t));
  95. hvx_len = 3 + p_lncp->pending_rsp.rsp_param_len;
  96. hvx_data[0] = LNCP_OP_RESPONSE_CODE;
  97. hvx_data[1] = p_lncp->pending_rsp.op_code;
  98. hvx_data[2] = p_lncp->pending_rsp.rsp_code;
  99. memcpy(&hvx_data[3], &p_lncp->pending_rsp.rsp_param[0], p_lncp->pending_rsp.rsp_param_len);
  100. lncp_req.type = NRF_BLE_GQ_REQ_GATTS_HVX;
  101. lncp_req.error_handler.cb = gatt_error_handler;
  102. lncp_req.error_handler.p_ctx = p_lncp;
  103. lncp_req.params.gatts_hvx.handle = p_lncp->ctrlpt_handles.value_handle;
  104. lncp_req.params.gatts_hvx.offset = 0;
  105. lncp_req.params.gatts_hvx.p_data = hvx_data;
  106. lncp_req.params.gatts_hvx.p_len = &hvx_len;
  107. lncp_req.params.gatts_hvx.type = BLE_GATT_HVX_INDICATION;
  108. p_lncp->is_indication_pending = true;
  109. err_code = nrf_ble_gq_item_add(p_lncp->p_gatt_queue, &lncp_req, p_lncp->conn_handle);
  110. if ((p_lncp->error_handler != NULL) &&
  111. (err_code != NRF_SUCCESS))
  112. {
  113. p_lncp->error_handler(err_code);
  114. }
  115. }
  116. static void on_connect(ble_lncp_t * p_lncp, ble_evt_t const * p_ble_evt)
  117. {
  118. memset(&p_lncp->mask, 0, sizeof(ble_lncp_mask_t));
  119. p_lncp->conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
  120. }
  121. static void on_disconnect(ble_lncp_t * p_lncp, ble_evt_t const * p_ble_evt)
  122. {
  123. UNUSED_PARAMETER(p_ble_evt);
  124. p_lncp->conn_handle = BLE_CONN_HANDLE_INVALID;
  125. }
  126. static void on_hvc_confirm(ble_lncp_t * p_lncp, ble_evt_t const * p_ble_evt)
  127. {
  128. if (p_ble_evt->evt.gatts_evt.params.hvc.handle == p_lncp->ctrlpt_handles.value_handle)
  129. {
  130. if (p_lncp->is_indication_pending)
  131. {
  132. p_lncp->is_indication_pending = false;
  133. }
  134. else
  135. {
  136. if (p_lncp->error_handler != NULL)
  137. {
  138. p_lncp->error_handler(NRF_ERROR_INVALID_STATE);
  139. }
  140. }
  141. }
  142. }
  143. /**@brief Handle write events to the control point cccd.
  144. *
  145. * @param[in] p_lncp Location and Navigation Service structure.
  146. * @param[in] p_evt_write Write event received from the BLE stack.
  147. */
  148. static void on_lncp_cccd_write(ble_lncp_t * p_lncp, ble_gatts_evt_write_t const * p_evt_write)
  149. {
  150. if (p_evt_write->len == BLE_CCCD_VALUE_LEN)
  151. {
  152. // CCCD written, update indications state
  153. p_lncp->is_ctrlpt_indication_enabled = ble_srv_is_indication_enabled(p_evt_write->data);
  154. }
  155. }
  156. /**@brief Handle write events to the navigation cccd.
  157. *
  158. * @param[in] p_lncp Location and Navigation Service structure.
  159. * @param[in] p_evt_write Write event received from the BLE stack.
  160. */
  161. static void on_nav_cccd_write(ble_lncp_t * p_lncp, ble_gatts_evt_write_t const * p_evt_write)
  162. {
  163. if (p_evt_write->len == BLE_CCCD_VALUE_LEN)
  164. {
  165. // CCCD written, update notification state
  166. p_lncp->is_nav_notification_enabled = ble_srv_is_notification_enabled(p_evt_write->data);
  167. }
  168. }
  169. /**@brief Event handler for control point write.
  170. *
  171. * @param[in] p_lncp Location and Navigation Service structure.
  172. * @param[in] p_ble_evt Event received from the BLE stack.
  173. */
  174. static void on_set_cumulative_value(ble_lncp_t * p_lncp, ble_gatts_evt_write_t const * p_evt_write)
  175. {
  176. if ( !(p_lncp->available_features & BLE_LNS_FEATURE_TOTAL_DISTANCE_SUPPORTED) )
  177. {
  178. p_lncp->pending_rsp.rsp_code = LNCP_RSP_OP_CODE_NOT_SUPPORTED;
  179. return;
  180. }
  181. if (p_evt_write->len != OPCODE_LENGTH + INT24_LEN)
  182. {
  183. p_lncp->pending_rsp.rsp_code = LNCP_RSP_INVALID_PARAMETER;
  184. return;
  185. }
  186. const uint32_t total_distance = uint24_decode(&p_evt_write->data[1]);
  187. const ble_lncp_evt_t evt = {
  188. .evt_type = LNCP_EVT_TOTAL_DISTANCE_SET,
  189. .params.total_distance = total_distance
  190. };
  191. p_lncp->pending_rsp.rsp_code = notify_app(p_lncp, &evt);
  192. if (p_lncp->pending_rsp.rsp_code == LNCP_RSP_SUCCESS)
  193. {
  194. p_lncp->total_distance = total_distance;
  195. }
  196. }
  197. /**@brief Event handler for control point write.
  198. *
  199. * @param[in] p_lncp Location and Navigation Service structure.
  200. * @param[in] p_ble_evt Event received from the BLE stack.
  201. */
  202. static void on_mask_loc_speed_content(ble_lncp_t * p_lncp, ble_gatts_evt_write_t const * p_evt_write)
  203. {
  204. if ( !(p_lncp->available_features & BLE_LNS_FEATURE_LOC_AND_SPEED_CONTENT_MASKING_SUPPORTED) )
  205. {
  206. p_lncp->pending_rsp.rsp_code = LNCP_RSP_OP_CODE_NOT_SUPPORTED;
  207. return;
  208. }
  209. if (p_evt_write->len != OPCODE_LENGTH + INT16_LEN)
  210. {
  211. p_lncp->pending_rsp.rsp_code = LNCP_RSP_INVALID_PARAMETER;
  212. return;
  213. }
  214. uint16_t rcvd_mask = uint16_decode(&p_evt_write->data[1]);
  215. if (rcvd_mask > 0x7F)
  216. {
  217. p_lncp->pending_rsp.rsp_code = LNCP_RSP_INVALID_PARAMETER;
  218. return;
  219. }
  220. const ble_lncp_evt_t evt = {
  221. .evt_type = LNCP_EVT_MASK_SET,
  222. .params.mask.flags = rcvd_mask
  223. };
  224. p_lncp->pending_rsp.rsp_code = notify_app(p_lncp, &evt);
  225. if (p_lncp->pending_rsp.rsp_code == LNCP_RSP_SUCCESS)
  226. {
  227. p_lncp->mask.flags = rcvd_mask;
  228. }
  229. }
  230. /**@brief Event handler for control point write.
  231. *
  232. * @param[in] p_lncp Location and Navigation Service structure.
  233. * @param[in] p_ble_evt Event received from the BLE stack.
  234. */
  235. static void on_nav_control(ble_lncp_t * p_lncp, ble_gatts_evt_write_t const * p_evt_write)
  236. {
  237. if ( !(p_lncp->is_navigation_present) )
  238. {
  239. p_lncp->pending_rsp.rsp_code = LNCP_RSP_OP_CODE_NOT_SUPPORTED;
  240. return;
  241. }
  242. if (p_evt_write->len != LNCP_NAV_CMD_LEN)
  243. {
  244. p_lncp->pending_rsp.rsp_code = LNCP_RSP_INVALID_PARAMETER;
  245. return;
  246. }
  247. /*lint --e{415} --e{416} -save suppress Warning 415: possible access out of bond */
  248. const uint8_t data_buf = p_evt_write->data[1];
  249. /*lint -restore*/
  250. if (data_buf > LNCP_NAV_CMD_MAX)
  251. {
  252. p_lncp->pending_rsp.rsp_code = LNCP_RSP_INVALID_PARAMETER;
  253. return;
  254. }
  255. const ble_lncp_nav_cmd_t cmd = (ble_lncp_nav_cmd_t) data_buf;
  256. if (cmd == LNCP_CMD_NAV_START || cmd == LNCP_CMD_NAV_CONTINUE || cmd == LNCP_CMD_NAV_NEAREST)
  257. {
  258. p_lncp->is_navigation_running = true;
  259. }
  260. else if (cmd == LNCP_CMD_NAV_STOP || cmd == LNCP_CMD_NAV_PAUSE)
  261. {
  262. p_lncp->is_navigation_running = false;
  263. }
  264. const ble_lncp_evt_t evt = {
  265. .evt_type = LNCP_EVT_NAV_COMMAND,
  266. .params.nav_cmd = cmd
  267. };
  268. p_lncp->pending_rsp.rsp_code = notify_app(p_lncp, &evt);
  269. }
  270. /**@brief Event handler for control point write.
  271. *
  272. * @param[in] p_lncp Location and Navigation Service structure.
  273. * @param[in] p_ble_evt Event received from the BLE stack.
  274. */
  275. static void on_req_num_routes(ble_lncp_t * p_lncp, ble_gatts_evt_write_t const * p_evt_write)
  276. {
  277. p_lncp->pending_rsp.rsp_code = LNCP_RSP_SUCCESS;
  278. if ( !(p_lncp->is_navigation_present) )
  279. {
  280. p_lncp->pending_rsp.rsp_code = LNCP_RSP_OP_CODE_NOT_SUPPORTED;
  281. return;
  282. }
  283. if (p_evt_write->len != OPCODE_LENGTH)
  284. {
  285. p_lncp->pending_rsp.rsp_code = LNCP_RSP_INVALID_PARAMETER;
  286. return;
  287. }
  288. const uint8_t num_records = ble_ln_db_num_records_get();
  289. p_lncp->pending_rsp.rsp_param_len = uint16_encode(num_records, &p_lncp->pending_rsp.rsp_param[0]);
  290. }
  291. /**@brief Event handler for control point write.
  292. *
  293. * @param[in] p_lncp Location and Navigation Service structure.
  294. * @param[in] p_ble_evt Event received from the BLE stack.
  295. */
  296. static void on_req_name_of_route(ble_lncp_t * p_lncp, ble_gatts_evt_write_t const * p_evt_write)
  297. {
  298. uint8_t * p_name;
  299. uint32_t err_code;
  300. p_lncp->pending_rsp.rsp_code = LNCP_RSP_SUCCESS;
  301. if ( !(p_lncp->is_navigation_present) )
  302. {
  303. p_lncp->pending_rsp.rsp_code = LNCP_RSP_OP_CODE_NOT_SUPPORTED;
  304. return;
  305. }
  306. if (p_evt_write->len != OPCODE_LENGTH + INT16_LEN)
  307. {
  308. p_lncp->pending_rsp.rsp_code = LNCP_RSP_INVALID_PARAMETER;
  309. return;
  310. }
  311. /*lint --e{415} --e{416} -save suppress Warning 415: possible access out of bond */
  312. const uint16_t route_num = uint16_decode(&p_evt_write->data[1]);
  313. /*lint -restore*/
  314. err_code = ble_ln_db_record_name_get(route_num, &p_name);
  315. if (err_code != NRF_SUCCESS)
  316. {
  317. p_lncp->pending_rsp.rsp_code = LNCP_RSP_OPERATION_FAILED;
  318. return;
  319. }
  320. memcpy(&p_lncp->pending_rsp.rsp_param[0], p_name, BLE_LNS_MAX_ROUTE_NAME_LEN);
  321. p_lncp->pending_rsp.rsp_param_len = BLE_LNS_MAX_ROUTE_NAME_LEN;
  322. }
  323. /**@brief Event handler for control point write.
  324. *
  325. * @param[in] p_lncp Location and Navigation Service structure.
  326. * @param[in] p_ble_evt Event received from the BLE stack.
  327. */
  328. static void on_select_route(ble_lncp_t * p_lncp, ble_gatts_evt_write_t const * p_evt_write)
  329. {
  330. if ( !(p_lncp->is_navigation_present))
  331. {
  332. p_lncp->pending_rsp.rsp_code = LNCP_RSP_OP_CODE_NOT_SUPPORTED;
  333. return;
  334. }
  335. if (p_evt_write->len != OPCODE_LENGTH + INT16_LEN)
  336. {
  337. p_lncp->pending_rsp.rsp_code = LNCP_RSP_INVALID_PARAMETER;
  338. return;
  339. }
  340. const uint16_t route_num = uint16_decode(&p_evt_write->data[1]);
  341. const uint16_t stored_num = ble_ln_db_num_records_get();
  342. if (route_num >= stored_num)
  343. {
  344. p_lncp->pending_rsp.rsp_code = LNCP_RSP_INVALID_PARAMETER;
  345. return;
  346. }
  347. const ble_lncp_evt_t evt = {
  348. .evt_type = LNCP_EVT_ROUTE_SELECTED,
  349. .params.selected_route = route_num
  350. };
  351. p_lncp->pending_rsp.rsp_code = notify_app(p_lncp, &evt);
  352. if (p_lncp->pending_rsp.rsp_code == LNCP_RSP_SUCCESS)
  353. {
  354. p_lncp->selected_route = route_num;
  355. }
  356. }
  357. /**@brief Event handler for control point write.
  358. *
  359. * @param[in] p_lncp Location and Navigation Service structure.
  360. * @param[in] p_ble_evt Event received from the BLE stack.
  361. */
  362. static void on_set_fix_rate(ble_lncp_t * p_lncp, ble_gatts_evt_write_t const * p_evt_write)
  363. {
  364. p_lncp->pending_rsp.rsp_code = LNCP_RSP_SUCCESS;
  365. if ( !(p_lncp->available_features & BLE_LNS_FEATURE_FIX_RATE_SETTING_SUPPORTED) )
  366. {
  367. p_lncp->pending_rsp.rsp_code = LNCP_RSP_OP_CODE_NOT_SUPPORTED;
  368. return;
  369. }
  370. if (p_evt_write->len != OPCODE_LENGTH + INT8_LEN)
  371. {
  372. p_lncp->pending_rsp.rsp_code = LNCP_RSP_INVALID_PARAMETER;
  373. return;
  374. }
  375. /*lint --e{415} --e{416} -save suppress Warning 415: possible access out of bond */
  376. const uint8_t fix_rate = p_evt_write->data[1];
  377. /*lint -restore*/
  378. const ble_lncp_evt_t evt = {
  379. .evt_type = LNCP_EVT_FIX_RATE_SET,
  380. .params.fix_rate = fix_rate
  381. };
  382. p_lncp->pending_rsp.rsp_code = notify_app(p_lncp, &evt);
  383. if (p_lncp->pending_rsp.rsp_code == LNCP_RSP_SUCCESS)
  384. {
  385. p_lncp->fix_rate = fix_rate;
  386. }
  387. }
  388. /**@brief Event handler for control point write.
  389. *
  390. * @param[in] p_lncp Location and Navigation Service structure.
  391. * @param[in] p_ble_evt Event received from the BLE stack.
  392. */
  393. static void on_set_elevation(ble_lncp_t * p_lncp, ble_gatts_evt_write_t const * p_evt_write)
  394. {
  395. p_lncp->pending_rsp.rsp_code = LNCP_RSP_SUCCESS;
  396. if ( !(p_lncp->available_features & BLE_LNS_FEATURE_ELEVATION_SETTING_SUPPORTED) )
  397. {
  398. p_lncp->pending_rsp.rsp_code = LNCP_RSP_OP_CODE_NOT_SUPPORTED;
  399. return;
  400. }
  401. if (p_evt_write->len != OPCODE_LENGTH + INT24_LEN)
  402. {
  403. p_lncp->pending_rsp.rsp_code = LNCP_RSP_INVALID_PARAMETER;
  404. return;
  405. }
  406. const uint32_t elevation = uint24_decode(&p_evt_write->data[1]);
  407. ble_lncp_evt_t evt = {
  408. .evt_type = LNCP_EVT_ELEVATION_SET,
  409. .params.elevation = elevation
  410. };
  411. p_lncp->pending_rsp.rsp_code = notify_app(p_lncp, &evt);
  412. if (p_lncp->pending_rsp.rsp_code == LNCP_RSP_SUCCESS)
  413. {
  414. p_lncp->elevation = elevation;
  415. }
  416. }
  417. /**@brief Handle write events to the Location and Navigation Service Control Point characteristic.
  418. *
  419. * @param[in] p_lncp Location and Navigation Service structure.
  420. * @param[in] p_evt_write Write event received from the BLE stack.
  421. */
  422. static void on_ctrlpt_write(ble_lncp_t * p_lncp, ble_gatts_evt_write_t const * p_evt_write)
  423. {
  424. uint32_t err_code;
  425. p_lncp->pending_rsp.rsp_param_len = 0;
  426. ble_gatts_rw_authorize_reply_params_t write_authorize_reply;
  427. memset(&write_authorize_reply, 0, sizeof(write_authorize_reply));
  428. write_authorize_reply.type = BLE_GATTS_AUTHORIZE_TYPE_WRITE;
  429. if (p_lncp->is_ctrlpt_indication_enabled)
  430. {
  431. if (!p_lncp->is_indication_pending)
  432. {
  433. write_authorize_reply.params.write.update = 1;
  434. write_authorize_reply.params.write.gatt_status = BLE_GATT_STATUS_SUCCESS;
  435. // if the op code is navigation control, its cccd must be checked
  436. if (p_evt_write->len > 0 && p_lncp->is_navigation_present)
  437. {
  438. if ( p_evt_write->data[0] == LNCP_OP_NAV_CONTROL
  439. || p_evt_write->data[0] == LNCP_OP_REQ_NAME_OF_ROUTE
  440. || p_evt_write->data[0] == LNCP_OP_REQ_NUM_ROUTES)
  441. {
  442. if (!p_lncp->is_nav_notification_enabled)
  443. {
  444. write_authorize_reply.params.write.gatt_status = BLE_GATT_STATUS_ATTERR_CPS_CCCD_CONFIG_ERROR;
  445. }
  446. }
  447. }
  448. }
  449. else
  450. {
  451. write_authorize_reply.params.write.gatt_status = BLE_GATT_STATUS_ATTERR_CPS_PROC_ALR_IN_PROG;
  452. }
  453. }
  454. else
  455. {
  456. write_authorize_reply.params.write.gatt_status = BLE_GATT_STATUS_ATTERR_CPS_CCCD_CONFIG_ERROR;
  457. }
  458. // reply to the write authorization
  459. do {
  460. err_code = sd_ble_gatts_rw_authorize_reply(p_lncp->conn_handle, &write_authorize_reply);
  461. if (err_code != NRF_SUCCESS && err_code != NRF_ERROR_BUSY)
  462. {
  463. if (p_lncp->error_handler != NULL)
  464. {
  465. p_lncp->error_handler(err_code);
  466. }
  467. }
  468. } while (err_code == NRF_ERROR_BUSY);
  469. if (write_authorize_reply.params.write.gatt_status != BLE_GATT_STATUS_SUCCESS)
  470. {
  471. return;
  472. }
  473. // Start executing the control point write action
  474. if (p_evt_write->len > 0)
  475. {
  476. p_lncp->pending_rsp.op_code = (ble_lncp_op_code_t) p_evt_write->data[0];
  477. switch (p_lncp->pending_rsp.op_code)
  478. {
  479. case LNCP_OP_SET_CUMULATIVE_VALUE:
  480. on_set_cumulative_value(p_lncp, p_evt_write);
  481. break;
  482. case LNCP_OP_MASK_LOC_SPEED_CONTENT:
  483. on_mask_loc_speed_content(p_lncp, p_evt_write);
  484. break;
  485. case LNCP_OP_NAV_CONTROL:
  486. on_nav_control(p_lncp, p_evt_write);
  487. break;
  488. case LNCP_OP_REQ_NUM_ROUTES:
  489. on_req_num_routes(p_lncp, p_evt_write);
  490. break;
  491. case LNCP_OP_REQ_NAME_OF_ROUTE:
  492. on_req_name_of_route(p_lncp, p_evt_write);
  493. break;
  494. case LNCP_OP_SELECT_ROUTE:
  495. on_select_route(p_lncp, p_evt_write);
  496. break;
  497. case LNCP_OP_SET_FIX_RATE:
  498. on_set_fix_rate(p_lncp, p_evt_write);
  499. break;
  500. case LNCP_OP_SET_ELEVATION:
  501. on_set_elevation(p_lncp, p_evt_write);
  502. break;
  503. // Unrecognized Op Code
  504. default:
  505. p_lncp->pending_rsp.rsp_code = LNCP_RSP_OP_CODE_NOT_SUPPORTED;
  506. break;
  507. }
  508. resp_send(p_lncp);
  509. }
  510. }
  511. /**@brief Write authorization request event handler.
  512. *
  513. * @details The write authorization request event handler is only called when writing to the control point.
  514. *
  515. * @param[in] p_lncp Location and Navigation Service structure.
  516. * @param[in] p_ble_evt Event received from the BLE stack.
  517. */
  518. static void on_rw_authorize_req(ble_lncp_t * p_lncp, ble_evt_t const * p_ble_evt)
  519. {
  520. const ble_gatts_evt_rw_authorize_request_t * p_auth_req =
  521. &p_ble_evt->evt.gatts_evt.params.authorize_request;
  522. if (
  523. (p_auth_req->type == BLE_GATTS_AUTHORIZE_TYPE_WRITE)
  524. &&
  525. (p_auth_req->request.write.handle == p_lncp->ctrlpt_handles.value_handle)
  526. &&
  527. (p_auth_req->request.write.op != BLE_GATTS_OP_PREP_WRITE_REQ)
  528. &&
  529. (p_auth_req->request.write.op != BLE_GATTS_OP_EXEC_WRITE_REQ_NOW)
  530. &&
  531. (p_auth_req->request.write.op != BLE_GATTS_OP_EXEC_WRITE_REQ_CANCEL)
  532. )
  533. {
  534. on_ctrlpt_write(p_lncp, &p_auth_req->request.write);
  535. }
  536. }
  537. /**@brief Write event handler.
  538. *
  539. * @param[in] p_lncp Location and Navigation Service structure.
  540. * @param[in] p_ble_evt Event received from the BLE stack.
  541. */
  542. static void on_write(ble_lncp_t * p_lncp, ble_evt_t const * p_ble_evt)
  543. {
  544. const ble_gatts_evt_write_t * p_evt_write = &p_ble_evt->evt.gatts_evt.params.write;
  545. if (p_evt_write->handle == p_lncp->ctrlpt_handles.cccd_handle)
  546. {
  547. on_lncp_cccd_write(p_lncp, p_evt_write);
  548. }
  549. else if (p_evt_write->handle == p_lncp->navigation_handles.cccd_handle)
  550. {
  551. on_nav_cccd_write(p_lncp, p_evt_write);
  552. }
  553. }
  554. void ble_lncp_on_ble_evt(ble_lncp_t * p_lncp, ble_evt_t const * p_ble_evt)
  555. {
  556. VERIFY_PARAM_NOT_NULL_VOID(p_lncp);
  557. VERIFY_PARAM_NOT_NULL_VOID(p_ble_evt);
  558. switch (p_ble_evt->header.evt_id)
  559. {
  560. case BLE_GAP_EVT_CONNECTED:
  561. on_connect(p_lncp, p_ble_evt);
  562. break;
  563. case BLE_GAP_EVT_DISCONNECTED:
  564. if (p_ble_evt->evt.gap_evt.conn_handle == p_lncp->conn_handle)
  565. {
  566. on_disconnect(p_lncp, p_ble_evt);
  567. }
  568. break;
  569. case BLE_GATTS_EVT_WRITE:
  570. if (p_ble_evt->evt.gatts_evt.conn_handle == p_lncp->conn_handle)
  571. {
  572. on_write(p_lncp, p_ble_evt);
  573. }
  574. break;
  575. case BLE_GATTS_EVT_RW_AUTHORIZE_REQUEST:
  576. if (p_ble_evt->evt.gatts_evt.conn_handle == p_lncp->conn_handle)
  577. {
  578. on_rw_authorize_req(p_lncp, p_ble_evt);
  579. }
  580. break;
  581. case BLE_GATTS_EVT_HVC:
  582. if (p_ble_evt->evt.gatts_evt.conn_handle == p_lncp->conn_handle)
  583. {
  584. on_hvc_confirm(p_lncp, p_ble_evt);
  585. }
  586. break;
  587. default:
  588. // no implementation
  589. break;
  590. }
  591. }
  592. uint32_t ble_lncp_total_distance_get(ble_lncp_t const * p_lncp)
  593. {
  594. if (p_lncp == NULL)
  595. {
  596. return 0;
  597. }
  598. return p_lncp->total_distance;
  599. }
  600. uint32_t ble_lncp_elevation_get(ble_lncp_t const * p_lncp)
  601. {
  602. if (p_lncp == NULL)
  603. {
  604. return 0;
  605. }
  606. return p_lncp->elevation;
  607. }
  608. ble_lncp_mask_t ble_lncp_mask_get(ble_lncp_t const * p_lncp)
  609. {
  610. if (p_lncp == NULL)
  611. {
  612. const ble_lncp_mask_t empty_mask = {0};
  613. return empty_mask;
  614. }
  615. return p_lncp->mask;
  616. }
  617. bool ble_lncp_is_navigation_running(ble_lncp_t const * p_lncp)
  618. {
  619. if (p_lncp == NULL)
  620. {
  621. return false;
  622. }
  623. return p_lncp->is_navigation_running;
  624. }
  625. ret_code_t ble_lncp_init(ble_lncp_t * p_lncp, ble_lncp_init_t const * p_lncp_init)
  626. {
  627. VERIFY_PARAM_NOT_NULL(p_lncp);
  628. VERIFY_PARAM_NOT_NULL(p_lncp_init);
  629. VERIFY_PARAM_NOT_NULL(p_lncp_init->p_gatt_queue);
  630. ble_add_char_params_t add_char_params;
  631. memset(&add_char_params, 0, sizeof(add_char_params));
  632. p_lncp->service_handle = p_lncp_init->service_handle;
  633. p_lncp->evt_handler = p_lncp_init->evt_handler;
  634. p_lncp->error_handler = p_lncp_init->error_handler;
  635. p_lncp->p_gatt_queue = p_lncp_init->p_gatt_queue;
  636. p_lncp->available_features = p_lncp_init->available_features;
  637. p_lncp->is_position_quality_present = p_lncp_init->is_position_quality_present;
  638. p_lncp->is_navigation_present = p_lncp_init->is_navigation_present;
  639. p_lncp->total_distance = p_lncp_init->total_distance;
  640. p_lncp->elevation = p_lncp_init->elevation;
  641. p_lncp->navigation_handles = p_lncp_init->navigation_handles;
  642. p_lncp->fix_rate = BLE_LNS_NO_FIX;
  643. p_lncp->selected_route = BLE_LNS_INVALID_ROUTE;
  644. p_lncp->conn_handle = BLE_CONN_HANDLE_INVALID;
  645. p_lncp->is_navigation_running = false;
  646. p_lncp->is_nav_notification_enabled = false;
  647. p_lncp->is_ctrlpt_indication_enabled = false;
  648. memset(&p_lncp->mask, 0, sizeof(ble_lncp_mask_t));
  649. add_char_params.uuid = BLE_UUID_LN_CONTROL_POINT_CHAR;
  650. add_char_params.max_len = 0;
  651. add_char_params.char_props.indicate = true;
  652. add_char_params.char_props.write = true;
  653. add_char_params.is_defered_write = true;
  654. add_char_params.is_var_len = true;
  655. add_char_params.max_len = BLE_GATT_ATT_MTU_DEFAULT;
  656. add_char_params.write_access = p_lncp_init->write_perm;
  657. add_char_params.cccd_write_access = p_lncp_init->cccd_write_perm;
  658. NRF_LOG_DEBUG("Initialized");
  659. return characteristic_add(p_lncp->service_handle,
  660. &add_char_params,
  661. &p_lncp->ctrlpt_handles);
  662. }