143625864Smatthias.ringwald /* 243625864Smatthias.ringwald * l2cap.c 343625864Smatthias.ringwald * 443625864Smatthias.ringwald * Logical Link Control and Adaption Protocl (L2CAP) 543625864Smatthias.ringwald * 643625864Smatthias.ringwald * Created by Matthias Ringwald on 5/16/09. 743625864Smatthias.ringwald */ 843625864Smatthias.ringwald 943625864Smatthias.ringwald #include "l2cap.h" 1043625864Smatthias.ringwald 1143625864Smatthias.ringwald #include <stdarg.h> 1243625864Smatthias.ringwald #include <string.h> 1343625864Smatthias.ringwald 1443625864Smatthias.ringwald #include <stdio.h> 1543625864Smatthias.ringwald 16*fcadd0caSmatthias.ringwald static void null_event_handler(uint8_t *packet, uint16_t size); 17*fcadd0caSmatthias.ringwald static void null_data_handler(uint16_t source_cid, uint8_t *packet, uint16_t size); 18*fcadd0caSmatthias.ringwald 191e6aba47Smatthias.ringwald static uint8_t * sig_buffer = NULL; 201e6aba47Smatthias.ringwald static linked_list_t l2cap_channels = NULL; 211e6aba47Smatthias.ringwald static uint8_t * acl_buffer = NULL; 22*fcadd0caSmatthias.ringwald static void (*event_packet_handler) (uint8_t *packet, uint16_t size) = null_event_handler; 23*fcadd0caSmatthias.ringwald static void (*data_packet_handler) (uint16_t source_cid, uint8_t *packet, uint16_t size) = null_data_handler; 241e6aba47Smatthias.ringwald 251e6aba47Smatthias.ringwald void l2cap_init(){ 261e6aba47Smatthias.ringwald sig_buffer = malloc( 48 ); 271e6aba47Smatthias.ringwald acl_buffer = malloc( 255 + 8 ); 28*fcadd0caSmatthias.ringwald 29*fcadd0caSmatthias.ringwald // 30*fcadd0caSmatthias.ringwald // register callbacks with HCI 31*fcadd0caSmatthias.ringwald // 32*fcadd0caSmatthias.ringwald hci_register_event_packet_handler(&l2cap_event_handler); 33*fcadd0caSmatthias.ringwald hci_register_acl_packet_handler(&l2cap_acl_handler); 34*fcadd0caSmatthias.ringwald } 35*fcadd0caSmatthias.ringwald 36*fcadd0caSmatthias.ringwald 37*fcadd0caSmatthias.ringwald /** Register L2CAP packet handlers */ 38*fcadd0caSmatthias.ringwald static void null_event_handler(uint8_t *packet, uint16_t size){ 39*fcadd0caSmatthias.ringwald } 40*fcadd0caSmatthias.ringwald static void null_data_handler(uint16_t source_cid, uint8_t *packet, uint16_t size){ 41*fcadd0caSmatthias.ringwald } 42*fcadd0caSmatthias.ringwald void l2cap_register_event_packet_handler(void (*handler)(uint8_t *packet, uint16_t size)){ 43*fcadd0caSmatthias.ringwald event_packet_handler = handler; 44*fcadd0caSmatthias.ringwald } 45*fcadd0caSmatthias.ringwald void l2cap_register_data_packet_handler (void (*handler)(uint16_t source_cid, uint8_t *packet, uint16_t size)){ 46*fcadd0caSmatthias.ringwald data_packet_handler = handler; 471e6aba47Smatthias.ringwald } 481e6aba47Smatthias.ringwald 4943625864Smatthias.ringwald 500af41d30Smatthias.ringwald int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 510af41d30Smatthias.ringwald va_list argptr; 520af41d30Smatthias.ringwald va_start(argptr, identifier); 530af41d30Smatthias.ringwald uint16_t len = l2cap_create_signaling_internal(sig_buffer, handle, cmd, identifier, argptr); 541e6aba47Smatthias.ringwald va_end(argptr); 550af41d30Smatthias.ringwald return hci_send_acl_packet(sig_buffer, len); 560af41d30Smatthias.ringwald } 570af41d30Smatthias.ringwald 581e6aba47Smatthias.ringwald // open outgoing L2CAP channel 591e6aba47Smatthias.ringwald void l2cap_create_channel_internal(connection_t * connection, bd_addr_t address, uint16_t psm){ 601e6aba47Smatthias.ringwald 611e6aba47Smatthias.ringwald // alloc structure 621e6aba47Smatthias.ringwald l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t)); 631e6aba47Smatthias.ringwald // TODO: emit error event 641e6aba47Smatthias.ringwald if (!chan) return; 651e6aba47Smatthias.ringwald 661e6aba47Smatthias.ringwald // fill in 671e6aba47Smatthias.ringwald BD_ADDR_COPY(chan->address, address); 681e6aba47Smatthias.ringwald chan->psm = psm; 691e6aba47Smatthias.ringwald chan->handle = 0; 701e6aba47Smatthias.ringwald chan->connection = connection; 711e6aba47Smatthias.ringwald 721e6aba47Smatthias.ringwald // set initial state 731e6aba47Smatthias.ringwald chan->state = L2CAP_STATE_CLOSED; 741e6aba47Smatthias.ringwald chan->sig_id = L2CAP_SIG_ID_INVALID; 751e6aba47Smatthias.ringwald 761e6aba47Smatthias.ringwald // add to connections list 771e6aba47Smatthias.ringwald linked_list_add(&l2cap_channels, (linked_item_t *) chan); 781e6aba47Smatthias.ringwald 791e6aba47Smatthias.ringwald // send connection request 801e6aba47Smatthias.ringwald // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 811e6aba47Smatthias.ringwald hci_send_cmd(&hci_create_connection, address, 0x18, 0, 0, 0, 0); 8243625864Smatthias.ringwald } 8343625864Smatthias.ringwald 841e6aba47Smatthias.ringwald void l2cap_disconnect_internal(uint16_t source_cid, uint8_t reason){ 851e6aba47Smatthias.ringwald // TODO: implement 8643625864Smatthias.ringwald } 871e6aba47Smatthias.ringwald 881e6aba47Smatthias.ringwald 891e6aba47Smatthias.ringwald void l2cap_event_handler( uint8_t *packet, uint16_t size ){ 901e6aba47Smatthias.ringwald // handle connection complete events 911e6aba47Smatthias.ringwald if (packet[0] == HCI_EVENT_CONNECTION_COMPLETE && packet[2] == 0){ 921e6aba47Smatthias.ringwald bd_addr_t address; 931e6aba47Smatthias.ringwald bt_flip_addr(address, &packet[5]); 941e6aba47Smatthias.ringwald 951e6aba47Smatthias.ringwald linked_item_t *it; 961e6aba47Smatthias.ringwald for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 971e6aba47Smatthias.ringwald l2cap_channel_t * chan = (l2cap_channel_t *) it; 981e6aba47Smatthias.ringwald if ( ! BD_ADDR_CMP( chan->address, address) ){ 991e6aba47Smatthias.ringwald if (chan->state == L2CAP_STATE_CLOSED) { 1001e6aba47Smatthias.ringwald chan->handle = READ_BT_16(packet, 3); 1011e6aba47Smatthias.ringwald chan->sig_id = l2cap_next_sig_id(); 1021e6aba47Smatthias.ringwald chan->source_cid = l2cap_next_source_cid(); 1031e6aba47Smatthias.ringwald 1041e6aba47Smatthias.ringwald l2cap_send_signaling_packet( chan->handle, CONNECTION_REQUEST, chan->sig_id, chan->psm, chan->source_cid); 1051e6aba47Smatthias.ringwald 1061e6aba47Smatthias.ringwald chan->state = L2CAP_STATE_WAIT_CONNECT_RSP; 1071e6aba47Smatthias.ringwald } 1081e6aba47Smatthias.ringwald } 1091e6aba47Smatthias.ringwald } 1101e6aba47Smatthias.ringwald } 1111e6aba47Smatthias.ringwald // handle disconnection complete events 1121e6aba47Smatthias.ringwald //@TODO:... 113*fcadd0caSmatthias.ringwald 114*fcadd0caSmatthias.ringwald // forward to higher layers 115*fcadd0caSmatthias.ringwald (*event_packet_handler)(packet, size); 116*fcadd0caSmatthias.ringwald 117*fcadd0caSmatthias.ringwald // forward event to clients 118*fcadd0caSmatthias.ringwald socket_connection_send_packet_all(HCI_EVENT_PACKET, 0, packet, size); 1191e6aba47Smatthias.ringwald } 1201e6aba47Smatthias.ringwald 1211e6aba47Smatthias.ringwald void l2cap_signaling_handler(l2cap_channel_t *channel, uint8_t *packet, uint16_t size){ 1221e6aba47Smatthias.ringwald 1231e6aba47Smatthias.ringwald static uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48 1241e6aba47Smatthias.ringwald 1251e6aba47Smatthias.ringwald uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 1261e6aba47Smatthias.ringwald uint8_t identifier = READ_L2CAP_SIGNALING_IDENTIFIER( packet ); 1271e6aba47Smatthias.ringwald 1281e6aba47Smatthias.ringwald switch (channel->state) { 1291e6aba47Smatthias.ringwald 1301e6aba47Smatthias.ringwald case L2CAP_STATE_WAIT_CONNECT_RSP: 1311e6aba47Smatthias.ringwald switch (code){ 1321e6aba47Smatthias.ringwald case CONNECTION_RESPONSE: 1331e6aba47Smatthias.ringwald if ( READ_BT_16 (packet, L2CAP_SIGNALING_DATA_OFFSET+3) == 0){ 1341e6aba47Smatthias.ringwald // successfull connection 1351e6aba47Smatthias.ringwald channel->dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET + 0); 1361e6aba47Smatthias.ringwald channel->sig_id = l2cap_next_sig_id(); 1371e6aba47Smatthias.ringwald l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->dest_cid, 0, 4, &config_options); 1381e6aba47Smatthias.ringwald channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP; 1391e6aba47Smatthias.ringwald } else { 1401e6aba47Smatthias.ringwald //@TODO: implement failed 1411e6aba47Smatthias.ringwald } 1421e6aba47Smatthias.ringwald break; 1431e6aba47Smatthias.ringwald //@TODO: implement other signaling packets 1441e6aba47Smatthias.ringwald } 1451e6aba47Smatthias.ringwald break; 1461e6aba47Smatthias.ringwald 1471e6aba47Smatthias.ringwald case L2CAP_STATE_WAIT_CONFIG_REQ_RSP: 1481e6aba47Smatthias.ringwald switch (code) { 1491e6aba47Smatthias.ringwald case CONFIGURE_RESPONSE: 1501e6aba47Smatthias.ringwald channel->state = L2CAP_STATE_WAIT_CONFIG_REQ; 1511e6aba47Smatthias.ringwald break; 1521e6aba47Smatthias.ringwald } 1531e6aba47Smatthias.ringwald break; 1541e6aba47Smatthias.ringwald 1551e6aba47Smatthias.ringwald case L2CAP_STATE_WAIT_CONFIG_REQ: 1561e6aba47Smatthias.ringwald switch (code) { 1571e6aba47Smatthias.ringwald case CONFIGURE_REQUEST: 1581e6aba47Smatthias.ringwald 1591e6aba47Smatthias.ringwald // accept the other's configuration options 1601e6aba47Smatthias.ringwald l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]); 1611e6aba47Smatthias.ringwald 1621e6aba47Smatthias.ringwald channel->state = L2CAP_STATE_OPEN; 1631e6aba47Smatthias.ringwald 1641e6aba47Smatthias.ringwald // notify client 1651e6aba47Smatthias.ringwald uint8_t event[8]; 1661e6aba47Smatthias.ringwald event[0] = HCI_EVENT_L2CAP_CHANNEL_OPENED; 1671e6aba47Smatthias.ringwald event[1] = 6; 1681e6aba47Smatthias.ringwald bt_store_16(event, 2, channel->handle); 1691e6aba47Smatthias.ringwald bt_store_16(event, 4, channel->source_cid); 1701e6aba47Smatthias.ringwald bt_store_16(event, 6, channel->dest_cid); 1711e6aba47Smatthias.ringwald socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 1721e6aba47Smatthias.ringwald break; 1731e6aba47Smatthias.ringwald } 1741e6aba47Smatthias.ringwald break; 1751e6aba47Smatthias.ringwald } 1761e6aba47Smatthias.ringwald } 1771e6aba47Smatthias.ringwald 1781e6aba47Smatthias.ringwald void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ 1791e6aba47Smatthias.ringwald 1801e6aba47Smatthias.ringwald // Get Channel ID and command code 1811e6aba47Smatthias.ringwald uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 1821e6aba47Smatthias.ringwald uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 1831e6aba47Smatthias.ringwald 1841e6aba47Smatthias.ringwald // Get Connection 1851e6aba47Smatthias.ringwald hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 1861e6aba47Smatthias.ringwald 1871e6aba47Smatthias.ringwald // Signaling Packet? 1881e6aba47Smatthias.ringwald if (channel_id == 1) { 1891e6aba47Smatthias.ringwald 1901e6aba47Smatthias.ringwald if (code < 1 || code == 2 || code >= 8){ 1911e6aba47Smatthias.ringwald // not for a particular channel 1921e6aba47Smatthias.ringwald return; 1931e6aba47Smatthias.ringwald } 1941e6aba47Smatthias.ringwald 1951e6aba47Smatthias.ringwald // Get Signaling Identifier and potential destination CID 1961e6aba47Smatthias.ringwald uint8_t sig_id = READ_L2CAP_SIGNALING_IDENTIFIER(packet); 1971e6aba47Smatthias.ringwald uint16_t dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 1981e6aba47Smatthias.ringwald 1991e6aba47Smatthias.ringwald // Find channel for this sig_id and connection handle 2001e6aba47Smatthias.ringwald linked_item_t *it; 2011e6aba47Smatthias.ringwald for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 2021e6aba47Smatthias.ringwald l2cap_channel_t * chan = (l2cap_channel_t *) it; 2031e6aba47Smatthias.ringwald if (chan->handle == handle) { 2041e6aba47Smatthias.ringwald if (code & 1) { 2051e6aba47Smatthias.ringwald // match odd commands by previous signaling identifier 2061e6aba47Smatthias.ringwald if (chan->sig_id == sig_id) { 2071e6aba47Smatthias.ringwald l2cap_signaling_handler( chan, packet, size); 2081e6aba47Smatthias.ringwald } 2091e6aba47Smatthias.ringwald } else { 2101e6aba47Smatthias.ringwald // match even commands by source channel id 2111e6aba47Smatthias.ringwald if (chan->source_cid == dest_cid) { 2121e6aba47Smatthias.ringwald l2cap_signaling_handler( chan, packet, size); 2131e6aba47Smatthias.ringwald } 2141e6aba47Smatthias.ringwald } 2151e6aba47Smatthias.ringwald } 2161e6aba47Smatthias.ringwald } 2171e6aba47Smatthias.ringwald return; 2181e6aba47Smatthias.ringwald } 2191e6aba47Smatthias.ringwald 2201e6aba47Smatthias.ringwald // Find channel for this channel_id and connection handle 2211e6aba47Smatthias.ringwald linked_item_t *it; 2221e6aba47Smatthias.ringwald for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 2231e6aba47Smatthias.ringwald l2cap_channel_t * channel = (l2cap_channel_t *) it; 2241e6aba47Smatthias.ringwald if ( channel->source_cid == channel_id && channel->handle == handle) { 2251e6aba47Smatthias.ringwald // send data packet back 2261e6aba47Smatthias.ringwald socket_connection_send_packet(channel->connection, HCI_ACL_DATA_PACKET, 0, packet, size); 2271e6aba47Smatthias.ringwald } 2281e6aba47Smatthias.ringwald } 229*fcadd0caSmatthias.ringwald 230*fcadd0caSmatthias.ringwald // forward to higher layers 231*fcadd0caSmatthias.ringwald (*data_packet_handler)(channel_id, packet, size); 2321e6aba47Smatthias.ringwald } 2331e6aba47Smatthias.ringwald 2341e6aba47Smatthias.ringwald void l2cap_send_internal(uint16_t source_cid, uint8_t *data, uint16_t len){ 2351e6aba47Smatthias.ringwald // find channel for source_cid, construct l2cap packet and send 2361e6aba47Smatthias.ringwald linked_item_t *it; 237*fcadd0caSmatthias.ringwald l2cap_channel_t * channel = NULL; 2381e6aba47Smatthias.ringwald for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 239*fcadd0caSmatthias.ringwald if ( ((l2cap_channel_t *) it)->source_cid == source_cid) { 240*fcadd0caSmatthias.ringwald channel = (l2cap_channel_t *) it; 241*fcadd0caSmatthias.ringwald break; 242*fcadd0caSmatthias.ringwald } 243*fcadd0caSmatthias.ringwald } 2441e6aba47Smatthias.ringwald 245*fcadd0caSmatthias.ringwald if (channel) { 2461e6aba47Smatthias.ringwald // 0 - Connection handle : PB=10 : BC=00 2471e6aba47Smatthias.ringwald bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); 2481e6aba47Smatthias.ringwald // 2 - ACL length 2491e6aba47Smatthias.ringwald bt_store_16(acl_buffer, 2, len + 4); 2501e6aba47Smatthias.ringwald // 4 - L2CAP packet length 2511e6aba47Smatthias.ringwald bt_store_16(acl_buffer, 4, len + 0); 2521e6aba47Smatthias.ringwald // 6 - L2CAP channel DEST 2531e6aba47Smatthias.ringwald bt_store_16(acl_buffer, 6, channel->dest_cid); 2541e6aba47Smatthias.ringwald // 8 - data 2551e6aba47Smatthias.ringwald memcpy(&acl_buffer[8], data, len); 2561e6aba47Smatthias.ringwald // send 2571e6aba47Smatthias.ringwald hci_send_acl_packet(acl_buffer, len+8); 2581e6aba47Smatthias.ringwald } 2591e6aba47Smatthias.ringwald } 2601e6aba47Smatthias.ringwald 2611e6aba47Smatthias.ringwald 262