aboutsummaryrefslogtreecommitdiff
path: root/lib/usbhost/USB_Host_Shield_2.0/BTD.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'lib/usbhost/USB_Host_Shield_2.0/BTD.cpp')
-rw-r--r--lib/usbhost/USB_Host_Shield_2.0/BTD.cpp1364
1 files changed, 1364 insertions, 0 deletions
diff --git a/lib/usbhost/USB_Host_Shield_2.0/BTD.cpp b/lib/usbhost/USB_Host_Shield_2.0/BTD.cpp
new file mode 100644
index 000000000..bcfba14b2
--- /dev/null
+++ b/lib/usbhost/USB_Host_Shield_2.0/BTD.cpp
@@ -0,0 +1,1364 @@
1/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
2
3 This software may be distributed and modified under the terms of the GNU
4 General Public License version 2 (GPL2) as published by the Free Software
5 Foundation and appearing in the file GPL2.TXT included in the packaging of
6 this file. Please note that GPL2 Section 2[b] requires that all works based
7 on this software must also be made publicly available under the terms of
8 the GPL2 ("Copyleft").
9
10 Contact information
11 -------------------
12
13 Kristian Lauszus, TKJ Electronics
14 Web : http://www.tkjelectronics.com
15 e-mail : kristianl@tkjelectronics.com
16 */
17
18#include "BTD.h"
19// To enable serial debugging see "settings.h"
20//#define EXTRADEBUG // Uncomment to get even more debugging data
21
22const uint8_t BTD::BTD_CONTROL_PIPE = 0;
23const uint8_t BTD::BTD_EVENT_PIPE = 1;
24const uint8_t BTD::BTD_DATAIN_PIPE = 2;
25const uint8_t BTD::BTD_DATAOUT_PIPE = 3;
26
27BTD::BTD(USB *p) :
28connectToWii(false),
29pairWithWii(false),
30connectToHIDDevice(false),
31pairWithHIDDevice(false),
32pUsb(p), // Pointer to USB class instance - mandatory
33bAddress(0), // Device address - mandatory
34bNumEP(1), // If config descriptor needs to be parsed
35qNextPollTime(0), // Reset NextPollTime
36pollInterval(0),
37bPollEnable(false) // Don't start polling before dongle is connected
38{
39 for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++)
40 btService[i] = NULL;
41
42 Initialize(); // Set all variables, endpoint structs etc. to default values
43
44 if(pUsb) // Register in USB subsystem
45 pUsb->RegisterDeviceClass(this); // Set devConfig[] entry
46}
47
48uint8_t BTD::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
49 const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR);
50 uint8_t buf[constBufSize];
51 USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
52 uint8_t rcode;
53 UsbDevice *p = NULL;
54 EpInfo *oldep_ptr = NULL;
55
56 Initialize(); // Set all variables, endpoint structs etc. to default values
57
58 AddressPool &addrPool = pUsb->GetAddressPool(); // Get memory address of USB device address pool
59#ifdef EXTRADEBUG
60 Notify(PSTR("\r\nBTD ConfigureDevice"), 0x80);
61#endif
62
63 if(bAddress) { // Check if address has already been assigned to an instance
64#ifdef DEBUG_USB_HOST
65 Notify(PSTR("\r\nAddress in use"), 0x80);
66#endif
67 return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
68 }
69
70 p = addrPool.GetUsbDevicePtr(0); // Get pointer to pseudo device with address 0 assigned
71 if(!p) {
72#ifdef DEBUG_USB_HOST
73 Notify(PSTR("\r\nAddress not found"), 0x80);
74#endif
75 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
76 }
77
78 if(!p->epinfo) {
79#ifdef DEBUG_USB_HOST
80 Notify(PSTR("\r\nepinfo is null"), 0x80);
81#endif
82 return USB_ERROR_EPINFO_IS_NULL;
83 }
84
85 oldep_ptr = p->epinfo; // Save old pointer to EP_RECORD of address 0
86 p->epinfo = epInfo; // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence
87 p->lowspeed = lowspeed;
88 rcode = pUsb->getDevDescr(0, 0, constBufSize, (uint8_t*)buf); // Get device descriptor - addr, ep, nbytes, data
89
90 p->epinfo = oldep_ptr; // Restore p->epinfo
91
92 if(rcode)
93 goto FailGetDevDescr;
94
95 bAddress = addrPool.AllocAddress(parent, false, port); // Allocate new address according to device class
96
97 if(!bAddress) {
98#ifdef DEBUG_USB_HOST
99 Notify(PSTR("\r\nOut of address space"), 0x80);
100#endif
101 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
102 }
103
104 epInfo[0].maxPktSize = udd->bMaxPacketSize0; // Extract Max Packet Size from device descriptor
105 epInfo[1].epAddr = udd->bNumConfigurations; // Steal and abuse from epInfo structure to save memory
106
107 VID = udd->idVendor;
108 PID = udd->idProduct;
109
110 return USB_ERROR_CONFIG_REQUIRES_ADDITIONAL_RESET;
111
112FailGetDevDescr:
113#ifdef DEBUG_USB_HOST
114 NotifyFailGetDevDescr(rcode);
115#endif
116 if(rcode != hrJERR)
117 rcode = USB_ERROR_FailGetDevDescr;
118 Release();
119 return rcode;
120};
121
122uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
123 uint8_t rcode;
124 uint8_t num_of_conf = epInfo[1].epAddr; // Number of configurations
125 epInfo[1].epAddr = 0;
126
127 AddressPool &addrPool = pUsb->GetAddressPool();
128#ifdef EXTRADEBUG
129 Notify(PSTR("\r\nBTD Init"), 0x80);
130#endif
131 UsbDevice *p = addrPool.GetUsbDevicePtr(bAddress); // Get pointer to assigned address record
132
133 if(!p) {
134#ifdef DEBUG_USB_HOST
135 Notify(PSTR("\r\nAddress not found"), 0x80);
136#endif
137 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
138 }
139
140 delay(300); // Assign new address to the device
141
142 rcode = pUsb->setAddr(0, 0, bAddress); // Assign new address to the device
143 if(rcode) {
144#ifdef DEBUG_USB_HOST
145 Notify(PSTR("\r\nsetAddr: "), 0x80);
146 D_PrintHex<uint8_t > (rcode, 0x80);
147#endif
148 p->lowspeed = false;
149 goto Fail;
150 }
151#ifdef EXTRADEBUG
152 Notify(PSTR("\r\nAddr: "), 0x80);
153 D_PrintHex<uint8_t > (bAddress, 0x80);
154#endif
155
156 p->lowspeed = false;
157
158 p = addrPool.GetUsbDevicePtr(bAddress); // Get pointer to assigned address record
159 if(!p) {
160#ifdef DEBUG_USB_HOST
161 Notify(PSTR("\r\nAddress not found"), 0x80);
162#endif
163 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
164 }
165
166 p->lowspeed = lowspeed;
167
168 rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); // Assign epInfo to epinfo pointer - only EP0 is known
169 if(rcode)
170 goto FailSetDevTblEntry;
171
172 if(VID == PS3_VID && (PID == PS3_PID || PID == PS3NAVIGATION_PID || PID == PS3MOVE_PID)) {
173 delay(100);
174 rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 1); // We only need the Control endpoint, so we don't have to initialize the other endpoints of device
175 if(rcode)
176 goto FailSetConfDescr;
177
178#ifdef DEBUG_USB_HOST
179 if(PID == PS3_PID || PID == PS3NAVIGATION_PID) {
180 if(PID == PS3_PID)
181 Notify(PSTR("\r\nDualshock 3 Controller Connected"), 0x80);
182 else // It must be a navigation controller
183 Notify(PSTR("\r\nNavigation Controller Connected"), 0x80);
184 } else // It must be a Motion controller
185 Notify(PSTR("\r\nMotion Controller Connected"), 0x80);
186#endif
187
188 if(my_bdaddr[0] == 0x00 && my_bdaddr[1] == 0x00 && my_bdaddr[2] == 0x00 && my_bdaddr[3] == 0x00 && my_bdaddr[4] == 0x00 && my_bdaddr[5] == 0x00) {
189#ifdef DEBUG_USB_HOST
190 Notify(PSTR("\r\nPlease plug in the dongle before trying to pair with the PS3 Controller\r\nor set the Bluetooth address in the constructor of the PS3BT class"), 0x80);
191#endif
192 } else {
193 if(PID == PS3_PID || PID == PS3NAVIGATION_PID)
194 setBdaddr(my_bdaddr); // Set internal Bluetooth address
195 else
196 setMoveBdaddr(my_bdaddr); // Set internal Bluetooth address
197#ifdef DEBUG_USB_HOST
198 Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80);
199 for(int8_t i = 5; i > 0; i--) {
200 D_PrintHex<uint8_t > (my_bdaddr[i], 0x80);
201 Notify(PSTR(":"), 0x80);
202 }
203 D_PrintHex<uint8_t > (my_bdaddr[0], 0x80);
204#endif
205 }
206
207 pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 0); // Reset configuration value
208 pUsb->setAddr(bAddress, 0, 0); // Reset address
209 Release(); // Release device
210 return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; // Return
211 } else {
212 // Check if attached device is a Bluetooth dongle and fill endpoint data structure
213 // First interface in the configuration must have Bluetooth assigned Class/Subclass/Protocol
214 // And 3 endpoints - interrupt-IN, bulk-IN, bulk-OUT, not necessarily in this order
215 for(uint8_t i = 0; i < num_of_conf; i++) {
216 if(VID == IOGEAR_GBU521_VID && PID == IOGEAR_GBU521_PID) {
217 ConfigDescParser<USB_CLASS_VENDOR_SPECIFIC, WI_SUBCLASS_RF, WI_PROTOCOL_BT, CP_MASK_COMPARE_ALL> confDescrParser(this); // Needed for the IOGEAR GBU521
218 rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
219 } else {
220 ConfigDescParser<USB_CLASS_WIRELESS_CTRL, WI_SUBCLASS_RF, WI_PROTOCOL_BT, CP_MASK_COMPARE_ALL> confDescrParser(this);
221 rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
222 }
223 if(rcode) // Check error code
224 goto FailGetConfDescr;
225 if(bNumEP >= BTD_MAX_ENDPOINTS) // All endpoints extracted
226 break;
227 }
228
229 if(bNumEP < BTD_MAX_ENDPOINTS)
230 goto FailUnknownDevice;
231
232 // Assign epInfo to epinfo pointer - this time all 3 endpoins
233 rcode = pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo);
234 if(rcode)
235 goto FailSetDevTblEntry;
236
237 // Set Configuration Value
238 rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bConfNum);
239 if(rcode)
240 goto FailSetConfDescr;
241
242 hci_num_reset_loops = 100; // only loop 100 times before trying to send the hci reset command
243 hci_counter = 0;
244 hci_state = HCI_INIT_STATE;
245 watingForConnection = false;
246 bPollEnable = true;
247
248#ifdef DEBUG_USB_HOST
249 Notify(PSTR("\r\nBluetooth Dongle Initialized"), 0x80);
250#endif
251 }
252 return 0; // Successful configuration
253
254 /* Diagnostic messages */
255FailSetDevTblEntry:
256#ifdef DEBUG_USB_HOST
257 NotifyFailSetDevTblEntry();
258 goto Fail;
259#endif
260
261FailGetConfDescr:
262#ifdef DEBUG_USB_HOST
263 NotifyFailGetConfDescr();
264 goto Fail;
265#endif
266
267FailSetConfDescr:
268#ifdef DEBUG_USB_HOST
269 NotifyFailSetConfDescr();
270#endif
271 goto Fail;
272
273FailUnknownDevice:
274#ifdef DEBUG_USB_HOST
275 NotifyFailUnknownDevice(VID, PID);
276#endif
277 pUsb->setAddr(bAddress, 0, 0); // Reset address
278 rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
279Fail:
280#ifdef DEBUG_USB_HOST
281 Notify(PSTR("\r\nBTD Init Failed, error code: "), 0x80);
282 NotifyFail(rcode);
283#endif
284 Release();
285 return rcode;
286}
287
288void BTD::Initialize() {
289 uint8_t i;
290 for(i = 0; i < BTD_MAX_ENDPOINTS; i++) {
291 epInfo[i].epAddr = 0;
292 epInfo[i].maxPktSize = (i) ? 0 : 8;
293 epInfo[i].epAttribs = 0;
294 epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
295 }
296 for(i = 0; i < BTD_NUM_SERVICES; i++) {
297 if(btService[i])
298 btService[i]->Reset(); // Reset all Bluetooth services
299 }
300
301 connectToWii = false;
302 incomingWii = false;
303 connectToHIDDevice = false;
304 incomingHIDDevice = false;
305 incomingPS4 = false;
306 bAddress = 0; // Clear device address
307 bNumEP = 1; // Must have to be reset to 1
308 qNextPollTime = 0; // Reset next poll time
309 pollInterval = 0;
310 bPollEnable = false; // Don't start polling before dongle is connected
311}
312
313/* Extracts interrupt-IN, bulk-IN, bulk-OUT endpoint information from config descriptor */
314void BTD::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *pep) {
315 //ErrorMessage<uint8_t>(PSTR("Conf.Val"),conf);
316 //ErrorMessage<uint8_t>(PSTR("Iface Num"),iface);
317 //ErrorMessage<uint8_t>(PSTR("Alt.Set"),alt);
318
319 if(alt) // Wrong interface - by BT spec, no alt setting
320 return;
321
322 bConfNum = conf;
323 uint8_t index;
324
325 if((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80) { // Interrupt In endpoint found
326 index = BTD_EVENT_PIPE;
327 epInfo[index].bmNakPower = USB_NAK_NOWAIT;
328 } else {
329 if((pep->bmAttributes & 0x02) == 2) // Bulk endpoint found
330 index = ((pep->bEndpointAddress & 0x80) == 0x80) ? BTD_DATAIN_PIPE : BTD_DATAOUT_PIPE;
331 else
332 return;
333 }
334
335 // Fill the rest of endpoint data structure
336 epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F);
337 epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize;
338#ifdef EXTRADEBUG
339 PrintEndpointDescriptor(pep);
340#endif
341 if(pollInterval < pep->bInterval) // Set the polling interval as the largest polling interval obtained from endpoints
342 pollInterval = pep->bInterval;
343 bNumEP++;
344}
345
346void BTD::PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR* ep_ptr) {
347#ifdef EXTRADEBUG
348 Notify(PSTR("\r\nEndpoint descriptor:"), 0x80);
349 Notify(PSTR("\r\nLength:\t\t"), 0x80);
350 D_PrintHex<uint8_t > (ep_ptr->bLength, 0x80);
351 Notify(PSTR("\r\nType:\t\t"), 0x80);
352 D_PrintHex<uint8_t > (ep_ptr->bDescriptorType, 0x80);
353 Notify(PSTR("\r\nAddress:\t"), 0x80);
354 D_PrintHex<uint8_t > (ep_ptr->bEndpointAddress, 0x80);
355 Notify(PSTR("\r\nAttributes:\t"), 0x80);
356 D_PrintHex<uint8_t > (ep_ptr->bmAttributes, 0x80);
357 Notify(PSTR("\r\nMaxPktSize:\t"), 0x80);
358 D_PrintHex<uint16_t > (ep_ptr->wMaxPacketSize, 0x80);
359 Notify(PSTR("\r\nPoll Intrv:\t"), 0x80);
360 D_PrintHex<uint8_t > (ep_ptr->bInterval, 0x80);
361#endif
362}
363
364/* Performs a cleanup after failed Init() attempt */
365uint8_t BTD::Release() {
366 Initialize(); // Set all variables, endpoint structs etc. to default values
367 pUsb->GetAddressPool().FreeAddress(bAddress);
368 return 0;
369}
370
371uint8_t BTD::Poll() {
372 if(!bPollEnable)
373 return 0;
374 if((long)(millis() - qNextPollTime) >= 0L) { // Don't poll if shorter than polling interval
375 qNextPollTime = millis() + pollInterval; // Set new poll time
376 HCI_event_task(); // Poll the HCI event pipe
377 HCI_task(); // HCI state machine
378 ACL_event_task(); // Poll the ACL input pipe too
379 }
380 return 0;
381}
382
383void BTD::disconnect() {
384 for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++)
385 if(btService[i])
386 btService[i]->disconnect();
387};
388
389void BTD::HCI_event_task() {
390 uint16_t length = BULK_MAXPKTSIZE; // Request more than 16 bytes anyway, the inTransfer routine will take care of this
391 uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_EVENT_PIPE ].epAddr, &length, hcibuf); // Input on endpoint 1
392
393 if(!rcode || rcode == hrNAK) { // Check for errors
394 switch(hcibuf[0]) { // Switch on event type
395 case EV_COMMAND_COMPLETE:
396 if(!hcibuf[5]) { // Check if command succeeded
397 hci_set_flag(HCI_FLAG_CMD_COMPLETE); // Set command complete flag
398 if((hcibuf[3] == 0x01) && (hcibuf[4] == 0x10)) { // Parameters from read local version information
399 hci_version = hcibuf[6]; // Used to check if it supports 2.0+EDR - see http://www.bluetooth.org/Technical/AssignedNumbers/hci.htm
400 hci_set_flag(HCI_FLAG_READ_VERSION);
401 } else if((hcibuf[3] == 0x09) && (hcibuf[4] == 0x10)) { // Parameters from read local bluetooth address
402 for(uint8_t i = 0; i < 6; i++)
403 my_bdaddr[i] = hcibuf[6 + i];
404 hci_set_flag(HCI_FLAG_READ_BDADDR);
405 }
406 }
407 break;
408
409 case EV_COMMAND_STATUS:
410 if(hcibuf[2]) { // Show status on serial if not OK
411#ifdef DEBUG_USB_HOST
412 Notify(PSTR("\r\nHCI Command Failed: "), 0x80);
413 D_PrintHex<uint8_t > (hcibuf[2], 0x80);
414#endif
415 }
416 break;
417
418 case EV_INQUIRY_COMPLETE:
419 if(inquiry_counter >= 5 && (pairWithWii || pairWithHIDDevice)) {
420 inquiry_counter = 0;
421#ifdef DEBUG_USB_HOST
422 if(pairWithWii)
423 Notify(PSTR("\r\nCouldn't find Wiimote"), 0x80);
424 else
425 Notify(PSTR("\r\nCouldn't find HID device"), 0x80);
426#endif
427 connectToWii = false;
428 pairWithWii = false;
429 connectToHIDDevice = false;
430 pairWithHIDDevice = false;
431 hci_state = HCI_SCANNING_STATE;
432 }
433 inquiry_counter++;
434 break;
435
436 case EV_INQUIRY_RESULT:
437 if(hcibuf[2]) { // Check that there is more than zero responses
438#ifdef EXTRADEBUG
439 Notify(PSTR("\r\nNumber of responses: "), 0x80);
440 Notify(hcibuf[2], 0x80);
441#endif
442 for(uint8_t i = 0; i < hcibuf[2]; i++) {
443 uint8_t offset = 8 * hcibuf[2] + 3 * i;
444
445 for(uint8_t j = 0; j < 3; j++)
446 classOfDevice[j] = hcibuf[j + 4 + offset];
447
448#ifdef EXTRADEBUG
449 Notify(PSTR("\r\nClass of device: "), 0x80);
450 D_PrintHex<uint8_t > (classOfDevice[2], 0x80);
451 Notify(PSTR(" "), 0x80);
452 D_PrintHex<uint8_t > (classOfDevice[1], 0x80);
453 Notify(PSTR(" "), 0x80);
454 D_PrintHex<uint8_t > (classOfDevice[0], 0x80);
455#endif
456
457 if(pairWithWii && classOfDevice[2] == 0x00 && (classOfDevice[1] & 0x05) && (classOfDevice[0] & 0x0C)) { // See http://wiibrew.org/wiki/Wiimote#SDP_information
458 checkRemoteName = true; // Check remote name to distinguish between the different controllers
459
460 for(uint8_t j = 0; j < 6; j++)
461 disc_bdaddr[j] = hcibuf[j + 3 + 6 * i];
462
463 hci_set_flag(HCI_FLAG_DEVICE_FOUND);
464 break;
465 } else if(pairWithHIDDevice && (classOfDevice[1] & 0x05) && (classOfDevice[0] & 0xC8)) { // Check if it is a mouse, keyboard or a gamepad - see: http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html
466#ifdef DEBUG_USB_HOST
467 if(classOfDevice[0] & 0x80)
468 Notify(PSTR("\r\nMouse found"), 0x80);
469 if(classOfDevice[0] & 0x40)
470 Notify(PSTR("\r\nKeyboard found"), 0x80);
471 if(classOfDevice[0] & 0x08)
472 Notify(PSTR("\r\nGamepad found"), 0x80);
473#endif
474
475 for(uint8_t j = 0; j < 6; j++)
476 disc_bdaddr[j] = hcibuf[j + 3 + 6 * i];
477
478 hci_set_flag(HCI_FLAG_DEVICE_FOUND);
479 break;
480 }
481 }
482 }
483 break;
484
485 case EV_CONNECT_COMPLETE:
486 hci_set_flag(HCI_FLAG_CONNECT_EVENT);
487 if(!hcibuf[2]) { // Check if connected OK
488#ifdef EXTRADEBUG
489 Notify(PSTR("\r\nConnection established"), 0x80);
490#endif
491 hci_handle = hcibuf[3] | ((hcibuf[4] & 0x0F) << 8); // Store the handle for the ACL connection
492 hci_set_flag(HCI_FLAG_CONNECT_COMPLETE); // Set connection complete flag
493 } else {
494 hci_state = HCI_CHECK_DEVICE_SERVICE;
495#ifdef DEBUG_USB_HOST
496 Notify(PSTR("\r\nConnection Failed: "), 0x80);
497 D_PrintHex<uint8_t > (hcibuf[2], 0x80);
498#endif
499 }
500 break;
501
502 case EV_DISCONNECT_COMPLETE:
503 if(!hcibuf[2]) { // Check if disconnected OK
504 hci_set_flag(HCI_FLAG_DISCONNECT_COMPLETE); // Set disconnect command complete flag
505 hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE); // Clear connection complete flag
506 }
507 break;
508
509 case EV_REMOTE_NAME_COMPLETE:
510 if(!hcibuf[2]) { // Check if reading is OK
511 for(uint8_t i = 0; i < min(sizeof (remote_name), sizeof (hcibuf) - 9); i++) {
512 remote_name[i] = hcibuf[9 + i];
513 if(remote_name[i] == '\0') // End of string
514 break;
515 }
516 // TODO: Altid sæt '\0' i remote name!
517 hci_set_flag(HCI_FLAG_REMOTE_NAME_COMPLETE);
518 }
519 break;
520
521 case EV_INCOMING_CONNECT:
522 for(uint8_t i = 0; i < 6; i++)
523 disc_bdaddr[i] = hcibuf[i + 2];
524
525 for(uint8_t i = 0; i < 3; i++)
526 classOfDevice[i] = hcibuf[i + 8];
527
528 if((classOfDevice[1] & 0x05) && (classOfDevice[0] & 0xC8)) { // Check if it is a mouse, keyboard or a gamepad
529#ifdef DEBUG_USB_HOST
530 if(classOfDevice[0] & 0x80)
531 Notify(PSTR("\r\nMouse is connecting"), 0x80);
532 if(classOfDevice[0] & 0x40)
533 Notify(PSTR("\r\nKeyboard is connecting"), 0x80);
534 if(classOfDevice[0] & 0x08)
535 Notify(PSTR("\r\nGamepad is connecting"), 0x80);
536#endif
537 incomingHIDDevice = true;
538 }
539
540#ifdef EXTRADEBUG
541 Notify(PSTR("\r\nClass of device: "), 0x80);
542 D_PrintHex<uint8_t > (classOfDevice[2], 0x80);
543 Notify(PSTR(" "), 0x80);
544 D_PrintHex<uint8_t > (classOfDevice[1], 0x80);
545 Notify(PSTR(" "), 0x80);
546 D_PrintHex<uint8_t > (classOfDevice[0], 0x80);
547#endif
548 hci_set_flag(HCI_FLAG_INCOMING_REQUEST);
549 break;
550
551 case EV_PIN_CODE_REQUEST:
552 if(pairWithWii) {
553#ifdef DEBUG_USB_HOST
554 Notify(PSTR("\r\nPairing with Wiimote"), 0x80);
555#endif
556 hci_pin_code_request_reply();
557 } else if(btdPin != NULL) {
558#ifdef DEBUG_USB_HOST
559 Notify(PSTR("\r\nBluetooth pin is set too: "), 0x80);
560 NotifyStr(btdPin, 0x80);
561#endif
562 hci_pin_code_request_reply();
563 } else {
564#ifdef DEBUG_USB_HOST
565 Notify(PSTR("\r\nNo pin was set"), 0x80);
566#endif
567 hci_pin_code_negative_request_reply();
568 }
569 break;
570
571 case EV_LINK_KEY_REQUEST:
572#ifdef DEBUG_USB_HOST
573 Notify(PSTR("\r\nReceived Key Request"), 0x80);
574#endif
575 hci_link_key_request_negative_reply();
576 break;
577
578 case EV_AUTHENTICATION_COMPLETE:
579 if(pairWithWii && !connectToWii) {
580#ifdef DEBUG_USB_HOST
581 Notify(PSTR("\r\nPairing successful with Wiimote"), 0x80);
582#endif
583 connectToWii = true; // Used to indicate to the Wii service, that it should connect to this device
584 } else if(pairWithHIDDevice && !connectToHIDDevice) {
585#ifdef DEBUG_USB_HOST
586 Notify(PSTR("\r\nPairing successful with HID device"), 0x80);
587#endif
588 connectToHIDDevice = true; // Used to indicate to the BTHID service, that it should connect to this device
589 }
590 break;
591 /* We will just ignore the following events */
592 case EV_NUM_COMPLETE_PKT:
593 case EV_ROLE_CHANGED:
594 case EV_PAGE_SCAN_REP_MODE:
595 case EV_LOOPBACK_COMMAND:
596 case EV_DATA_BUFFER_OVERFLOW:
597 case EV_CHANGE_CONNECTION_LINK:
598 case EV_MAX_SLOTS_CHANGE:
599 case EV_QOS_SETUP_COMPLETE:
600 case EV_LINK_KEY_NOTIFICATION:
601 case EV_ENCRYPTION_CHANGE:
602 case EV_READ_REMOTE_VERSION_INFORMATION_COMPLETE:
603 break;
604#ifdef EXTRADEBUG
605 default:
606 if(hcibuf[0] != 0x00) {
607 Notify(PSTR("\r\nUnmanaged HCI Event: "), 0x80);
608 D_PrintHex<uint8_t > (hcibuf[0], 0x80);
609 }
610 break;
611#endif
612 } // Switch
613 }
614#ifdef EXTRADEBUG
615 else {
616 Notify(PSTR("\r\nHCI event error: "), 0x80);
617 D_PrintHex<uint8_t > (rcode, 0x80);
618 }
619#endif
620}
621
622/* Poll Bluetooth and print result */
623void BTD::HCI_task() {
624 switch(hci_state) {
625 case HCI_INIT_STATE:
626 hci_counter++;
627 if(hci_counter > hci_num_reset_loops) { // wait until we have looped x times to clear any old events
628 hci_reset();
629 hci_state = HCI_RESET_STATE;
630 hci_counter = 0;
631 }
632 break;
633
634 case HCI_RESET_STATE:
635 hci_counter++;
636 if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
637 hci_counter = 0;
638#ifdef DEBUG_USB_HOST
639 Notify(PSTR("\r\nHCI Reset complete"), 0x80);
640#endif
641 hci_state = HCI_CLASS_STATE;
642 hci_write_class_of_device();
643 } else if(hci_counter > hci_num_reset_loops) {
644 hci_num_reset_loops *= 10;
645 if(hci_num_reset_loops > 2000)
646 hci_num_reset_loops = 2000;
647#ifdef DEBUG_USB_HOST
648 Notify(PSTR("\r\nNo response to HCI Reset"), 0x80);
649#endif
650 hci_state = HCI_INIT_STATE;
651 hci_counter = 0;
652 }
653 break;
654
655 case HCI_CLASS_STATE:
656 if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
657#ifdef DEBUG_USB_HOST
658 Notify(PSTR("\r\nWrite class of device"), 0x80);
659#endif
660 hci_state = HCI_BDADDR_STATE;
661 hci_read_bdaddr();
662 }
663 break;
664
665 case HCI_BDADDR_STATE:
666 if(hci_check_flag(HCI_FLAG_READ_BDADDR)) {
667#ifdef DEBUG_USB_HOST
668 Notify(PSTR("\r\nLocal Bluetooth Address: "), 0x80);
669 for(int8_t i = 5; i > 0; i--) {
670 D_PrintHex<uint8_t > (my_bdaddr[i], 0x80);
671 Notify(PSTR(":"), 0x80);
672 }
673 D_PrintHex<uint8_t > (my_bdaddr[0], 0x80);
674#endif
675 hci_read_local_version_information();
676 hci_state = HCI_LOCAL_VERSION_STATE;
677 }
678 break;
679
680 case HCI_LOCAL_VERSION_STATE: // The local version is used by the PS3BT class
681 if(hci_check_flag(HCI_FLAG_READ_VERSION)) {
682 if(btdName != NULL) {
683 hci_set_local_name(btdName);
684 hci_state = HCI_SET_NAME_STATE;
685 } else
686 hci_state = HCI_CHECK_DEVICE_SERVICE;
687 }
688 break;
689
690 case HCI_SET_NAME_STATE:
691 if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
692#ifdef DEBUG_USB_HOST
693 Notify(PSTR("\r\nThe name is set to: "), 0x80);
694 NotifyStr(btdName, 0x80);
695#endif
696 hci_state = HCI_CHECK_DEVICE_SERVICE;
697 }
698 break;
699
700 case HCI_CHECK_DEVICE_SERVICE:
701 if(pairWithHIDDevice || pairWithWii) { // Check if it should try to connect to a Wiimote
702#ifdef DEBUG_USB_HOST
703 if(pairWithWii)
704 Notify(PSTR("\r\nStarting inquiry\r\nPress 1 & 2 on the Wiimote\r\nOr press the SYNC button if you are using a Wii U Pro Controller or a Wii Balance Board"), 0x80);
705 else
706 Notify(PSTR("\r\nPlease enable discovery of your device"), 0x80);
707#endif
708 hci_inquiry();
709 hci_state = HCI_INQUIRY_STATE;
710 } else
711 hci_state = HCI_SCANNING_STATE; // Don't try to connect to a Wiimote
712 break;
713
714 case HCI_INQUIRY_STATE:
715 if(hci_check_flag(HCI_FLAG_DEVICE_FOUND)) {
716 hci_inquiry_cancel(); // Stop inquiry
717#ifdef DEBUG_USB_HOST
718 if(pairWithWii)
719 Notify(PSTR("\r\nWiimote found"), 0x80);
720 else
721 Notify(PSTR("\r\nHID device found"), 0x80);
722
723 Notify(PSTR("\r\nNow just create the instance like so:"), 0x80);
724 if(pairWithWii)
725 Notify(PSTR("\r\nWII Wii(&Btd);"), 0x80);
726 else
727 Notify(PSTR("\r\nBTHID bthid(&Btd);"), 0x80);
728
729 Notify(PSTR("\r\nAnd then press any button on the "), 0x80);
730 if(pairWithWii)
731 Notify(PSTR("Wiimote"), 0x80);
732 else
733 Notify(PSTR("device"), 0x80);
734#endif
735 if(checkRemoteName) {
736 hci_remote_name(); // We need to know the name to distinguish between the Wiimote, the new Wiimote with Motion Plus inside, a Wii U Pro Controller and a Wii Balance Board
737 hci_state = HCI_REMOTE_NAME_STATE;
738 } else
739 hci_state = HCI_CONNECT_DEVICE_STATE;
740 }
741 break;
742
743 case HCI_CONNECT_DEVICE_STATE:
744 if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
745#ifdef DEBUG_USB_HOST
746 if(pairWithWii)
747 Notify(PSTR("\r\nConnecting to Wiimote"), 0x80);
748 else
749 Notify(PSTR("\r\nConnecting to HID device"), 0x80);
750#endif
751 checkRemoteName = false;
752 hci_connect();
753 hci_state = HCI_CONNECTED_DEVICE_STATE;
754 }
755 break;
756
757 case HCI_CONNECTED_DEVICE_STATE:
758 if(hci_check_flag(HCI_FLAG_CONNECT_EVENT)) {
759 if(hci_check_flag(HCI_FLAG_CONNECT_COMPLETE)) {
760#ifdef DEBUG_USB_HOST
761 if(pairWithWii)
762 Notify(PSTR("\r\nConnected to Wiimote"), 0x80);
763 else
764 Notify(PSTR("\r\nConnected to HID device"), 0x80);
765#endif
766 hci_authentication_request(); // This will start the pairing with the Wiimote
767 hci_state = HCI_SCANNING_STATE;
768 } else {
769#ifdef DEBUG_USB_HOST
770 Notify(PSTR("\r\nTrying to connect one more time..."), 0x80);
771#endif
772 hci_connect(); // Try to connect one more time
773 }
774 }
775 break;
776
777 case HCI_SCANNING_STATE:
778 if(!connectToWii && !pairWithWii && !connectToHIDDevice && !pairWithHIDDevice) {
779#ifdef DEBUG_USB_HOST
780 Notify(PSTR("\r\nWait For Incoming Connection Request"), 0x80);
781#endif
782 hci_write_scan_enable();
783 watingForConnection = true;
784 hci_state = HCI_CONNECT_IN_STATE;
785 }
786 break;
787
788 case HCI_CONNECT_IN_STATE:
789 if(hci_check_flag(HCI_FLAG_INCOMING_REQUEST)) {
790 watingForConnection = false;
791#ifdef DEBUG_USB_HOST
792 Notify(PSTR("\r\nIncoming Connection Request"), 0x80);
793#endif
794 hci_remote_name();
795 hci_state = HCI_REMOTE_NAME_STATE;
796 } else if(hci_check_flag(HCI_FLAG_DISCONNECT_COMPLETE))
797 hci_state = HCI_DISCONNECT_STATE;
798 break;
799
800 case HCI_REMOTE_NAME_STATE:
801 if(hci_check_flag(HCI_FLAG_REMOTE_NAME_COMPLETE)) {
802#ifdef DEBUG_USB_HOST
803 Notify(PSTR("\r\nRemote Name: "), 0x80);
804 for(uint8_t i = 0; i < strlen(remote_name); i++)
805 Notifyc(remote_name[i], 0x80);
806#endif
807 if(strncmp((const char*)remote_name, "Nintendo", 8) == 0) {
808 incomingWii = true;
809 motionPlusInside = false;
810 wiiUProController = false;
811 pairWiiUsingSync = false;
812#ifdef DEBUG_USB_HOST
813 Notify(PSTR("\r\nWiimote is connecting"), 0x80);
814#endif
815 if(strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-TR", 22) == 0) {
816#ifdef DEBUG_USB_HOST
817 Notify(PSTR(" with Motion Plus Inside"), 0x80);
818#endif
819 motionPlusInside = true;
820 } else if(strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-UC", 22) == 0) {
821#ifdef DEBUG_USB_HOST
822 Notify(PSTR(" - Wii U Pro Controller"), 0x80);
823#endif
824 wiiUProController = motionPlusInside = pairWiiUsingSync = true;
825 } else if(strncmp((const char*)remote_name, "Nintendo RVL-WBC-01", 19) == 0) {
826#ifdef DEBUG_USB_HOST
827 Notify(PSTR(" - Wii Balance Board"), 0x80);
828#endif
829 pairWiiUsingSync = true;
830 }
831 }
832 if(classOfDevice[2] == 0 && classOfDevice[1] == 0x25 && classOfDevice[0] == 0x08 && strncmp((const char*)remote_name, "Wireless Controller", 19) == 0) {
833#ifdef DEBUG_USB_HOST
834 Notify(PSTR("\r\nPS4 controller is connecting"), 0x80);
835#endif
836 incomingPS4 = true;
837 }
838 if(pairWithWii && checkRemoteName)
839 hci_state = HCI_CONNECT_DEVICE_STATE;
840 else {
841 hci_accept_connection();
842 hci_state = HCI_CONNECTED_STATE;
843 }
844 }
845 break;
846
847 case HCI_CONNECTED_STATE:
848 if(hci_check_flag(HCI_FLAG_CONNECT_COMPLETE)) {
849#ifdef DEBUG_USB_HOST
850 Notify(PSTR("\r\nConnected to Device: "), 0x80);
851 for(int8_t i = 5; i > 0; i--) {
852 D_PrintHex<uint8_t > (disc_bdaddr[i], 0x80);
853 Notify(PSTR(":"), 0x80);
854 }
855 D_PrintHex<uint8_t > (disc_bdaddr[0], 0x80);
856#endif
857 if(incomingPS4)
858 connectToHIDDevice = true; // We should always connect to the PS4 controller
859
860 // Clear these flags for a new connection
861 l2capConnectionClaimed = false;
862 sdpConnectionClaimed = false;
863 rfcommConnectionClaimed = false;
864
865 hci_event_flag = 0;
866 hci_state = HCI_DONE_STATE;
867 }
868 break;
869
870 case HCI_DONE_STATE:
871 hci_counter++;
872 if(hci_counter > 1000) { // Wait until we have looped 1000 times to make sure that the L2CAP connection has been started
873 hci_counter = 0;
874 hci_state = HCI_SCANNING_STATE;
875 }
876 break;
877
878 case HCI_DISCONNECT_STATE:
879 if(hci_check_flag(HCI_FLAG_DISCONNECT_COMPLETE)) {
880#ifdef DEBUG_USB_HOST
881 Notify(PSTR("\r\nHCI Disconnected from Device"), 0x80);
882#endif
883 hci_event_flag = 0; // Clear all flags
884
885 // Reset all buffers
886 memset(hcibuf, 0, BULK_MAXPKTSIZE);
887 memset(l2capinbuf, 0, BULK_MAXPKTSIZE);
888
889 connectToWii = incomingWii = pairWithWii = false;
890 connectToHIDDevice = incomingHIDDevice = pairWithHIDDevice = checkRemoteName = false;
891 incomingPS4 = false;
892
893 hci_state = HCI_SCANNING_STATE;
894 }
895 break;
896 default:
897 break;
898 }
899}
900
901void BTD::ACL_event_task() {
902 uint16_t length = BULK_MAXPKTSIZE;
903 uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_DATAIN_PIPE ].epAddr, &length, l2capinbuf); // Input on endpoint 2
904
905 if(!rcode) { // Check for errors
906 if(length > 0) { // Check if any data was read
907 for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++) {
908 if(btService[i])
909 btService[i]->ACLData(l2capinbuf);
910 }
911 }
912 }
913#ifdef EXTRADEBUG
914 else if(rcode != hrNAK) {
915 Notify(PSTR("\r\nACL data in error: "), 0x80);
916 D_PrintHex<uint8_t > (rcode, 0x80);
917 }
918#endif
919 for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++)
920 if(btService[i])
921 btService[i]->Run();
922}
923
924/************************************************************/
925/* HCI Commands */
926
927/************************************************************/
928void BTD::HCI_Command(uint8_t* data, uint16_t nbytes) {
929 hci_clear_flag(HCI_FLAG_CMD_COMPLETE);
930 pUsb->ctrlReq(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bmREQ_HCI_OUT, 0x00, 0x00, 0x00, 0x00, nbytes, nbytes, data, NULL);
931}
932
933void BTD::hci_reset() {
934 hci_event_flag = 0; // Clear all the flags
935 hcibuf[0] = 0x03; // HCI OCF = 3
936 hcibuf[1] = 0x03 << 2; // HCI OGF = 3
937 hcibuf[2] = 0x00;
938
939 HCI_Command(hcibuf, 3);
940}
941
942void BTD::hci_write_scan_enable() {
943 hci_clear_flag(HCI_FLAG_INCOMING_REQUEST);
944 hcibuf[0] = 0x1A; // HCI OCF = 1A
945 hcibuf[1] = 0x03 << 2; // HCI OGF = 3
946 hcibuf[2] = 0x01; // parameter length = 1
947 if(btdName != NULL)
948 hcibuf[3] = 0x03; // Inquiry Scan enabled. Page Scan enabled.
949 else
950 hcibuf[3] = 0x02; // Inquiry Scan disabled. Page Scan enabled.
951
952 HCI_Command(hcibuf, 4);
953}
954
955void BTD::hci_write_scan_disable() {
956 hcibuf[0] = 0x1A; // HCI OCF = 1A
957 hcibuf[1] = 0x03 << 2; // HCI OGF = 3
958 hcibuf[2] = 0x01; // parameter length = 1
959 hcibuf[3] = 0x00; // Inquiry Scan disabled. Page Scan disabled.
960
961 HCI_Command(hcibuf, 4);
962}
963
964void BTD::hci_read_bdaddr() {
965 hci_clear_flag(HCI_FLAG_READ_BDADDR);
966 hcibuf[0] = 0x09; // HCI OCF = 9
967 hcibuf[1] = 0x04 << 2; // HCI OGF = 4
968 hcibuf[2] = 0x00;
969
970 HCI_Command(hcibuf, 3);
971}
972
973void BTD::hci_read_local_version_information() {
974 hci_clear_flag(HCI_FLAG_READ_VERSION);
975 hcibuf[0] = 0x01; // HCI OCF = 1
976 hcibuf[1] = 0x04 << 2; // HCI OGF = 4
977 hcibuf[2] = 0x00;
978
979 HCI_Command(hcibuf, 3);
980}
981
982void BTD::hci_accept_connection() {
983 hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE);
984 hcibuf[0] = 0x09; // HCI OCF = 9
985 hcibuf[1] = 0x01 << 2; // HCI OGF = 1
986 hcibuf[2] = 0x07; // parameter length 7
987 hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
988 hcibuf[4] = disc_bdaddr[1];
989 hcibuf[5] = disc_bdaddr[2];
990 hcibuf[6] = disc_bdaddr[3];
991 hcibuf[7] = disc_bdaddr[4];
992 hcibuf[8] = disc_bdaddr[5];
993 hcibuf[9] = 0x00; // Switch role to master
994
995 HCI_Command(hcibuf, 10);
996}
997
998void BTD::hci_remote_name() {
999 hci_clear_flag(HCI_FLAG_REMOTE_NAME_COMPLETE);
1000 hcibuf[0] = 0x19; // HCI OCF = 19
1001 hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1002 hcibuf[2] = 0x0A; // parameter length = 10
1003 hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
1004 hcibuf[4] = disc_bdaddr[1];
1005 hcibuf[5] = disc_bdaddr[2];
1006 hcibuf[6] = disc_bdaddr[3];
1007 hcibuf[7] = disc_bdaddr[4];
1008 hcibuf[8] = disc_bdaddr[5];
1009 hcibuf[9] = 0x01; // Page Scan Repetition Mode
1010 hcibuf[10] = 0x00; // Reserved
1011 hcibuf[11] = 0x00; // Clock offset - low byte
1012 hcibuf[12] = 0x00; // Clock offset - high byte
1013
1014 HCI_Command(hcibuf, 13);
1015}
1016
1017void BTD::hci_set_local_name(const char* name) {
1018 hcibuf[0] = 0x13; // HCI OCF = 13
1019 hcibuf[1] = 0x03 << 2; // HCI OGF = 3
1020 hcibuf[2] = strlen(name) + 1; // parameter length = the length of the string + end byte
1021 uint8_t i;
1022 for(i = 0; i < strlen(name); i++)
1023 hcibuf[i + 3] = name[i];
1024 hcibuf[i + 3] = 0x00; // End of string
1025
1026 HCI_Command(hcibuf, 4 + strlen(name));
1027}
1028
1029void BTD::hci_inquiry() {
1030 hci_clear_flag(HCI_FLAG_DEVICE_FOUND);
1031 hcibuf[0] = 0x01;
1032 hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1033 hcibuf[2] = 0x05; // Parameter Total Length = 5
1034 hcibuf[3] = 0x33; // LAP: Genera/Unlimited Inquiry Access Code (GIAC = 0x9E8B33) - see https://www.bluetooth.org/Technical/AssignedNumbers/baseband.htm
1035 hcibuf[4] = 0x8B;
1036 hcibuf[5] = 0x9E;
1037 hcibuf[6] = 0x30; // Inquiry time = 61.44 sec (maximum)
1038 hcibuf[7] = 0x0A; // 10 number of responses
1039
1040 HCI_Command(hcibuf, 8);
1041}
1042
1043void BTD::hci_inquiry_cancel() {
1044 hcibuf[0] = 0x02;
1045 hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1046 hcibuf[2] = 0x00; // Parameter Total Length = 0
1047
1048 HCI_Command(hcibuf, 3);
1049}
1050
1051void BTD::hci_connect() {
1052 hci_connect(disc_bdaddr); // Use last discovered device
1053}
1054
1055void BTD::hci_connect(uint8_t *bdaddr) {
1056 hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE | HCI_FLAG_CONNECT_EVENT);
1057 hcibuf[0] = 0x05;
1058 hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1059 hcibuf[2] = 0x0D; // parameter Total Length = 13
1060 hcibuf[3] = bdaddr[0]; // 6 octet bdaddr (LSB)
1061 hcibuf[4] = bdaddr[1];
1062 hcibuf[5] = bdaddr[2];
1063 hcibuf[6] = bdaddr[3];
1064 hcibuf[7] = bdaddr[4];
1065 hcibuf[8] = bdaddr[5];
1066 hcibuf[9] = 0x18; // DM1 or DH1 may be used
1067 hcibuf[10] = 0xCC; // DM3, DH3, DM5, DH5 may be used
1068 hcibuf[11] = 0x01; // Page repetition mode R1
1069 hcibuf[12] = 0x00; // Reserved
1070 hcibuf[13] = 0x00; // Clock offset
1071 hcibuf[14] = 0x00; // Invalid clock offset
1072 hcibuf[15] = 0x00; // Do not allow role switch
1073
1074 HCI_Command(hcibuf, 16);
1075}
1076
1077void BTD::hci_pin_code_request_reply() {
1078 hcibuf[0] = 0x0D; // HCI OCF = 0D
1079 hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1080 hcibuf[2] = 0x17; // parameter length 23
1081 hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
1082 hcibuf[4] = disc_bdaddr[1];
1083 hcibuf[5] = disc_bdaddr[2];
1084 hcibuf[6] = disc_bdaddr[3];
1085 hcibuf[7] = disc_bdaddr[4];
1086 hcibuf[8] = disc_bdaddr[5];
1087 if(pairWithWii) {
1088 hcibuf[9] = 6; // Pin length is the length of the Bluetooth address
1089 if(pairWiiUsingSync) {
1090#ifdef DEBUG_USB_HOST
1091 Notify(PSTR("\r\nParing with Wii controller via SYNC"), 0x80);
1092#endif
1093 for(uint8_t i = 0; i < 6; i++)
1094 hcibuf[10 + i] = my_bdaddr[i]; // The pin is the Bluetooth dongles Bluetooth address backwards
1095 } else {
1096 for(uint8_t i = 0; i < 6; i++)
1097 hcibuf[10 + i] = disc_bdaddr[i]; // The pin is the Wiimote's Bluetooth address backwards
1098 }
1099 for(uint8_t i = 16; i < 26; i++)
1100 hcibuf[i] = 0x00; // The rest should be 0
1101 } else {
1102 hcibuf[9] = strlen(btdPin); // Length of pin
1103 uint8_t i;
1104 for(i = 0; i < strlen(btdPin); i++) // The maximum size of the pin is 16
1105 hcibuf[i + 10] = btdPin[i];
1106 for(; i < 16; i++)
1107 hcibuf[i + 10] = 0x00; // The rest should be 0
1108 }
1109
1110 HCI_Command(hcibuf, 26);
1111}
1112
1113void BTD::hci_pin_code_negative_request_reply() {
1114 hcibuf[0] = 0x0E; // HCI OCF = 0E
1115 hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1116 hcibuf[2] = 0x06; // parameter length 6
1117 hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
1118 hcibuf[4] = disc_bdaddr[1];
1119 hcibuf[5] = disc_bdaddr[2];
1120 hcibuf[6] = disc_bdaddr[3];
1121 hcibuf[7] = disc_bdaddr[4];
1122 hcibuf[8] = disc_bdaddr[5];
1123
1124 HCI_Command(hcibuf, 9);
1125}
1126
1127void BTD::hci_link_key_request_negative_reply() {
1128 hcibuf[0] = 0x0C; // HCI OCF = 0C
1129 hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1130 hcibuf[2] = 0x06; // parameter length 6
1131 hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
1132 hcibuf[4] = disc_bdaddr[1];
1133 hcibuf[5] = disc_bdaddr[2];
1134 hcibuf[6] = disc_bdaddr[3];
1135 hcibuf[7] = disc_bdaddr[4];
1136 hcibuf[8] = disc_bdaddr[5];
1137
1138 HCI_Command(hcibuf, 9);
1139}
1140
1141void BTD::hci_authentication_request() {
1142 hcibuf[0] = 0x11; // HCI OCF = 11
1143 hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1144 hcibuf[2] = 0x02; // parameter length = 2
1145 hcibuf[3] = (uint8_t)(hci_handle & 0xFF); //connection handle - low byte
1146 hcibuf[4] = (uint8_t)((hci_handle >> 8) & 0x0F); //connection handle - high byte
1147
1148 HCI_Command(hcibuf, 5);
1149}
1150
1151void BTD::hci_disconnect(uint16_t handle) { // This is called by the different services
1152 hci_clear_flag(HCI_FLAG_DISCONNECT_COMPLETE);
1153 hcibuf[0] = 0x06; // HCI OCF = 6
1154 hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1155 hcibuf[2] = 0x03; // parameter length = 3
1156 hcibuf[3] = (uint8_t)(handle & 0xFF); //connection handle - low byte
1157 hcibuf[4] = (uint8_t)((handle >> 8) & 0x0F); //connection handle - high byte
1158 hcibuf[5] = 0x13; // reason
1159
1160 HCI_Command(hcibuf, 6);
1161}
1162
1163void BTD::hci_write_class_of_device() { // See http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html
1164 hcibuf[0] = 0x24; // HCI OCF = 24
1165 hcibuf[1] = 0x03 << 2; // HCI OGF = 3
1166 hcibuf[2] = 0x03; // parameter length = 3
1167 hcibuf[3] = 0x04; // Robot
1168 hcibuf[4] = 0x08; // Toy
1169 hcibuf[5] = 0x00;
1170
1171 HCI_Command(hcibuf, 6);
1172}
1173/*******************************************************************
1174 * *
1175 * HCI ACL Data Packet *
1176 * *
1177 * buf[0] buf[1] buf[2] buf[3]
1178 * 0 4 8 11 12 16 24 31 MSB
1179 * .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
1180 * | HCI Handle |PB |BC | Data Total Length | HCI ACL Data Packet
1181 * .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
1182 *
1183 * buf[4] buf[5] buf[6] buf[7]
1184 * 0 8 16 31 MSB
1185 * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
1186 * | Length | Channel ID | Basic L2CAP header
1187 * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
1188 *
1189 * buf[8] buf[9] buf[10] buf[11]
1190 * 0 8 16 31 MSB
1191 * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
1192 * | Code | Identifier | Length | Control frame (C-frame)
1193 * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. (signaling packet format)
1194 */
1195/************************************************************/
1196/* L2CAP Commands */
1197
1198/************************************************************/
1199void BTD::L2CAP_Command(uint16_t handle, uint8_t* data, uint8_t nbytes, uint8_t channelLow, uint8_t channelHigh) {
1200 uint8_t buf[8 + nbytes];
1201 buf[0] = (uint8_t)(handle & 0xff); // HCI handle with PB,BC flag
1202 buf[1] = (uint8_t)(((handle >> 8) & 0x0f) | 0x20);
1203 buf[2] = (uint8_t)((4 + nbytes) & 0xff); // HCI ACL total data length
1204 buf[3] = (uint8_t)((4 + nbytes) >> 8);
1205 buf[4] = (uint8_t)(nbytes & 0xff); // L2CAP header: Length
1206 buf[5] = (uint8_t)(nbytes >> 8);
1207 buf[6] = channelLow;
1208 buf[7] = channelHigh;
1209
1210 for(uint16_t i = 0; i < nbytes; i++) // L2CAP C-frame
1211 buf[8 + i] = data[i];
1212
1213 uint8_t rcode = pUsb->outTransfer(bAddress, epInfo[ BTD_DATAOUT_PIPE ].epAddr, (8 + nbytes), buf);
1214 if(rcode) {
1215 delay(100); // This small delay prevents it from overflowing if it fails
1216#ifdef DEBUG_USB_HOST
1217 Notify(PSTR("\r\nError sending L2CAP message: 0x"), 0x80);
1218 D_PrintHex<uint8_t > (rcode, 0x80);
1219 Notify(PSTR(" - Channel ID: "), 0x80);
1220 D_PrintHex<uint8_t > (channelHigh, 0x80);
1221 Notify(PSTR(" "), 0x80);
1222 D_PrintHex<uint8_t > (channelLow, 0x80);
1223#endif
1224 }
1225}
1226
1227void BTD::l2cap_connection_request(uint16_t handle, uint8_t rxid, uint8_t* scid, uint16_t psm) {
1228 l2capoutbuf[0] = L2CAP_CMD_CONNECTION_REQUEST; // Code
1229 l2capoutbuf[1] = rxid; // Identifier
1230 l2capoutbuf[2] = 0x04; // Length
1231 l2capoutbuf[3] = 0x00;
1232 l2capoutbuf[4] = (uint8_t)(psm & 0xff); // PSM
1233 l2capoutbuf[5] = (uint8_t)(psm >> 8);
1234 l2capoutbuf[6] = scid[0]; // Source CID
1235 l2capoutbuf[7] = scid[1];
1236
1237 L2CAP_Command(handle, l2capoutbuf, 8);
1238}
1239
1240void BTD::l2cap_connection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid, uint8_t result) {
1241 l2capoutbuf[0] = L2CAP_CMD_CONNECTION_RESPONSE; // Code
1242 l2capoutbuf[1] = rxid; // Identifier
1243 l2capoutbuf[2] = 0x08; // Length
1244 l2capoutbuf[3] = 0x00;
1245 l2capoutbuf[4] = dcid[0]; // Destination CID
1246 l2capoutbuf[5] = dcid[1];
1247 l2capoutbuf[6] = scid[0]; // Source CID
1248 l2capoutbuf[7] = scid[1];
1249 l2capoutbuf[8] = result; // Result: Pending or Success
1250 l2capoutbuf[9] = 0x00;
1251 l2capoutbuf[10] = 0x00; // No further information
1252 l2capoutbuf[11] = 0x00;
1253
1254 L2CAP_Command(handle, l2capoutbuf, 12);
1255}
1256
1257void BTD::l2cap_config_request(uint16_t handle, uint8_t rxid, uint8_t* dcid) {
1258 l2capoutbuf[0] = L2CAP_CMD_CONFIG_REQUEST; // Code
1259 l2capoutbuf[1] = rxid; // Identifier
1260 l2capoutbuf[2] = 0x08; // Length
1261 l2capoutbuf[3] = 0x00;
1262 l2capoutbuf[4] = dcid[0]; // Destination CID
1263 l2capoutbuf[5] = dcid[1];
1264 l2capoutbuf[6] = 0x00; // Flags
1265 l2capoutbuf[7] = 0x00;
1266 l2capoutbuf[8] = 0x01; // Config Opt: type = MTU (Maximum Transmission Unit) - Hint
1267 l2capoutbuf[9] = 0x02; // Config Opt: length
1268 l2capoutbuf[10] = 0xFF; // MTU
1269 l2capoutbuf[11] = 0xFF;
1270
1271 L2CAP_Command(handle, l2capoutbuf, 12);
1272}
1273
1274void BTD::l2cap_config_response(uint16_t handle, uint8_t rxid, uint8_t* scid) {
1275 l2capoutbuf[0] = L2CAP_CMD_CONFIG_RESPONSE; // Code
1276 l2capoutbuf[1] = rxid; // Identifier
1277 l2capoutbuf[2] = 0x0A; // Length
1278 l2capoutbuf[3] = 0x00;
1279 l2capoutbuf[4] = scid[0]; // Source CID
1280 l2capoutbuf[5] = scid[1];
1281 l2capoutbuf[6] = 0x00; // Flag
1282 l2capoutbuf[7] = 0x00;
1283 l2capoutbuf[8] = 0x00; // Result
1284 l2capoutbuf[9] = 0x00;
1285 l2capoutbuf[10] = 0x01; // Config
1286 l2capoutbuf[11] = 0x02;
1287 l2capoutbuf[12] = 0xA0;
1288 l2capoutbuf[13] = 0x02;
1289
1290 L2CAP_Command(handle, l2capoutbuf, 14);
1291}
1292
1293void BTD::l2cap_disconnection_request(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid) {
1294 l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_REQUEST; // Code
1295 l2capoutbuf[1] = rxid; // Identifier
1296 l2capoutbuf[2] = 0x04; // Length
1297 l2capoutbuf[3] = 0x00;
1298 l2capoutbuf[4] = dcid[0];
1299 l2capoutbuf[5] = dcid[1];
1300 l2capoutbuf[6] = scid[0];
1301 l2capoutbuf[7] = scid[1];
1302
1303 L2CAP_Command(handle, l2capoutbuf, 8);
1304}
1305
1306void BTD::l2cap_disconnection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid) {
1307 l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_RESPONSE; // Code
1308 l2capoutbuf[1] = rxid; // Identifier
1309 l2capoutbuf[2] = 0x04; // Length
1310 l2capoutbuf[3] = 0x00;
1311 l2capoutbuf[4] = dcid[0];
1312 l2capoutbuf[5] = dcid[1];
1313 l2capoutbuf[6] = scid[0];
1314 l2capoutbuf[7] = scid[1];
1315
1316 L2CAP_Command(handle, l2capoutbuf, 8);
1317}
1318
1319void BTD::l2cap_information_response(uint16_t handle, uint8_t rxid, uint8_t infoTypeLow, uint8_t infoTypeHigh) {
1320 l2capoutbuf[0] = L2CAP_CMD_INFORMATION_RESPONSE; // Code
1321 l2capoutbuf[1] = rxid; // Identifier
1322 l2capoutbuf[2] = 0x08; // Length
1323 l2capoutbuf[3] = 0x00;
1324 l2capoutbuf[4] = infoTypeLow;
1325 l2capoutbuf[5] = infoTypeHigh;
1326 l2capoutbuf[6] = 0x00; // Result = success
1327 l2capoutbuf[7] = 0x00; // Result = success
1328 l2capoutbuf[8] = 0x00;
1329 l2capoutbuf[9] = 0x00;
1330 l2capoutbuf[10] = 0x00;
1331 l2capoutbuf[11] = 0x00;
1332
1333 L2CAP_Command(handle, l2capoutbuf, 12);
1334}
1335
1336/* PS3 Commands - only set Bluetooth address is implemented in this library */
1337void BTD::setBdaddr(uint8_t* bdaddr) {
1338 /* Set the internal Bluetooth address */
1339 uint8_t buf[8];
1340 buf[0] = 0x01;
1341 buf[1] = 0x00;
1342
1343 for(uint8_t i = 0; i < 6; i++)
1344 buf[i + 2] = bdaddr[5 - i]; // Copy into buffer, has to be written reversed, so it is MSB first
1345
1346 // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0xF5), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
1347 pUsb->ctrlReq(bAddress, epInfo[BTD_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL);
1348}
1349
1350void BTD::setMoveBdaddr(uint8_t* bdaddr) {
1351 /* Set the internal Bluetooth address */
1352 uint8_t buf[11];
1353 buf[0] = 0x05;
1354 buf[7] = 0x10;
1355 buf[8] = 0x01;
1356 buf[9] = 0x02;
1357 buf[10] = 0x12;
1358
1359 for(uint8_t i = 0; i < 6; i++)
1360 buf[i + 1] = bdaddr[i];
1361
1362 // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x05), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
1363 pUsb->ctrlReq(bAddress, epInfo[BTD_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00, 11, 11, buf, NULL);
1364}
generated by cgit v1.2.3 (git 2.25.1) at 2025-07-06 06:44:55 +0000