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