xref: /btstack/src/l2cap.c (revision fcadd0ca34dea9fd124dbf7316e77852015c9ef9)
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