diff --git a/src/hci.c b/src/hci.c index edb7abd09..8e3987a1c 100644 --- a/src/hci.c +++ b/src/hci.c @@ -58,6 +58,11 @@ hci_cmd_t hci_host_buffer_size = { static hci_transport_t * hci_transport; static uint8_t * hci_cmd_buffer; +void bt_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){ + buffer[pos] = value & 0xff; + buffer[pos+1] = value >> 8; +} + void hexdump(uint8_t *data, int size){ int i; for (i=0; isend_acl_packet(packet, size); +} + int hci_send_cmd(hci_cmd_t *cmd, ...){ hci_cmd_buffer[0] = cmd->opcode & 0xff; hci_cmd_buffer[1] = cmd->opcode >> 8; diff --git a/src/hci.h b/src/hci.h index c608e79ba..53c3e7c02 100644 --- a/src/hci.h +++ b/src/hci.h @@ -18,6 +18,7 @@ #define READ_BT_24( buffer, pos) ( ((uint32_t) buffer[pos]) | (((uint32_t)buffer[pos+1]) << 8) | (((uint32_t)buffer[pos+2]) << 16)) #define READ_BT_32( buffer, pos) ( ((uint32_t) buffer[pos]) | (((uint32_t)buffer[pos+1]) << 8) | (((uint32_t)buffer[pos+2]) << 16) | (((uint32_t) buffer[pos+3])) << 24) +// #define STORE_BT_16( buffer, pos, value ) { buffer[pos] = (value) & 0xff; buffer[pos+1] = (value) >> 8; } // packet headers #define HCI_CMD_DATA_PKT_HDR 0x03 @@ -31,13 +32,16 @@ #define BD_ADDR_LEN 6 typedef uint8_t bd_addr_t[BD_ADDR_LEN]; - /** * @brief The link key type */ #define LINK_KEY_LEN 16 typedef uint8_t link_key_t[LINK_KEY_LEN]; +/** + * @brief hci connection handle type + */ +typedef uint16_t hci_con_handle_t; typedef enum { HCI_POWER_OFF = 0, @@ -65,6 +69,12 @@ void hexdump(uint8_t *data, int size); // create and send hci command packet based on a template and a list of parameters int hci_send_cmd(hci_cmd_t *cmd, ...); +// send ACL packet +int hci_send_acl_packet(uint8_t *packet, int size); + +// helper +extern void bt_store_16(uint8_t *buffer, uint16_t pos, uint16_t value); + extern hci_cmd_t hci_inquiry; extern hci_cmd_t hci_link_key_request_negative_reply; extern hci_cmd_t hci_pin_code_request_reply; diff --git a/src/l2cap.c b/src/l2cap.c new file mode 100644 index 000000000..e3d82d75b --- /dev/null +++ b/src/l2cap.c @@ -0,0 +1,94 @@ +/* + * l2cap.c + * + * Logical Link Control and Adaption Protocl (L2CAP) + * + * Created by Matthias Ringwald on 5/16/09. + */ + +#include "l2cap.h" + +#include +#include + +#include + +static char *l2cap_signaling_commands_format[] = { + "D", // 0x01 command reject: reason {cmd not understood (0), sig MTU exceeded (2:max sig MTU), invalid CID (4:req CID)}, data len, data + "22", // 0x02 connection request: PSM, Source CID + "2222", // 0x03 connection response: Dest CID, Source CID, Result, Status + "22D", // 0x04 config request: Dest CID, Flags, Configuration options + "222D", // 0x05 config response: Source CID, Flags, Result, Configuration options + "22", // 0x06 disconection request: Dest CID, Source CID + "22", // 0x07 disconection response: Dest CID, Source CID + "D", // 0x08 echo request: Data + "D", // 0x09 echo response: Data + "2", // 0x0a information request: InfoType {1=Connectionless MTU, 2=Extended features supported} + "22D", // 0x0b information response: InfoType, Result, Data +}; + +static uint8_t * sig_buffer; + +uint8_t sig_seq_nr; +uint16_t local_cid; + +int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ + + // 0 - Connection handle : PB=10 : BC=00 + bt_store_16(sig_buffer, 0, handle | (2 << 12) | (0 << 14)); + // 6 - L2CAP channel = 1 + bt_store_16(sig_buffer, 6, 1); + // 8 - Code + sig_buffer[8] = cmd; + // 9 - id (!= 0 sequentially) + sig_buffer[9] = identifier; + + // 12 - L2CAP signaling parameters + uint16_t pos = 12; + va_list argptr; + va_start(argptr, identifier); + const char *format = l2cap_signaling_commands_format[cmd-1]; + uint16_t word; + uint8_t * ptr; + while (*format) { + switch(*format) { + case '1': // 8 bit value + case '2': // 16 bit value + word = va_arg(argptr, int); + // minimal va_arg is int: 2 bytes on 8+16 bit CPUs + sig_buffer[pos++] = word & 0xff; + if (*format == '2') { + sig_buffer[pos++] = word >> 8; + } + break; + case 'D': // variable data. passed: len, ptr + word = va_arg(argptr, int); + ptr = va_arg(argptr, uint8_t *); + memcpy(&sig_buffer[pos], ptr, word); + pos += word; + break; + default: + break; + } + format++; + }; + va_end(argptr); + + // Fill in various length fields: it's the number of bytes following for ACL lenght and l2cap parameter length + // - the l2cap payload length is counted after the following channel id (only payload) + + // 2 - ACL length + bt_store_16(sig_buffer, 2, pos - 4); + // 4 - L2CAP packet length + bt_store_16(sig_buffer, 4, pos - 6 - 2); + // 10 - L2CAP signaling parameter length + bt_store_16(sig_buffer, 10, pos - 12); + + return hci_send_acl_packet(sig_buffer, pos); +} + +void l2cap_init(){ + sig_buffer = malloc( 48 ); + sig_seq_nr = 1; + local_cid = 0x40; +} \ No newline at end of file diff --git a/src/l2cap.h b/src/l2cap.h new file mode 100644 index 000000000..3763be216 --- /dev/null +++ b/src/l2cap.h @@ -0,0 +1,29 @@ +/* + * l2cap.h + * + * Logical Link Control and Adaption Protocl (L2CAP) + * + * Created by Matthias Ringwald on 5/16/09. + */ + +#include "hci.h" + +typedef enum { + COMMAND_REJECT = 1, + CONNECTION_REQUEST, + CONNECTION_RESPONSE, + CONFIGURE_REQUEST, + CONFIGURE_RESPONSE, + DISCONNECTION_REQUEST, + DISCONNECTION_RESPONSE, + ECHO_REQUEST, + ECHO_RESPONSE, + INFORMATIONAL_REQUEST, + INFORMATIONAL_RESPONSE +} L2CAP_SIGNALING_COMMANDS; + +void l2cap_init(); +int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...); + +extern uint16_t local_cid; +extern uint8_t sig_seq_nr; diff --git a/src/main.c b/src/main.c index 347a196f9..aaf6d4a15 100644 --- a/src/main.c +++ b/src/main.c @@ -14,10 +14,14 @@ #include "hci.h" #include "hci_transport_h4.h" +#include "l2cap.h" static hci_transport_t * transport; static hci_uart_config_t config; +hci_con_handle_t con_handle= 0; +uint16_t dest_cid; + #define COMMAND_COMPLETE_EVENT(event,cmd) ( event[0] == 0x0e && READ_BT_16(event,3) == cmd.opcode) #if 0 @@ -55,11 +59,56 @@ void event_handler(uint8_t *packet, int size){ if (packet[0] == 0x16){ hci_send_cmd(&hci_pin_code_request_reply, &addr, 4, "1234"); } + + // connection established -> start L2CAP conection + if (packet[0] == 0x03){ + if (packet[2] == 0){ + // get new connection handle + con_handle = READ_BT_16(packet, 3); + + // request l2cap connection + printf("> CONNECTION REQUEST\n"); + l2cap_send_signaling_packet(con_handle, CONNECTION_REQUEST, sig_seq_nr++, 0x13, local_cid); + } + } } +void acl_handler(uint8_t *packet, int size){ + uint16_t source_cid; + uint16_t result; + uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48 + // connection response + if (packet[8] == CONNECTION_RESPONSE){ + dest_cid = READ_BT_16(packet, 12); + source_cid = READ_BT_16(packet, 14); + result = READ_BT_16(packet, 16); + uint16_t status = READ_BT_16(packet, 18); + printf("< CONNECTION_RESPONSE: id %u, dest cid %u, src cid %u, result %u, status %u\n", packet[9], dest_cid, source_cid, result, status); + if (result == 0){ + printf("> CONFIGURE_REQUEST: id %u\n", sig_seq_nr); + l2cap_send_signaling_packet(con_handle, CONFIGURE_REQUEST, sig_seq_nr++, dest_cid, 0, 4, &config_options); + } + } + else if (packet[8] == CONFIGURE_RESPONSE){ + source_cid = READ_BT_16(packet, 12); + uint16_t flags = READ_BT_16(packet, 14); + result = READ_BT_16(packet, 16); + printf("< CONFIGURE_RESPONSE: id %u, src cid %u, flags %u, result %u!!!\n", packet[9], flags, result); + hexdump(&packet[18], size-18); + } + else if (packet[8] == CONFIGURE_REQUEST){ + printf("< CONFIGURE_REQUEST: id %u\n", packet[9]); + hexdump(&packet[16], size-16); + printf("> CONFIGURE_RESPONSE: id %u\n", packet[9]); + l2cap_send_signaling_packet(con_handle, CONFIGURE_RESPONSE, packet[9], local_cid, 0, 0, size - 16, &packet[16]); + } + else { + printf("Unknown ACL ^^^ \n"); + } +} int main (int argc, const char * argv[]) { - + // if (argc <= 1){ printf("HCI Daemon tester. Specify device name for Ericsson ROK 101 007\n"); @@ -69,22 +118,28 @@ int main (int argc, const char * argv[]) { // H4 UART transport = &hci_h4_transport; + // Ancient Ericsson ROK 101 007 (ca. 2001) config.device_name = argv[1]; config.baudrate = 57600; config.flowcontrol = 1; - hci_init(transport, &config); - hci_power_control(HCI_POWER_ON); - // open low-level device transport->open(&config); + + // init HCI + hci_init(transport, &config); + hci_power_control(HCI_POWER_ON); + + // init L2CAP + l2cap_init(); // // register callbacks // transport->register_event_packet_handler(&event_handler); - + transport->register_acl_packet_handler(&acl_handler); + // get fd for select call int transport_fd = transport->get_fd();