diff --git a/firmware-bluetooth/CMakeLists.txt b/firmware-bluetooth/CMakeLists.txt index 2980c5c6..26a6bbd8 100644 --- a/firmware-bluetooth/CMakeLists.txt +++ b/firmware-bluetooth/CMakeLists.txt @@ -13,6 +13,7 @@ target_include_directories(app PRIVATE ${REMAPPER_SRC}) target_sources(app PRIVATE src/main.cc + src/joycon2.cc ${REMAPPER_SRC}/config.cc ${REMAPPER_SRC}/crc.cc ${REMAPPER_SRC}/descriptor_parser.cc diff --git a/firmware-bluetooth/src/joycon2.cc b/firmware-bluetooth/src/joycon2.cc new file mode 100644 index 00000000..d7387eef --- /dev/null +++ b/firmware-bluetooth/src/joycon2.cc @@ -0,0 +1,214 @@ +#include "joycon2.h" +#include +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(joycon2, LOG_LEVEL_DBG); + +// Joy-Con 2 GATT UUIDs - using manual initialization to avoid macro issues +static const struct bt_uuid_128 joycon2_input_report_uuid = { + .uuid = { BT_UUID_TYPE_128 }, + .val = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } +}; +static const struct bt_uuid_128 joycon2_write_command_uuid = { + .uuid = { BT_UUID_TYPE_128 }, + .val = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } +}; + +// Joy-Con 2 connection management +#define MAX_JOYCON2 2 +struct joycon2_connection joycon2_conns[MAX_JOYCON2]; +int joycon2_count = 0; + +// Device info storage per connection +struct joycon2_device_info joycon2_device_info_per_conn[CONFIG_BT_MAX_CONN] = {0}; + +// Define report_type here since it's needed for the message queue +struct report_type { + uint16_t interface; + uint8_t len; + uint8_t data[65]; +}; + +// Forward declarations for report handling +extern struct k_msgq report_q; + +// Helper function to get 16-bit little-endian value +static inline uint16_t get_le16(const uint8_t* buf) { + return (uint16_t)buf[0] | ((uint16_t)buf[1] << 8); +} + +bool joycon2_is_device(const struct bt_scan_device_info* device_info) { + // Check manufacturer data for Nintendo Joy-Con + // Use the correct Zephyr API to access advertisement data + const struct bt_le_scan_recv_info* recv_info = device_info->recv_info; + + // In Zephyr, advertisement data is accessed through net_buf_simple + // We need to parse the advertisement data to find manufacturer specific data + // For now, let's use a simpler approach - check by device name + char addr[BT_ADDR_LE_STR_LEN]; + bt_addr_le_to_str(recv_info->addr, addr, sizeof(addr)); + + LOG_INF("Checking device at %s for Joy-Con 2", addr); + + // TODO: Implement proper advertisement data parsing using net_buf_simple + // The advertisement data should be accessed through a separate parameter + // in the scan callback function, not through recv_info->data + + // For debugging purposes, let's assume any device with "Joy-Con" in the name + // or Nintendo manufacturer ID is a Joy-Con 2 + // This is a temporary implementation until we can properly parse advertisement data + + // For now, return false to avoid false positives + // TODO: Implement proper advertisement data parsing + return false; +} + +void joycon2_store_device_info(const struct bt_scan_device_info* device_info, uint8_t conn_idx) { + if (conn_idx < CONFIG_BT_MAX_CONN) { + // Store a pointer to the device info (const cast needed for compatibility) + joycon2_device_info_per_conn[conn_idx].device_info = (struct bt_scan_device_info*)device_info; + joycon2_device_info_per_conn[conn_idx].is_joycon2 = true; + LOG_INF("Joy-Con 2 device info stored for connection %d", conn_idx); + } +} + +bool joycon2_is_connection(uint8_t conn_idx) { + return (conn_idx < CONFIG_BT_MAX_CONN && joycon2_device_info_per_conn[conn_idx].is_joycon2); +} + +void joycon2_discover_characteristics(struct bt_conn* conn, void* dm) { + LOG_INF("Joy-Con 2 discovery characteristics called"); + + if (joycon2_count < MAX_JOYCON2) { + joycon2_conns[joycon2_count].conn = conn; + joycon2_conns[joycon2_count].input_handle = 0; + joycon2_conns[joycon2_count].write_handle = 0; + joycon2_conns[joycon2_count].sensors_enabled = false; + + // Parse GATT discovery data to find Joy-Con characteristics + const struct bt_gatt_dm* gatt_dm = (const struct bt_gatt_dm*)dm; + const struct bt_gatt_dm_attr* attr = NULL; + while (NULL != (attr = bt_gatt_dm_attr_next(gatt_dm, attr))) { + if (attr->uuid->type == BT_UUID_TYPE_128) { + const struct bt_uuid_128* uuid = (const struct bt_uuid_128*)attr->uuid; + + // Check for input report characteristic + if (memcmp(uuid->val, JOYCON2_INPUT_REPORT_UUID_VAL, 16) == 0) { + joycon2_conns[joycon2_count].input_handle = attr->handle; + LOG_INF("Found Joy-Con input report characteristic at handle: 0x%04x", attr->handle); + } + + // Check for write command characteristic + if (memcmp(uuid->val, JOYCON2_WRITE_COMMAND_UUID_VAL, 16) == 0) { + joycon2_conns[joycon2_count].write_handle = attr->handle; + LOG_INF("Found Joy-Con write command characteristic at handle: 0x%04x", attr->handle); + } + } + } + + joycon2_count++; + LOG_INF("Joy-Con 2 connection added with input_handle: 0x%04x, write_handle: 0x%04x", + joycon2_conns[joycon2_count-1].input_handle, + joycon2_conns[joycon2_count-1].write_handle); + } +} + +void joycon2_enable_sensors(struct bt_conn* conn, uint16_t write_handle) { + static const uint8_t enable_imu_cmd1[] = { 0x0c, 0x91, 0x01, 0x02, 0x00, 0x04, 0x00, 0x00, 0xFF, 0x00, 0x00, 0x00 }; + static const uint8_t enable_imu_cmd2[] = { 0x0c, 0x91, 0x01, 0x04, 0x00, 0x04, 0x00, 0x00, 0xFF, 0x00, 0x00, 0x00 }; + struct bt_gatt_write_params params = {0}; + int err; + + params.handle = write_handle; + params.offset = 0; + params.data = enable_imu_cmd1; + params.length = sizeof(enable_imu_cmd1); + params.func = NULL; + err = bt_gatt_write(conn, ¶ms); + if (err) { + LOG_ERR("Failed to send Joy-Con 2 IMU enable cmd1: %d", err); + } else { + LOG_INF("Sent Joy-Con 2 IMU enable cmd1"); + } + + k_sleep(K_MSEC(100)); + + params.data = enable_imu_cmd2; + params.length = sizeof(enable_imu_cmd2); + err = bt_gatt_write(conn, ¶ms); + if (err) { + LOG_ERR("Failed to send Joy-Con 2 IMU enable cmd2: %d", err); + } else { + LOG_INF("Sent Joy-Con 2 IMU enable cmd2"); + } + + // Mark sensors as enabled for this connection + for (int i = 0; i < joycon2_count; i++) { + if (joycon2_conns[i].conn == conn) { + joycon2_conns[i].sensors_enabled = true; + break; + } + } +} + +void joycon2_decode_and_handle_report(const uint8_t* data, uint8_t len, uint16_t interface) { + if (len < 2) { + LOG_ERR("[Joy-Con 2] Report too short: %d", len); + return; + } + + uint8_t report_id = data[0]; + LOG_INF("[Joy-Con 2] Received report ID: %d, len: %d, interface: 0x%04x", report_id, len, interface); + + // For debugging, log the first few bytes of the report + if (len >= 8) { + LOG_INF("[Joy-Con 2] Report data: %02x %02x %02x %02x %02x %02x %02x %02x...", + data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]); + } + + // Create a standard HID report structure for the remapper + struct report_type decoded_report; + decoded_report.interface = interface; + decoded_report.len = len; + + // Copy the entire report data as-is + if (len <= sizeof(decoded_report.data)) { + memcpy(decoded_report.data, data, len); + + // Process the report through the normal remapper pipeline + if (k_msgq_put(&report_q, &decoded_report, K_NO_WAIT)) { + LOG_ERR("[Joy-Con 2] Failed to queue report"); + } else { + LOG_INF("[Joy-Con 2] Successfully queued report (len: %d)", len); + } + } else { + LOG_ERR("[Joy-Con 2] Report too large: %d bytes", len); + } +} + +void joycon2_cleanup_connection(uint8_t conn_idx) { + if (conn_idx < CONFIG_BT_MAX_CONN) { + joycon2_device_info_per_conn[conn_idx].device_info = NULL; + joycon2_device_info_per_conn[conn_idx].is_joycon2 = false; + LOG_INF("Joy-Con 2 device info cleared for connection %d", conn_idx); + } +} + +// Helper function to find Joy-Con 2 connection by bt_conn +struct joycon2_connection* joycon2_find_connection(struct bt_conn* conn) { + for (int i = 0; i < joycon2_count; i++) { + if (joycon2_conns[i].conn == conn) { + return &joycon2_conns[i]; + } + } + return NULL; +} + +// Helper function to check if a connection is Joy-Con 2 +bool joycon2_is_connection_by_conn(struct bt_conn* conn) { + return joycon2_find_connection(conn) != NULL; +} \ No newline at end of file diff --git a/firmware-bluetooth/src/joycon2.h b/firmware-bluetooth/src/joycon2.h new file mode 100644 index 00000000..fb49c6dc --- /dev/null +++ b/firmware-bluetooth/src/joycon2.h @@ -0,0 +1,51 @@ +#ifndef JOYCON2_H +#define JOYCON2_H + +#include +#include +#include +#include +#include + +// Joy-Con 2 GATT UUIDs +static const uint8_t JOYCON2_INPUT_REPORT_UUID_VAL[16] = {0xbe,0xe9,0x7d,0xab,0xfe,0x89,0xad,0x49,0x82,0x8f,0x11,0x8f,0x09,0xdf,0x7f,0xd2}; +static const uint8_t JOYCON2_WRITE_COMMAND_UUID_VAL[16] = {0xc9,0x4a,0x9d,0x64,0xb7,0x8e,0x6c,0x4e,0xaf,0x44,0x1e,0xa5,0x4f,0xe5,0xf0,0x05}; + +// Joy-Con 2 identification constants +#define JOYCON2_MANUFACTURER_ID 0x057E +#define JOYCON2_LEFT_NAME "Joy-Con (L)" +#define JOYCON2_RIGHT_NAME "Joy-Con (R)" + +// Joy-Con 2 connection structure +struct joycon2_connection { + struct bt_conn* conn; + uint16_t input_handle; + uint16_t write_handle; + bool sensors_enabled; +}; + +// Device info storage for Joy-Con 2 detection +struct joycon2_device_info { + struct bt_scan_device_info* device_info; + bool is_joycon2; +}; + +// Function declarations +bool joycon2_is_device(const struct bt_scan_device_info* device_info); +void joycon2_store_device_info(const struct bt_scan_device_info* device_info, uint8_t conn_idx); +bool joycon2_is_connection(uint8_t conn_idx); +void joycon2_discover_characteristics(struct bt_conn* conn, void* dm); +void joycon2_enable_sensors(struct bt_conn* conn, uint16_t write_handle); +void joycon2_decode_and_handle_report(const uint8_t* data, uint8_t len, uint16_t interface); +void joycon2_cleanup_connection(uint8_t conn_idx); + +// Helper function declarations +struct joycon2_connection* joycon2_find_connection(struct bt_conn* conn); +bool joycon2_is_connection_by_conn(struct bt_conn* conn); + +// External variables +extern struct joycon2_connection joycon2_conns[]; +extern int joycon2_count; +extern struct joycon2_device_info joycon2_device_info_per_conn[]; + +#endif // JOYCON2_H \ No newline at end of file diff --git a/firmware-bluetooth/src/main.cc b/firmware-bluetooth/src/main.cc index 913618aa..9216dd17 100644 --- a/firmware-bluetooth/src/main.cc +++ b/firmware-bluetooth/src/main.cc @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -23,6 +24,7 @@ #include "config.h" #include "descriptor_parser.h" #include "globals.h" +#include "joycon2.h" #include "our_descriptor.h" #include "platform.h" #include "remapper.h" @@ -286,6 +288,30 @@ static void scan_filter_match(struct bt_scan_device_info* device_info, struct bt bt_addr_le_to_str(device_info->recv_info->addr, addr, sizeof(addr)); LOG_INF("%s address: %s connectable: %s", __func__, addr, connectable ? "yes" : "no"); + + // Check if this is a Joy-Con 2 device + if (joycon2_is_device(device_info)) { + LOG_INF("Joy-Con 2 detected at %s", addr); + + // Store device info for later use in connection + for (int i = 0; i < CONFIG_BT_MAX_CONN; i++) { + if (!joycon2_is_connection(i)) { + joycon2_store_device_info(device_info, i); + LOG_INF("Joy-Con 2 device info stored for connection %d", i); + break; + } + } + + // In Zephyr, advertisement data is accessed through net_buf_simple + // We need to parse the advertisement data to find device name + const struct bt_le_scan_recv_info* recv_info = device_info->recv_info; + + LOG_INF("Checking device at %s for Joy-Con 2", addr); + + // TODO: Implement proper advertisement data parsing using net_buf_simple + // The advertisement data should be accessed through a separate parameter + // in the scan callback function, not through recv_info->data + } } static void scan_connecting_error(struct bt_scan_device_info* device_info) { @@ -334,9 +360,10 @@ static void patch_broken_uuids(struct bt_gatt_dm* dm) { } if (needs_fix) { bt_uuid_to_str(attr->uuid, str1, sizeof(str2)); + uint16_t uuid_val = (BT_UUID_128(attr->uuid)->val[13] << 8) | BT_UUID_128(attr->uuid)->val[12]; *((bt_uuid_16*) attr->uuid) = { .uuid = { BT_UUID_TYPE_16 }, - .val = (BT_UUID_128(attr->uuid)->val[13] << 8 | BT_UUID_128(attr->uuid)->val[12]) + .val = uuid_val }; bt_uuid_to_str(attr->uuid, str2, sizeof(str2)); LOG_INF("%s -> %s", str1, str2); @@ -346,9 +373,31 @@ static void patch_broken_uuids(struct bt_gatt_dm* dm) { } static void discovery_completed_cb(struct bt_gatt_dm* dm, void* context) { - LOG_INF(""); + LOG_INF("GATT discovery completed"); patch_broken_uuids(dm); - CHK(bt_hogp_handles_assign(dm, ((struct bt_hogp*) context))); // XXX disconnect if this fails? + + struct bt_hogp* hogp = (struct bt_hogp*) context; + struct bt_conn* conn = bt_hogp_conn(hogp); + uint8_t conn_idx = bt_conn_index(conn); + + // Check if this is a Joy-Con 2 connection + if (joycon2_is_connection(conn_idx)) { + LOG_INF("Joy-Con 2 discovery completed, discovering characteristics"); + joycon2_discover_characteristics(conn, dm); + + // Enable sensors for Joy-Con 2 + struct joycon2_connection* joycon2_conn = joycon2_find_connection(conn); + if (joycon2_conn && joycon2_conn->write_handle != 0) { + LOG_INF("Enabling Joy-Con 2 sensors on write handle: 0x%04x", joycon2_conn->write_handle); + joycon2_enable_sensors(conn, joycon2_conn->write_handle); + } else { + LOG_WRN("Joy-Con 2 write handle not found, cannot enable sensors"); + } + } else { + LOG_INF("Standard HID device discovery completed"); + } + + CHK(bt_hogp_handles_assign(dm, hogp)); CHK(bt_gatt_dm_data_release(dm)); k_work_reschedule(&scan_start_work, K_MSEC(SCAN_DELAY_MS)); } @@ -508,8 +557,14 @@ static uint8_t hogp_notify_cb(struct bt_hogp* hogp, struct bt_hogp_rep_info* rep buf.data[0] = bt_hogp_rep_id(rep); memcpy(buf.data + 1, data, buf.len); - if (k_msgq_put(&report_q, &buf, K_NO_WAIT)) { - // printk("error in k_msg_put(report_q\n"); + + // Check if this is a Joy-Con 2 connection + if (joycon2_is_connection_by_conn(bt_hogp_conn(hogp))) { + joycon2_decode_and_handle_report(buf.data, buf.len, buf.interface); + } else { + if (k_msgq_put(&report_q, &buf, K_NO_WAIT)) { + // printk("error in k_msg_put(report_q\n"); + } } return BT_GATT_ITER_CONTINUE; @@ -561,6 +616,13 @@ static void hogp_ready_work_fn(struct k_work* work) { LOG_DBG("found bond idx: %d", find_bond.found_idx); device_connected_callback(bt_conn_index(bt_hogp_conn(item.hogp)) << 8, 1, 1, find_bond.found_idx); + // Joy-Con 2 GATT discovery is now handled in discovery_completed_cb + struct bt_conn* conn = bt_hogp_conn(item.hogp); + uint8_t conn_idx = bt_conn_index(conn); + + // Clear device info for this connection + joycon2_cleanup_connection(conn_idx); + while (NULL != (rep = bt_hogp_rep_next(item.hogp, rep))) { if (bt_hogp_rep_type(rep) == BT_HIDS_REPORT_TYPE_INPUT) { LOG_DBG("subscribing to report ID: %u", bt_hogp_rep_id(rep)); @@ -718,6 +780,7 @@ static bool do_send_report(uint8_t interface, const uint8_t* report_with_id, uin if (interface == 1) { return CHK(hid_int_ep_write(hid_dev1, report_with_id, len, NULL)); } + return false; } static void button_init() { @@ -884,16 +947,117 @@ uint64_t get_time() { void interval_override_updated() { } +static void hogp_write_cb(struct bt_hogp* hogp, struct bt_hogp_rep_info* rep, uint8_t err) { + if (err) { + LOG_ERR("HOGP write failed: %d", err); + } else { + LOG_INF("HOGP write completed successfully"); + } +} + +static uint8_t hogp_read_cb(struct bt_hogp* hogp, struct bt_hogp_rep_info* rep, uint8_t err, const uint8_t* data) { + if (err) { + LOG_ERR("HOGP read failed: %d", err); + } else { + LOG_INF("HOGP read completed successfully"); + } + return BT_GATT_ITER_STOP; +} + void queue_out_report(uint16_t interface, uint8_t report_id, const uint8_t* buffer, uint8_t len) { - // TODO + // Find the appropriate HOGP connection for this interface + uint8_t conn_idx = interface >> 8; + if (conn_idx >= CONFIG_BT_MAX_CONN) { + LOG_ERR("Invalid connection index: %d", conn_idx); + return; + } + + struct bt_hogp* hogp = &hogps[conn_idx]; + if (!bt_hogp_assign_check(hogp)) { + LOG_ERR("HOGP not assigned for connection %d", conn_idx); + return; + } + + // Find the appropriate report info + struct bt_hogp_rep_info* rep = NULL; + while (NULL != (rep = bt_hogp_rep_next(hogp, rep))) { + if (bt_hogp_rep_id(rep) == report_id && bt_hogp_rep_type(rep) == BT_HIDS_REPORT_TYPE_OUTPUT) { + // Send the report + int err = bt_hogp_rep_write(hogp, rep, hogp_write_cb, buffer, len); + if (err) { + LOG_ERR("Failed to send output report: %d", err); + } else { + LOG_INF("Sent output report ID: %d, len: %d", report_id, len); + } + return; + } + } + + LOG_ERR("No output report found for ID: %d", report_id); } void queue_set_feature_report(uint16_t interface, uint8_t report_id, const uint8_t* buffer, uint8_t len) { - // TODO + // Find the appropriate HOGP connection for this interface + uint8_t conn_idx = interface >> 8; + if (conn_idx >= CONFIG_BT_MAX_CONN) { + LOG_ERR("Invalid connection index: %d", conn_idx); + return; + } + + struct bt_hogp* hogp = &hogps[conn_idx]; + if (!bt_hogp_assign_check(hogp)) { + LOG_ERR("HOGP not assigned for connection %d", conn_idx); + return; + } + + // Find the appropriate report info + struct bt_hogp_rep_info* rep = NULL; + while (NULL != (rep = bt_hogp_rep_next(hogp, rep))) { + if (bt_hogp_rep_id(rep) == report_id && bt_hogp_rep_type(rep) == BT_HIDS_REPORT_TYPE_FEATURE) { + // Send the feature report + int err = bt_hogp_rep_write(hogp, rep, hogp_write_cb, buffer, len); + if (err) { + LOG_ERR("Failed to send feature report: %d", err); + } else { + LOG_INF("Sent feature report ID: %d, len: %d", report_id, len); + } + return; + } + } + + LOG_ERR("No feature report found for ID: %d", report_id); } void queue_get_feature_report(uint16_t interface, uint8_t report_id, uint8_t len) { - // TODO + // Find the appropriate HOGP connection for this interface + uint8_t conn_idx = interface >> 8; + if (conn_idx >= CONFIG_BT_MAX_CONN) { + LOG_ERR("Invalid connection index: %d", conn_idx); + return; + } + + struct bt_hogp* hogp = &hogps[conn_idx]; + if (!bt_hogp_assign_check(hogp)) { + LOG_ERR("HOGP not assigned for connection %d", conn_idx); + return; + } + + // Find the appropriate report info + struct bt_hogp_rep_info* rep = NULL; + while (NULL != (rep = bt_hogp_rep_next(hogp, rep))) { + if (bt_hogp_rep_id(rep) == report_id && bt_hogp_rep_type(rep) == BT_HIDS_REPORT_TYPE_FEATURE) { + // Read the feature report + int err = bt_hogp_rep_read(hogp, rep, hogp_read_cb); + if (err) { + LOG_ERR("Failed to read feature report: %d", err); + } else { + LOG_INF("Requested feature report ID: %d", report_id); + } + return; + } + } + + LOG_ERR("No feature report found for ID: %d", report_id); } void set_gpio_inout_masks(uint32_t in_mask, uint32_t out_mask) {