tmk_core/protocol/usb_hid/USB_Host_Shield_2.0/BTD.cpp

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