From b1bc6ebdd8b6942d8972914ba8386822a2553be7 Mon Sep 17 00:00:00 2001 From: CTCaer Date: Fri, 4 Oct 2024 21:39:35 +0300 Subject: [PATCH] bdk: joycon: utilize packet id per joycon Also fix a possible infinite loop --- bdk/input/joycon.c | 69 ++++++++++++++++++++-------------------------- 1 file changed, 30 insertions(+), 39 deletions(-) diff --git a/bdk/input/joycon.c b/bdk/input/joycon.c index 37d3086..5e3243e 100644 --- a/bdk/input/joycon.c +++ b/bdk/input/joycon.c @@ -22,7 +22,6 @@ #include #include #include -#include #include #include #include @@ -223,8 +222,8 @@ typedef struct _jc_hid_in_rpt_t { u8 cmd; u8 pkt_id; - u8 conn_info:4; - u8 batt_info:4; + u8 conn_info:4; // Connection detect. + u8 batt_info:4; // Power info. u8 btn_right; u8 btn_shared; u8 btn_left; @@ -317,9 +316,10 @@ typedef struct _joycon_ctxt_t u8 uart; u8 type; u8 state; - u8 mac[6]; u32 last_received_time; u32 last_status_req_time; + u8 mac[6]; + u8 pkt_id; u8 rumble_sent; u8 connected; u8 detected; @@ -330,11 +330,10 @@ static joycon_ctxt_t jc_l = {0}; static joycon_ctxt_t jc_r = {0}; static bool jc_init_done = false; -static u32 hid_pkt_inc = 0; static jc_gamepad_rpt_t jc_gamepad; -static u8 _jc_crc(u8 *data, u16 len, u8 init) +static u8 _jc_crc(const u8 *data, u16 len, u8 init) { u8 crc = init; for (u16 i = 0; i < len; i++) @@ -448,7 +447,7 @@ static void _jc_conn_check() if (jc_l.connected) _jc_power_supply(UART_C, false); - hid_pkt_inc = 0; + jc_l.pkt_id = 0; jc_l.connected = false; jc_l.rumble_sent = false; @@ -465,7 +464,7 @@ static void _jc_conn_check() if (jc_r.connected) _jc_power_supply(UART_B, false); - hid_pkt_inc = 0; + jc_r.pkt_id = 0; jc_r.connected = false; jc_r.rumble_sent = false; @@ -484,7 +483,7 @@ static void _joycon_send_raw(u8 uart_port, const u8 *buf, u16 size) uart_wait_xfer(uart_port, UART_TX_IDLE); } -static u16 _jc_packet_add_uart_hdr(jc_wired_hdr_t *out, u8 wired_cmd, u8 *data, u16 size, bool crc) +static u16 _jc_packet_add_uart_hdr(jc_wired_hdr_t *out, u8 wired_cmd, const u8 *data, u16 size, bool crc) { out->uart_hdr.magic[0] = 0x19; out->uart_hdr.magic[1] = 0x01; @@ -504,7 +503,7 @@ static u16 _jc_packet_add_uart_hdr(jc_wired_hdr_t *out, u8 wired_cmd, u8 *data, return sizeof(jc_wired_hdr_t); } -static u16 _jc_hid_output_rpt_craft(jc_wired_hdr_t *rpt, u8 *payload, u16 size, bool crc) +static u16 _jc_hid_output_rpt_craft(jc_wired_hdr_t *rpt, const u8 *payload, u16 size, bool crc) { u16 pkt_size = _jc_packet_add_uart_hdr(rpt, JC_WIRED_HID, NULL, 0, crc); pkt_size += size; @@ -519,22 +518,18 @@ static u16 _jc_hid_output_rpt_craft(jc_wired_hdr_t *rpt, u8 *payload, u16 size, return pkt_size; } -static void _jc_send_hid_output_rpt(u8 uart, u8 *payload, u16 size, bool crc) +static void _jc_send_hid_output_rpt(joycon_ctxt_t *jc, jc_hid_out_rpt_t *hid_pkt, u16 size, bool crc) { u8 rpt[0x50]; memset(rpt, 0, sizeof(rpt)); - u32 rpt_size = _jc_hid_output_rpt_craft((jc_wired_hdr_t *)rpt, payload, size, crc); + hid_pkt->pkt_id = (jc->pkt_id++ & 0xF); + u32 rpt_size = _jc_hid_output_rpt_craft((jc_wired_hdr_t *)rpt, (u8 *)hid_pkt, size, crc); - _joycon_send_raw(uart, rpt, rpt_size); + _joycon_send_raw(jc->uart, rpt, rpt_size); } -static u8 _jc_hid_pkt_id_incr() -{ - return (hid_pkt_inc++ & 0xF); -} - -static void _jc_send_hid_cmd(u8 uart, u8 subcmd, u8 *data, u16 size) +static void _jc_send_hid_cmd(joycon_ctxt_t *jc, u8 subcmd, const u8 *data, u16 size) { static const u8 rumble_neutral[8] = { 0x00, 0x01, 0x40, 0x40, 0x00, 0x01, 0x40, 0x40 }; static const u8 rumble_init[8] = { 0xc2, 0xc8, 0x03, 0x72, 0xc2, 0xc8, 0x03, 0x72 }; @@ -551,47 +546,43 @@ static void _jc_send_hid_cmd(u8 uart, u8 subcmd, u8 *data, u16 size) // Enable rumble. hid_pkt->cmd = JC_HID_OUTPUT_RPT; - hid_pkt->pkt_id = _jc_hid_pkt_id_incr(); hid_pkt->subcmd = JC_HID_SUBCMD_RUMBLE_CTL; hid_pkt->subcmd_data[0] = 1; if (send_r_rumble) - _jc_send_hid_output_rpt(UART_B, (u8 *)hid_pkt, 0x10, false); + _jc_send_hid_output_rpt(&jc_r, hid_pkt, 0x10, false); if (send_l_rumble) - _jc_send_hid_output_rpt(UART_C, (u8 *)hid_pkt, 0x10, false); + _jc_send_hid_output_rpt(&jc_l, hid_pkt, 0x10, false); // Send rumble. - hid_pkt->cmd = JC_HID_RUMBLE_RPT; - hid_pkt->pkt_id = _jc_hid_pkt_id_incr(); + hid_pkt->cmd = JC_HID_RUMBLE_RPT; memcpy(hid_pkt->rumble, rumble_init, sizeof(rumble_init)); if (send_r_rumble) - _jc_send_hid_output_rpt(UART_B, (u8 *)hid_pkt, 10, false); + _jc_send_hid_output_rpt(&jc_r, hid_pkt, 10, false); if (send_l_rumble) - _jc_send_hid_output_rpt(UART_C, (u8 *)hid_pkt, 10, false); + _jc_send_hid_output_rpt(&jc_l, hid_pkt, 10, false); msleep(15); // Disable rumble. hid_pkt->cmd = JC_HID_OUTPUT_RPT; - hid_pkt->pkt_id = _jc_hid_pkt_id_incr(); hid_pkt->subcmd = JC_HID_SUBCMD_RUMBLE_CTL; hid_pkt->subcmd_data[0] = 0; memcpy(hid_pkt->rumble, rumble_neutral, sizeof(rumble_neutral)); if (send_r_rumble) - _jc_send_hid_output_rpt(UART_B, (u8 *)hid_pkt, 0x10, false); + _jc_send_hid_output_rpt(&jc_r, hid_pkt, 0x10, false); if (send_l_rumble) - _jc_send_hid_output_rpt(UART_C, (u8 *)hid_pkt, 0x10, false); + _jc_send_hid_output_rpt(&jc_l, hid_pkt, 0x10, false); } else { - bool crc_needed = (jc_l.uart == uart) ? (jc_l.type & JC_ID_HORI) : (jc_r.type & JC_ID_HORI); + bool crc_needed = jc->type & JC_ID_HORI; hid_pkt->cmd = JC_HID_OUTPUT_RPT; - hid_pkt->pkt_id = _jc_hid_pkt_id_incr(); hid_pkt->subcmd = subcmd; if (data) memcpy(hid_pkt->subcmd_data, data, size); - _jc_send_hid_output_rpt(uart, (u8 *)hid_pkt, sizeof(jc_hid_out_rpt_t) + size, crc_needed); + _jc_send_hid_output_rpt(jc, hid_pkt, sizeof(jc_hid_out_rpt_t) + size, crc_needed); } } @@ -823,7 +814,7 @@ static bool _jc_send_init_rumble(joycon_ctxt_t *jc) // Send init rumble or request nx pad status report. if ((jc_r.connected && !jc_r.rumble_sent) || (jc_l.connected && !jc_l.rumble_sent)) { - _jc_send_hid_cmd(jc->uart, JC_HID_SUBCMD_SND_RUMBLE, NULL, 0); + _jc_send_hid_cmd(jc, JC_HID_SUBCMD_SND_RUMBLE, NULL, 0); if (jc_l.connected) jc_l.rumble_sent = true; @@ -864,7 +855,7 @@ static void _jc_req_nx_pad_status(joycon_ctxt_t *jc) jc->last_status_req_time = get_tmr_ms() + (!jc->sio_mode ? 15 : 7); } -static bool _jc_validate_pairing_info(u8 *buf, bool *is_hos) +static bool _jc_validate_pairing_info(const u8 *buf, bool *is_hos) { u8 crc = 0; for (u32 i = 0; i < 0x22; i++) @@ -933,13 +924,13 @@ retry: { if (!jc_l_found) { - _jc_send_hid_cmd(jc_l.uart, JC_HID_SUBCMD_SPI_READ, (u8 *)&subcmd_data_l, 5); + _jc_send_hid_cmd(&jc_l, JC_HID_SUBCMD_SPI_READ, (u8 *)&subcmd_data_l, 5); jc_l.last_status_req_time = get_tmr_ms() + 15; } if (!jc_r_found) { - _jc_send_hid_cmd(jc_r.uart, JC_HID_SUBCMD_SPI_READ, (u8 *)&subcmd_data_r, 5); + _jc_send_hid_cmd(&jc_r, JC_HID_SUBCMD_SPI_READ, (u8 *)&subcmd_data_r, 5); jc_r.last_status_req_time = get_tmr_ms() + 15; } @@ -1130,7 +1121,7 @@ static void _jc_init_conn(joycon_ctxt_t *jc) // Initialize the controller. u32 retries = 10; - while (!jc->connected) + while (!jc->connected && retries) { _joycon_send_raw(jc->uart, sio_init, sizeof(sio_init)); msleep(5); @@ -1239,12 +1230,12 @@ void jc_deinit() u8 data = HCI_STATE_SLEEP; if (jc_r.connected && !(jc_r.type & JC_ID_HORI)) { - _jc_send_hid_cmd(UART_B, JC_HID_SUBCMD_HCI_STATE, &data, 1); + _jc_send_hid_cmd(&jc_r, JC_HID_SUBCMD_HCI_STATE, &data, 1); _jc_rcv_pkt(&jc_r); } if (jc_l.connected && !(jc_l.type & JC_ID_HORI)) { - _jc_send_hid_cmd(UART_C, JC_HID_SUBCMD_HCI_STATE, &data, 1); + _jc_send_hid_cmd(&jc_l, JC_HID_SUBCMD_HCI_STATE, &data, 1); _jc_rcv_pkt(&jc_l); } }