1 /*
2 * Copyright 2008 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
3 * All rights reserved. Distributed under the terms of the MIT License.
4 */
5 #include <KernelExport.h>
6 #include <string.h>
7
8 #include <net/if_dl.h>
9 #include <net_datalink.h>
10 #include <NetBufferUtilities.h>
11
12 #include <l2cap.h>
13 #include "L2capEndpoint.h"
14 #include "L2capEndpointManager.h"
15 #include "l2cap_internal.h"
16 #include "l2cap_signal.h"
17 #include "l2cap_command.h"
18
19 #include <btDebug.h>
20
21 #include <bluetooth/L2CAP/btL2CAP.h>
22 #include <bluetooth/HCI/btHCI_command.h>
23
24
25 struct l2cap_config_options {
26 uint16 mtu;
27 bool mtu_set;
28 uint16 flush_timeout;
29 bool flush_timeout_set;
30 l2cap_qos qos;
31 bool qos_set;
32 net_buffer* rejected;
33 };
34
35
36 // #pragma mark - incoming signals
37
38
39 static status_t
l2cap_handle_connection_req(HciConnection * conn,uint8 ident,net_buffer * buffer)40 l2cap_handle_connection_req(HciConnection* conn, uint8 ident, net_buffer* buffer)
41 {
42 NetBufferHeaderReader<l2cap_connection_req> command(buffer);
43 if (command.Status() != B_OK)
44 return ENOBUFS;
45
46 const uint16 psm = le16toh(command->psm);
47 const uint16 scid = le16toh(command->scid);
48
49 L2capEndpoint* endpoint = gL2capEndpointManager.ForPSM(psm);
50 if (endpoint == NULL) {
51 // Refuse connection.
52 send_l2cap_connection_rsp(conn, ident, 0, scid,
53 l2cap_connection_rsp::RESULT_PSM_NOT_SUPPORTED, 0);
54 return B_OK;
55 }
56
57 endpoint->_HandleConnectionReq(conn, ident, psm, scid);
58 return B_OK;
59 }
60
61
62 static void
l2cap_handle_connection_rsp(L2capEndpoint * endpoint,uint8 ident,net_buffer * buffer)63 l2cap_handle_connection_rsp(L2capEndpoint* endpoint, uint8 ident, net_buffer* buffer)
64 {
65 NetBufferHeaderReader<l2cap_connection_rsp> command(buffer);
66 if (command.Status() != B_OK)
67 return;
68
69 l2cap_connection_rsp response;
70 response.dcid = le16toh(command->dcid);
71 response.scid = le16toh(command->scid);
72 response.result = le16toh(command->result);
73 response.status = le16toh(command->status);
74
75 TRACE("%s: dcid=%d scid=%d result=%d status%d\n",
76 __func__, response.dcid, response.scid, response.result, response.status);
77
78 endpoint->_HandleConnectionRsp(ident, response);
79 }
80
81
82 static void
parse_configuration_options(net_buffer * buffer,size_t offset,uint16 length,l2cap_config_options & options)83 parse_configuration_options(net_buffer* buffer, size_t offset, uint16 length,
84 l2cap_config_options& options)
85 {
86 while (offset < length) {
87 l2cap_configuration_option option;
88 if (gBufferModule->read(buffer, offset, &option, sizeof(option)) != B_OK)
89 break;
90
91 l2cap_configuration_option_value value;
92 if (gBufferModule->read(buffer, offset + sizeof(option), &value,
93 min_c(sizeof(value), option.length)) != B_OK) {
94 break;
95 }
96
97 const bool hint = (option.type & l2cap_configuration_option::OPTION_HINT_BIT);
98 option.type &= ~l2cap_configuration_option::OPTION_HINT_BIT;
99
100 switch (option.type) {
101 case l2cap_configuration_option::OPTION_MTU:
102 options.mtu = le16toh(value.mtu);
103 options.mtu_set = true;
104 break;
105
106 case l2cap_configuration_option::OPTION_FLUSH_TIMEOUT:
107 options.flush_timeout = le16toh(value.flush_timeout);
108 options.flush_timeout_set = true;
109 break;
110
111 case l2cap_configuration_option::OPTION_QOS:
112 options.qos.flags = value.qos.flags;
113 options.qos.service_type = value.qos.service_type;
114 options.qos.token_rate = le32toh(value.qos.token_rate);
115 options.qos.token_bucket_size = le32toh(value.qos.token_bucket_size);
116 options.qos.peak_bandwidth = le32toh(value.qos.peak_bandwidth);
117 options.qos.access_latency = le32toh(value.qos.access_latency);
118 options.qos.delay_variation = le32toh(value.qos.delay_variation);
119 options.qos_set = true;
120 break;
121
122 default: {
123 if (hint)
124 break;
125
126 // Unknown option: we need to reject it.
127 if (options.rejected == NULL)
128 options.rejected = gBufferModule->create(128);
129 gBufferModule->append_cloned(options.rejected, buffer, offset,
130 sizeof(option) + option.length);
131 }
132 }
133
134 offset += sizeof(option) + option.length;
135 }
136 }
137
138
139 static status_t
l2cap_handle_configuration_req(HciConnection * conn,uint8 ident,net_buffer * buffer,uint16 length)140 l2cap_handle_configuration_req(HciConnection* conn, uint8 ident, net_buffer* buffer, uint16 length)
141 {
142 NetBufferHeaderReader<l2cap_configuration_req> command(buffer);
143 if (command.Status() != B_OK)
144 return ENOBUFS;
145
146 const uint16 dcid = le16toh(command->dcid);
147 L2capEndpoint* endpoint = gL2capEndpointManager.ForChannel(dcid);
148 if (endpoint == NULL) {
149 ERROR("l2cap: unexpected configuration req: channel does not exist (cid=%d)\n", dcid);
150 send_l2cap_command_reject(conn, ident,
151 l2cap_command_reject::REJECTED_INVALID_CID, 0, 0, 0);
152 return B_ERROR;
153 }
154
155 const uint16 flags = le16toh(command->flags);
156
157 // Read options (if any).
158 l2cap_config_options options = {};
159 parse_configuration_options(buffer, sizeof(l2cap_configuration_req), length, options);
160
161 if (options.rejected != NULL) {
162 // Reject without doing anything else.
163 send_l2cap_configuration_rsp(conn, ident, dcid, 0,
164 l2cap_configuration_rsp::RESULT_UNKNOWN_OPTION, options.rejected);
165 return B_OK;
166 }
167
168 endpoint->_HandleConfigurationReq(ident, flags,
169 options.mtu_set ? &options.mtu : NULL,
170 options.flush_timeout_set ? &options.flush_timeout : NULL,
171 options.qos_set ? &options.qos : NULL);
172 return B_OK;
173 }
174
175
176 static status_t
l2cap_handle_configuration_rsp(HciConnection * conn,L2capEndpoint * endpoint,uint8 ident,net_buffer * buffer,uint16 length,bool & releaseIdent)177 l2cap_handle_configuration_rsp(HciConnection* conn, L2capEndpoint* endpoint,
178 uint8 ident, net_buffer* buffer, uint16 length, bool& releaseIdent)
179 {
180 if (endpoint == NULL) {
181 // The endpoint seems to be gone.
182 return B_ERROR;
183 }
184
185 NetBufferHeaderReader<l2cap_configuration_rsp> command(buffer);
186 if (command.Status() != B_OK)
187 return ENOBUFS;
188
189 const uint16 scid = le16toh(command->scid);
190 const uint16 flags = le16toh(command->flags);
191 const uint16 result = le16toh(command->result);
192 releaseIdent = (flags & L2CAP_CFG_FLAG_CONTINUATION) == 0;
193
194 // Read options (if any).
195 l2cap_config_options options = {};
196 parse_configuration_options(buffer, sizeof(l2cap_configuration_rsp), length, options);
197
198 if (options.rejected != NULL) {
199 // Reject without doing anything else.
200 send_l2cap_command_reject(conn, ident,
201 l2cap_command_reject::REJECTED_NOT_UNDERSTOOD, 0, 0, 0);
202 return B_OK;
203 }
204
205 endpoint->_HandleConfigurationRsp(ident, scid, flags, result,
206 options.mtu_set ? &options.mtu : NULL,
207 options.flush_timeout_set ? &options.flush_timeout : NULL,
208 options.qos_set ? &options.qos : NULL);
209 return B_OK;
210 }
211
212
213 static status_t
l2cap_handle_disconnection_req(HciConnection * conn,uint8 ident,net_buffer * buffer)214 l2cap_handle_disconnection_req(HciConnection* conn, uint8 ident, net_buffer* buffer)
215 {
216 NetBufferHeaderReader<l2cap_disconnection_req> command(buffer);
217 if (command.Status() != B_OK)
218 return ENOBUFS;
219
220 const uint16 dcid = le16toh(command->dcid);
221 L2capEndpoint* endpoint = gL2capEndpointManager.ForChannel(dcid);
222 if (endpoint == NULL) {
223 ERROR("l2cap: unexpected disconnection req: channel does not exist (cid=%d)\n", dcid);
224 send_l2cap_command_reject(conn, ident,
225 l2cap_command_reject::REJECTED_INVALID_CID, 0, 0, 0);
226 return B_ERROR;
227 }
228
229 const uint16 scid = le16toh(command->scid);
230
231 endpoint->_HandleDisconnectionReq(ident, scid);
232 return B_OK;
233 }
234
235
236 static status_t
l2cap_handle_disconnection_rsp(L2capEndpoint * endpoint,uint8 ident,net_buffer * buffer)237 l2cap_handle_disconnection_rsp(L2capEndpoint* endpoint, uint8 ident, net_buffer* buffer)
238 {
239 NetBufferHeaderReader<l2cap_disconnection_rsp> command(buffer);
240 if (command.Status() != B_OK)
241 return ENOBUFS;
242
243 const uint16 dcid = le16toh(command->dcid);
244 const uint16 scid = le16toh(command->scid);
245
246 endpoint->_HandleDisconnectionRsp(ident, dcid, scid);
247 return B_OK;
248 }
249
250
251 static status_t
l2cap_handle_echo_req(HciConnection * conn,uint8 ident,net_buffer * buffer,uint16 length)252 l2cap_handle_echo_req(HciConnection *conn, uint8 ident, net_buffer* buffer, uint16 length)
253 {
254 net_buffer* reply = gBufferModule->clone(buffer, false);
255 if (reply == NULL)
256 return ENOMEM;
257
258 gBufferModule->trim(reply, length);
259 return send_l2cap_command(conn, L2CAP_ECHO_RSP, ident, reply);
260 }
261
262
263 static status_t
l2cap_handle_echo_rsp(L2capEndpoint * endpoint,uint8 ident,net_buffer * buffer,uint16 length)264 l2cap_handle_echo_rsp(L2capEndpoint* endpoint, uint8 ident, net_buffer* buffer, uint16 length)
265 {
266 // Not currently triggered by this module.
267 return B_OK;
268 }
269
270
271 static status_t
l2cap_handle_info_req(HciConnection * conn,uint8 ident,net_buffer * buffer)272 l2cap_handle_info_req(HciConnection* conn, uint8 ident, net_buffer* buffer)
273 {
274 NetBufferHeaderReader<l2cap_information_req> command(buffer);
275 if (command.Status() != B_OK)
276 return ENOBUFS;
277
278 const uint16 type = le16toh(command->type);
279
280 net_buffer* reply = NULL;
281 uint8 replyCode = 0;
282 switch (type) {
283 case l2cap_information_req::TYPE_CONNECTIONLESS_MTU:
284 reply = make_l2cap_information_rsp(replyCode, type,
285 l2cap_information_rsp::RESULT_SUCCESS, L2CAP_MTU_DEFAULT);
286 break;
287
288 default:
289 ERROR("l2cap: unhandled information request type %d\n", type);
290 reply = make_l2cap_information_rsp(replyCode, type,
291 l2cap_information_rsp::RESULT_NOT_SUPPORTED, 0);
292 break;
293 }
294
295 return send_l2cap_command(conn, replyCode, ident, reply);
296 }
297
298
299 static status_t
l2cap_handle_info_rsp(L2capEndpoint * endpoint,uint8 ident,net_buffer * buffer)300 l2cap_handle_info_rsp(L2capEndpoint* endpoint, uint8 ident, net_buffer* buffer)
301 {
302 // Not currently triggered by this module.
303 return B_OK;
304 }
305
306
307 static status_t
l2cap_handle_command_reject(L2capEndpoint * endpoint,uint8 ident,net_buffer * buffer)308 l2cap_handle_command_reject(L2capEndpoint* endpoint, uint8 ident, net_buffer* buffer)
309 {
310 if (endpoint == NULL) {
311 ERROR("l2cap: unexpected command rejected: ident %d unknown\n", ident);
312 return B_ERROR;
313 }
314
315 NetBufferHeaderReader<l2cap_command_reject> command(buffer);
316 if (command.Status() != B_OK)
317 return ENOBUFS;
318
319 const uint16 reason = le16toh(command->reason);
320 TRACE("%s: reason=%d\n", __func__, command->reason);
321
322 l2cap_command_reject_data data = {};
323 gBufferModule->read(buffer, sizeof(l2cap_command_reject),
324 &data, min_c(buffer->size, sizeof(l2cap_command_reject_data)));
325
326 endpoint->_HandleCommandRejected(ident, reason, data);
327
328 return B_OK;
329 }
330
331
332 // #pragma mark - outgoing signals
333
334
335 status_t
send_l2cap_command(HciConnection * conn,uint8 code,uint8 ident,net_buffer * command)336 send_l2cap_command(HciConnection* conn, uint8 code, uint8 ident, net_buffer* command)
337 {
338 // TODO: Command timeouts (probably in L2capEndpoint.)
339 {
340 NetBufferPrepend<l2cap_command_header> header(command);
341 header->code = code;
342 header->ident = ident;
343 header->length = htole16(command->size - sizeof(l2cap_command_header));
344 } {
345 NetBufferPrepend<l2cap_basic_header> basicHeader(command);
346 basicHeader->length = htole16(command->size - sizeof(l2cap_basic_header));
347 basicHeader->dcid = htole16(L2CAP_SIGNALING_CID);
348 }
349 command->type = conn->handle;
350 status_t status = btDevices->PostACL(conn->Hid, command);
351 if (status != B_OK)
352 gBufferModule->free(command);
353 return status;
354 }
355
356
357 status_t
send_l2cap_command_reject(HciConnection * conn,uint8 ident,uint16 reason,uint16 mtu,uint16 scid,uint16 dcid)358 send_l2cap_command_reject(HciConnection* conn, uint8 ident, uint16 reason,
359 uint16 mtu, uint16 scid, uint16 dcid)
360 {
361 uint8 code = 0;
362 net_buffer* buffer = make_l2cap_command_reject(code, reason, mtu, scid, dcid);
363 if (buffer == NULL)
364 return ENOMEM;
365
366 return send_l2cap_command(conn, code, ident, buffer);
367 }
368
369
370 status_t
send_l2cap_configuration_req(HciConnection * conn,uint8 ident,uint16 dcid,uint16 flags,uint16 * mtu,uint16 * flush_timeout,l2cap_qos * flow)371 send_l2cap_configuration_req(HciConnection* conn, uint8 ident, uint16 dcid, uint16 flags,
372 uint16* mtu, uint16* flush_timeout, l2cap_qos* flow)
373 {
374 uint8 code = 0;
375 net_buffer* buffer = make_l2cap_configuration_req(code, dcid, flags, mtu, flush_timeout, flow);
376 if (buffer == NULL)
377 return ENOMEM;
378
379 return send_l2cap_command(conn, code, ident, buffer);
380 }
381
382
383 status_t
send_l2cap_connection_req(HciConnection * conn,uint8 ident,uint16 psm,uint16 scid)384 send_l2cap_connection_req(HciConnection* conn, uint8 ident, uint16 psm, uint16 scid)
385 {
386 uint8 code = 0;
387 net_buffer* buffer = make_l2cap_connection_req(code, psm, scid);
388 if (buffer == NULL)
389 return ENOMEM;
390
391 return send_l2cap_command(conn, code, ident, buffer);
392 }
393
394
395 status_t
send_l2cap_connection_rsp(HciConnection * conn,uint8 ident,uint16 dcid,uint16 scid,uint16 result,uint16 status)396 send_l2cap_connection_rsp(HciConnection* conn, uint8 ident, uint16 dcid, uint16 scid,
397 uint16 result, uint16 status)
398 {
399 uint8 code = 0;
400 net_buffer* buffer = make_l2cap_connection_rsp(code, dcid, scid, result, status);
401 if (buffer == NULL)
402 return ENOMEM;
403
404 return send_l2cap_command(conn, code, ident, buffer);
405 }
406
407
408 status_t
send_l2cap_configuration_rsp(HciConnection * conn,uint8 ident,uint16 scid,uint16 flags,uint16 result,net_buffer * opt)409 send_l2cap_configuration_rsp(HciConnection* conn, uint8 ident, uint16 scid, uint16 flags,
410 uint16 result, net_buffer* opt)
411 {
412 uint8 code = 0;
413 net_buffer* buffer = make_l2cap_configuration_rsp(code, scid, flags, result, opt);
414 if (buffer == NULL)
415 return ENOMEM;
416
417 return send_l2cap_command(conn, code, ident, buffer);
418 }
419
420
421 status_t
send_l2cap_disconnection_req(HciConnection * conn,uint8 ident,uint16 dcid,uint16 scid)422 send_l2cap_disconnection_req(HciConnection* conn, uint8 ident, uint16 dcid, uint16 scid)
423 {
424 uint8 code = 0;
425 net_buffer* buffer = make_l2cap_disconnection_req(code, dcid, scid);
426 if (buffer == NULL)
427 return ENOMEM;
428
429 return send_l2cap_command(conn, code, ident, buffer);
430 }
431
432
433 status_t
send_l2cap_disconnection_rsp(HciConnection * conn,uint8 ident,uint16 dcid,uint16 scid)434 send_l2cap_disconnection_rsp(HciConnection* conn, uint8 ident, uint16 dcid, uint16 scid)
435 {
436 uint8 code = 0;
437 net_buffer* buffer = make_l2cap_disconnection_rsp(code, dcid, scid);
438 if (buffer == NULL)
439 return ENOMEM;
440
441 return send_l2cap_command(conn, code, ident, buffer);
442 }
443
444
445 // #pragma mark - dispatcher
446
447
448 status_t
l2cap_handle_signaling_command(HciConnection * connection,net_buffer * buffer)449 l2cap_handle_signaling_command(HciConnection* connection, net_buffer* buffer)
450 {
451 TRACE("%s: signal size=%" B_PRIu32 "\n", __func__, buffer->size);
452
453 // TODO: If there are multiple commands in a packet, we could accumulate the
454 // responses into a single packet also...
455
456 while (buffer->size != 0) {
457 NetBufferHeaderReader<l2cap_command_header> commandHeader(buffer);
458 if (commandHeader.Status() != B_OK)
459 return ENOBUFS;
460
461 const uint8 code = commandHeader->code;
462 const uint8 ident = commandHeader->ident;
463 const uint16 length = le16toh(commandHeader->length);
464
465 L2capEndpoint* endpoint = NULL;
466 bool releaseIdent = false;
467 if (L2CAP_IS_SIGNAL_RSP(code)) {
468 endpoint = static_cast<L2capEndpoint*>(
469 btCoreData->lookup_command_ident(connection, ident));
470 releaseIdent = true;
471 }
472
473 if (buffer->size < length) {
474 ERROR("%s: invalid L2CAP signaling command packet, code=%#x, "
475 "ident=%d, length=%d, buffer size=%" B_PRIu32 "\n", __func__,
476 code, ident, length, buffer->size);
477 return EMSGSIZE;
478 }
479
480 commandHeader.Remove();
481
482 switch (code) {
483 case L2CAP_COMMAND_REJECT_RSP:
484 l2cap_handle_command_reject(endpoint, ident, buffer);
485 break;
486
487 case L2CAP_CONNECTION_REQ:
488 l2cap_handle_connection_req(connection, ident, buffer);
489 break;
490
491 case L2CAP_CONNECTION_RSP:
492 l2cap_handle_connection_rsp(endpoint, ident, buffer);
493 break;
494
495 case L2CAP_CONFIGURATION_REQ:
496 l2cap_handle_configuration_req(connection, ident,
497 buffer, length);
498 break;
499
500 case L2CAP_CONFIGURATION_RSP:
501 l2cap_handle_configuration_rsp(connection, endpoint, ident,
502 buffer, length, releaseIdent);
503 break;
504
505 case L2CAP_DISCONNECTION_REQ:
506 l2cap_handle_disconnection_req(connection, ident, buffer);
507 break;
508
509 case L2CAP_DISCONNECTION_RSP:
510 l2cap_handle_disconnection_rsp(endpoint, ident, buffer);
511 break;
512
513 case L2CAP_ECHO_REQ:
514 l2cap_handle_echo_req(connection, ident, buffer, length);
515 break;
516
517 case L2CAP_ECHO_RSP:
518 l2cap_handle_echo_rsp(endpoint, ident, buffer, length);
519 break;
520
521 case L2CAP_INFORMATION_REQ:
522 l2cap_handle_info_req(connection, ident, buffer);
523 break;
524
525 case L2CAP_INFORMATION_RSP:
526 l2cap_handle_info_rsp(endpoint, ident, buffer);
527 break;
528
529 default:
530 ERROR("l2cap: unknown L2CAP signaling command, "
531 "code=%#x\n", code);
532 send_l2cap_command_reject(connection, ident,
533 l2cap_command_reject::REJECTED_NOT_UNDERSTOOD, 0, 0, 0);
534 break;
535 }
536
537 // Only release the ident if no more signals with it are expected.
538 if (releaseIdent)
539 btCoreData->free_command_ident(connection, ident);
540
541 // Advance to the next command (if any.)
542 gBufferModule->remove_header(buffer, length);
543 }
544
545 gBufferModule->free(buffer);
546 return B_OK;
547 }
548