-
-
Notifications
You must be signed in to change notification settings - Fork 335
/
bluetooth.cpp
486 lines (365 loc) · 11 KB
/
bluetooth.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
/*
* Copyright 2008, Haiku, Inc. All Rights Reserved.
* Distributed under the terms of the MIT License.
*
* Authors:
* Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
*/
#include <new>
#include <errno.h>
#include <stdlib.h>
#include <string.h>
#include <KernelExport.h>
#include <lock.h>
#include <SupportDefs.h>
#include <util/AutoLock.h>
#include <util/DoublyLinkedList.h>
#include <net_buffer.h>
#include <net_device.h>
#include <net_stack.h>
#include <NetBufferUtilities.h>
#include <btDebug.h>
#include <btCoreData.h>
#include <btModules.h>
#include <CodeHandler.h>
#define KERNEL_LAND
#include <PortListener.h>
#undef KERNEL_LAND
#include <bluetooth/HCI/btHCI.h>
#include <bluetooth/HCI/btHCI_acl.h>
#include <bluetooth/HCI/btHCI_command.h>
#include <bluetooth/HCI/btHCI_event.h>
#include <bluetooth/HCI/btHCI_transport.h>
#include <bluetooth/HCI/btHCI_sco.h>
#include <bluetooth/bdaddrUtils.h>
#include "acl.h"
int32 api_version = B_CUR_DRIVER_API_VERSION;
typedef PortListener<void,
HCI_MAX_FRAME_SIZE, // Event Body can hold max 255 + 2 header
24 // Some devices have sent chunks of 24 events(inquiry result)
> BluetoothRawDataPort;
// Modules references
net_buffer_module_info* gBufferModule = NULL;
struct bluetooth_core_data_module_info* btCoreData = NULL;
static mutex sListLock;
static sem_id sLinkChangeSemaphore;
static DoublyLinkedList<bluetooth_device> sDeviceList;
BluetoothRawDataPort* BluetoothRXPort;
// forward declarations
status_t HciPacketHandler(void* data, int32 code, size_t size);
bluetooth_device*
FindDeviceByID(hci_id hid)
{
bluetooth_device* device;
DoublyLinkedList<bluetooth_device>::Iterator iterator
= sDeviceList.GetIterator();
while (iterator.HasNext()) {
device = iterator.Next();
if (device->index == hid)
return device;
}
return NULL;
}
status_t
PostTransportPacket(hci_id hid, bt_packet_t type, void* data, size_t count)
{
uint32 code = 0;
Bluetooth::CodeHandler::SetDevice(&code, hid);
Bluetooth::CodeHandler::SetProtocol(&code, type);
return BluetoothRXPort->Trigger(code, data, count);
}
status_t
Assemble(bluetooth_device* bluetoothDevice, bt_packet_t type, void* data,
size_t count)
{
net_buffer* nbuf = bluetoothDevice->fBuffersRx[type];
size_t currentPacketLen = 0;
while (count) {
if (nbuf == NULL) {
// new buffer incoming
switch (type) {
case BT_EVENT:
if (count >= HCI_EVENT_HDR_SIZE) {
struct hci_event_header* headerPacket
= (struct hci_event_header*)data;
bluetoothDevice->fExpectedPacketSize[type]
= HCI_EVENT_HDR_SIZE + headerPacket->elen;
if (count >= bluetoothDevice->fExpectedPacketSize[type]) {
// the whole packet is here so it can be already posted.
ERROR("%s: EVENT posted in HCI!!!\n", __func__);
btCoreData->PostEvent(bluetoothDevice, data,
bluetoothDevice->fExpectedPacketSize[type]);
} else {
nbuf = gBufferModule->create(
bluetoothDevice->fExpectedPacketSize[type]);
bluetoothDevice->fBuffersRx[type] = nbuf;
nbuf->protocol = type;
}
} else {
panic("EVENT frame corrupted\n");
return EILSEQ;
}
break;
case BT_ACL:
if (count >= HCI_ACL_HDR_SIZE) {
struct hci_acl_header* headerPkt = (struct hci_acl_header*)data;
bluetoothDevice->fExpectedPacketSize[type] = HCI_ACL_HDR_SIZE
+ B_LENDIAN_TO_HOST_INT16(headerPkt->alen);
// Create the buffer -> TODO: this allocation can fail
nbuf = gBufferModule->create(
bluetoothDevice->fExpectedPacketSize[type]);
bluetoothDevice->fBuffersRx[type] = nbuf;
nbuf->protocol = type;
} else {
panic("ACL frame corrupted\n");
return EILSEQ;
}
break;
case BT_SCO:
break;
default:
panic("unknown packet type in assembly");
break;
}
currentPacketLen = bluetoothDevice->fExpectedPacketSize[type];
} else {
// Continuation of a packet
currentPacketLen = bluetoothDevice->fExpectedPacketSize[type] - nbuf->size;
}
if (nbuf != NULL) {
currentPacketLen = min_c(currentPacketLen, count);
gBufferModule->append(nbuf, data, currentPacketLen);
if ((bluetoothDevice->fExpectedPacketSize[type] - nbuf->size) == 0) {
switch (nbuf->protocol) {
case BT_EVENT:
panic("need to send full buffer to btdatacore!\n");
btCoreData->PostEvent(bluetoothDevice, data,
bluetoothDevice->fExpectedPacketSize[type]);
break;
case BT_ACL:
// TODO: device descriptor has been fetched better not
// pass id again
TRACE("%s: ACL parsed in ACL!\n", __func__);
AclAssembly(nbuf, bluetoothDevice->index);
break;
default:
break;
}
bluetoothDevice->fBuffersRx[type] = nbuf = NULL;
bluetoothDevice->fExpectedPacketSize[type] = 0;
} else {
if (type == BT_ACL) {
TRACE("%s: ACL Packet not filled size %" B_PRIu32
" expected=%" B_PRIuSIZE "\n", __func__, nbuf->size,
bluetoothDevice->fExpectedPacketSize[type]);
}
}
}
// in case in the pipe there is info about the next buffer
count -= currentPacketLen;
data = (void*)((uint8*)data + currentPacketLen);
}
return B_OK;
}
status_t
HciPacketHandler(void* data, int32 code, size_t size)
{
hci_id deviceId = Bluetooth::CodeHandler::Device(code);
bluetooth_device* bluetoothDevice = FindDeviceByID(deviceId);
TRACE("%s: to assemble %" B_PRIuSIZE " bytes of 0x%" B_PRIx32 "\n",
__func__, size, deviceId);
if (bluetoothDevice != NULL) {
return Assemble(bluetoothDevice, Bluetooth::CodeHandler::Protocol(code),
data, size);
} else {
ERROR("%s: Device 0x%" B_PRIx32 " could not be matched\n", __func__,
deviceId);
}
return B_ERROR;
}
// #pragma mark -
status_t
RegisterDriver(bt_hci_transport_hooks* hooks, bluetooth_device** _device)
{
bluetooth_device* device = new (std::nothrow) bluetooth_device;
if (device == NULL)
return B_NO_MEMORY;
for (int index = 0; index < HCI_NUM_PACKET_TYPES; index++) {
device->fBuffersRx[index] = NULL;
device->fExpectedPacketSize[index] = 0;
}
device->info = NULL; // not yet used
device->hooks = hooks;
device->supportedPacketTypes = (HCI_DM1 | HCI_DH1 | HCI_HV1);
device->linkMode = (HCI_LM_ACCEPT);
device->mtu = L2CAP_MTU_MINIMUM; // TODO: ensure specs min value
MutexLocker _(&sListLock);
if (sDeviceList.IsEmpty())
device->index = HCI_DEVICE_INDEX_OFFSET; // REVIEW: dev index
else {
device->index = (sDeviceList.Tail())->index + 1; // REVIEW!
TRACE("%s: List not empty\n", __func__);
}
sDeviceList.Add(device);
TRACE("%s: Device %" B_PRIx32 "\n", __func__, device->index);
*_device = device;
return B_OK;
}
status_t
UnregisterDriver(hci_id id)
{
bluetooth_device* device = FindDeviceByID(id);
if (device == NULL)
return B_ERROR;
if (device->GetDoublyLinkedListLink()->next != NULL
|| device->GetDoublyLinkedListLink()->previous != NULL
|| device == sDeviceList.Head())
sDeviceList.Remove(device);
delete device;
return B_OK;
}
// PostACL
status_t
PostACL(hci_id hciId, net_buffer* buffer)
{
net_buffer* curr_frame = NULL;
net_buffer* next_frame = buffer;
uint8 flag = HCI_ACL_PACKET_START;
if (buffer == NULL)
panic("passing null buffer");
uint16 handle = buffer->type; // TODO: CodeHandler
bluetooth_device* device = FindDeviceByID(hciId);
if (device == NULL) {
ERROR("%s: No device 0x%" B_PRIx32 "\n", __func__, hciId);
return B_ERROR;
}
TRACE("%s: index 0x%" B_PRIx32 " try to send bt packet of %" B_PRIu32
" bytes (flags 0x%" B_PRIx32 "):\n", __func__, device->index,
buffer->size, buffer->flags);
// TODO: ATOMIC! any other thread should stop here
do {
// Divide packet if big enough
curr_frame = next_frame;
if (curr_frame->size > device->mtu) {
next_frame = gBufferModule->split(curr_frame, device->mtu);
} else {
next_frame = NULL;
}
// Attach acl header
{
NetBufferPrepend<struct hci_acl_header> bufferHeader(curr_frame);
status_t status = bufferHeader.Status();
if (status < B_OK) {
// free the buffer
continue;
}
bufferHeader->handle = pack_acl_handle_flags(handle, flag, 0);
bufferHeader->alen = curr_frame->size - sizeof(struct hci_acl_header);
}
// Send to driver
curr_frame->protocol = BT_ACL;
TRACE("%s: Tolower nbuf %p!\n", __func__, curr_frame);
// We could pass a cookie and avoid the driver fetch the Id
device->hooks->SendACL(device->index, curr_frame);
flag = HCI_ACL_PACKET_FRAGMENT;
} while (next_frame != NULL);
return B_OK;
}
status_t
PostSCO(hci_id hciId, net_buffer* buffer)
{
return B_ERROR;
}
status_t
PostESCO(hci_id hciId, net_buffer* buffer)
{
return B_ERROR;
}
static int
dump_bluetooth_devices(int argc, char** argv)
{
bluetooth_device* device;
DoublyLinkedList<bluetooth_device>::Iterator iterator
= sDeviceList.GetIterator();
while (iterator.HasNext()) {
device = iterator.Next();
kprintf("\tindex=%" B_PRIx32 " @%p hooks=%p\n", device->index,
device, device->hooks);
}
return 0;
}
static status_t
bluetooth_std_ops(int32 op, ...)
{
switch (op) {
case B_MODULE_INIT:
{
status_t status;
status = get_module(NET_BUFFER_MODULE_NAME,
(module_info**)&gBufferModule);
if (status < B_OK) {
panic("no way Dude we need that!");
return status;
}
status = get_module(BT_CORE_DATA_MODULE_NAME,
(module_info**)&btCoreData);
if (status < B_OK) {
ERROR("%s: problem getting bt core data module\n", __func__);
return status;
}
new (&sDeviceList) DoublyLinkedList<bluetooth_device>;
// static C++ objects are not initialized in the module startup
BluetoothRXPort = new BluetoothRawDataPort(BT_RX_PORT_NAME,
(BluetoothRawDataPort::port_listener_func)&HciPacketHandler);
if (BluetoothRXPort->Launch() != B_OK) {
ERROR("%s: RX thread creation failed!\n", __func__);
// we Cannot do much here ... avoid registering
} else {
TRACE("%s: RX thread launched!\n", __func__);
}
sLinkChangeSemaphore = create_sem(0, "bt sem");
if (sLinkChangeSemaphore < B_OK) {
put_module(NET_STACK_MODULE_NAME);
ERROR("%s: Link change semaphore failed\n", __func__);
return sLinkChangeSemaphore;
}
mutex_init(&sListLock, "bluetooth devices");
// status = InitializeAclConnectionThread();
ERROR("%s: Connection Thread error\n", __func__);
add_debugger_command("btLocalDevices", &dump_bluetooth_devices,
"Lists Bluetooth LocalDevices registered in the Stack");
return B_OK;
}
case B_MODULE_UNINIT:
{
delete_sem(sLinkChangeSemaphore);
mutex_destroy(&sListLock);
put_module(NET_BUFFER_MODULE_NAME);
put_module(NET_STACK_MODULE_NAME);
put_module(BT_CORE_DATA_MODULE_NAME);
remove_debugger_command("btLocalDevices", &dump_bluetooth_devices);
// status_t status = QuitAclConnectionThread();
return B_OK;
}
default:
return B_ERROR;
}
}
bt_hci_module_info sBluetoothModule = {
{
BT_HCI_MODULE_NAME,
B_KEEP_LOADED,
bluetooth_std_ops
},
RegisterDriver,
UnregisterDriver,
FindDeviceByID,
PostTransportPacket,
PostACL,
PostSCO,
PostESCO
};
module_info* modules[] = {
(module_info*)&sBluetoothModule,
NULL
};