Implement a GATT server

CMakeLists.txt

@@ -9,20 +9,26 @@ pico_sdk_init()
 
 include_directories(
     ${CMAKE_SOURCE_DIR}/lib/rtt/RTT
+    ${CMAKE_SOURCE_DIR}/src
 )
 
 add_executable(vibe-check
-    lib/rtt/RTT/SEGGER_RTT.c
+    # BTStack seems to embed and link against RTT(!)
+    # lib/rtt/RTT/SEGGER_RTT.c
     src/stdio_rtt.c
     src/main.c
+    src/vibe_bt.c
 )
 
-# Add pico_stdlib library which aggregates commonly used features
 target_link_libraries(vibe-check
     pico_stdlib
     pico_cyw43_arch_none
+    pico_btstack_ble
+    pico_btstack_cyw43
 )
 
+pico_btstack_make_gatt_header(vibe-check PRIVATE "${CMAKE_SOURCE_DIR}/src/vibe.gatt")
+
 # enable debug logging to USB and UART
 pico_enable_stdio_usb(vibe-check 1)
 pico_enable_stdio_uart(vibe-check 1)

README.md

@@ -1,8 +1,22 @@
+# Bluetooth mod for the Doxy Massager
+
+Connections: TBD
+
+We're pretending to be a Sakuraneko Dokidoki:
+ * BTLE GATT Service `0000ffe0-0000-1000-8000-00805f9b34fb`
+ * BTLE GATT Characteristic `0000ffe1-0000-1000-8000-00805f9b34fb`
+ * [buttplug.io's implementation](https://github.com/buttplugio/buttplug/blob/master/buttplug/src/server/device/protocol/sakuraneko.rs)
+
+The implementation is simply complete enough to satisfy buttplug.io; it almost certainly
+doesn't work with Sakuraneko's app. We just picked this one to emulate because the protocol
+was trivial.
+
 ## Build
 
 ```
-# env -C lib/pico-sdk/ git submodule update --init
-# env -C build/ cmake .. -G Ninja -DPICO_BOARD=pico_w
-# env -C build/ ninja
-# picotool load -f build/vibe-check.uf2
+$ env -C lib/pico-sdk/ git submodule update --init
+$ env -C build/ cmake .. -G Ninja -DPICO_BOARD=pico_w
+$ env -C build/ ninja
+$ picotool load -f build/vibe-check.uf2
+# (or your preferred way of loading things onto a Pico)
 ```

src/btstack_config.h

@@ -0,0 +1,52 @@
+#ifndef BTSTACK_CONFIG_H
+#define BTSTACK_CONFIG_H
+
+#ifndef ENABLE_BLE
+#error Please link to pico_btstack_ble
+#endif
+
+// BTstack features that can be enabled
+#define ENABLE_LE_PERIPHERAL
+#define ENABLE_LOG_INFO
+#define ENABLE_LOG_ERROR
+#define ENABLE_PRINTF_HEXDUMP
+#define ENABLE_SEGGER_RTT
+#define MAX_NR_GATT_CLIENTS 0
+
+// BTstack configuration. buffers, sizes, ...
+#define HCI_OUTGOING_PRE_BUFFER_SIZE 4
+#define HCI_ACL_PAYLOAD_SIZE (255 + 4)
+#define HCI_ACL_CHUNK_SIZE_ALIGNMENT 4
+#define MAX_NR_HCI_CONNECTIONS 1
+#define MAX_NR_SM_LOOKUP_ENTRIES 3
+#define MAX_NR_WHITELIST_ENTRIES 16
+#define MAX_NR_LE_DEVICE_DB_ENTRIES 16
+
+// Limit number of ACL/SCO Buffer to use by stack to avoid cyw43 shared bus overrun
+#define MAX_NR_CONTROLLER_ACL_BUFFERS 3
+#define MAX_NR_CONTROLLER_SCO_PACKETS 3
+
+// Enable and configure HCI Controller to Host Flow Control to avoid cyw43 shared bus overrun
+#define ENABLE_HCI_CONTROLLER_TO_HOST_FLOW_CONTROL
+#define HCI_HOST_ACL_PACKET_LEN (255+4)
+#define HCI_HOST_ACL_PACKET_NUM 3
+#define HCI_HOST_SCO_PACKET_LEN 120
+#define HCI_HOST_SCO_PACKET_NUM 3
+
+// Link Key DB and LE Device DB using TLV on top of Flash Sector interface
+#define NVM_NUM_DEVICE_DB_ENTRIES 16
+#define NVM_NUM_LINK_KEYS 16
+
+// We don't give btstack a malloc, so use a fixed-size ATT DB.
+#define MAX_ATT_DB_SIZE 512
+
+// BTstack HAL configuration
+#define HAVE_EMBEDDED_TIME_MS
+// map btstack_assert onto Pico SDK assert()
+#define HAVE_ASSERT
+// Some USB dongles take longer to respond to HCI reset (e.g. BCM20702A).
+#define HCI_RESET_RESEND_TIMEOUT_MS 1000
+#define ENABLE_SOFTWARE_AES128
+#define ENABLE_MICRO_ECC_FOR_LE_SECURE_CONNECTIONS
+
+#endif

src/main.c

@@ -1,32 +1,58 @@
 #include <stdio.h>
-#include "pico/stdlib.h"
+#include "btstack.h"
+#include "hci_dump.h"
+#include "hci_dump_segger_rtt_stdout.h"
 #include "pico/cyw43_arch.h"
+#include "pico/btstack_cyw43.h"
+#include "pico/stdlib.h"
 #include "stdio_rtt.h"
-
-// #include "btstack.h"
+#include "vibe_bt.h"
 
 #ifndef CYW43_WL_GPIO_LED_PIN
     #error no WiFi LED on board, wrong -DPICO_BOARD?
 #endif
 
-// Sakuraneko Dokidoki - sakuraneko-03  - 0000ffe1-0000-1000-8000-00805f9b34fb
-// https://github.com/buttplugio/buttplug/blob/master/buttplug/src/server/device/protocol/sakuraneko.rs
+#define HEARTBEAT_PERIOD_MS 100
+
+static btstack_timer_source_t heartbeat;
+static btstack_packet_callback_registration_t hci_event_callback_registration;
+
+static void heartbeat_handler(struct btstack_timer_source *ts) {
+    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, bt_connected);
+
+    // Restart timer
+    btstack_run_loop_set_timer(ts, HEARTBEAT_PERIOD_MS);
+    btstack_run_loop_add_timer(ts);
+}
 
 int main() {
     stdio_init_all();
     stdio_rtt_init();
 
     if (cyw43_arch_init()) {
-        printf("WiFi init failed");
+        printf("CYW43 init failed");
         return -1;
     }
-    while (true) {
-        printf("Hello, world!\n");
-        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
-        sleep_ms(1000);
-        printf("Goodbye, world!\n");
-        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 0);
-        sleep_ms(1000);
-    }
+
+    // hci_dump_init(hci_dump_segger_rtt_stdout_get_instance());
+    l2cap_init();
+    sm_init();
+
+    att_server_init(profile_data, bt_att_read_callback, bt_att_write_callback);    
+    
+    hci_event_callback_registration.callback = &bt_packet_handler;
+    hci_add_event_handler(&hci_event_callback_registration);
+    // register for ATT event
+    att_server_register_packet_handler(bt_packet_handler);
+
+    // set one-shot btstack timer
+    heartbeat.process = &heartbeat_handler;
+    btstack_run_loop_set_timer(&heartbeat, HEARTBEAT_PERIOD_MS);
+    btstack_run_loop_add_timer(&heartbeat);
+
+    // turn on bluetooth!
+    hci_power_control(HCI_POWER_ON);
+
+    btstack_run_loop_execute();
     return 0;
 }

src/vibe.gatt

@@ -0,0 +1,8 @@
+PRIMARY_SERVICE, GAP_SERVICE
+CHARACTERISTIC, GAP_DEVICE_NAME, READ, "sakuraneko-03"
+
+PRIMARY_SERVICE, GATT_SERVICE
+CHARACTERISTIC, GATT_DATABASE_HASH, READ,
+
+PRIMARY_SERVICE, 0000ffe0-0000-1000-8000-00805f9b34fb
+CHARACTERISTIC, 0000ffe1-0000-1000-8000-00805f9b34fb, WRITE_WITHOUT_RESPONSE | DYNAMIC,

src/vibe_bt.c

@@ -0,0 +1,98 @@
+#include <stdio.h>
+#include "vibe_bt.h"
+#include "vibe.h"
+
+#define APP_AD_FLAGS 0x06
+static const uint8_t adv_data[] = {
+    // Flags general discoverable
+    0x02, BLUETOOTH_DATA_TYPE_FLAGS, APP_AD_FLAGS,
+    // Name
+    0x0E, BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME, 's', 'a', 'k', 'u', 'r', 'a', 'n', 'e', 'k', 'o', '-', '0', '3',
+};
+static const uint8_t adv_data_len = sizeof(adv_data);
+
+bool bt_connected = false;
+
+void bt_packet_handler(
+	uint8_t packet_type, 
+	uint16_t channel, 
+	uint8_t *packet, 
+	uint16_t size)
+{
+    UNUSED(size);
+    UNUSED(channel);
+    bd_addr_t local_addr;
+    if (packet_type != HCI_EVENT_PACKET) return;
+
+    uint8_t event_type = hci_event_packet_get_type(packet);
+    switch(event_type){
+    case BTSTACK_EVENT_STATE:
+        if (btstack_event_state_get_state(packet) != HCI_STATE_WORKING) return;
+        gap_local_bd_addr(local_addr);
+        printf("BTstack up and running on %s.\n", bd_addr_to_str(local_addr));
+
+        // setup advertisements
+        uint16_t adv_int_min = 800;
+        uint16_t adv_int_max = 800;
+        uint8_t adv_type = 0;
+        bd_addr_t null_addr;
+        memset(null_addr, 0, 6);
+        gap_advertisements_set_params(adv_int_min, adv_int_max, adv_type, 0, null_addr, 0x07, 0x00);
+        assert(adv_data_len <= 31); // ble limitation
+        gap_advertisements_set_data(adv_data_len, (uint8_t*) adv_data);
+        gap_advertisements_enable(1);
+        break;
+    case HCI_EVENT_LE_META:
+		switch (hci_event_le_meta_get_subevent_code(packet)) {
+		case HCI_SUBEVENT_LE_CONNECTION_COMPLETE:
+			bt_connected = true;
+			break;
+		default:
+			break;
+		}
+		break;
+    case HCI_EVENT_DISCONNECTION_COMPLETE:
+    	bt_connected = false;
+        break;
+    default:
+        break;
+    }
+}
+
+uint16_t bt_att_read_callback(
+	hci_con_handle_t connection_handle, 
+	uint16_t att_handle, 
+	uint16_t offset, 
+	uint8_t * buffer, 
+	uint16_t buffer_size)
+{
+	printf("GATT Read %d\n", att_handle);
+	return 0;
+}
+
+int bt_att_write_callback(
+	hci_con_handle_t connection_handle, 
+	uint16_t att_handle, 
+	uint16_t transaction_mode, 
+	uint16_t offset, 
+	uint8_t *buffer, 
+	uint16_t buffer_size)
+{
+	printf("GATT Write %d, %db: ", att_handle, buffer_size);
+	for (int i = 0; i < buffer_size; i++)
+		printf("%02x ", buffer[i]);
+	putchar('\n');
+
+	if (att_handle != ATT_CHARACTERISTIC_0000ffe1_0000_1000_8000_00805f9b34fb_01_VALUE_HANDLE) return 0;
+
+	// Buttplug.io always sends
+	// 0xa1, 0x08, 0x01, 0x00, 0x00, 0x00, 0x64, PERCENTAGE, 0x00, 0x64, 0xdf, 0x55,
+	if (buffer_size != 12) return 0;
+	if (memcmp(buffer, "\xA1\x08\x01\x00\x00\x00\x64", 7) != 0) return 0;
+	if (memcmp(buffer + 8, "\x00\x64\xDF\x55", 4) != 0) return 0;
+
+	// In case it somehow came unset
+	bt_connected = true;
+	printf("!! Write of %d\n", buffer[7]);
+	return 0;
+}

src/vibe_bt.h

@@ -0,0 +1,29 @@
+#ifndef VIBE_BT_H
+#define VIBE_BT_H
+#include "btstack.h"
+
+extern bool bt_connected;
+extern const uint8_t profile_data[];
+
+void bt_packet_handler(
+	uint8_t packet_type, 
+	uint16_t channel, 
+	uint8_t *packet, 
+	uint16_t size);
+
+uint16_t bt_att_read_callback(
+	hci_con_handle_t connection_handle, 
+	uint16_t att_handle, 
+	uint16_t offset, 
+	uint8_t * buffer, 
+	uint16_t buffer_size);
+
+int bt_att_write_callback(
+	hci_con_handle_t connection_handle, 
+	uint16_t att_handle, 
+	uint16_t transaction_mode, 
+	uint16_t offset, 
+	uint8_t *buffer, 
+	uint16_t buffer_size);
+
+#endif