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