hci_inject.cc revision 0f9b91e150e153229235c163861198e23600e636
109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati/****************************************************************************** 209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * 309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * Copyright (C) 2014 Google, Inc. 409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * 509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * Licensed under the Apache License, Version 2.0 (the "License"); 609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * you may not use this file except in compliance with the License. 709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * You may obtain a copy of the License at: 809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * 909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * http://www.apache.org/licenses/LICENSE-2.0 1009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * 1109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * Unless required by applicable law or agreed to in writing, software 1209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * distributed under the License is distributed on an "AS IS" BASIS, 1309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * See the License for the specific language governing permissions and 1509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * limitations under the License. 1609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati * 1709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati ******************************************************************************/ 1809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 1909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati#define LOG_TAG "bt_hci_inject" 2009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 2109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati#include <assert.h> 2209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati#include <errno.h> 2309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 240f9b91e150e153229235c163861198e23600e636Sharvil Nanavati#include "osi/include/allocator.h" 2509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati#include "bt_types.h" 26bf8193bc81cc077e3acd245cacbe8e3789c4b9ffZach Johnson#include "buffer_allocator.h" 2709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati#include "hci_inject.h" 28fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson#include "hci_layer.h" 290f9b91e150e153229235c163861198e23600e636Sharvil Nanavati#include "osi/include/list.h" 300f9b91e150e153229235c163861198e23600e636Sharvil Nanavati#include "osi/include/osi.h" 3144802768c447ab480d4227b3a852a97d923b816dSharvil Nanavati#include "osi/include/log.h" 320f9b91e150e153229235c163861198e23600e636Sharvil Nanavati#include "osi/include/socket.h" 330f9b91e150e153229235c163861198e23600e636Sharvil Nanavati#include "osi/include/thread.h" 3409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 3509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatitypedef enum { 3609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati HCI_PACKET_COMMAND = 1, 3709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati HCI_PACKET_ACL_DATA = 2, 3809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati HCI_PACKET_SCO_DATA = 3, 3909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati HCI_PACKET_EVENT = 4, 4009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati} hci_packet_t; 4109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 4209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatitypedef struct { 4309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati socket_t *socket; 4409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati uint8_t buffer[65536 + 3]; // 2 bytes length prefix, 1 byte type prefix. 4509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati size_t buffer_size; 4609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati} client_t; 4709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 4809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic const port_t LISTEN_PORT = 8873; 4909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 50bf8193bc81cc077e3acd245cacbe8e3789c4b9ffZach Johnsonstatic const hci_inject_t interface; 51bf8193bc81cc077e3acd245cacbe8e3789c4b9ffZach Johnsonstatic const hci_t *hci; 52bf8193bc81cc077e3acd245cacbe8e3789c4b9ffZach Johnsonstatic const allocator_t *buffer_allocator; 5309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic socket_t *listen_socket; 5409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic thread_t *thread; 5509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic list_t *clients; 5609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 5709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic int hci_packet_to_event(hci_packet_t packet); 5809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic void accept_ready(socket_t *socket, void *context); 5909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic void read_ready(socket_t *socket, void *context); 6009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic void client_free(void *ptr); 6109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 62bf8193bc81cc077e3acd245cacbe8e3789c4b9ffZach Johnsonbool hci_inject_open(const hci_t *hci_interface) { 6309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati assert(listen_socket == NULL); 6409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati assert(thread == NULL); 6509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati assert(clients == NULL); 66fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson assert(hci_interface != NULL); 6709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 68fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson hci = hci_interface; 6909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 7009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati thread = thread_new("hci_inject"); 7109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati if (!thread) 7209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati goto error; 7309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 74fbf89085bf308a98b00da77d1538539f6dd58604Sharvil Nanavati clients = list_new(client_free); 75fbf89085bf308a98b00da77d1538539f6dd58604Sharvil Nanavati if (!clients) 76fbf89085bf308a98b00da77d1538539f6dd58604Sharvil Nanavati goto error; 77fbf89085bf308a98b00da77d1538539f6dd58604Sharvil Nanavati 7809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati listen_socket = socket_new(); 7909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati if (!listen_socket) 8009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati goto error; 8109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 8209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati if (!socket_listen(listen_socket, LISTEN_PORT)) 8309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati goto error; 8409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 85fbf89085bf308a98b00da77d1538539f6dd58604Sharvil Nanavati socket_register(listen_socket, thread_get_reactor(thread), NULL, accept_ready, NULL); 8609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati return true; 8709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 8809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatierror:; 89fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson interface.close(); 9009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati return false; 9109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati} 9209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 9309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavativoid hci_inject_close(void) { 9409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati socket_free(listen_socket); 9509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati list_free(clients); 96fbf89085bf308a98b00da77d1538539f6dd58604Sharvil Nanavati thread_free(thread); 9709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 9809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati listen_socket = NULL; 9909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati thread = NULL; 10009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati clients = NULL; 10109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati} 10209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 10309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic int hci_packet_to_event(hci_packet_t packet) { 10409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati switch (packet) { 10509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati case HCI_PACKET_COMMAND: 10609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati return MSG_STACK_TO_HC_HCI_CMD; 10709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati case HCI_PACKET_ACL_DATA: 10809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati return MSG_STACK_TO_HC_HCI_ACL; 10909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati case HCI_PACKET_SCO_DATA: 11009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati return MSG_STACK_TO_HC_HCI_SCO; 11109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati default: 11244802768c447ab480d4227b3a852a97d923b816dSharvil Nanavati LOG_ERROR("%s unsupported packet type: %d", __func__, packet); 11309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati return -1; 11409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati } 11509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati} 11609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 11709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic void accept_ready(socket_t *socket, UNUSED_ATTR void *context) { 11809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati assert(socket != NULL); 11909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati assert(socket == listen_socket); 12009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 12109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati socket = socket_accept(socket); 12209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati if (!socket) 12309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati return; 12409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 1251e0ede7f5522a853fc39d4f183e508ed38f01636Zach Johnson client_t *client = (client_t *)osi_calloc(sizeof(client_t)); 12609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati if (!client) { 12744802768c447ab480d4227b3a852a97d923b816dSharvil Nanavati LOG_ERROR("%s unable to allocate memory for client.", __func__); 12809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati socket_free(socket); 12909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati return; 13009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati } 13109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 13209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati client->socket = socket; 13309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 13409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati if (!list_append(clients, client)) { 13544802768c447ab480d4227b3a852a97d923b816dSharvil Nanavati LOG_ERROR("%s unable to add client to list.", __func__); 13609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati client_free(client); 13709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati return; 13809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati } 13909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 140fbf89085bf308a98b00da77d1538539f6dd58604Sharvil Nanavati socket_register(socket, thread_get_reactor(thread), client, read_ready, NULL); 14109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati} 14209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 14309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic void read_ready(UNUSED_ATTR socket_t *socket, void *context) { 14409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati assert(socket != NULL); 14509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati assert(context != NULL); 14609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 14709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati client_t *client = (client_t *)context; 14809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 14909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati ssize_t ret = socket_read(client->socket, client->buffer + client->buffer_size, sizeof(client->buffer) - client->buffer_size); 15009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati if (ret == 0 || (ret == -1 && ret != EWOULDBLOCK && ret != EAGAIN)) { 15109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati list_remove(clients, client); 15209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati return; 15309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati } 15409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati client->buffer_size += ret; 15509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 15609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati while (client->buffer_size > 3) { 15709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati uint8_t *buffer = client->buffer; 15809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati hci_packet_t packet_type = (hci_packet_t)buffer[0]; 15909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati size_t packet_len = (buffer[2] << 8) | buffer[1]; 16009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati size_t frame_len = 3 + packet_len; 16109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 16209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati if (client->buffer_size < frame_len) 16309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati break; 16409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 16509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati // TODO(sharvil): validate incoming HCI messages. 16609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati // TODO(sharvil): once we have an HCI parser, we can eliminate 16709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati // the 2-byte size field since it will be contained in the packet. 16809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 169bf8193bc81cc077e3acd245cacbe8e3789c4b9ffZach Johnson BT_HDR *buf = (BT_HDR *)buffer_allocator->alloc(packet_len); 17009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati if (buf) { 17109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati buf->event = hci_packet_to_event(packet_type); 17209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati buf->offset = 0; 17309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati buf->layer_specific = 0; 17409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati buf->len = packet_len; 17509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati memcpy(buf->data, buffer + 3, packet_len); 176fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson hci->transmit_downward(buf->event, buf); 17709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati } else { 17844802768c447ab480d4227b3a852a97d923b816dSharvil Nanavati LOG_ERROR("%s dropping injected packet of length %zu", __func__, packet_len); 17909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati } 18009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 18109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati size_t remainder = client->buffer_size - frame_len; 18209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati memmove(buffer, buffer + frame_len, remainder); 18309f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati client->buffer_size -= frame_len; 18409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati } 18509f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati} 18609f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 18709f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavatistatic void client_free(void *ptr) { 18809f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati if (!ptr) 18909f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati return; 19009f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati 19109f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati client_t *client = (client_t *)ptr; 19209f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati socket_free(client->socket); 1931e0ede7f5522a853fc39d4f183e508ed38f01636Zach Johnson osi_free(client); 19409f31c121df79afe045163e0142dec5c09caa072Sharvil Nanavati} 195fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson 196bf8193bc81cc077e3acd245cacbe8e3789c4b9ffZach Johnsonstatic const hci_inject_t interface = { 197fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson hci_inject_open, 198fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson hci_inject_close 199fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson}; 200fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson 201bf8193bc81cc077e3acd245cacbe8e3789c4b9ffZach Johnsonconst hci_inject_t *hci_inject_get_interface() { 202bf8193bc81cc077e3acd245cacbe8e3789c4b9ffZach Johnson buffer_allocator = buffer_allocator_get_interface(); 203fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson return &interface; 204fbbd42b1fc1aae7a106f46275ab1fd86452cce78Zach Johnson} 205