static void send_arp_probe_ipv4(void){ // "random address" static uint8_t requested_address[4] = {169, 254, 1, 0}; requested_address[3]++; int pos = setup_ethernet_header(1, 0, 1, NETWORK_TYPE_IPv4); net_store_16(network_buffer, pos, HARDWARE_TYPE_ETHERNET); pos += 2; net_store_16(network_buffer, pos, NETWORK_TYPE_IPv4); pos += 2; network_buffer[pos++] = 6; // Hardware length (HLEN) - 6 MAC Address network_buffer[pos++] = 4; // Protocol length (PLEN) - 4 IPv4 Address net_store_16(network_buffer, pos, ARP_OPERATION_REQUEST); pos += 2; BD_ADDR_COPY(&network_buffer[pos], local_addr); // Sender Hardware Address (SHA) pos += 6; bzero(&network_buffer[pos], 4); // Sender Protocol Adress (SPA) pos += 4; BD_ADDR_COPY(&network_buffer[pos], other_addr); // Target Hardware Address (THA) (ignored for requests) pos += 6; memcpy(&network_buffer[pos], requested_address, 4); pos += 4; // magically, add some extra bytes for Ethernet padding pos += 18; send_buffer(pos); }
static uint16_t setup_ethernet_header(int src_compressed, int dst_compressed, int broadcast, uint16_t network_protocol_type){ // setup packet int pos = 0; // destination if (broadcast){ BD_ADDR_COPY(&network_buffer[pos], broadcast_addr); } else { BD_ADDR_COPY(&network_buffer[pos], dst_compressed ? pts_addr : other_addr); } pos += 6; // source BD_ADDR_COPY(&network_buffer[pos], src_compressed ? local_addr : other_addr); pos += 6; net_store_16(network_buffer, pos, network_protocol_type); pos += 2; return pos; }
static void l2cap_handle_connection_request(hci_con_handle_t handle, uint8_t sig_id, uint16_t psm, uint16_t source_cid){ log_info("l2cap_handle_connection_request for handle %u, psm %u cid %u\n", handle, psm, source_cid); l2cap_service_t *service = l2cap_get_service(psm); if (!service) { // 0x0002 PSM not supported l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, 0x0002); return; } hci_connection_t * hci_connection = connection_for_handle( handle ); if (!hci_connection) { // log_error("no hci_connection for handle %u\n", handle); return; } // alloc structure // log_info("l2cap_handle_connection_request register channel\n"); l2cap_channel_t * channel = (l2cap_channel_t*) btstack_memory_l2cap_channel_get(); if (!channel){ // 0x0004 No resources available l2cap_register_signaling_response(handle, CONNECTION_REQUEST, sig_id, 0x0004); return; } // fill in BD_ADDR_COPY(channel->address, hci_connection->address); channel->psm = psm; channel->handle = handle; channel->connection = service->connection; channel->packet_handler = service->packet_handler; channel->local_cid = l2cap_next_local_cid(); channel->remote_cid = source_cid; channel->local_mtu = service->mtu; channel->remote_mtu = L2CAP_DEFAULT_MTU; channel->packets_granted = 0; channel->remote_sig_id = sig_id; // limit local mtu to max acl packet length if (channel->local_mtu > l2cap_max_mtu()) { channel->local_mtu = l2cap_max_mtu(); } // set initial state channel->state = L2CAP_STATE_WAIT_CLIENT_ACCEPT_OR_REJECT; channel->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; // add to connections list linked_list_add(&l2cap_channels, (linked_item_t *) channel); // emit incoming connection request l2cap_emit_connection_request(channel); }
// open outgoing L2CAP channel void l2cap_create_channel_internal(void * connection, btstack_packet_handler_t packet_handler, bd_addr_t address, uint16_t psm, uint16_t mtu){ // alloc structure l2cap_channel_t * chan = (l2cap_channel_t*) btstack_memory_l2cap_channel_get(); if (!chan) { // emit error event l2cap_channel_t dummy_channel; BD_ADDR_COPY(dummy_channel.address, address); dummy_channel.psm = psm; l2cap_emit_channel_opened(&dummy_channel, BTSTACK_MEMORY_ALLOC_FAILED); return; } // limit local mtu to max acl packet length if (mtu > l2cap_max_mtu()) { mtu = l2cap_max_mtu(); } // fill in BD_ADDR_COPY(chan->address, address); chan->psm = psm; chan->handle = 0; chan->connection = connection; chan->packet_handler = packet_handler; chan->remote_mtu = L2CAP_MINIMAL_MTU; chan->local_mtu = mtu; chan->packets_granted = 0; // set initial state chan->state = L2CAP_STATE_WILL_SEND_CREATE_CONNECTION; chan->state_var = L2CAP_CHANNEL_STATE_VAR_NONE; chan->remote_sig_id = L2CAP_SIG_ID_INVALID; chan->local_sig_id = L2CAP_SIG_ID_INVALID; // add to connections list linked_list_add(&l2cap_channels, (linked_item_t *) chan); l2cap_run(); }
/** * create connection for given address * * @return connection OR NULL, if no memory left */ static hci_connection_t * create_connection_for_addr(bd_addr_t addr){ hci_connection_t * conn = btstack_memory_hci_connection_get(); if (!conn) return NULL; BD_ADDR_COPY(conn->address, addr); conn->con_handle = 0xffff; conn->authentication_flags = AUTH_FLAGS_NONE; linked_item_set_user(&conn->timeout.item, conn); conn->timeout.process = hci_connection_timeout_handler; hci_connection_timestamp(conn); conn->acl_recombination_length = 0; conn->acl_recombination_pos = 0; conn->num_acl_packets_sent = 0; linked_list_add(&hci_stack.connections, (linked_item_t *) conn); return conn; }
static void event_handler(uint8_t *packet, int size){ bd_addr_t addr; uint8_t link_type; hci_con_handle_t handle; hci_connection_t * conn; int i; switch (packet[0]) { case HCI_EVENT_COMMAND_COMPLETE: // get num cmd packets // log_info("HCI_EVENT_COMMAND_COMPLETE cmds old %u - new %u\n", hci_stack.num_cmd_packets, packet[2]); hci_stack.num_cmd_packets = packet[2]; if (COMMAND_COMPLETE_EVENT(packet, hci_read_buffer_size)){ // from offset 5 // status // "The HC_ACL_Data_Packet_Length return parameter will be used to determine the size of the L2CAP segments contained in ACL Data Packets" hci_stack.acl_data_packet_length = READ_BT_16(packet, 6); // ignore: SCO data packet len (8) hci_stack.total_num_acl_packets = packet[9]; // ignore: total num SCO packets if (hci_stack.state == HCI_STATE_INITIALIZING){ // determine usable ACL payload size if (HCI_ACL_PAYLOAD_SIZE < hci_stack.acl_data_packet_length){ hci_stack.acl_data_packet_length = HCI_ACL_PAYLOAD_SIZE; } // determine usable ACL packet types hci_stack.packet_types = hci_acl_packet_types_for_buffer_size(hci_stack.acl_data_packet_length); log_error("hci_read_buffer_size: used size %u, count %u, packet types %04x\n", hci_stack.acl_data_packet_length, hci_stack.total_num_acl_packets, hci_stack.packet_types); } } if (COMMAND_COMPLETE_EVENT(packet, hci_write_scan_enable)){ hci_emit_discoverable_enabled(hci_stack.discoverable); } break; case HCI_EVENT_COMMAND_STATUS: // get num cmd packets // log_info("HCI_EVENT_COMMAND_STATUS cmds - old %u - new %u\n", hci_stack.num_cmd_packets, packet[3]); hci_stack.num_cmd_packets = packet[3]; break; case HCI_EVENT_NUMBER_OF_COMPLETED_PACKETS: for (i=0; i<packet[2];i++){ handle = READ_BT_16(packet, 3 + 2*i); uint16_t num_packets = READ_BT_16(packet, 3 + packet[2]*2 + 2*i); conn = connection_for_handle(handle); if (!conn){ log_error("hci_number_completed_packet lists unused con handle %u\n", handle); continue; } conn->num_acl_packets_sent -= num_packets; // log_info("hci_number_completed_packet %u processed for handle %u, outstanding %u\n", num_packets, handle, conn->num_acl_packets_sent); } break; case HCI_EVENT_CONNECTION_REQUEST: bt_flip_addr(addr, &packet[2]); // TODO: eval COD 8-10 link_type = packet[11]; log_info("Connection_incoming: %s, type %u\n", bd_addr_to_str(addr), link_type); if (link_type == 1) { // ACL conn = connection_for_address(addr); if (!conn) { conn = create_connection_for_addr(addr); } if (!conn) { // CONNECTION REJECTED DUE TO LIMITED RESOURCES (0X0D) hci_stack.decline_reason = 0x0d; BD_ADDR_COPY(hci_stack.decline_addr, addr); break; } conn->state = RECEIVED_CONNECTION_REQUEST; hci_run(); } else { // SYNCHRONOUS CONNECTION LIMIT TO A DEVICE EXCEEDED (0X0A) hci_stack.decline_reason = 0x0a; BD_ADDR_COPY(hci_stack.decline_addr, addr); } break; case HCI_EVENT_CONNECTION_COMPLETE: // Connection management bt_flip_addr(addr, &packet[5]); log_info("Connection_complete (status=%u) %s\n", packet[2], bd_addr_to_str(addr)); conn = connection_for_address(addr); if (conn) { if (!packet[2]){ conn->state = OPEN; conn->con_handle = READ_BT_16(packet, 3); #ifdef HAVE_TICK // restart timer run_loop_set_timer(&conn->timeout, HCI_CONNECTION_TIMEOUT_MS); run_loop_add_timer(&conn->timeout); #endif log_info("New connection: handle %u, %s\n", conn->con_handle, bd_addr_to_str(conn->address)); hci_emit_nr_connections_changed(); } else { // connection failed, remove entry linked_list_remove(&hci_stack.connections, (linked_item_t *) conn); btstack_memory_hci_connection_free( conn ); // if authentication error, also delete link key if (packet[2] == 0x05) { hci_drop_link_key_for_bd_addr(&addr); } } } break; case HCI_EVENT_LINK_KEY_REQUEST: log_info("HCI_EVENT_LINK_KEY_REQUEST\n"); hci_add_connection_flags_for_flipped_bd_addr(&packet[2], RECV_LINK_KEY_REQUEST); if (!hci_stack.remote_device_db) break; hci_add_connection_flags_for_flipped_bd_addr(&packet[2], HANDLE_LINK_KEY_REQUEST); hci_run(); // request already answered return; case HCI_EVENT_LINK_KEY_NOTIFICATION: hci_add_connection_flags_for_flipped_bd_addr(&packet[2], RECV_LINK_KEY_NOTIFICATION); if (!hci_stack.remote_device_db) break; bt_flip_addr(addr, &packet[2]); hci_stack.remote_device_db->put_link_key(&addr, (link_key_t *) &packet[8]); // still forward event to allow dismiss of pairing dialog break; case HCI_EVENT_PIN_CODE_REQUEST: hci_add_connection_flags_for_flipped_bd_addr(&packet[2], RECV_PIN_CODE_REQUEST); break; #ifndef EMBEDDED case HCI_EVENT_REMOTE_NAME_REQUEST_COMPLETE: if (!hci_stack.remote_device_db) break; if (packet[2]) break; // status not ok bt_flip_addr(addr, &packet[3]); // fix for invalid remote names - terminate on 0xff for (i=0; i<248;i++){ if (packet[9+i] == 0xff){ packet[9+i] = 0; break; } } memset(&device_name, 0, sizeof(device_name_t)); strncpy((char*) device_name, (char*) &packet[9], 248); hci_stack.remote_device_db->put_name(&addr, &device_name); break; case HCI_EVENT_INQUIRY_RESULT: case HCI_EVENT_INQUIRY_RESULT_WITH_RSSI: if (!hci_stack.remote_device_db) break; // first send inq result packet hci_stack.packet_handler(HCI_EVENT_PACKET, packet, size); // then send cached remote names for (i=0; i<packet[2];i++){ bt_flip_addr(addr, &packet[3+i*6]); if (hci_stack.remote_device_db->get_name(&addr, &device_name)){ hci_emit_remote_name_cached(&addr, &device_name); } } return; #endif case HCI_EVENT_DISCONNECTION_COMPLETE: if (!packet[2]){ handle = READ_BT_16(packet, 3); hci_connection_t * conn = connection_for_handle(handle); if (conn) { hci_shutdown_connection(conn); } } break; case HCI_EVENT_HARDWARE_ERROR: if(hci_stack.control->hw_error){ (*hci_stack.control->hw_error)(); } break; #ifdef HAVE_BLE case HCI_EVENT_LE_META: switch (packet[2]) { case HCI_SUBEVENT_LE_CONNECTION_COMPLETE: // Connection management bt_flip_addr(addr, &packet[8]); log_info("LE Connection_complete (status=%u) %s\n", packet[3], bd_addr_to_str(addr)); // LE connections are auto-accepted, so just create a connection if there isn't one already conn = connection_for_address(addr); if (packet[3]){ if (conn){ // outgoing connection failed, remove entry linked_list_remove(&hci_stack.connections, (linked_item_t *) conn); btstack_memory_hci_connection_free( conn ); } // if authentication error, also delete link key if (packet[3] == 0x05) { hci_drop_link_key_for_bd_addr(&addr); } break; } if (!conn){ conn = create_connection_for_addr(addr); } if (!conn){ // no memory break; } conn->state = OPEN; conn->con_handle = READ_BT_16(packet, 4); // TODO: store - role, peer address type, conn_interval, conn_latency, supervision timeout, master clock // restart timer // run_loop_set_timer(&conn->timeout, HCI_CONNECTION_TIMEOUT_MS); // run_loop_add_timer(&conn->timeout); log_info("New connection: handle %u, %s\n", conn->con_handle, bd_addr_to_str(conn->address)); hci_emit_nr_connections_changed(); break; default: break; } break; #endif default: break; } // handle BT initialization if (hci_stack.state == HCI_STATE_INITIALIZING){ // handle H4 synchronization loss on restart // if (hci_stack.substate == 1 && packet[0] == HCI_EVENT_HARDWARE_ERROR){ // hci_stack.substate = 0; // } // handle normal init sequence if (hci_stack.substate % 2){ // odd: waiting for event if (packet[0] == HCI_EVENT_COMMAND_COMPLETE){ hci_stack.substate++; } } } // help with BT sleep if (hci_stack.state == HCI_STATE_FALLING_ASLEEP && hci_stack.substate == 1 && COMMAND_COMPLETE_EVENT(packet, hci_write_scan_enable)){ hci_stack.substate++; } hci_stack.packet_handler(HCI_EVENT_PACKET, packet, size); // execute main loop hci_run(); }
// enable LE, setup ADV data static void packet_handler (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ uint8_t adv_data[] = { 02, 01, 05, 03, 02, 0xf0, 0xff }; sm_run(); switch (packet_type) { case HCI_EVENT_PACKET: switch (packet[0]) { case BTSTACK_EVENT_STATE: // bt stack activated, get started if (packet[2] == HCI_STATE_WORKING) { printf("Working!\n"); hci_send_cmd(&hci_le_set_advertising_data, sizeof(adv_data), adv_data); } break; case DAEMON_EVENT_HCI_PACKET_SENT: att_try_respond(); break; case HCI_EVENT_LE_META: switch (packet[2]) { case HCI_SUBEVENT_LE_CONNECTION_COMPLETE: sm_response_handle = READ_BT_16(packet, 4); sm_m_addr_type = packet[7]; BD_ADDR_COPY(sm_m_address, &packet[8]); // TODO use non-null TK if appropriate sm_reset_tk(); // TODO support private addresses sm_s_addr_type = 0; BD_ADDR_COPY(sm_s_address, hci_local_bd_addr()); // request security sm_send_security_request = 1; // reset connection MTU att_connection.mtu = 23; break; case HCI_SUBEVENT_LE_LONG_TERM_KEY_REQUEST: sm_s1(sm_tk, sm_m_random, sm_m_random, sm_s_ltk); hci_send_cmd(&hci_le_long_term_key_request_reply, READ_BT_16(packet, 3), sm_s_ltk); break; default: break; } break; case HCI_EVENT_ENCRYPTION_CHANGE: // distribute keys as requested by initiator // TODO: handle initiator case here if (sm_key_distribution_set & SM_KEYDIST_ENC_KEY) sm_send_encryption_information = 1; sm_send_master_identification = 1; if (sm_key_distribution_set & SM_KEYDIST_ID_KEY) sm_send_identity_information = 1; sm_send_identity_address_information = 1; if (sm_key_distribution_set & SM_KEYDIST_SIGN) sm_send_signing_identification = 1; break; case HCI_EVENT_DISCONNECTION_COMPLETE: att_response_handle = 0; att_response_size = 0; // restart advertising hci_send_cmd(&hci_le_set_advertise_enable, 1); break; case HCI_EVENT_COMMAND_COMPLETE: if (COMMAND_COMPLETE_EVENT(packet, hci_le_set_advertising_parameters)){ // only needed for BLE Peripheral hci_send_cmd(&hci_le_set_advertising_data, sizeof(adv_data), adv_data); break; } if (COMMAND_COMPLETE_EVENT(packet, hci_le_set_advertising_data)){ // only needed for BLE Peripheral hci_send_cmd(&hci_le_set_scan_response_data, 10, adv_data); break; } if (COMMAND_COMPLETE_EVENT(packet, hci_le_set_scan_response_data)){ // only needed for BLE Peripheral hci_send_cmd(&hci_le_set_advertise_enable, 1); break; } if (COMMAND_COMPLETE_EVENT(packet, hci_le_rand)){ switch (sm_state_responding){ case SM_STATE_C1_W4_RANDOM_A: memcpy(&sm_s_random[0], &packet[6], 8); hci_send_cmd(&hci_le_rand); sm_state_responding++; break; case SM_STATE_C1_W4_RANDOM_B: memcpy(&sm_s_random[8], &packet[6], 8); // calculate s_confirm sm_c1(sm_tk, sm_s_random, sm_preq, sm_pres, sm_m_addr_type, sm_s_addr_type, sm_m_address, sm_s_address, sm_s_confirm); // send data sm_state_responding = SM_STATE_C1_SEND; break; default: break; } break; } } } sm_run(); }
static void sm_packet_handler(uint8_t packet_type, uint16_t handle, uint8_t *packet, uint16_t size){ if (packet_type != SM_DATA_PACKET) return; printf("sm_packet_handler, request %0x\n", packet[0]); switch (packet[0]){ case SM_CODE_PAIRING_REQUEST: // store key distribtion request sm_key_distribution_set = packet[6]; // for validate memcpy(sm_preq, packet, 7); // TODO use provided IO capabilites // TOOD use local MITM flag // TODO provide callback to request OOB data memcpy(sm_response_buffer, packet, size); sm_response_buffer[0] = SM_CODE_PAIRING_RESPONSE; // sm_response_buffer[1] = 0x00; // io capability: DisplayOnly // sm_response_buffer[1] = 0x02; // io capability: KeyboardOnly // sm_response_buffer[1] = 0x03; // io capability: NoInputNoOutput sm_response_buffer[1] = 0x04; // io capability: KeyboardDisplay sm_response_buffer[2] = 0x00; // no oob data available sm_response_buffer[3] = sm_response_buffer[3] & 3; // remove MITM flag sm_response_buffer[4] = 0x10; // maxium encryption key size sm_response_size = 7; // for validate memcpy(sm_pres, sm_response_buffer, 7); break; case SM_CODE_PAIRING_CONFIRM: // received confirm value memcpy(sm_m_confirm, &packet[1], 16); // dummy sm_response_size = 17; memcpy(sm_response_buffer, packet, size); // for real // sm_state_responding = SM_STATE_C1_GET_RANDOM_A; break; case SM_CODE_PAIRING_RANDOM: // received confirm value memcpy(sm_m_random, &packet[1], 16); sm_response_size = 17; // validate sm_validate(); // memcpy(sm_response_buffer, packet, size); break; case SM_CODE_ENCRYPTION_INFORMATION: sm_received_encryption_information = 1; memcpy(sm_m_ltk, &packet[1], 16); break; case SM_CODE_MASTER_IDENTIFICATION: sm_received_master_identification = 1; sm_m_ediv = READ_BT_16(packet, 1); memcpy(sm_m_rand, &packet[3],8); break; case SM_CODE_IDENTITY_INFORMATION: sm_received_identity_information = 1; memcpy(sm_m_irk, &packet[1], 16); break; case SM_CODE_IDENTITY_ADDRESS_INFORMATION: sm_received_identity_address_information = 1; sm_m_addr_type = packet[1]; BD_ADDR_COPY(sm_m_address, &packet[2]); break; case SM_CODE_SIGNING_INFORMATION: sm_received_signing_identification = 1; memcpy(sm_m_csrk, &packet[1], 16); break; } // try to send preparared packet sm_run(); }
static void sm_run(void){ // assert that we can send either one if (!hci_can_send_packet_now(HCI_COMMAND_DATA_PACKET)) return; if (!hci_can_send_packet_now(HCI_ACL_DATA_PACKET)) return; switch (sm_state_responding){ case SM_STATE_C1_GET_RANDOM_A: case SM_STATE_C1_GET_RANDOM_B: hci_send_cmd(&hci_le_rand); sm_state_responding++; return; case SM_STATE_C1_GET_ENC_A: case SM_STATE_C1_GET_ENC_B: break; case SM_STATE_C1_SEND: { uint8_t buffer[17]; buffer[0] = SM_CODE_PAIRING_CONFIRM; memcpy(&buffer[1], sm_s_confirm, 16); l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); sm_state_responding = SM_STATE_IDLE; return; } default: break; } // send security manager packet if (sm_response_size){ uint16_t size = sm_response_size; sm_response_size = 0; l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) sm_response_buffer, size); } // send security request if (sm_send_security_request){ sm_send_security_request = 0; uint8_t buffer[2]; buffer[0] = SM_CODE_SECURITY_REQUEST; buffer[1] = SM_AUTHREQ_BONDING; l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); return; } if (sm_send_encryption_information){ sm_send_encryption_information = 0; uint8_t buffer[17]; buffer[0] = SM_CODE_ENCRYPTION_INFORMATION; memcpy(&buffer[1], sm_s_ltk, 16); l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); return; } if (sm_send_master_identification){ sm_send_master_identification = 0; uint8_t buffer[11]; buffer[0] = SM_CODE_MASTER_IDENTIFICATION; bt_store_16(buffer, 1, sm_s_ediv); memcpy(&buffer[3],sm_s_rand,8); l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); return; } if (sm_send_identity_information){ sm_send_identity_information = 0; uint8_t buffer[17]; buffer[0] = SM_CODE_IDENTITY_INFORMATION; memcpy(&buffer[1], sm_s_irk, 16); l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); return; } if (sm_send_identity_address_information ){ sm_send_identity_address_information = 0; uint8_t buffer[8]; buffer[0] = SM_CODE_IDENTITY_ADDRESS_INFORMATION; buffer[1] = sm_s_addr_type; BD_ADDR_COPY(&buffer[2], sm_s_address); l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); return; } if (sm_send_signing_identification){ sm_send_signing_identification = 0; uint8_t buffer[17]; buffer[0] = SM_CODE_SIGNING_INFORMATION; memcpy(&buffer[1], sm_s_csrk, 16); l2cap_send_connectionless(sm_response_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer)); return; } }