diff --git a/input/drivers_hid/btstack_hid.c b/input/drivers_hid/btstack_hid.c index 00f7322316..8e1e18305a 100644 --- a/input/drivers_hid/btstack_hid.c +++ b/input/drivers_hid/btstack_hid.c @@ -115,8 +115,6 @@ typedef struct timer /* btdynamic.h */ -bool btstack_try_load(void); - #ifndef BUILDING_BTDYNAMIC #define BTDIMPORT extern #else @@ -759,6 +757,248 @@ static void *btstack_get_handle(void) return NULL; } +static void btpad_increment_position(uint32_t *ptr) +{ + *ptr = (*ptr + 1) % 64; +} + +static void btpad_connection_send_control(void *data, + uint8_t* data_buf, size_t size) +{ + struct btstack_hid_adapter *connection = (struct btstack_hid_adapter*)data; + + if (connection) + bt_send_l2cap_ptr(connection->channels[0], data_buf, size); +} + +static void btpad_queue_process(void) +{ + for (; can_run && (insert_position != read_position); can_run--) + { + struct btpad_queue_command* cmd = &commands[read_position]; + btpad_queue_process_cmd(cmd); + btpad_increment_position(&read_position); + } +} + +static void btpad_queue_run(uint32_t count) +{ + can_run = count; + + btpad_queue_process(); +} + +static void btpad_queue_hci_read_bd_addr( + struct btpad_queue_command *cmd) +{ + if (!cmd) + return; + + cmd->command = hci_read_bd_addr_ptr; + + btpad_increment_position(&insert_position); + btpad_queue_process(); +} + +static void btpad_queue_hci_disconnect( + struct btpad_queue_command *cmd, + uint16_t handle, uint8_t reason) +{ + if (!cmd) + return; + + cmd->command = hci_disconnect_ptr; + cmd->hci_disconnect.handle = handle; + cmd->hci_disconnect.reason = reason; + + btpad_increment_position(&insert_position); + btpad_queue_process(); +} + +static void btpad_queue_hci_inquiry( + struct btpad_queue_command *cmd, + uint32_t lap, + uint8_t length, uint8_t num_responses) +{ + if (!cmd) + return; + + cmd->command = hci_inquiry_ptr; + cmd->hci_inquiry.lap = lap; + cmd->hci_inquiry.length = length; + cmd->hci_inquiry.num_responses = num_responses; + + btpad_increment_position(&insert_position); + btpad_queue_process(); +} + +static void btpad_queue_hci_remote_name_request( + struct btpad_queue_command *cmd, + bd_addr_t bd_addr, + uint8_t page_scan_repetition_mode, + uint8_t reserved, uint16_t clock_offset) +{ + if (!cmd) + return; + + cmd->command = hci_remote_name_request_ptr; + memcpy(cmd->hci_remote_name_request.bd_addr, bd_addr, sizeof(bd_addr_t)); + cmd->hci_remote_name_request.page_scan_repetition_mode = + page_scan_repetition_mode; + cmd->hci_remote_name_request.reserved = reserved; + cmd->hci_remote_name_request.clock_offset = clock_offset; + + btpad_increment_position(&insert_position); + btpad_queue_process(); +} + +static void btpad_queue_hci_pin_code_request_reply( + struct btpad_queue_command *cmd, + bd_addr_t bd_addr, bd_addr_t pin) +{ + if (!cmd) + return; + + cmd->command = hci_pin_code_request_reply_ptr; + memcpy(cmd->hci_pin_code_request_reply.bd_addr, bd_addr, sizeof(bd_addr_t)); + memcpy(cmd->hci_pin_code_request_reply.pin, pin, sizeof(bd_addr_t)); + + btpad_increment_position(&insert_position); + btpad_queue_process(); +} + +static void btpad_close_connection(struct btstack_hid_adapter* connection) +{ + if (!connection) + return; + + if (connection->handle) + btpad_queue_hci_disconnect(&commands[insert_position], + connection->handle, 0x15); + + memset(connection, 0, sizeof(struct btstack_hid_adapter)); +} + +static void btpad_close_all_connections(void) +{ + unsigned i; + + for (i = 0; i < MAX_USERS; i ++) + btpad_close_connection(&g_connections[i]); + +#ifdef __APPLE__ + CFRunLoopStop(CFRunLoopGetCurrent()); +#endif +} + +static struct btstack_hid_adapter *btpad_find_empty_connection(void) +{ + unsigned i; + + for (i = 0; i < MAX_USERS; i++) + { + if (g_connections[i].state == BTPAD_EMPTY) + return &g_connections[i]; + } + + return 0; +} + +static struct btstack_hid_adapter *btpad_find_connection_for( + uint16_t handle, bd_addr_t address) +{ + unsigned i; + + for (i = 0; i < MAX_USERS; i++) + { + if (!g_connections[i].handle && !g_connections[i].has_address) + continue; + + if (handle && g_connections[i].handle + && handle != g_connections[i].handle) + continue; + + if (address && g_connections[i].has_address + && (BD_ADDR_CMP(address, g_connections[i].address))) + continue; + + return &g_connections[i]; + } + + return 0; +} + +static void btpad_queue_process_cmd(struct btpad_queue_command *cmd) +{ + if (!cmd) + return; + + if (cmd->command == btstack_set_power_mode_ptr) + bt_send_cmd_ptr( + cmd->command, + cmd->btstack_set_power_mode.on); + else if (cmd->command == hci_read_bd_addr_ptr) + bt_send_cmd_ptr(cmd->command); + else if (cmd->command == hci_disconnect_ptr) + bt_send_cmd_ptr( + cmd->command, + cmd->hci_disconnect.handle, + cmd->hci_disconnect.reason); + else if (cmd->command == hci_inquiry_ptr) + bt_send_cmd_ptr( + cmd->command, + cmd->hci_inquiry.lap, + cmd->hci_inquiry.length, + cmd->hci_inquiry.num_responses); + else if (cmd->command == hci_remote_name_request_ptr) + bt_send_cmd_ptr( + cmd->command, + cmd->hci_remote_name_request.bd_addr, + cmd->hci_remote_name_request.page_scan_repetition_mode, + cmd->hci_remote_name_request.reserved, + cmd->hci_remote_name_request.clock_offset); + + else if (cmd->command == hci_pin_code_request_reply_ptr) + bt_send_cmd_ptr( + cmd->command, + cmd->hci_pin_code_request_reply.bd_addr, + 6, + cmd->hci_pin_code_request_reply.pin); +} + + +static void btpad_queue_reset(void) +{ + insert_position = 0; + read_position = 0; + can_run = 1; +} + + +static void btpad_queue_btstack_set_power_mode( + struct btpad_queue_command *cmd, uint8_t on) +{ + if (!cmd) + return; + + cmd->command = btstack_set_power_mode_ptr; + cmd->btstack_set_power_mode.on = on; + + btpad_increment_position(&insert_position); + btpad_queue_process(); +} + + + +static void btpad_set_inquiry_state(bool on) +{ + inquiry_off = !on; + + if (!inquiry_off && !inquiry_running) + btpad_queue_hci_inquiry(&commands[insert_position], + HCI_INQUIRY_LAP, 3, 1); +} + static void btpad_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { @@ -1095,245 +1335,6 @@ static void btstack_set_poweron(bool on) } } -static void btpad_increment_position(uint32_t *ptr) -{ - *ptr = (*ptr + 1) % 64; -} - -static void btpad_queue_process_cmd(struct btpad_queue_command *cmd) -{ - if (!cmd) - return; - - if (cmd->command == btstack_set_power_mode_ptr) - bt_send_cmd_ptr( - cmd->command, - cmd->btstack_set_power_mode.on); - else if (cmd->command == hci_read_bd_addr_ptr) - bt_send_cmd_ptr(cmd->command); - else if (cmd->command == hci_disconnect_ptr) - bt_send_cmd_ptr( - cmd->command, - cmd->hci_disconnect.handle, - cmd->hci_disconnect.reason); - else if (cmd->command == hci_inquiry_ptr) - bt_send_cmd_ptr( - cmd->command, - cmd->hci_inquiry.lap, - cmd->hci_inquiry.length, - cmd->hci_inquiry.num_responses); - else if (cmd->command == hci_remote_name_request_ptr) - bt_send_cmd_ptr( - cmd->command, - cmd->hci_remote_name_request.bd_addr, - cmd->hci_remote_name_request.page_scan_repetition_mode, - cmd->hci_remote_name_request.reserved, - cmd->hci_remote_name_request.clock_offset); - - else if (cmd->command == hci_pin_code_request_reply_ptr) - bt_send_cmd_ptr( - cmd->command, - cmd->hci_pin_code_request_reply.bd_addr, - 6, - cmd->hci_pin_code_request_reply.pin); -} - -static void btpad_queue_process(void) -{ - for (; can_run && (insert_position != read_position); can_run--) - { - struct btpad_queue_command* cmd = &commands[read_position]; - btpad_queue_process_cmd(cmd); - btpad_increment_position(&read_position); - } -} - -static void btpad_queue_reset(void) -{ - insert_position = 0; - read_position = 0; - can_run = 1; -} - -static void btpad_queue_run(uint32_t count) -{ - can_run = count; - - btpad_queue_process(); -} - -static void btpad_queue_btstack_set_power_mode( - struct btpad_queue_command *cmd, uint8_t on) -{ - if (!cmd) - return; - - cmd->command = btstack_set_power_mode_ptr; - cmd->btstack_set_power_mode.on = on; - - btpad_increment_position(&insert_position); - btpad_queue_process(); -} - -static void btpad_queue_hci_read_bd_addr( - struct btpad_queue_command *cmd) -{ - if (!cmd) - return; - - cmd->command = hci_read_bd_addr_ptr; - - btpad_increment_position(&insert_position); - btpad_queue_process(); -} - -static void btpad_queue_hci_disconnect( - struct btpad_queue_command *cmd, - uint16_t handle, uint8_t reason) -{ - if (!cmd) - return; - - cmd->command = hci_disconnect_ptr; - cmd->hci_disconnect.handle = handle; - cmd->hci_disconnect.reason = reason; - - btpad_increment_position(&insert_position); - btpad_queue_process(); -} - -static void btpad_queue_hci_inquiry( - struct btpad_queue_command *cmd, - uint32_t lap, - uint8_t length, uint8_t num_responses) -{ - if (!cmd) - return; - - cmd->command = hci_inquiry_ptr; - cmd->hci_inquiry.lap = lap; - cmd->hci_inquiry.length = length; - cmd->hci_inquiry.num_responses = num_responses; - - btpad_increment_position(&insert_position); - btpad_queue_process(); -} - -static void btpad_queue_hci_remote_name_request( - struct btpad_queue_command *cmd, - bd_addr_t bd_addr, - uint8_t page_scan_repetition_mode, - uint8_t reserved, uint16_t clock_offset) -{ - if (!cmd) - return; - - cmd->command = hci_remote_name_request_ptr; - memcpy(cmd->hci_remote_name_request.bd_addr, bd_addr, sizeof(bd_addr_t)); - cmd->hci_remote_name_request.page_scan_repetition_mode = - page_scan_repetition_mode; - cmd->hci_remote_name_request.reserved = reserved; - cmd->hci_remote_name_request.clock_offset = clock_offset; - - btpad_increment_position(&insert_position); - btpad_queue_process(); -} - -static void btpad_queue_hci_pin_code_request_reply( - struct btpad_queue_command *cmd, - bd_addr_t bd_addr, bd_addr_t pin) -{ - if (!cmd) - return; - - cmd->command = hci_pin_code_request_reply_ptr; - memcpy(cmd->hci_pin_code_request_reply.bd_addr, bd_addr, sizeof(bd_addr_t)); - memcpy(cmd->hci_pin_code_request_reply.pin, pin, sizeof(bd_addr_t)); - - btpad_increment_position(&insert_position); - btpad_queue_process(); -} - -static void btpad_connection_send_control(void *data, - uint8_t* data_buf, size_t size) -{ - struct btstack_hid_adapter *connection = (struct btstack_hid_adapter*)data; - - if (connection) - bt_send_l2cap_ptr(connection->channels[0], data_buf, size); -} - -static void btpad_set_inquiry_state(bool on) -{ - inquiry_off = !on; - - if (!inquiry_off && !inquiry_running) - btpad_queue_hci_inquiry(&commands[insert_position], - HCI_INQUIRY_LAP, 3, 1); -} - -/* Internal interface. */ -static struct btstack_hid_adapter *btpad_find_empty_connection(void) -{ - unsigned i; - - for (i = 0; i < MAX_USERS; i++) - { - if (g_connections[i].state == BTPAD_EMPTY) - return &g_connections[i]; - } - - return 0; -} - -static struct btstack_hid_adapter *btpad_find_connection_for( - uint16_t handle, bd_addr_t address) -{ - unsigned i; - - for (i = 0; i < MAX_USERS; i++) - { - if (!g_connections[i].handle && !g_connections[i].has_address) - continue; - - if (handle && g_connections[i].handle - && handle != g_connections[i].handle) - continue; - - if (address && g_connections[i].has_address - && (BD_ADDR_CMP(address, g_connections[i].address))) - continue; - - return &g_connections[i]; - } - - return 0; -} - -static void btpad_close_connection(struct btstack_hid_adapter* connection) -{ - if (!connection) - return; - - if (connection->handle) - btpad_queue_hci_disconnect(&commands[insert_position], - connection->handle, 0x15); - - memset(connection, 0, sizeof(struct btstack_hid_adapter)); -} - -static void btpad_close_all_connections(void) -{ - unsigned i; - - for (i = 0; i < MAX_USERS; i ++) - btpad_close_connection(&g_connections[i]); - -#ifdef __APPLE__ - CFRunLoopStop(CFRunLoopGetCurrent()); -#endif -} - static bool btstack_hid_joypad_query(void *data, unsigned pad) @@ -1447,7 +1448,6 @@ error: return NULL; } - static void btstack_hid_poll(void *data) { (void)data;