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