void hci_emit_hci_open_failed(){
uint8_t event[2];
event[0] = BTSTACK_EVENT_POWERON_FAILED;
event[1] = sizeof(event) - 2;
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event));
}
static int h4_send_packet(uint8_t packet_type, uint8_t * packet, int size){
if (hci_transport_h4->ds == NULL) return -1;
if (hci_transport_h4->uart_fd == 0) return -1;
// wake Bluetooth module
h4_enforce_wake_on();
hci_dump_packet( (uint8_t) packet_type, 0, packet, size);
char *data = (char*) packet;
int bytes_written = write(hci_transport_h4->uart_fd, &packet_type, 1);
while (bytes_written < 1) {
usleep(5000);
bytes_written = write(hci_transport_h4->uart_fd, &packet_type, 1);
};
while (size > 0) {
int bytes_written = write(hci_transport_h4->uart_fd, data, size);
if (bytes_written < 0) {
usleep(5000);
continue;
}
data += bytes_written;
size -= bytes_written;
}
return 0;
}
int hci_send_cmd(const hci_cmd_t *cmd, ...) {
va_list argptr;
va_start(argptr, cmd);
uint16_t len = hci_create_cmd_internal(packet_buffer, cmd, argptr);
va_end(argptr);
hci_dump_packet(HCI_COMMAND_DATA_PACKET, 0, packet_buffer, len);
dump_packet(HCI_COMMAND_DATA_PACKET, packet_buffer, len);
packet_buffer_len = len;
// track le encrypt and le rand
if (cmd->opcode == hci_le_encrypt.opcode) {
uint8_t * key_flipped = &packet_buffer[3];
uint8_t key[16];
swap128(key_flipped, key);
// printf("le_encrypt key ");
// hexdump(key, 16);
uint8_t * plaintext_flipped = &packet_buffer[19];
uint8_t plaintext[16];
swap128(plaintext_flipped, plaintext);
// printf("le_encrypt txt ");
// hexdump(plaintext, 16);
aes128_calc_cyphertext(key, plaintext, aes128_cyphertext);
// printf("le_encrypt res ");
// hexdump(aes128_cyphertext, 16);
}
return 0;
}
void usb_bt_callback(uint8_t event_type, uint8_t *data, uint16_t size)
{
switch(event_type)
{
case USB_BT_EVENT_INTERRUPT_READ:
hci_dump_packet( HCI_EVENT_PACKET, 1, data, size);
packet_handler(HCI_EVENT_PACKET,data,size);
break;
case USB_BT_EVENT_BULK_READ:
hci_dump_packet( HCI_ACL_DATA_PACKET, 1, data, size);
packet_handler(HCI_ACL_DATA_PACKET,data,size);
break;
default:
break;
}
}
void hci_emit_l2cap_check_timeout(hci_connection_t *conn){
uint8_t event[4];
event[0] = L2CAP_EVENT_TIMEOUT_CHECK;
event[1] = sizeof(event) - 2;
bt_store_16(event, 2, conn->con_handle);
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event));
}
void hci_emit_state(){
uint8_t event[3];
event[0] = BTSTACK_EVENT_STATE;
event[1] = sizeof(event) - 2;
event[2] = hci_stack.state;
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event));
}
void l2cap_emit_channel_closed(l2cap_channel_t *channel) {
uint8_t event[4];
event[0] = L2CAP_EVENT_CHANNEL_CLOSED;
event[1] = sizeof(event) - 2;
bt_store_16(event, 2, channel->local_cid);
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
}
void hci_emit_nr_connections_changed(){
uint8_t event[3];
event[0] = BTSTACK_EVENT_NR_CONNECTIONS_CHANGED;
event[1] = sizeof(event) - 2;
event[2] = nr_hci_connections();
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event));
}
void hci_emit_system_bluetooth_enabled(uint8_t enabled){
uint8_t event[3];
event[0] = BTSTACK_EVENT_SYSTEM_BLUETOOTH_ENABLED;
event[1] = sizeof(event) - 2;
event[2] = enabled;
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event));
}
void hci_emit_discoverable_enabled(uint8_t enabled){
uint8_t event[3];
event[0] = BTSTACK_EVENT_DISCOVERABLE_ENABLED;
event[1] = sizeof(event) - 2;
event[2] = enabled;
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event));
}
static void l2cap_emit_service_registered(void *connection, uint8_t status, uint16_t psm){
uint8_t event[5];
event[0] = L2CAP_EVENT_SERVICE_REGISTERED;
event[1] = sizeof(event) - 2;
event[2] = status;
bt_store_16(event, 3, psm);
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
(*packet_handler)(connection, HCI_EVENT_PACKET, 0, event, sizeof(event));
}
/*=============================================================================
=============================================================================*/
static int h4_send_packet(uint8_t packetType, uint8_t *packet, int size)
{
hci_dump_packet(packetType, 0, packet, size);
BSP_BTUART_Transmit(&packetType, 1);
BSP_BTUART_Transmit(packet, size);
return 0;
}
static void h4_deliver_packet(void){
if (read_pos < 3) return; // sanity check
hci_dump_packet( hci_packet[0], 1, &hci_packet[1], read_pos-1);
packet_handler(hci_packet[0], &hci_packet[1], read_pos-1);
h4_state = H4_W4_PACKET_TYPE;
read_pos = 0;
bytes_to_read = 1;
}
void mock_simulate_hci_event(uint8_t * packet, uint16_t size) {
hci_dump_packet(HCI_EVENT_PACKET, 1, packet, size);
if (event_packet_handler) {
event_packet_handler(NULL, HCI_EVENT_PACKET, NULL, packet, size);
}
if (le_data_handler) {
le_data_handler(HCI_EVENT_PACKET, NULL, packet, size);
}
}
void hci_emit_disconnection_complete(uint16_t handle, uint8_t reason){
uint8_t event[6];
event[0] = HCI_EVENT_DISCONNECTION_COMPLETE;
event[1] = sizeof(event) - 2;
event[2] = 0; // status = OK
bt_store_16(event, 3, handle);
event[5] = reason;
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event));
}
void hci_emit_btstack_version() {
uint8_t event[6];
event[0] = BTSTACK_EVENT_VERSION;
event[1] = sizeof(event) - 2;
event[2] = BTSTACK_MAJOR;
event[3] = BTSTACK_MINOR;
bt_store_16(event, 4, BTSTACK_REVISION);
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event));
}
void hci_emit_remote_name_cached(bd_addr_t *addr, device_name_t *name){
uint8_t event[2+1+6+248];
event[0] = BTSTACK_EVENT_REMOTE_NAME_CACHED;
event[1] = sizeof(event) - 2;
event[2] = 0; // just to be compatible with HCI_EVENT_REMOTE_NAME_REQUEST_COMPLETE
bt_flip_addr(&event[3], *addr);
memcpy(&event[9], name, 248);
hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event));
hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event));
}
// data: event(8), len(8), status(8), service_record_handle(32)
static void sdp_emit_service_registered(void *connection, uint32_t handle, uint8_t status) {
if (!app_packet_handler) return;
uint8_t event[7];
event[0] = SDP_SERVICE_REGISTERED;
event[1] = sizeof(event) - 2;
event[2] = status;
bt_store_32(event, 3, handle);
hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event));
(*app_packet_handler)(connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event));
}
void hci_emit_connection_complete(hci_connection_t *conn, uint8_t status){
uint8_t event[13];
event[0] = HCI_EVENT_CONNECTION_COMPLETE;
event[1] = sizeof(event) - 2;
event[2] = status;
bt_store_16(event, 3, conn->con_handle);
bt_flip_addr(&event[5], conn->address);
event[11] = 1; // ACL connection
event[12] = 0; // encryption disabled
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
hci_stack.packet_handler(HCI_EVENT_PACKET, event, sizeof(event));
}
void l2cap_emit_connection_request(l2cap_channel_t *channel) {
uint8_t event[16];
event[0] = L2CAP_EVENT_INCOMING_CONNECTION;
event[1] = sizeof(event) - 2;
bt_flip_addr(&event[2], channel->address);
bt_store_16(event, 8, channel->handle);
bt_store_16(event, 10, channel->psm);
bt_store_16(event, 12, channel->local_cid);
bt_store_16(event, 14, channel->remote_cid);
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
}
void l2cap_emit_credits(l2cap_channel_t *channel, uint8_t credits) {
// track credits
channel->packets_granted += credits;
// log_info("l2cap_emit_credits for cid %u, credits given: %u (+%u)\n", channel->local_cid, channel->packets_granted, credits);
uint8_t event[5];
event[0] = L2CAP_EVENT_CREDITS;
event[1] = sizeof(event) - 2;
bt_store_16(event, 2, channel->local_cid);
event[4] = credits;
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
}
void hci_dump_log(int log_level, const char * format, ...){
if (!hci_dump_log_level_active(log_level)) return;
va_list argptr;
va_start(argptr, format);
#ifdef HAVE_POSIX_FILE_IO
int len = vsnprintf(log_message_buffer, sizeof(log_message_buffer), format, argptr);
hci_dump_packet(LOG_MESSAGE_PACKET, 0, (uint8_t*) log_message_buffer, len);
#else
printf("LOG -- ");
vprintf(format, argptr);
printf("\n");
#endif
va_end(argptr);
}
static int usb_send_acl_packet(uint8_t *packet, int size)
{
int r;
hci_dump_packet( HCI_ACL_DATA_PACKET, 0, packet, size);
// Use synchronous call to sent out data
r = usb_bt_bulk_write(packet, size);
if(r < 0){
log_error("Error submitting data transfer");
}
return 0;
}
// define new packet type SDP_CLIENT_PACKET
static void handle_sdp_client_query_result(sdp_query_event_t * event){
sdp_query_attribute_value_event_t * ve;
sdp_query_complete_event_t * complete_event;
switch (event->type){
case SDP_QUERY_ATTRIBUTE_VALUE:
ve = (sdp_query_attribute_value_event_t*) event;
sdp_client_assert_buffer(ve->attribute_length);
attribute_value[ve->data_offset] = ve->data;
if ((uint16_t)(ve->data_offset+1) == ve->attribute_length){
hexdump(attribute_value, ve->attribute_length);
int event_len = 1 + 3 * 2 + ve->attribute_length;
uint8_t event[event_len];
event[0] = SDP_QUERY_ATTRIBUTE_VALUE;
event[1] = ve->record_id;
event[3] = ve->attribute_id;
event[5] = ve->attribute_length;
memcpy(&event[7], attribute_value, ve->attribute_length);
hci_dump_packet(SDP_CLIENT_PACKET, 0, event, event_len);
socket_connection_send_packet(NULL, SDP_CLIENT_PACKET, 0, event, event_len);
}
break;
case SDP_QUERY_COMPLETE:
complete_event = (sdp_query_complete_event_t*) event;
uint8_t event[] = { SDP_QUERY_COMPLETE, 1, complete_event->status};
hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event));
socket_connection_send_packet(NULL, HCI_EVENT_PACKET, 0, event, sizeof(event));
break;
}
}
static void handle_sdp_rfcomm_service_result(sdp_query_event_t * rfcomm_event, void * context){
switch (rfcomm_event->type){
case SDP_QUERY_RFCOMM_SERVICE: {
sdp_query_rfcomm_service_event_t * service_event = (sdp_query_rfcomm_service_event_t*) rfcomm_event;
int name_len = strlen((const char*)service_event);
int event_len = 3 + name_len;
uint8_t event[event_len];
event[0] = rfcomm_event->type;
event[1] = 1 + name_len;
event[2] = service_event->channel_nr;
memcpy(&event[3], service_event->service_name, name_len);
hci_dump_packet(HCI_EVENT_PACKET, 0, event, event_len);
socket_connection_send_packet(context, HCI_EVENT_PACKET, 0, event, event_len);
break;
}
case SDP_QUERY_COMPLETE: {
sdp_query_complete_event_t * complete_event = (sdp_query_complete_event_t*) rfcomm_event;
uint8_t event[] = { rfcomm_event->type, 1, complete_event->status};
hci_dump_packet(HCI_EVENT_PACKET, 0, event, sizeof(event));
socket_connection_send_packet(context, HCI_EVENT_PACKET, 0, event, sizeof(event));
break;
}
}
}
void l2cap_emit_channel_opened(l2cap_channel_t *channel, uint8_t status) {
uint8_t event[21];
event[0] = L2CAP_EVENT_CHANNEL_OPENED;
event[1] = sizeof(event) - 2;
event[2] = status;
bt_flip_addr(&event[3], channel->address);
bt_store_16(event, 9, channel->handle);
bt_store_16(event, 11, channel->psm);
bt_store_16(event, 13, channel->local_cid);
bt_store_16(event, 15, channel->remote_cid);
bt_store_16(event, 17, channel->local_mtu);
bt_store_16(event, 19, channel->remote_mtu);
hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
l2cap_dispatch(channel, HCI_EVENT_PACKET, event, sizeof(event));
}
static int usb_send_cmd_packet(uint8_t *packet, int size)
{
int r;
hci_dump_packet( HCI_COMMAND_DATA_PACKET, 0, packet, size);
// Use synchronous call to sent out command
r = usb_bt_control_write(packet, size);
if (r < 0 || r !=size ) {
log_error("Error submitting control transfer %d\n", r);
return r;
}
return 0;
}
static int h5_send_packet(uint8_t packet_type, uint8_t *packet, int size){
if (hci_transport_h5->ds == NULL) return -1;
if (hci_transport_h5->ds->fd == 0) return -1;
char *data = (char*) packet;
hci_dump_packet( (uint8_t) packet_type, 0, packet, size);
write(hci_transport_h5->ds->fd, &packet_type, 1);
while (size > 0) {
int bytes_written = write(hci_transport_h5->ds->fd, data, size);
if (bytes_written < 0) {
usleep(5000);
continue;
}
data += bytes_written;
size -= bytes_written;
}
return 0;
}
void mock_simulate_sm_data_packet(uint8_t * packet, uint16_t len) {
uint16_t handle = 0x40;
uint16_t cid = 0x06;
uint8_t acl_buffer[len + 8];
// 0 - Connection handle : PB=10 : BC=00
bt_store_16(acl_buffer, 0, handle | (0 << 12) | (0 << 14));
// 2 - ACL length
bt_store_16(acl_buffer, 2, len + 4);
// 4 - L2CAP packet length
bt_store_16(acl_buffer, 4, len + 0);
// 6 - L2CAP channel DEST
bt_store_16(acl_buffer, 6, cid);
memcpy(&acl_buffer[8], packet, len);
hci_dump_packet(HCI_ACL_DATA_PACKET, 1, &acl_buffer[0], len + 8);
le_data_handler(SM_DATA_PACKET, handle, packet, len);
}
int l2cap_send_connectionless(uint16_t handle, uint16_t cid, uint8_t * buffer, uint16_t len) {
// printf("l2cap_send_connectionless\n");
int pb = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02;
// 0 - Connection handle : PB=pb : BC=00
bt_store_16(packet_buffer, 0, handle | (pb << 12) | (0 << 14));
// 2 - ACL length
bt_store_16(packet_buffer, 2, len + 4);
// 4 - L2CAP packet length
bt_store_16(packet_buffer, 4, len + 0);
// 6 - L2CAP channel DEST
bt_store_16(packet_buffer, 6, cid);
memcpy(&packet_buffer[8], buffer, len);
hci_dump_packet(HCI_ACL_DATA_PACKET, 0, &packet_buffer[0], len + 8);
dump_packet(HCI_ACL_DATA_PACKET, packet_buffer, len + 8);
packet_buffer_len = len + 8;
return 0;
}
