1 /* 2 * l2cap.c 3 * 4 * Logical Link Control and Adaption Protocl (L2CAP) 5 * 6 * Created by Matthias Ringwald on 5/16/09. 7 */ 8 9 #include "l2cap.h" 10 11 #include <stdarg.h> 12 #include <string.h> 13 14 #include <stdio.h> 15 16 static void null_event_handler(uint8_t *packet, uint16_t size); 17 static void null_data_handler(uint16_t source_cid, uint8_t *packet, uint16_t size); 18 19 static uint8_t * sig_buffer = NULL; 20 static linked_list_t l2cap_channels = NULL; 21 static uint8_t * acl_buffer = NULL; 22 static void (*event_packet_handler) (uint8_t *packet, uint16_t size) = null_event_handler; 23 static void (*data_packet_handler) (uint16_t source_cid, uint8_t *packet, uint16_t size) = null_data_handler; 24 25 void l2cap_init(){ 26 sig_buffer = malloc( 48 ); 27 acl_buffer = malloc( 255 + 8 ); 28 29 // 30 // register callbacks with HCI 31 // 32 hci_register_event_packet_handler(&l2cap_event_handler); 33 hci_register_acl_packet_handler(&l2cap_acl_handler); 34 } 35 36 37 /** Register L2CAP packet handlers */ 38 static void null_event_handler(uint8_t *packet, uint16_t size){ 39 } 40 static void null_data_handler(uint16_t source_cid, uint8_t *packet, uint16_t size){ 41 } 42 void l2cap_register_event_packet_handler(void (*handler)(uint8_t *packet, uint16_t size)){ 43 event_packet_handler = handler; 44 } 45 void l2cap_register_data_packet_handler (void (*handler)(uint16_t source_cid, uint8_t *packet, uint16_t size)){ 46 data_packet_handler = handler; 47 } 48 49 50 int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){ 51 va_list argptr; 52 va_start(argptr, identifier); 53 uint16_t len = l2cap_create_signaling_internal(sig_buffer, handle, cmd, identifier, argptr); 54 va_end(argptr); 55 return hci_send_acl_packet(sig_buffer, len); 56 } 57 58 // open outgoing L2CAP channel 59 void l2cap_create_channel_internal(connection_t * connection, bd_addr_t address, uint16_t psm){ 60 61 // alloc structure 62 l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t)); 63 // TODO: emit error event 64 if (!chan) return; 65 66 // fill in 67 BD_ADDR_COPY(chan->address, address); 68 chan->psm = psm; 69 chan->handle = 0; 70 chan->connection = connection; 71 72 // set initial state 73 chan->state = L2CAP_STATE_CLOSED; 74 chan->sig_id = L2CAP_SIG_ID_INVALID; 75 76 // add to connections list 77 linked_list_add(&l2cap_channels, (linked_item_t *) chan); 78 79 // send connection request 80 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch 81 hci_send_cmd(&hci_create_connection, address, 0x18, 0, 0, 0, 0); 82 } 83 84 void l2cap_disconnect_internal(uint16_t source_cid, uint8_t reason){ 85 // TODO: implement 86 } 87 88 89 void l2cap_event_handler( uint8_t *packet, uint16_t size ){ 90 // handle connection complete events 91 if (packet[0] == HCI_EVENT_CONNECTION_COMPLETE && packet[2] == 0){ 92 bd_addr_t address; 93 bt_flip_addr(address, &packet[5]); 94 95 linked_item_t *it; 96 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 97 l2cap_channel_t * chan = (l2cap_channel_t *) it; 98 if ( ! BD_ADDR_CMP( chan->address, address) ){ 99 if (chan->state == L2CAP_STATE_CLOSED) { 100 chan->handle = READ_BT_16(packet, 3); 101 chan->sig_id = l2cap_next_sig_id(); 102 chan->source_cid = l2cap_next_source_cid(); 103 104 l2cap_send_signaling_packet( chan->handle, CONNECTION_REQUEST, chan->sig_id, chan->psm, chan->source_cid); 105 106 chan->state = L2CAP_STATE_WAIT_CONNECT_RSP; 107 } 108 } 109 } 110 } 111 // handle disconnection complete events 112 //@TODO:... 113 114 // forward to higher layers 115 (*event_packet_handler)(packet, size); 116 117 // forward event to clients 118 socket_connection_send_packet_all(HCI_EVENT_PACKET, 0, packet, size); 119 } 120 121 void l2cap_signaling_handler(l2cap_channel_t *channel, uint8_t *packet, uint16_t size){ 122 123 static uint8_t config_options[] = { 1, 2, 150, 0}; // mtu = 48 124 125 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 126 uint8_t identifier = READ_L2CAP_SIGNALING_IDENTIFIER( packet ); 127 128 switch (channel->state) { 129 130 case L2CAP_STATE_WAIT_CONNECT_RSP: 131 switch (code){ 132 case CONNECTION_RESPONSE: 133 if ( READ_BT_16 (packet, L2CAP_SIGNALING_DATA_OFFSET+3) == 0){ 134 // successfull connection 135 channel->dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET + 0); 136 channel->sig_id = l2cap_next_sig_id(); 137 l2cap_send_signaling_packet(channel->handle, CONFIGURE_REQUEST, channel->sig_id, channel->dest_cid, 0, 4, &config_options); 138 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ_RSP; 139 } else { 140 //@TODO: implement failed 141 } 142 break; 143 //@TODO: implement other signaling packets 144 } 145 break; 146 147 case L2CAP_STATE_WAIT_CONFIG_REQ_RSP: 148 switch (code) { 149 case CONFIGURE_RESPONSE: 150 channel->state = L2CAP_STATE_WAIT_CONFIG_REQ; 151 break; 152 } 153 break; 154 155 case L2CAP_STATE_WAIT_CONFIG_REQ: 156 switch (code) { 157 case CONFIGURE_REQUEST: 158 159 // accept the other's configuration options 160 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]); 161 162 channel->state = L2CAP_STATE_OPEN; 163 164 // notify client 165 uint8_t event[8]; 166 event[0] = HCI_EVENT_L2CAP_CHANNEL_OPENED; 167 event[1] = 6; 168 bt_store_16(event, 2, channel->handle); 169 bt_store_16(event, 4, channel->source_cid); 170 bt_store_16(event, 6, channel->dest_cid); 171 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event)); 172 break; 173 } 174 break; 175 } 176 } 177 178 void l2cap_acl_handler( uint8_t *packet, uint16_t size ){ 179 180 // Get Channel ID and command code 181 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet); 182 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet ); 183 184 // Get Connection 185 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet); 186 187 // Signaling Packet? 188 if (channel_id == 1) { 189 190 if (code < 1 || code == 2 || code >= 8){ 191 // not for a particular channel 192 return; 193 } 194 195 // Get Signaling Identifier and potential destination CID 196 uint8_t sig_id = READ_L2CAP_SIGNALING_IDENTIFIER(packet); 197 uint16_t dest_cid = READ_BT_16(packet, L2CAP_SIGNALING_DATA_OFFSET); 198 199 // Find channel for this sig_id and connection handle 200 linked_item_t *it; 201 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 202 l2cap_channel_t * chan = (l2cap_channel_t *) it; 203 if (chan->handle == handle) { 204 if (code & 1) { 205 // match odd commands by previous signaling identifier 206 if (chan->sig_id == sig_id) { 207 l2cap_signaling_handler( chan, packet, size); 208 } 209 } else { 210 // match even commands by source channel id 211 if (chan->source_cid == dest_cid) { 212 l2cap_signaling_handler( chan, packet, size); 213 } 214 } 215 } 216 } 217 return; 218 } 219 220 // Find channel for this channel_id and connection handle 221 linked_item_t *it; 222 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 223 l2cap_channel_t * channel = (l2cap_channel_t *) it; 224 if ( channel->source_cid == channel_id && channel->handle == handle) { 225 // send data packet back 226 socket_connection_send_packet(channel->connection, HCI_ACL_DATA_PACKET, 0, packet, size); 227 } 228 } 229 230 // forward to higher layers 231 (*data_packet_handler)(channel_id, packet, size); 232 } 233 234 void l2cap_send_internal(uint16_t source_cid, uint8_t *data, uint16_t len){ 235 // find channel for source_cid, construct l2cap packet and send 236 linked_item_t *it; 237 l2cap_channel_t * channel = NULL; 238 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){ 239 if ( ((l2cap_channel_t *) it)->source_cid == source_cid) { 240 channel = (l2cap_channel_t *) it; 241 break; 242 } 243 } 244 245 if (channel) { 246 // 0 - Connection handle : PB=10 : BC=00 247 bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14)); 248 // 2 - ACL length 249 bt_store_16(acl_buffer, 2, len + 4); 250 // 4 - L2CAP packet length 251 bt_store_16(acl_buffer, 4, len + 0); 252 // 6 - L2CAP channel DEST 253 bt_store_16(acl_buffer, 6, channel->dest_cid); 254 // 8 - data 255 memcpy(&acl_buffer[8], data, len); 256 // send 257 hci_send_acl_packet(acl_buffer, len+8); 258 } 259 } 260 261 262