l2cap.c (c8e4258af80ac01c2b8c90df5fb94a264ad985db) l2cap.c (f62db1e31aee1f37c3f088521ef5f5c8d96e68cc)
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

--- 32 unchanged lines hidden (view full) ---

41}
42void l2cap_register_event_packet_handler(void (*handler)(uint8_t *packet, uint16_t size)){
43 event_packet_handler = handler;
44}
45void l2cap_register_data_packet_handler (void (*handler)(uint16_t source_cid, uint8_t *packet, uint16_t size)){
46 data_packet_handler = handler;
47}
48
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

--- 32 unchanged lines hidden (view full) ---

41}
42void l2cap_register_event_packet_handler(void (*handler)(uint8_t *packet, uint16_t size)){
43 event_packet_handler = handler;
44}
45void 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
50int 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
49int l2cap_send_signaling_packet(hci_con_handle_t handle, L2CAP_SIGNALING_COMMANDS cmd, uint8_t identifier, ...){
50 va_list argptr;
51 va_start(argptr, identifier);
52 uint16_t len = l2cap_create_signaling_internal(sig_buffer, handle, cmd, identifier, argptr);
53 va_end(argptr);
54 return hci_send_acl_packet(sig_buffer, len);
55}
56
57l2cap_channel_t * l2cap_get_channel_for_source_cid(uint16_t source_cid){
58 linked_item_t *it;
59 l2cap_channel_t * channel;
60 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
61 channel = (l2cap_channel_t *) it;
62 if ( channel->source_cid == source_cid) {
63 return channel;
64 }
65 }
66 return NULL;
67}
68
58// open outgoing L2CAP channel
59void 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

--- 11 unchanged lines hidden (view full) ---

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
84void l2cap_disconnect_internal(uint16_t source_cid, uint8_t reason){
69// open outgoing L2CAP channel
70void l2cap_create_channel_internal(connection_t * connection, bd_addr_t address, uint16_t psm){
71
72 // alloc structure
73 l2cap_channel_t * chan = malloc(sizeof(l2cap_channel_t));
74 // TODO: emit error event
75 if (!chan) return;
76

--- 11 unchanged lines hidden (view full) ---

88 linked_list_add(&l2cap_channels, (linked_item_t *) chan);
89
90 // send connection request
91 // BD_ADDR, Packet_Type, Page_Scan_Repetition_Mode, Reserved, Clock_Offset, Allow_Role_Switch
92 hci_send_cmd(&hci_create_connection, address, 0x18, 0, 0, 0, 0);
93}
94
95void l2cap_disconnect_internal(uint16_t source_cid, uint8_t reason){
85 // TODO: implement
96 // find channel for source_cid
97 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid);
98 if (channel) {
99 channel->sig_id = l2cap_next_sig_id();
100 l2cap_send_signaling_packet( channel->handle, DISCONNECTION_REQUEST, channel->sig_id, channel->dest_cid, channel->source_cid);
101 channel->state = L2CAP_STATE_WAIT_DISCONNECT;
102 }
86}
87
88
89void 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]);

--- 65 unchanged lines hidden (view full) ---

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 l2cap_emit_channel_opened(channel);
164 break;
165 }
166 break;
103}
104
105
106void l2cap_event_handler( uint8_t *packet, uint16_t size ){
107 // handle connection complete events
108 if (packet[0] == HCI_EVENT_CONNECTION_COMPLETE && packet[2] == 0){
109 bd_addr_t address;
110 bt_flip_addr(address, &packet[5]);

--- 65 unchanged lines hidden (view full) ---

176 // accept the other's configuration options
177 l2cap_send_signaling_packet(channel->handle, CONFIGURE_RESPONSE, identifier, channel->dest_cid, 0, 0, size - 16, &packet[16]);
178
179 channel->state = L2CAP_STATE_OPEN;
180 l2cap_emit_channel_opened(channel);
181 break;
182 }
183 break;
184
185 case L2CAP_STATE_WAIT_DISCONNECT:
186 switch (code) {
187 case DISCONNECTION_RESPONSE:
188 channel->state = L2CAP_STATE_CLOSED;
189 l2cap_emit_channel_closed(channel);
190
191 // discard channel
192 linked_list_remove(&l2cap_channels, (linked_item_t *) channel);
193 free (channel);
194 break;
195 }
196 break;
167 }
168}
169
170// notify client
171void l2cap_emit_channel_opened(l2cap_channel_t *channel) {
172 uint8_t event[16];
173 event[0] = HCI_EVENT_L2CAP_CHANNEL_OPENED;
174 event[1] = sizeof(event) - 2;
175 bt_flip_addr(&event[2], channel->address);
176 bt_store_16(event, 8, channel->handle);
177 bt_store_16(event, 10, channel->psm);
178 bt_store_16(event, 12, channel->source_cid);
179 bt_store_16(event, 14, channel->dest_cid);
180 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event));
181}
182
197 }
198}
199
200// notify client
201void l2cap_emit_channel_opened(l2cap_channel_t *channel) {
202 uint8_t event[16];
203 event[0] = HCI_EVENT_L2CAP_CHANNEL_OPENED;
204 event[1] = sizeof(event) - 2;
205 bt_flip_addr(&event[2], channel->address);
206 bt_store_16(event, 8, channel->handle);
207 bt_store_16(event, 10, channel->psm);
208 bt_store_16(event, 12, channel->source_cid);
209 bt_store_16(event, 14, channel->dest_cid);
210 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event));
211}
212
213void l2cap_emit_channel_closed(l2cap_channel_t *channel) {
214 uint8_t event[4];
215 event[0] = HCI_EVENT_L2CAP_CHANNEL_CLOSED;
216 event[1] = sizeof(event) - 2;
217 bt_store_16(event, 2, channel->source_cid);
218 socket_connection_send_packet(channel->connection, HCI_EVENT_PACKET, 0, event, sizeof(event));
219}
220
183void l2cap_acl_handler( uint8_t *packet, uint16_t size ){
184
185 // Get Channel ID and command code
186 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet);
187 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet );
188
189 // Get Connection
190 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet);

--- 27 unchanged lines hidden (view full) ---

218 }
219 }
220 }
221 }
222 return;
223 }
224
225 // Find channel for this channel_id and connection handle
221void l2cap_acl_handler( uint8_t *packet, uint16_t size ){
222
223 // Get Channel ID and command code
224 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet);
225 uint8_t code = READ_L2CAP_SIGNALING_CODE( packet );
226
227 // Get Connection
228 hci_con_handle_t handle = READ_ACL_CONNECTION_HANDLE(packet);

--- 27 unchanged lines hidden (view full) ---

256 }
257 }
258 }
259 }
260 return;
261 }
262
263 // Find channel for this channel_id and connection handle
226 linked_item_t *it;
227 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
228 l2cap_channel_t * channel = (l2cap_channel_t *) it;
229 if ( channel->source_cid == channel_id && channel->handle == handle) {
230 // send data packet back
231 socket_connection_send_packet(channel->connection, HCI_ACL_DATA_PACKET, 0, packet, size);
232 }
264 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(channel_id);
265 if (channel) {
266 socket_connection_send_packet(channel->connection, HCI_ACL_DATA_PACKET, 0, packet, size);
233 }
234
235 // forward to higher layers
236 (*data_packet_handler)(channel_id, packet, size);
237}
238
267 }
268
269 // forward to higher layers
270 (*data_packet_handler)(channel_id, packet, size);
271}
272
273
239void l2cap_send_internal(uint16_t source_cid, uint8_t *data, uint16_t len){
240 // find channel for source_cid, construct l2cap packet and send
274void l2cap_send_internal(uint16_t source_cid, uint8_t *data, uint16_t len){
275 // find channel for source_cid, construct l2cap packet and send
241 linked_item_t *it;
242 l2cap_channel_t * channel = NULL;
243 for (it = (linked_item_t *) l2cap_channels; it ; it = it->next){
244 if ( ((l2cap_channel_t *) it)->source_cid == source_cid) {
245 channel = (l2cap_channel_t *) it;
246 break;
247 }
248 }
249
276 l2cap_channel_t * channel = l2cap_get_channel_for_source_cid(source_cid);
250 if (channel) {
251 // 0 - Connection handle : PB=10 : BC=00
252 bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14));
253 // 2 - ACL length
254 bt_store_16(acl_buffer, 2, len + 4);
255 // 4 - L2CAP packet length
256 bt_store_16(acl_buffer, 4, len + 0);
257 // 6 - L2CAP channel DEST
258 bt_store_16(acl_buffer, 6, channel->dest_cid);
259 // 8 - data
260 memcpy(&acl_buffer[8], data, len);
261 // send
262 hci_send_acl_packet(acl_buffer, len+8);
263 }
264}
265
266
277 if (channel) {
278 // 0 - Connection handle : PB=10 : BC=00
279 bt_store_16(acl_buffer, 0, channel->handle | (2 << 12) | (0 << 14));
280 // 2 - ACL length
281 bt_store_16(acl_buffer, 2, len + 4);
282 // 4 - L2CAP packet length
283 bt_store_16(acl_buffer, 4, len + 0);
284 // 6 - L2CAP channel DEST
285 bt_store_16(acl_buffer, 6, channel->dest_cid);
286 // 8 - data
287 memcpy(&acl_buffer[8], data, len);
288 // send
289 hci_send_acl_packet(acl_buffer, len+8);
290 }
291}
292
293