S&B Volcano vaporizer remote control with Pi Pico W
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

ble.c 28KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006
  1. /*
  2. * ble.c
  3. *
  4. * https://github.com/raspberrypi/pico-examples/blob/master/pico_w/bt/standalone/client.c
  5. * https://vanhunteradams.com/Pico/BLE/BTStack_HCI.html
  6. *
  7. * Copyright (c) 2023 Thomas Buck (thomas@xythobuz.de)
  8. *
  9. * This program is free software: you can redistribute it and/or modify
  10. * it under the terms of the GNU General Public License as published by
  11. * the Free Software Foundation, either version 3 of the License, or
  12. * (at your option) any later version.
  13. *
  14. * This program is distributed in the hope that it will be useful,
  15. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  16. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  17. * GNU General Public License for more details.
  18. *
  19. * See <http://www.gnu.org/licenses/>.
  20. */
  21. #include "pico/cyw43_arch.h"
  22. #include "hardware/watchdog.h"
  23. #include "config.h"
  24. #include "log.h"
  25. #include "main.h"
  26. #include "util.h"
  27. #include "ble.h"
  28. #define BLE_READ_TIMEOUT_MS (3 * 500)
  29. #define BLE_SRVC_TIMEOUT_MS (3 * 500)
  30. #define BLE_CHAR_TIMEOUT_MS (3 * 2000)
  31. #define BLE_WRTE_TIMEOUT_MS (3 * 500)
  32. #define BLE_NOTY_TIMEOUT_MS (3 * 500)
  33. #define BLE_MAX_SCAN_AGE_MS (10 * 1000)
  34. #define BLE_MAX_SERVICES 8
  35. #define BLE_MAX_CHARACTERISTICS 8
  36. enum ble_state {
  37. TC_OFF = 0,
  38. TC_IDLE,
  39. TC_W4_SCAN,
  40. TC_W4_CONNECT,
  41. TC_READY,
  42. TC_W4_READ,
  43. TC_READ_COMPLETE,
  44. TC_W4_SERVICE,
  45. TC_W4_CHARACTERISTIC,
  46. TC_W4_WRITE,
  47. TC_WRITE_COMPLETE,
  48. TC_W4_NOTIFY_ENABLE,
  49. TC_NOTIFY_ENABLED,
  50. };
  51. struct ble_characteristic {
  52. bool set;
  53. gatt_client_characteristic_t c;
  54. gatt_client_notification_t n;
  55. };
  56. struct ble_service {
  57. bool set;
  58. gatt_client_service_t service;
  59. struct ble_characteristic chars[BLE_MAX_CHARACTERISTICS];
  60. };
  61. static btstack_packet_callback_registration_t hci_event_callback_registration;
  62. static hci_con_handle_t connection_handle;
  63. static enum ble_state state = TC_OFF;
  64. static struct ble_scan_result scans[BLE_MAX_SCAN_RESULTS] = {0};
  65. static uint16_t read_len = 0;
  66. static uint8_t data_buff[BLE_MAX_VALUE_LEN] = {0};
  67. static uint16_t value_handle = 0;
  68. static struct ble_service services[BLE_MAX_SERVICES] = {0};
  69. static uint8_t service_idx = 0;
  70. static uint8_t characteristic_idx = 0;
  71. static void hci_add_scan_result(bd_addr_t addr, bd_addr_type_t type, int8_t rssi) {
  72. int unused = -1;
  73. for (uint i = 0; i < BLE_MAX_SCAN_RESULTS; i++) {
  74. if (!scans[i].set) {
  75. if (unused < 0) {
  76. unused = i;
  77. }
  78. continue;
  79. }
  80. if (memcmp(addr, scans[i].addr, sizeof(bd_addr_t)) == 0) {
  81. // already in list, just update changing values
  82. scans[i].time = to_ms_since_boot(get_absolute_time());
  83. scans[i].rssi = rssi;
  84. return;
  85. }
  86. }
  87. if (unused < 0) {
  88. debug("no space in scan results for %s", bd_addr_to_str(addr));
  89. return;
  90. }
  91. debug("new device with addr %s", bd_addr_to_str(addr));
  92. scans[unused].set = true;
  93. scans[unused].time = to_ms_since_boot(get_absolute_time());
  94. memcpy(scans[unused].addr, addr, sizeof(bd_addr_t));
  95. scans[unused].type = type;
  96. scans[unused].rssi = rssi;
  97. scans[unused].name[0] = '\0';
  98. scans[unused].data_len = 0;
  99. }
  100. static void hci_scan_result_add_name(bd_addr_t addr, const uint8_t *data, uint8_t data_size) {
  101. for (uint i = 0; i < BLE_MAX_SCAN_RESULTS; i++) {
  102. if (!scans[i].set) {
  103. continue;
  104. }
  105. if (memcmp(addr, scans[i].addr, sizeof(bd_addr_t)) != 0) {
  106. continue;
  107. }
  108. uint8_t len = data_size;
  109. if (len > BLE_MAX_NAME_LENGTH) {
  110. len = BLE_MAX_NAME_LENGTH;
  111. }
  112. memcpy(scans[i].name, data, len);
  113. scans[i].name[len] = '\0';
  114. scans[i].time = to_ms_since_boot(get_absolute_time());
  115. return;
  116. }
  117. debug("no matching entry for %s to add name '%.*s' to", bd_addr_to_str(addr), data_size, data);
  118. }
  119. static void hci_scan_result_add_data(bd_addr_t addr, const uint8_t *data, uint8_t data_size) {
  120. for (uint i = 0; i < BLE_MAX_SCAN_RESULTS; i++) {
  121. if (!scans[i].set) {
  122. continue;
  123. }
  124. if (memcmp(addr, scans[i].addr, sizeof(bd_addr_t)) != 0) {
  125. continue;
  126. }
  127. uint8_t len = data_size;
  128. if (len > BLE_MAX_DATA_LENGTH) {
  129. len = BLE_MAX_DATA_LENGTH;
  130. }
  131. memcpy(scans[i].data, data, len);
  132. scans[i].data_len = len;
  133. scans[i].time = to_ms_since_boot(get_absolute_time());
  134. return;
  135. }
  136. debug("no matching entry for %s to add data to", bd_addr_to_str(addr));
  137. }
  138. static void hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
  139. UNUSED(size);
  140. UNUSED(channel);
  141. //debug("type=0x%02X size=%d", packet_type, size);
  142. //hexdump(packet, size);
  143. if (packet_type != HCI_EVENT_PACKET) {
  144. //debug("unexpected packet 0x%02X", packet_type);
  145. return;
  146. }
  147. switch (hci_event_packet_get_type(packet)) {
  148. case BTSTACK_EVENT_STATE:
  149. if (btstack_event_state_get_state(packet) == HCI_STATE_WORKING) {
  150. bd_addr_t local_addr;
  151. gap_local_bd_addr(local_addr);
  152. debug("BTstack up with %s", bd_addr_to_str(local_addr));
  153. state = TC_IDLE;
  154. } else {
  155. debug("BTstack down (%d)", btstack_event_state_get_state(packet));
  156. state = TC_OFF;
  157. }
  158. break;
  159. case GAP_EVENT_ADVERTISING_REPORT: {
  160. if (state != TC_W4_SCAN) {
  161. debug("scan result in invalid state %d", state);
  162. return;
  163. }
  164. bd_addr_t addr;
  165. gap_event_advertising_report_get_address(packet, addr);
  166. bd_addr_type_t type;
  167. type = gap_event_advertising_report_get_address_type(packet);
  168. int8_t rssi;
  169. rssi = (int8_t)gap_event_advertising_report_get_rssi(packet);
  170. // add data received so far
  171. hci_add_scan_result(addr, type, rssi);
  172. // get advertisement from report event
  173. const uint8_t *adv_data = gap_event_advertising_report_get_data(packet);
  174. uint8_t adv_len = gap_event_advertising_report_get_data_length(packet);
  175. // iterate over advertisement data
  176. ad_context_t context;
  177. for (ad_iterator_init(&context, adv_len, adv_data);
  178. ad_iterator_has_more(&context);
  179. ad_iterator_next(&context)) {
  180. uint8_t data_type = ad_iterator_get_data_type(&context);
  181. uint8_t data_size = ad_iterator_get_data_len(&context);
  182. const uint8_t *data = ad_iterator_get_data(&context);
  183. switch (data_type) {
  184. case BLUETOOTH_DATA_TYPE_SHORTENED_LOCAL_NAME:
  185. case BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME:
  186. hci_scan_result_add_name(addr, data, data_size);
  187. break;
  188. case BLUETOOTH_DATA_TYPE_SERVICE_DATA:
  189. case BLUETOOTH_DATA_TYPE_MANUFACTURER_SPECIFIC_DATA:
  190. hci_scan_result_add_data(addr, data, data_size);
  191. break;
  192. default:
  193. //debug("Unexpected advertisement type 0x%02X from %s", data_type, bd_addr_to_str(addr));
  194. //hexdump(data, data_size);
  195. break;
  196. }
  197. }
  198. break;
  199. }
  200. case HCI_EVENT_LE_META:
  201. switch (hci_event_le_meta_get_subevent_code(packet)) {
  202. case HCI_SUBEVENT_LE_CONNECTION_COMPLETE:
  203. if (state != TC_W4_CONNECT) {
  204. return;
  205. }
  206. debug("connection complete");
  207. connection_handle = hci_subevent_le_connection_complete_get_connection_handle(packet);
  208. state = TC_READY;
  209. break;
  210. default:
  211. //debug("unexpected LE meta event 0x%02X", hci_event_le_meta_get_subevent_code(packet));
  212. break;
  213. }
  214. break;
  215. case HCI_EVENT_DISCONNECTION_COMPLETE:
  216. debug("disconnected");
  217. connection_handle = HCI_CON_HANDLE_INVALID;
  218. state = TC_IDLE;
  219. break;
  220. case GATT_EVENT_CHARACTERISTIC_VALUE_QUERY_RESULT:
  221. if (state != TC_W4_READ) {
  222. debug("gatt value query result in invalid state %d", state);
  223. return;
  224. }
  225. uint16_t len = gatt_event_characteristic_value_query_result_get_value_length(packet);
  226. if ((read_len + len) > BLE_MAX_VALUE_LEN) {
  227. debug("not enough space for value (%d + %d > %d)", read_len, len, BLE_MAX_VALUE_LEN);
  228. return;
  229. }
  230. memcpy(data_buff + read_len,
  231. gatt_event_characteristic_value_query_result_get_value(packet),
  232. len);
  233. read_len += len;
  234. break;
  235. case GATT_EVENT_SERVICE_QUERY_RESULT:
  236. if (state != TC_W4_SERVICE) {
  237. debug("gatt service query result in invalid state %d", state);
  238. return;
  239. }
  240. gatt_event_service_query_result_get_service(packet, &services[service_idx].service);
  241. //debug("got service %s result", uuid128_to_str(services[service_idx].service.uuid128));
  242. break;
  243. case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT:
  244. if (state != TC_W4_CHARACTERISTIC) {
  245. debug("gatt characteristic query result in invalid state %d", state);
  246. return;
  247. }
  248. gatt_event_characteristic_query_result_get_characteristic(packet, &services[service_idx].chars[characteristic_idx].c);
  249. //debug("got characteristic %s result", uuid128_to_str(services[service_idx].chars[characteristic_idx].c.uuid128));
  250. break;
  251. case GATT_EVENT_QUERY_COMPLETE: {
  252. uint8_t att_status = gatt_event_query_complete_get_att_status(packet);
  253. if (att_status != ATT_ERROR_SUCCESS){
  254. debug("query result has ATT Error 0x%02x in %d", att_status, state);
  255. state = TC_READY;
  256. break;
  257. }
  258. switch (state) {
  259. case TC_W4_READ:
  260. state = TC_READ_COMPLETE;
  261. break;
  262. case TC_W4_SERVICE:
  263. //debug("service %s complete", uuid128_to_str(services[service_idx].service.uuid128));
  264. state = TC_READY;
  265. break;
  266. case TC_W4_CHARACTERISTIC:
  267. //debug("characteristic %s complete", uuid128_to_str(services[service_idx].chars[characteristic_idx].c.uuid128));
  268. state = TC_READY;
  269. break;
  270. case TC_W4_WRITE:
  271. //debug("write complete");
  272. state = TC_WRITE_COMPLETE;
  273. break;
  274. case TC_W4_NOTIFY_ENABLE:
  275. //debug("notify enable complete");
  276. state = TC_NOTIFY_ENABLED;
  277. break;
  278. default:
  279. debug("gatt query complete in invalid state %d", state);
  280. break;
  281. }
  282. break;
  283. }
  284. case GATT_EVENT_NOTIFICATION: {
  285. if ((state != TC_READY) && (state != TC_WRITE_COMPLETE)) {
  286. debug("gatt notification in invalid state %d", state);
  287. return;
  288. }
  289. uint16_t value_length = gatt_event_notification_get_value_length(packet);
  290. const uint8_t *value = gatt_event_notification_get_value(packet);
  291. value_handle = gatt_event_notification_get_value_handle(packet);
  292. if ((read_len + value_length) <= BLE_MAX_VALUE_LEN) {
  293. memcpy(data_buff + read_len, value, value_length);
  294. read_len += value_length;
  295. }
  296. break;
  297. }
  298. default:
  299. //debug("unexpected event 0x%02X", hci_event_packet_get_type(packet));
  300. break;
  301. }
  302. }
  303. void ble_init(void) {
  304. cyw43_thread_enter();
  305. state = TC_OFF;
  306. for (uint i = 0; i < BLE_MAX_SCAN_RESULTS; i++) {
  307. scans[i].set = false;
  308. }
  309. for (uint i = 0; i < BLE_MAX_SERVICES; i++) {
  310. services[i].set = false;
  311. for (uint j = 0; j < BLE_MAX_CHARACTERISTICS; j++) {
  312. services[i].chars[j].set = false;
  313. }
  314. }
  315. cyw43_thread_exit();
  316. l2cap_init();
  317. sm_init();
  318. sm_set_io_capabilities(IO_CAPABILITY_NO_INPUT_NO_OUTPUT);
  319. gatt_client_init();
  320. hci_event_callback_registration.callback = &hci_event_handler;
  321. hci_add_event_handler(&hci_event_callback_registration);
  322. hci_power_control(HCI_POWER_ON);
  323. }
  324. bool ble_is_ready(void) {
  325. cyw43_thread_enter();
  326. bool v = (state != TC_OFF);
  327. cyw43_thread_exit();
  328. return v;
  329. }
  330. void ble_scan(enum ble_scan_mode mode) {
  331. cyw43_thread_enter();
  332. if (state == TC_OFF) {
  333. cyw43_thread_exit();
  334. return;
  335. }
  336. switch (mode) {
  337. case BLE_SCAN_OFF:
  338. debug("stopping BLE scan");
  339. gap_stop_scan();
  340. state = TC_IDLE;
  341. break;
  342. case BLE_SCAN_ON:
  343. debug("starting BLE scan");
  344. state = TC_W4_SCAN;
  345. gap_set_scan_parameters(1, 0x0030, 0x0030);
  346. gap_start_scan();
  347. break;
  348. case BLE_SCAN_TOGGLE:
  349. switch (state) {
  350. case TC_W4_SCAN:
  351. cyw43_thread_exit();
  352. ble_scan(0);
  353. return;
  354. case TC_IDLE:
  355. cyw43_thread_exit();
  356. ble_scan(1);
  357. return;
  358. default:
  359. debug("invalid state %d", state);
  360. break;
  361. }
  362. break;
  363. default:
  364. debug("invalid mode %d", mode);
  365. break;
  366. }
  367. cyw43_thread_exit();
  368. }
  369. int32_t ble_get_scan_results(struct ble_scan_result *buf, uint16_t len) {
  370. if (!buf || (len <= 0)) {
  371. return -1;
  372. }
  373. cyw43_thread_enter();
  374. if (state == TC_OFF) {
  375. cyw43_thread_exit();
  376. return -1;
  377. }
  378. uint16_t pos = 0;
  379. for (uint16_t i = 0; i < BLE_MAX_SCAN_RESULTS; i++) {
  380. if (!scans[i].set) {
  381. continue;
  382. }
  383. // only age out entries while scanning, otherwise keep results cached
  384. if (state == TC_W4_SCAN) {
  385. uint32_t diff = to_ms_since_boot(get_absolute_time()) - scans[i].time;
  386. if (diff >= BLE_MAX_SCAN_AGE_MS) {
  387. //debug("removing %s due to age", bd_addr_to_str(scans[i].addr));
  388. scans[i].set = false;
  389. }
  390. }
  391. memcpy(buf + pos, scans + i, sizeof(struct ble_scan_result));
  392. pos++;
  393. if (pos >= len) {
  394. break;
  395. }
  396. }
  397. cyw43_thread_exit();
  398. return pos;
  399. }
  400. void ble_connect(bd_addr_t addr, bd_addr_type_t type) {
  401. cyw43_thread_enter();
  402. switch (state) {
  403. case TC_W4_SCAN:
  404. cyw43_thread_exit();
  405. ble_scan(0);
  406. cyw43_thread_enter();
  407. break;
  408. case TC_READY:
  409. gap_disconnect(connection_handle);
  410. break;
  411. case TC_IDLE:
  412. break;
  413. default:
  414. debug("invalid state for connect %d", state);
  415. cyw43_thread_exit();
  416. return;
  417. }
  418. debug("connecting to %s", bd_addr_to_str(addr));
  419. state = TC_W4_CONNECT;
  420. gap_connect(addr, type);
  421. cyw43_thread_exit();
  422. }
  423. bool ble_is_connected(void) {
  424. cyw43_thread_enter();
  425. bool v = (state == TC_READY)
  426. || (state == TC_W4_READ)
  427. || (state == TC_READ_COMPLETE)
  428. || (state == TC_W4_SERVICE)
  429. || (state == TC_W4_CHARACTERISTIC)
  430. || (state == TC_W4_WRITE)
  431. || (state == TC_WRITE_COMPLETE);
  432. cyw43_thread_exit();
  433. return v;
  434. }
  435. void ble_disconnect(void) {
  436. cyw43_thread_enter();
  437. if (state == TC_READY) {
  438. debug("disconnecting");
  439. gap_disconnect(connection_handle);
  440. } else {
  441. debug("invalid state for disconnect %d", state);
  442. }
  443. cyw43_thread_exit();
  444. }
  445. int32_t ble_read(const uint8_t *characteristic, uint8_t *buff, uint16_t buff_len) {
  446. cyw43_thread_enter();
  447. if (state != TC_READY) {
  448. cyw43_thread_exit();
  449. debug("invalid state for read (%d)", state);
  450. return -1;
  451. }
  452. uint8_t r = gatt_client_read_value_of_characteristics_by_uuid128(hci_event_handler,
  453. connection_handle,
  454. 0x0001, 0xFFFF,
  455. characteristic);
  456. if (r != ERROR_CODE_SUCCESS) {
  457. cyw43_thread_exit();
  458. debug("gatt read failed %d", r);
  459. return -2;
  460. }
  461. state = TC_W4_READ;
  462. read_len = 0;
  463. cyw43_thread_exit();
  464. uint32_t start_time = to_ms_since_boot(get_absolute_time());
  465. while (1) {
  466. sleep_ms(1);
  467. main_loop_hw();
  468. uint32_t now = to_ms_since_boot(get_absolute_time());
  469. if ((now - start_time) >= BLE_READ_TIMEOUT_MS) {
  470. debug("timeout waiting for read");
  471. cyw43_thread_enter();
  472. state = TC_READY;
  473. cyw43_thread_exit();
  474. return -3;
  475. }
  476. cyw43_thread_enter();
  477. enum ble_state state_cached = state;
  478. cyw43_thread_exit();
  479. if (state_cached == TC_READ_COMPLETE) {
  480. break;
  481. }
  482. }
  483. cyw43_thread_enter();
  484. state = TC_READY;
  485. if (read_len > buff_len) {
  486. debug("buffer too short (%d < %d)", buff_len, read_len);
  487. cyw43_thread_exit();
  488. return -4;
  489. }
  490. memcpy(buff, data_buff, read_len);
  491. uint16_t tmp = read_len;
  492. read_len = 0;
  493. cyw43_thread_exit();
  494. return tmp;
  495. }
  496. static int discover_service(const uint8_t *service) {
  497. // check if service has already been discovered
  498. int srvc = -1, free_srvc = -1;
  499. for (int i = 0; i < BLE_MAX_SERVICES; i++) {
  500. if (!services[i].set) {
  501. if (free_srvc < 0) {
  502. free_srvc = i;
  503. }
  504. continue;
  505. }
  506. if (memcmp(services[i].service.uuid128, service, 16) == 0) {
  507. srvc = i;
  508. break;
  509. }
  510. }
  511. // if this service has not been discovered yet, add it
  512. if (srvc < 0) {
  513. if (free_srvc < 0) {
  514. debug("no space left for BLE service. overwriting.");
  515. free_srvc = 0;
  516. }
  517. srvc = free_srvc;
  518. services[srvc].set = true;
  519. debug("discovering service %s at %d", uuid128_to_str(service), srvc);
  520. uint8_t r = gatt_client_discover_primary_services_by_uuid128(hci_event_handler,
  521. connection_handle,
  522. service);
  523. if (r != ERROR_CODE_SUCCESS) {
  524. cyw43_thread_exit();
  525. debug("gatt service discovery failed %d", r);
  526. return -2;
  527. }
  528. state = TC_W4_SERVICE;
  529. service_idx = srvc;
  530. cyw43_thread_exit();
  531. uint32_t start_time = to_ms_since_boot(get_absolute_time());
  532. while (1) {
  533. sleep_ms(1);
  534. main_loop_hw();
  535. uint32_t now = to_ms_since_boot(get_absolute_time());
  536. if ((now - start_time) >= BLE_SRVC_TIMEOUT_MS) {
  537. debug("timeout waiting for service");
  538. cyw43_thread_enter();
  539. state = TC_READY;
  540. cyw43_thread_exit();
  541. return -3;
  542. }
  543. cyw43_thread_enter();
  544. enum ble_state state_cached = state;
  545. cyw43_thread_exit();
  546. if (state_cached == TC_READY) {
  547. break;
  548. }
  549. }
  550. cyw43_thread_enter();
  551. }
  552. return srvc;
  553. }
  554. static int discover_characteristic(int srvc, const uint8_t *characteristic) {
  555. // check if characteristic has already been discovered
  556. int ch = -1, free_ch = -1;
  557. for (int i = 0; i < BLE_MAX_CHARACTERISTICS; i++) {
  558. if (!services[srvc].chars[i].set) {
  559. if (free_ch < 0) {
  560. free_ch = i;
  561. }
  562. continue;
  563. }
  564. if (memcmp(services[srvc].chars[i].c.uuid128, characteristic, 16) == 0) {
  565. ch = i;
  566. break;
  567. }
  568. }
  569. // if this characteristic has not been discovered yet, add it
  570. if (ch < 0) {
  571. if (free_ch < 0) {
  572. debug("no space left for BLE characteristic. overwriting.");
  573. free_ch = 0;
  574. }
  575. ch = free_ch;
  576. services[srvc].chars[ch].set = true;
  577. debug("discovering characteristic %s at %d", uuid128_to_str(characteristic), ch);
  578. uint8_t r = gatt_client_discover_characteristics_for_service_by_uuid128(hci_event_handler,
  579. connection_handle,
  580. &services[srvc].service,
  581. characteristic);
  582. if (r != ERROR_CODE_SUCCESS) {
  583. cyw43_thread_exit();
  584. debug("gatt characteristic discovery failed %d", r);
  585. return -4;
  586. }
  587. state = TC_W4_CHARACTERISTIC;
  588. characteristic_idx = ch;
  589. cyw43_thread_exit();
  590. uint32_t start_time = to_ms_since_boot(get_absolute_time());
  591. while (1) {
  592. sleep_ms(1);
  593. main_loop_hw();
  594. uint32_t now = to_ms_since_boot(get_absolute_time());
  595. if ((now - start_time) >= BLE_CHAR_TIMEOUT_MS) {
  596. debug("timeout waiting for characteristic");
  597. cyw43_thread_enter();
  598. state = TC_READY;
  599. cyw43_thread_exit();
  600. return -5;
  601. }
  602. cyw43_thread_enter();
  603. enum ble_state state_cached = state;
  604. cyw43_thread_exit();
  605. if (state_cached == TC_READY) {
  606. break;
  607. }
  608. }
  609. cyw43_thread_enter();
  610. }
  611. return ch;
  612. }
  613. int8_t ble_write(const uint8_t *service, const uint8_t *characteristic,
  614. const uint8_t *buff, uint16_t buff_len) {
  615. cyw43_thread_enter();
  616. if (state != TC_READY) {
  617. cyw43_thread_exit();
  618. debug("invalid state for write (%d)", state);
  619. return -1;
  620. }
  621. int srvc = discover_service(service);
  622. if (srvc < 0) {
  623. debug("error discovering service (%d)", srvc);
  624. return srvc;
  625. }
  626. int ch = discover_characteristic(srvc, characteristic);
  627. if (ch < 0) {
  628. debug("error discovering characteristic (%d)", ch);
  629. return ch;
  630. }
  631. if (buff_len > BLE_MAX_VALUE_LEN) {
  632. buff_len = BLE_MAX_VALUE_LEN;
  633. }
  634. memcpy(data_buff, buff, buff_len);
  635. uint8_t r = gatt_client_write_value_of_characteristic(hci_event_handler,
  636. connection_handle,
  637. services[srvc].chars[ch].c.value_handle,
  638. buff_len, data_buff);
  639. if (r != ERROR_CODE_SUCCESS) {
  640. cyw43_thread_exit();
  641. debug("gatt write failed %d", r);
  642. return -6;
  643. }
  644. state = TC_W4_WRITE;
  645. cyw43_thread_exit();
  646. uint32_t start_time = to_ms_since_boot(get_absolute_time());
  647. while (1) {
  648. sleep_ms(1);
  649. main_loop_hw();
  650. uint32_t now = to_ms_since_boot(get_absolute_time());
  651. if ((now - start_time) >= BLE_WRTE_TIMEOUT_MS) {
  652. debug("timeout waiting for write");
  653. cyw43_thread_enter();
  654. state = TC_READY;
  655. cyw43_thread_exit();
  656. return -7;
  657. }
  658. cyw43_thread_enter();
  659. enum ble_state state_cached = state;
  660. cyw43_thread_exit();
  661. if ((state_cached == TC_WRITE_COMPLETE) || (state_cached == TC_READY)) {
  662. break;
  663. }
  664. }
  665. cyw43_thread_enter();
  666. int8_t ret = (state == TC_WRITE_COMPLETE) ? 0 : -8;
  667. state = TC_READY;
  668. cyw43_thread_exit();
  669. return ret;
  670. }
  671. int8_t ble_discover(const uint8_t *service, const uint8_t *characteristic) {
  672. cyw43_thread_enter();
  673. if (state != TC_READY) {
  674. cyw43_thread_exit();
  675. debug("invalid state for discovery (%d)", state);
  676. return -1;
  677. }
  678. int srvc = discover_service(service);
  679. if (srvc < 0) {
  680. debug("error discovering service (%d)", srvc);
  681. return srvc;
  682. }
  683. int ch = discover_characteristic(srvc, characteristic);
  684. if (ch < 0) {
  685. debug("error discovering characteristic (%d)", ch);
  686. return ch;
  687. }
  688. cyw43_thread_exit();
  689. return 0;
  690. }
  691. int8_t ble_notification_disable(const uint8_t *service, const uint8_t *characteristic) {
  692. cyw43_thread_enter();
  693. if (state != TC_READY) {
  694. cyw43_thread_exit();
  695. debug("invalid state for notify (%d)", state);
  696. return -1;
  697. }
  698. int srvc = discover_service(service);
  699. if (srvc < 0) {
  700. debug("error discovering service (%d)", srvc);
  701. return srvc;
  702. }
  703. int ch = discover_characteristic(srvc, characteristic);
  704. if (ch < 0) {
  705. debug("error discovering characteristic (%d)", ch);
  706. return ch;
  707. }
  708. gatt_client_stop_listening_for_characteristic_value_updates(&services[srvc].chars[ch].n);
  709. cyw43_thread_exit();
  710. return 0;
  711. }
  712. int8_t ble_notification_enable(const uint8_t *service, const uint8_t *characteristic) {
  713. cyw43_thread_enter();
  714. if (state != TC_READY) {
  715. cyw43_thread_exit();
  716. debug("invalid state for notify (%d)", state);
  717. return -1;
  718. }
  719. int srvc = discover_service(service);
  720. if (srvc < 0) {
  721. debug("error discovering service (%d)", srvc);
  722. return srvc;
  723. }
  724. int ch = discover_characteristic(srvc, characteristic);
  725. if (ch < 0) {
  726. debug("error discovering characteristic (%d)", ch);
  727. return ch;
  728. }
  729. gatt_client_listen_for_characteristic_value_updates(&services[srvc].chars[ch].n,
  730. hci_event_handler,
  731. connection_handle,
  732. &services[srvc].chars[ch].c);
  733. gatt_client_write_client_characteristic_configuration(hci_event_handler,
  734. connection_handle,
  735. &services[srvc].chars[ch].c,
  736. GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION);
  737. state = TC_W4_NOTIFY_ENABLE;
  738. cyw43_thread_exit();
  739. uint32_t start_time = to_ms_since_boot(get_absolute_time());
  740. while (1) {
  741. sleep_ms(1);
  742. main_loop_hw();
  743. uint32_t now = to_ms_since_boot(get_absolute_time());
  744. if ((now - start_time) >= BLE_NOTY_TIMEOUT_MS) {
  745. debug("timeout waiting for notify enable");
  746. cyw43_thread_enter();
  747. state = TC_READY;
  748. cyw43_thread_exit();
  749. return -6;
  750. }
  751. cyw43_thread_enter();
  752. enum ble_state state_cached = state;
  753. cyw43_thread_exit();
  754. if ((state_cached == TC_NOTIFY_ENABLED) || (state_cached == TC_READY)) {
  755. break;
  756. }
  757. }
  758. cyw43_thread_enter();
  759. int8_t ret = (state == TC_NOTIFY_ENABLED) ? 0 : -7;
  760. state = TC_READY;
  761. cyw43_thread_exit();
  762. return ret;
  763. }
  764. bool ble_notification_ready(void) {
  765. cyw43_thread_enter();
  766. if (state != TC_READY) {
  767. cyw43_thread_exit();
  768. debug("invalid state for notify (%d)", state);
  769. return -1;
  770. }
  771. uint16_t tmp = read_len;
  772. cyw43_thread_exit();
  773. return (tmp > 0);
  774. }
  775. uint16_t ble_notification_get(uint8_t *buff, uint16_t buff_len, uint8_t *characteristic) {
  776. if ((buff == NULL) || (characteristic == NULL) || (buff_len <= 0)) {
  777. debug("invalid params");
  778. return -1;
  779. }
  780. cyw43_thread_enter();
  781. if (state != TC_READY) {
  782. cyw43_thread_exit();
  783. debug("invalid state for notify (%d)", state);
  784. return -2;
  785. }
  786. if (read_len <= 0) {
  787. debug("no data available");
  788. cyw43_thread_exit();
  789. return -3;
  790. }
  791. if (read_len > buff_len) {
  792. debug("buffer too short (%d < %d)", buff_len, read_len);
  793. cyw43_thread_exit();
  794. return -4;
  795. }
  796. memcpy(buff, data_buff, read_len);
  797. bool found = false;
  798. for (int i = 0; i < BLE_MAX_SERVICES; i++) {
  799. if (!services[i].set) {
  800. continue;
  801. }
  802. for (int j = 0; j < BLE_MAX_CHARACTERISTICS; j++) {
  803. if (!services[i].chars[j].set) {
  804. continue;
  805. }
  806. if (services[i].chars[j].c.value_handle == value_handle) {
  807. memcpy(characteristic, services[i].chars[j].c.uuid128, 16);
  808. found = true;
  809. break;
  810. }
  811. }
  812. }
  813. if (!found) {
  814. debug("can not find characteristic for value handle 0x%04X", value_handle);
  815. memset(characteristic, 0, 16);
  816. }
  817. uint16_t tmp = read_len;
  818. read_len = 0;
  819. cyw43_thread_exit();
  820. return tmp;
  821. }