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