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