Takumi Odashima
/
ArduinoUsbHostShield
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
BTD.cpp
00001 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 00002 00003 This software may be distributed and modified under the terms of the GNU 00004 General Public License version 2 (GPL2) as published by the Free Software 00005 Foundation and appearing in the file GPL2.TXT included in the packaging of 00006 this file. Please note that GPL2 Section 2[b] requires that all works based 00007 on this software must also be made publicly available under the terms of 00008 the GPL2 ("Copyleft"). 00009 00010 Contact information 00011 ------------------- 00012 00013 Kristian Lauszus, TKJ Electronics 00014 Web : http://www.tkjelectronics.com 00015 e-mail : kristianl@tkjelectronics.com 00016 */ 00017 00018 #include "BTD.h" 00019 // To enable serial debugging see "settings.h" 00020 //#define EXTRADEBUG // Uncomment to get even more debugging data 00021 00022 const uint8_t BTD::BTD_CONTROL_PIPE = 0; 00023 const uint8_t BTD::BTD_EVENT_PIPE = 1; 00024 const uint8_t BTD::BTD_DATAIN_PIPE = 2; 00025 const uint8_t BTD::BTD_DATAOUT_PIPE = 3; 00026 00027 BTD::BTD(USB *p) : 00028 connectToWii(false), 00029 pairWithWii(false), 00030 connectToHIDDevice(false), 00031 pairWithHIDDevice(false), 00032 pUsb(p), // Pointer to USB class instance - mandatory 00033 bAddress(0), // Device address - mandatory 00034 bNumEP(1), // If config descriptor needs to be parsed 00035 qNextPollTime(0), // Reset NextPollTime 00036 pollInterval(0), 00037 bPollEnable(false) // Don't start polling before dongle is connected 00038 { 00039 for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++) 00040 btService[i] = NULL; 00041 00042 Initialize(); // Set all variables, endpoint structs etc. to default values 00043 00044 if(pUsb) // Register in USB subsystem 00045 pUsb->RegisterDeviceClass(this); // Set devConfig[] entry 00046 } 00047 00048 uint8_t BTD::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) { 00049 const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR); 00050 uint8_t buf[constBufSize]; 00051 USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf); 00052 uint8_t rcode; 00053 UsbDevice *p = NULL; 00054 EpInfo *oldep_ptr = NULL; 00055 00056 Initialize(); // Set all variables, endpoint structs etc. to default values 00057 00058 AddressPool &addrPool = pUsb->GetAddressPool(); // Get memory address of USB device address pool 00059 #ifdef EXTRADEBUG 00060 Notify(PSTR("\r\nBTD ConfigureDevice"), 0x80); 00061 #endif 00062 00063 if(bAddress) { // Check if address has already been assigned to an instance 00064 #ifdef DEBUG_USB_HOST 00065 Notify(PSTR("\r\nAddress in use"), 0x80); 00066 #endif 00067 return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; 00068 } 00069 00070 p = addrPool.GetUsbDevicePtr(0); // Get pointer to pseudo device with address 0 assigned 00071 if(!p) { 00072 #ifdef DEBUG_USB_HOST 00073 Notify(PSTR("\r\nAddress not found"), 0x80); 00074 #endif 00075 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; 00076 } 00077 00078 if(!p->epinfo) { 00079 #ifdef DEBUG_USB_HOST 00080 Notify(PSTR("\r\nepinfo is null"), 0x80); 00081 #endif 00082 return USB_ERROR_EPINFO_IS_NULL; 00083 } 00084 00085 oldep_ptr = p->epinfo; // Save old pointer to EP_RECORD of address 0 00086 p->epinfo = epInfo; // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence 00087 p->lowspeed = lowspeed; 00088 rcode = pUsb->getDevDescr(0, 0, constBufSize, (uint8_t*)buf); // Get device descriptor - addr, ep, nbytes, data 00089 00090 p->epinfo = oldep_ptr; // Restore p->epinfo 00091 00092 if(rcode) 00093 goto FailGetDevDescr; 00094 00095 bAddress = addrPool.AllocAddress(parent, false, port); // Allocate new address according to device class 00096 00097 if(!bAddress) { 00098 #ifdef DEBUG_USB_HOST 00099 Notify(PSTR("\r\nOut of address space"), 0x80); 00100 #endif 00101 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; 00102 } 00103 00104 if (udd->bDeviceClass == 0x09) // Some dongles have an USB hub inside 00105 goto FailHub; 00106 00107 epInfo[0].maxPktSize = udd->bMaxPacketSize0; // Extract Max Packet Size from device descriptor 00108 epInfo[1].epAddr = udd->bNumConfigurations; // Steal and abuse from epInfo structure to save memory 00109 00110 VID = udd->idVendor; 00111 PID = udd->idProduct; 00112 00113 return USB_ERROR_CONFIG_REQUIRES_ADDITIONAL_RESET; 00114 00115 FailHub: 00116 #ifdef DEBUG_USB_HOST 00117 Notify(PSTR("\r\nPlease create a hub instance in your code: \"USBHub Hub1(&Usb);\""), 0x80); 00118 #endif 00119 pUsb->setAddr(bAddress, 0, 0); // Reset address 00120 rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; 00121 Release(); 00122 return rcode; 00123 00124 FailGetDevDescr: 00125 #ifdef DEBUG_USB_HOST 00126 NotifyFailGetDevDescr(rcode); 00127 #endif 00128 if(rcode != hrJERR) 00129 rcode = USB_ERROR_FailGetDevDescr; 00130 Release(); 00131 return rcode; 00132 }; 00133 00134 uint8_t BTD::Init(uint8_t parent __attribute__((unused)), uint8_t port __attribute__((unused)), bool lowspeed) { 00135 uint8_t rcode; 00136 uint8_t num_of_conf = epInfo[1].epAddr; // Number of configurations 00137 epInfo[1].epAddr = 0; 00138 00139 AddressPool &addrPool = pUsb->GetAddressPool(); 00140 #ifdef EXTRADEBUG 00141 Notify(PSTR("\r\nBTD Init"), 0x80); 00142 #endif 00143 UsbDevice *p = addrPool.GetUsbDevicePtr(bAddress); // Get pointer to assigned address record 00144 00145 if(!p) { 00146 #ifdef DEBUG_USB_HOST 00147 Notify(PSTR("\r\nAddress not found"), 0x80); 00148 #endif 00149 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; 00150 } 00151 00152 delay(300); // Assign new address to the device 00153 00154 rcode = pUsb->setAddr(0, 0, bAddress); // Assign new address to the device 00155 if(rcode) { 00156 #ifdef DEBUG_USB_HOST 00157 Notify(PSTR("\r\nsetAddr: "), 0x80); 00158 D_PrintHex<uint8_t > (rcode, 0x80); 00159 #endif 00160 p->lowspeed = false; 00161 goto Fail; 00162 } 00163 #ifdef EXTRADEBUG 00164 Notify(PSTR("\r\nAddr: "), 0x80); 00165 D_PrintHex<uint8_t > (bAddress, 0x80); 00166 #endif 00167 00168 p->lowspeed = false; 00169 00170 p = addrPool.GetUsbDevicePtr(bAddress); // Get pointer to assigned address record 00171 if(!p) { 00172 #ifdef DEBUG_USB_HOST 00173 Notify(PSTR("\r\nAddress not found"), 0x80); 00174 #endif 00175 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; 00176 } 00177 00178 p->lowspeed = lowspeed; 00179 00180 rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); // Assign epInfo to epinfo pointer - only EP0 is known 00181 if(rcode) 00182 goto FailSetDevTblEntry; 00183 00184 if(VID == PS3_VID && (PID == PS3_PID || PID == PS3NAVIGATION_PID || PID == PS3MOVE_PID)) { 00185 delay(100); 00186 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 00187 if(rcode) 00188 goto FailSetConfDescr; 00189 00190 #ifdef DEBUG_USB_HOST 00191 if(PID == PS3_PID || PID == PS3NAVIGATION_PID) { 00192 if(PID == PS3_PID) 00193 Notify(PSTR("\r\nDualshock 3 Controller Connected"), 0x80); 00194 else // It must be a navigation controller 00195 Notify(PSTR("\r\nNavigation Controller Connected"), 0x80); 00196 } else // It must be a Motion controller 00197 Notify(PSTR("\r\nMotion Controller Connected"), 0x80); 00198 #endif 00199 00200 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) { 00201 #ifdef DEBUG_USB_HOST 00202 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); 00203 #endif 00204 } else { 00205 if(PID == PS3_PID || PID == PS3NAVIGATION_PID) 00206 setBdaddr(my_bdaddr); // Set internal Bluetooth address 00207 else 00208 setMoveBdaddr(my_bdaddr); // Set internal Bluetooth address 00209 #ifdef DEBUG_USB_HOST 00210 Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80); 00211 for(int8_t i = 5; i > 0; i--) { 00212 D_PrintHex<uint8_t > (my_bdaddr[i], 0x80); 00213 Notify(PSTR(":"), 0x80); 00214 } 00215 D_PrintHex<uint8_t > (my_bdaddr[0], 0x80); 00216 #endif 00217 } 00218 00219 pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 0); // Reset configuration value 00220 pUsb->setAddr(bAddress, 0, 0); // Reset address 00221 Release(); // Release device 00222 return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; // Return 00223 } else { 00224 // Check if attached device is a Bluetooth dongle and fill endpoint data structure 00225 // First interface in the configuration must have Bluetooth assigned Class/Subclass/Protocol 00226 // And 3 endpoints - interrupt-IN, bulk-IN, bulk-OUT, not necessarily in this order 00227 for(uint8_t i = 0; i < num_of_conf; i++) { 00228 if((VID == IOGEAR_GBU521_VID && PID == IOGEAR_GBU521_PID) || (VID == BELKIN_F8T065BF_VID && PID == BELKIN_F8T065BF_PID)) { 00229 ConfigDescParser<USB_CLASS_VENDOR_SPECIFIC, WI_SUBCLASS_RF, WI_PROTOCOL_BT, CP_MASK_COMPARE_ALL> confDescrParser(this); // Workaround issue with some dongles 00230 rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser); 00231 } else { 00232 ConfigDescParser<USB_CLASS_WIRELESS_CTRL, WI_SUBCLASS_RF, WI_PROTOCOL_BT, CP_MASK_COMPARE_ALL> confDescrParser(this); // Set class id according to the specification 00233 rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser); 00234 } 00235 if(rcode) // Check error code 00236 goto FailGetConfDescr; 00237 if(bNumEP >= BTD_MAX_ENDPOINTS) // All endpoints extracted 00238 break; 00239 } 00240 00241 if(bNumEP < BTD_MAX_ENDPOINTS) 00242 goto FailUnknownDevice; 00243 00244 // Assign epInfo to epinfo pointer - this time all 3 endpoins 00245 rcode = pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo); 00246 if(rcode) 00247 goto FailSetDevTblEntry; 00248 00249 // Set Configuration Value 00250 rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bConfNum); 00251 if(rcode) 00252 goto FailSetConfDescr; 00253 00254 hci_num_reset_loops = 100; // only loop 100 times before trying to send the hci reset command 00255 hci_counter = 0; 00256 hci_state = HCI_INIT_STATE; 00257 waitingForConnection = false; 00258 bPollEnable = true; 00259 00260 #ifdef DEBUG_USB_HOST 00261 Notify(PSTR("\r\nBluetooth Dongle Initialized"), 0x80); 00262 #endif 00263 } 00264 return 0; // Successful configuration 00265 00266 /* Diagnostic messages */ 00267 FailSetDevTblEntry: 00268 #ifdef DEBUG_USB_HOST 00269 NotifyFailSetDevTblEntry(); 00270 goto Fail; 00271 #endif 00272 00273 FailGetConfDescr: 00274 #ifdef DEBUG_USB_HOST 00275 NotifyFailGetConfDescr(); 00276 goto Fail; 00277 #endif 00278 00279 FailSetConfDescr: 00280 #ifdef DEBUG_USB_HOST 00281 NotifyFailSetConfDescr(); 00282 #endif 00283 goto Fail; 00284 00285 FailUnknownDevice: 00286 #ifdef DEBUG_USB_HOST 00287 NotifyFailUnknownDevice(VID, PID); 00288 #endif 00289 pUsb->setAddr(bAddress, 0, 0); // Reset address 00290 rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; 00291 Fail: 00292 #ifdef DEBUG_USB_HOST 00293 Notify(PSTR("\r\nBTD Init Failed, error code: "), 0x80); 00294 NotifyFail(rcode); 00295 #endif 00296 Release(); 00297 return rcode; 00298 } 00299 00300 void BTD::Initialize() { 00301 uint8_t i; 00302 for(i = 0; i < BTD_MAX_ENDPOINTS; i++) { 00303 epInfo[i].epAddr = 0; 00304 epInfo[i].maxPktSize = (i) ? 0 : 8; 00305 epInfo[i].bmSndToggle = 0; 00306 epInfo[i].bmRcvToggle = 0; 00307 epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER; 00308 } 00309 for(i = 0; i < BTD_NUM_SERVICES; i++) { 00310 if(btService[i]) 00311 btService[i]->Reset(); // Reset all Bluetooth services 00312 } 00313 00314 connectToWii = false; 00315 incomingWii = false; 00316 connectToHIDDevice = false; 00317 incomingHIDDevice = false; 00318 incomingPS4 = false; 00319 bAddress = 0; // Clear device address 00320 bNumEP = 1; // Must have to be reset to 1 00321 qNextPollTime = 0; // Reset next poll time 00322 pollInterval = 0; 00323 bPollEnable = false; // Don't start polling before dongle is connected 00324 } 00325 00326 /* Extracts interrupt-IN, bulk-IN, bulk-OUT endpoint information from config descriptor */ 00327 void BTD::EndpointXtract(uint8_t conf, uint8_t iface __attribute__((unused)), uint8_t alt, uint8_t proto __attribute__((unused)), const USB_ENDPOINT_DESCRIPTOR *pep) { 00328 //ErrorMessage<uint8_t>(PSTR("Conf.Val"),conf); 00329 //ErrorMessage<uint8_t>(PSTR("Iface Num"),iface); 00330 //ErrorMessage<uint8_t>(PSTR("Alt.Set"),alt); 00331 00332 if(alt) // Wrong interface - by BT spec, no alt setting 00333 return; 00334 00335 bConfNum = conf; 00336 uint8_t index; 00337 00338 if((pep->bmAttributes & bmUSB_TRANSFER_TYPE) == USB_TRANSFER_TYPE_INTERRUPT && (pep->bEndpointAddress & 0x80) == 0x80) { // Interrupt In endpoint found 00339 index = BTD_EVENT_PIPE; 00340 epInfo[index].bmNakPower = USB_NAK_NOWAIT; 00341 } else if((pep->bmAttributes & bmUSB_TRANSFER_TYPE) == USB_TRANSFER_TYPE_BULK) // Bulk endpoint found 00342 index = ((pep->bEndpointAddress & 0x80) == 0x80) ? BTD_DATAIN_PIPE : BTD_DATAOUT_PIPE; 00343 else 00344 return; 00345 00346 // Fill the rest of endpoint data structure 00347 epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F); 00348 epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize; 00349 #ifdef EXTRADEBUG 00350 PrintEndpointDescriptor(pep); 00351 #endif 00352 if(pollInterval < pep->bInterval) // Set the polling interval as the largest polling interval obtained from endpoints 00353 pollInterval = pep->bInterval; 00354 bNumEP++; 00355 } 00356 00357 void BTD::PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR* ep_ptr __attribute__((unused))) { 00358 #ifdef EXTRADEBUG 00359 Notify(PSTR("\r\nEndpoint descriptor:"), 0x80); 00360 Notify(PSTR("\r\nLength:\t\t"), 0x80); 00361 D_PrintHex<uint8_t > (ep_ptr->bLength, 0x80); 00362 Notify(PSTR("\r\nType:\t\t"), 0x80); 00363 D_PrintHex<uint8_t > (ep_ptr->bDescriptorType, 0x80); 00364 Notify(PSTR("\r\nAddress:\t"), 0x80); 00365 D_PrintHex<uint8_t > (ep_ptr->bEndpointAddress, 0x80); 00366 Notify(PSTR("\r\nAttributes:\t"), 0x80); 00367 D_PrintHex<uint8_t > (ep_ptr->bmAttributes, 0x80); 00368 Notify(PSTR("\r\nMaxPktSize:\t"), 0x80); 00369 D_PrintHex<uint16_t > (ep_ptr->wMaxPacketSize, 0x80); 00370 Notify(PSTR("\r\nPoll Intrv:\t"), 0x80); 00371 D_PrintHex<uint8_t > (ep_ptr->bInterval, 0x80); 00372 #endif 00373 } 00374 00375 /* Performs a cleanup after failed Init() attempt */ 00376 uint8_t BTD::Release() { 00377 Initialize(); // Set all variables, endpoint structs etc. to default values 00378 pUsb->GetAddressPool().FreeAddress(bAddress); 00379 return 0; 00380 } 00381 00382 uint8_t BTD::Poll() { 00383 if(!bPollEnable) 00384 return 0; 00385 if((int32_t)((uint32_t)millis() - qNextPollTime) >= 0L) { // Don't poll if shorter than polling interval 00386 qNextPollTime = (uint32_t)millis() + pollInterval; // Set new poll time 00387 HCI_event_task(); // Poll the HCI event pipe 00388 HCI_task(); // HCI state machine 00389 ACL_event_task(); // Poll the ACL input pipe too 00390 } 00391 return 0; 00392 } 00393 00394 void BTD::disconnect() { 00395 for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++) 00396 if(btService[i]) 00397 btService[i]->disconnect(); 00398 }; 00399 00400 void BTD::HCI_event_task() { 00401 uint16_t length = BULK_MAXPKTSIZE; // Request more than 16 bytes anyway, the inTransfer routine will take care of this 00402 uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_EVENT_PIPE ].epAddr, &length, hcibuf, pollInterval); // Input on endpoint 1 00403 00404 if(!rcode || rcode == hrNAK) { // Check for errors 00405 switch(hcibuf[0]) { // Switch on event type 00406 case EV_COMMAND_COMPLETE: 00407 if(!hcibuf[5]) { // Check if command succeeded 00408 hci_set_flag(HCI_FLAG_CMD_COMPLETE); // Set command complete flag 00409 if((hcibuf[3] == 0x01) && (hcibuf[4] == 0x10)) { // Parameters from read local version information 00410 hci_version = hcibuf[6]; // Used to check if it supports 2.0+EDR - see http://www.bluetooth.org/Technical/AssignedNumbers/hci.htm 00411 hci_set_flag(HCI_FLAG_READ_VERSION); 00412 } else if((hcibuf[3] == 0x09) && (hcibuf[4] == 0x10)) { // Parameters from read local bluetooth address 00413 for(uint8_t i = 0; i < 6; i++) 00414 my_bdaddr[i] = hcibuf[6 + i]; 00415 hci_set_flag(HCI_FLAG_READ_BDADDR); 00416 } 00417 } 00418 break; 00419 00420 case EV_COMMAND_STATUS: 00421 if(hcibuf[2]) { // Show status on serial if not OK 00422 #ifdef DEBUG_USB_HOST 00423 Notify(PSTR("\r\nHCI Command Failed: "), 0x80); 00424 D_PrintHex<uint8_t > (hcibuf[2], 0x80); 00425 #endif 00426 } 00427 break; 00428 00429 case EV_INQUIRY_COMPLETE: 00430 if(inquiry_counter >= 5 && (pairWithWii || pairWithHIDDevice)) { 00431 inquiry_counter = 0; 00432 #ifdef DEBUG_USB_HOST 00433 if(pairWithWii) 00434 Notify(PSTR("\r\nCouldn't find Wiimote"), 0x80); 00435 else 00436 Notify(PSTR("\r\nCouldn't find HID device"), 0x80); 00437 #endif 00438 connectToWii = false; 00439 pairWithWii = false; 00440 connectToHIDDevice = false; 00441 pairWithHIDDevice = false; 00442 hci_state = HCI_SCANNING_STATE; 00443 } 00444 inquiry_counter++; 00445 break; 00446 00447 case EV_INQUIRY_RESULT: 00448 if(hcibuf[2]) { // Check that there is more than zero responses 00449 #ifdef EXTRADEBUG 00450 Notify(PSTR("\r\nNumber of responses: "), 0x80); 00451 Notify(hcibuf[2], 0x80); 00452 #endif 00453 for(uint8_t i = 0; i < hcibuf[2]; i++) { 00454 uint8_t offset = 8 * hcibuf[2] + 3 * i; 00455 00456 for(uint8_t j = 0; j < 3; j++) 00457 classOfDevice[j] = hcibuf[j + 4 + offset]; 00458 00459 #ifdef EXTRADEBUG 00460 Notify(PSTR("\r\nClass of device: "), 0x80); 00461 D_PrintHex<uint8_t > (classOfDevice[2], 0x80); 00462 Notify(PSTR(" "), 0x80); 00463 D_PrintHex<uint8_t > (classOfDevice[1], 0x80); 00464 Notify(PSTR(" "), 0x80); 00465 D_PrintHex<uint8_t > (classOfDevice[0], 0x80); 00466 #endif 00467 00468 if(pairWithWii && classOfDevice[2] == 0x00 && (classOfDevice[1] & 0x05) && (classOfDevice[0] & 0x0C)) { // See http://wiibrew.org/wiki/Wiimote#SDP_information 00469 checkRemoteName = true; // Check remote name to distinguish between the different controllers 00470 00471 for(uint8_t j = 0; j < 6; j++) 00472 disc_bdaddr[j] = hcibuf[j + 3 + 6 * i]; 00473 00474 hci_set_flag(HCI_FLAG_DEVICE_FOUND); 00475 break; 00476 } 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 00477 #ifdef DEBUG_USB_HOST 00478 if(classOfDevice[0] & 0x80) 00479 Notify(PSTR("\r\nMouse found"), 0x80); 00480 if(classOfDevice[0] & 0x40) 00481 Notify(PSTR("\r\nKeyboard found"), 0x80); 00482 if(classOfDevice[0] & 0x08) 00483 Notify(PSTR("\r\nGamepad found"), 0x80); 00484 #endif 00485 00486 for(uint8_t j = 0; j < 6; j++) 00487 disc_bdaddr[j] = hcibuf[j + 3 + 6 * i]; 00488 00489 hci_set_flag(HCI_FLAG_DEVICE_FOUND); 00490 break; 00491 } 00492 } 00493 } 00494 break; 00495 00496 case EV_CONNECT_COMPLETE: 00497 hci_set_flag(HCI_FLAG_CONNECT_EVENT); 00498 if(!hcibuf[2]) { // Check if connected OK 00499 #ifdef EXTRADEBUG 00500 Notify(PSTR("\r\nConnection established"), 0x80); 00501 #endif 00502 hci_handle = hcibuf[3] | ((hcibuf[4] & 0x0F) << 8); // Store the handle for the ACL connection 00503 hci_set_flag(HCI_FLAG_CONNECT_COMPLETE); // Set connection complete flag 00504 } else { 00505 hci_state = HCI_CHECK_DEVICE_SERVICE; 00506 #ifdef DEBUG_USB_HOST 00507 Notify(PSTR("\r\nConnection Failed: "), 0x80); 00508 D_PrintHex<uint8_t > (hcibuf[2], 0x80); 00509 #endif 00510 } 00511 break; 00512 00513 case EV_DISCONNECT_COMPLETE: 00514 if(!hcibuf[2]) { // Check if disconnected OK 00515 hci_set_flag(HCI_FLAG_DISCONNECT_COMPLETE); // Set disconnect command complete flag 00516 hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE); // Clear connection complete flag 00517 } 00518 break; 00519 00520 case EV_REMOTE_NAME_COMPLETE: 00521 if(!hcibuf[2]) { // Check if reading is OK 00522 for(uint8_t i = 0; i < min(sizeof (remote_name), sizeof (hcibuf) - 9); i++) { 00523 remote_name[i] = hcibuf[9 + i]; 00524 if(remote_name[i] == '\0') // End of string 00525 break; 00526 } 00527 // TODO: Altid sæt '\0' i remote name! 00528 hci_set_flag(HCI_FLAG_REMOTE_NAME_COMPLETE); 00529 } 00530 break; 00531 00532 case EV_INCOMING_CONNECT: 00533 for(uint8_t i = 0; i < 6; i++) 00534 disc_bdaddr[i] = hcibuf[i + 2]; 00535 00536 for(uint8_t i = 0; i < 3; i++) 00537 classOfDevice[i] = hcibuf[i + 8]; 00538 00539 if((classOfDevice[1] & 0x05) && (classOfDevice[0] & 0xC8)) { // Check if it is a mouse, keyboard or a gamepad 00540 #ifdef DEBUG_USB_HOST 00541 if(classOfDevice[0] & 0x80) 00542 Notify(PSTR("\r\nMouse is connecting"), 0x80); 00543 if(classOfDevice[0] & 0x40) 00544 Notify(PSTR("\r\nKeyboard is connecting"), 0x80); 00545 if(classOfDevice[0] & 0x08) 00546 Notify(PSTR("\r\nGamepad is connecting"), 0x80); 00547 #endif 00548 incomingHIDDevice = true; 00549 } 00550 00551 #ifdef EXTRADEBUG 00552 Notify(PSTR("\r\nClass of device: "), 0x80); 00553 D_PrintHex<uint8_t > (classOfDevice[2], 0x80); 00554 Notify(PSTR(" "), 0x80); 00555 D_PrintHex<uint8_t > (classOfDevice[1], 0x80); 00556 Notify(PSTR(" "), 0x80); 00557 D_PrintHex<uint8_t > (classOfDevice[0], 0x80); 00558 #endif 00559 hci_set_flag(HCI_FLAG_INCOMING_REQUEST); 00560 break; 00561 00562 case EV_PIN_CODE_REQUEST: 00563 if(pairWithWii) { 00564 #ifdef DEBUG_USB_HOST 00565 Notify(PSTR("\r\nPairing with Wiimote"), 0x80); 00566 #endif 00567 hci_pin_code_request_reply(); 00568 } else if(btdPin != NULL) { 00569 #ifdef DEBUG_USB_HOST 00570 Notify(PSTR("\r\nBluetooth pin is set too: "), 0x80); 00571 NotifyStr(btdPin, 0x80); 00572 #endif 00573 hci_pin_code_request_reply(); 00574 } else { 00575 #ifdef DEBUG_USB_HOST 00576 Notify(PSTR("\r\nNo pin was set"), 0x80); 00577 #endif 00578 hci_pin_code_negative_request_reply(); 00579 } 00580 break; 00581 00582 case EV_LINK_KEY_REQUEST: 00583 #ifdef DEBUG_USB_HOST 00584 Notify(PSTR("\r\nReceived Key Request"), 0x80); 00585 #endif 00586 hci_link_key_request_negative_reply(); 00587 break; 00588 00589 case EV_AUTHENTICATION_COMPLETE: 00590 if(!hcibuf[2]) { // Check if pairing was successful 00591 if(pairWithWii && !connectToWii) { 00592 #ifdef DEBUG_USB_HOST 00593 Notify(PSTR("\r\nPairing successful with Wiimote"), 0x80); 00594 #endif 00595 connectToWii = true; // Used to indicate to the Wii service, that it should connect to this device 00596 } else if(pairWithHIDDevice && !connectToHIDDevice) { 00597 #ifdef DEBUG_USB_HOST 00598 Notify(PSTR("\r\nPairing successful with HID device"), 0x80); 00599 #endif 00600 connectToHIDDevice = true; // Used to indicate to the BTHID service, that it should connect to this device 00601 } 00602 } else { 00603 #ifdef DEBUG_USB_HOST 00604 Notify(PSTR("\r\nPairing Failed: "), 0x80); 00605 D_PrintHex<uint8_t > (hcibuf[2], 0x80); 00606 #endif 00607 hci_disconnect(hci_handle); 00608 hci_state = HCI_DISCONNECT_STATE; 00609 } 00610 break; 00611 /* We will just ignore the following events */ 00612 case EV_NUM_COMPLETE_PKT: 00613 case EV_ROLE_CHANGED: 00614 case EV_PAGE_SCAN_REP_MODE: 00615 case EV_LOOPBACK_COMMAND: 00616 case EV_DATA_BUFFER_OVERFLOW: 00617 case EV_CHANGE_CONNECTION_LINK: 00618 case EV_MAX_SLOTS_CHANGE: 00619 case EV_QOS_SETUP_COMPLETE: 00620 case EV_LINK_KEY_NOTIFICATION: 00621 case EV_ENCRYPTION_CHANGE: 00622 case EV_READ_REMOTE_VERSION_INFORMATION_COMPLETE: 00623 break; 00624 #ifdef EXTRADEBUG 00625 default: 00626 if(hcibuf[0] != 0x00) { 00627 Notify(PSTR("\r\nUnmanaged HCI Event: "), 0x80); 00628 D_PrintHex<uint8_t > (hcibuf[0], 0x80); 00629 } 00630 break; 00631 #endif 00632 } // Switch 00633 } 00634 #ifdef EXTRADEBUG 00635 else { 00636 Notify(PSTR("\r\nHCI event error: "), 0x80); 00637 D_PrintHex<uint8_t > (rcode, 0x80); 00638 } 00639 #endif 00640 } 00641 00642 /* Poll Bluetooth and print result */ 00643 void BTD::HCI_task() { 00644 switch(hci_state) { 00645 case HCI_INIT_STATE: 00646 hci_counter++; 00647 if(hci_counter > hci_num_reset_loops) { // wait until we have looped x times to clear any old events 00648 hci_reset(); 00649 hci_state = HCI_RESET_STATE; 00650 hci_counter = 0; 00651 } 00652 break; 00653 00654 case HCI_RESET_STATE: 00655 hci_counter++; 00656 if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) { 00657 hci_counter = 0; 00658 #ifdef DEBUG_USB_HOST 00659 Notify(PSTR("\r\nHCI Reset complete"), 0x80); 00660 #endif 00661 hci_state = HCI_CLASS_STATE; 00662 hci_write_class_of_device(); 00663 } else if(hci_counter > hci_num_reset_loops) { 00664 hci_num_reset_loops *= 10; 00665 if(hci_num_reset_loops > 2000) 00666 hci_num_reset_loops = 2000; 00667 #ifdef DEBUG_USB_HOST 00668 Notify(PSTR("\r\nNo response to HCI Reset"), 0x80); 00669 #endif 00670 hci_state = HCI_INIT_STATE; 00671 hci_counter = 0; 00672 } 00673 break; 00674 00675 case HCI_CLASS_STATE: 00676 if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) { 00677 #ifdef DEBUG_USB_HOST 00678 Notify(PSTR("\r\nWrite class of device"), 0x80); 00679 #endif 00680 hci_state = HCI_BDADDR_STATE; 00681 hci_read_bdaddr(); 00682 } 00683 break; 00684 00685 case HCI_BDADDR_STATE: 00686 if(hci_check_flag(HCI_FLAG_READ_BDADDR)) { 00687 #ifdef DEBUG_USB_HOST 00688 Notify(PSTR("\r\nLocal Bluetooth Address: "), 0x80); 00689 for(int8_t i = 5; i > 0; i--) { 00690 D_PrintHex<uint8_t > (my_bdaddr[i], 0x80); 00691 Notify(PSTR(":"), 0x80); 00692 } 00693 D_PrintHex<uint8_t > (my_bdaddr[0], 0x80); 00694 #endif 00695 hci_read_local_version_information(); 00696 hci_state = HCI_LOCAL_VERSION_STATE; 00697 } 00698 break; 00699 00700 case HCI_LOCAL_VERSION_STATE: // The local version is used by the PS3BT class 00701 if(hci_check_flag(HCI_FLAG_READ_VERSION)) { 00702 if(btdName != NULL) { 00703 hci_set_local_name(btdName); 00704 hci_state = HCI_SET_NAME_STATE; 00705 } else 00706 hci_state = HCI_CHECK_DEVICE_SERVICE; 00707 } 00708 break; 00709 00710 case HCI_SET_NAME_STATE: 00711 if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) { 00712 #ifdef DEBUG_USB_HOST 00713 Notify(PSTR("\r\nThe name is set to: "), 0x80); 00714 NotifyStr(btdName, 0x80); 00715 #endif 00716 hci_state = HCI_CHECK_DEVICE_SERVICE; 00717 } 00718 break; 00719 00720 case HCI_CHECK_DEVICE_SERVICE: 00721 if(pairWithHIDDevice || pairWithWii) { // Check if it should try to connect to a Wiimote 00722 #ifdef DEBUG_USB_HOST 00723 if(pairWithWii) 00724 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); 00725 else 00726 Notify(PSTR("\r\nPlease enable discovery of your device"), 0x80); 00727 #endif 00728 hci_inquiry(); 00729 hci_state = HCI_INQUIRY_STATE; 00730 } else 00731 hci_state = HCI_SCANNING_STATE; // Don't try to connect to a Wiimote 00732 break; 00733 00734 case HCI_INQUIRY_STATE: 00735 if(hci_check_flag(HCI_FLAG_DEVICE_FOUND)) { 00736 hci_inquiry_cancel(); // Stop inquiry 00737 #ifdef DEBUG_USB_HOST 00738 if(pairWithWii) 00739 Notify(PSTR("\r\nWiimote found"), 0x80); 00740 else 00741 Notify(PSTR("\r\nHID device found"), 0x80); 00742 00743 Notify(PSTR("\r\nNow just create the instance like so:"), 0x80); 00744 if(pairWithWii) 00745 Notify(PSTR("\r\nWII Wii(&Btd);"), 0x80); 00746 else 00747 Notify(PSTR("\r\nBTHID bthid(&Btd);"), 0x80); 00748 00749 Notify(PSTR("\r\nAnd then press any button on the "), 0x80); 00750 if(pairWithWii) 00751 Notify(PSTR("Wiimote"), 0x80); 00752 else 00753 Notify(PSTR("device"), 0x80); 00754 #endif 00755 if(checkRemoteName) { 00756 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 00757 hci_state = HCI_REMOTE_NAME_STATE; 00758 } else 00759 hci_state = HCI_CONNECT_DEVICE_STATE; 00760 } 00761 break; 00762 00763 case HCI_CONNECT_DEVICE_STATE: 00764 if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) { 00765 #ifdef DEBUG_USB_HOST 00766 if(pairWithWii) 00767 Notify(PSTR("\r\nConnecting to Wiimote"), 0x80); 00768 else 00769 Notify(PSTR("\r\nConnecting to HID device"), 0x80); 00770 #endif 00771 checkRemoteName = false; 00772 hci_connect(); 00773 hci_state = HCI_CONNECTED_DEVICE_STATE; 00774 } 00775 break; 00776 00777 case HCI_CONNECTED_DEVICE_STATE: 00778 if(hci_check_flag(HCI_FLAG_CONNECT_EVENT)) { 00779 if(hci_check_flag(HCI_FLAG_CONNECT_COMPLETE)) { 00780 #ifdef DEBUG_USB_HOST 00781 if(pairWithWii) 00782 Notify(PSTR("\r\nConnected to Wiimote"), 0x80); 00783 else 00784 Notify(PSTR("\r\nConnected to HID device"), 0x80); 00785 #endif 00786 hci_authentication_request(); // This will start the pairing with the Wiimote 00787 hci_state = HCI_SCANNING_STATE; 00788 } else { 00789 #ifdef DEBUG_USB_HOST 00790 Notify(PSTR("\r\nTrying to connect one more time..."), 0x80); 00791 #endif 00792 hci_connect(); // Try to connect one more time 00793 } 00794 } 00795 break; 00796 00797 case HCI_SCANNING_STATE: 00798 if(!connectToWii && !pairWithWii && !connectToHIDDevice && !pairWithHIDDevice) { 00799 #ifdef DEBUG_USB_HOST 00800 Notify(PSTR("\r\nWait For Incoming Connection Request"), 0x80); 00801 #endif 00802 hci_write_scan_enable(); 00803 waitingForConnection = true; 00804 hci_state = HCI_CONNECT_IN_STATE; 00805 } 00806 break; 00807 00808 case HCI_CONNECT_IN_STATE: 00809 if(hci_check_flag(HCI_FLAG_INCOMING_REQUEST)) { 00810 waitingForConnection = false; 00811 #ifdef DEBUG_USB_HOST 00812 Notify(PSTR("\r\nIncoming Connection Request"), 0x80); 00813 #endif 00814 hci_remote_name(); 00815 hci_state = HCI_REMOTE_NAME_STATE; 00816 } else if(hci_check_flag(HCI_FLAG_DISCONNECT_COMPLETE)) 00817 hci_state = HCI_DISCONNECT_STATE; 00818 break; 00819 00820 case HCI_REMOTE_NAME_STATE: 00821 if(hci_check_flag(HCI_FLAG_REMOTE_NAME_COMPLETE)) { 00822 #ifdef DEBUG_USB_HOST 00823 Notify(PSTR("\r\nRemote Name: "), 0x80); 00824 for(uint8_t i = 0; i < strlen(remote_name); i++) 00825 Notifyc(remote_name[i], 0x80); 00826 #endif 00827 if(strncmp((const char*)remote_name, "Nintendo", 8) == 0) { 00828 incomingWii = true; 00829 motionPlusInside = false; 00830 wiiUProController = false; 00831 pairWiiUsingSync = false; 00832 #ifdef DEBUG_USB_HOST 00833 Notify(PSTR("\r\nWiimote is connecting"), 0x80); 00834 #endif 00835 if(strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-TR", 22) == 0) { 00836 #ifdef DEBUG_USB_HOST 00837 Notify(PSTR(" with Motion Plus Inside"), 0x80); 00838 #endif 00839 motionPlusInside = true; 00840 } else if(strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-UC", 22) == 0) { 00841 #ifdef DEBUG_USB_HOST 00842 Notify(PSTR(" - Wii U Pro Controller"), 0x80); 00843 #endif 00844 wiiUProController = motionPlusInside = pairWiiUsingSync = true; 00845 } else if(strncmp((const char*)remote_name, "Nintendo RVL-WBC-01", 19) == 0) { 00846 #ifdef DEBUG_USB_HOST 00847 Notify(PSTR(" - Wii Balance Board"), 0x80); 00848 #endif 00849 pairWiiUsingSync = true; 00850 } 00851 } 00852 if(classOfDevice[2] == 0 && classOfDevice[1] == 0x25 && classOfDevice[0] == 0x08 && strncmp((const char*)remote_name, "Wireless Controller", 19) == 0) { 00853 #ifdef DEBUG_USB_HOST 00854 Notify(PSTR("\r\nPS4 controller is connecting"), 0x80); 00855 #endif 00856 incomingPS4 = true; 00857 } 00858 if(pairWithWii && checkRemoteName) 00859 hci_state = HCI_CONNECT_DEVICE_STATE; 00860 else { 00861 hci_accept_connection(); 00862 hci_state = HCI_CONNECTED_STATE; 00863 } 00864 } 00865 break; 00866 00867 case HCI_CONNECTED_STATE: 00868 if(hci_check_flag(HCI_FLAG_CONNECT_COMPLETE)) { 00869 #ifdef DEBUG_USB_HOST 00870 Notify(PSTR("\r\nConnected to Device: "), 0x80); 00871 for(int8_t i = 5; i > 0; i--) { 00872 D_PrintHex<uint8_t > (disc_bdaddr[i], 0x80); 00873 Notify(PSTR(":"), 0x80); 00874 } 00875 D_PrintHex<uint8_t > (disc_bdaddr[0], 0x80); 00876 #endif 00877 if(incomingPS4) 00878 connectToHIDDevice = true; // We should always connect to the PS4 controller 00879 00880 // Clear these flags for a new connection 00881 l2capConnectionClaimed = false; 00882 sdpConnectionClaimed = false; 00883 rfcommConnectionClaimed = false; 00884 00885 hci_event_flag = 0; 00886 hci_state = HCI_DONE_STATE; 00887 } 00888 break; 00889 00890 case HCI_DONE_STATE: 00891 hci_counter++; 00892 if(hci_counter > 1000) { // Wait until we have looped 1000 times to make sure that the L2CAP connection has been started 00893 hci_counter = 0; 00894 hci_state = HCI_SCANNING_STATE; 00895 } 00896 break; 00897 00898 case HCI_DISCONNECT_STATE: 00899 if(hci_check_flag(HCI_FLAG_DISCONNECT_COMPLETE)) { 00900 #ifdef DEBUG_USB_HOST 00901 Notify(PSTR("\r\nHCI Disconnected from Device"), 0x80); 00902 #endif 00903 hci_event_flag = 0; // Clear all flags 00904 00905 // Reset all buffers 00906 memset(hcibuf, 0, BULK_MAXPKTSIZE); 00907 memset(l2capinbuf, 0, BULK_MAXPKTSIZE); 00908 00909 connectToWii = incomingWii = pairWithWii = false; 00910 connectToHIDDevice = incomingHIDDevice = pairWithHIDDevice = checkRemoteName = false; 00911 incomingPS4 = false; 00912 00913 hci_state = HCI_SCANNING_STATE; 00914 } 00915 break; 00916 default: 00917 break; 00918 } 00919 } 00920 00921 void BTD::ACL_event_task() { 00922 uint16_t length = BULK_MAXPKTSIZE; 00923 uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_DATAIN_PIPE ].epAddr, &length, l2capinbuf, pollInterval); // Input on endpoint 2 00924 00925 if(!rcode) { // Check for errors 00926 if(length > 0) { // Check if any data was read 00927 for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++) { 00928 if(btService[i]) 00929 btService[i]->ACLData(l2capinbuf); 00930 } 00931 } 00932 } 00933 #ifdef EXTRADEBUG 00934 else if(rcode != hrNAK) { 00935 Notify(PSTR("\r\nACL data in error: "), 0x80); 00936 D_PrintHex<uint8_t > (rcode, 0x80); 00937 } 00938 #endif 00939 for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++) 00940 if(btService[i]) 00941 btService[i]->Run(); 00942 } 00943 00944 /************************************************************/ 00945 /* HCI Commands */ 00946 00947 /************************************************************/ 00948 void BTD::HCI_Command(uint8_t* data, uint16_t nbytes) { 00949 hci_clear_flag(HCI_FLAG_CMD_COMPLETE); 00950 pUsb->ctrlReq(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bmREQ_HCI_OUT, 0x00, 0x00, 0x00, 0x00, nbytes, nbytes, data, NULL); 00951 } 00952 00953 void BTD::hci_reset() { 00954 hci_event_flag = 0; // Clear all the flags 00955 hcibuf[0] = 0x03; // HCI OCF = 3 00956 hcibuf[1] = 0x03 << 2; // HCI OGF = 3 00957 hcibuf[2] = 0x00; 00958 00959 HCI_Command(hcibuf, 3); 00960 } 00961 00962 void BTD::hci_write_scan_enable() { 00963 hci_clear_flag(HCI_FLAG_INCOMING_REQUEST); 00964 hcibuf[0] = 0x1A; // HCI OCF = 1A 00965 hcibuf[1] = 0x03 << 2; // HCI OGF = 3 00966 hcibuf[2] = 0x01; // parameter length = 1 00967 if(btdName != NULL) 00968 hcibuf[3] = 0x03; // Inquiry Scan enabled. Page Scan enabled. 00969 else 00970 hcibuf[3] = 0x02; // Inquiry Scan disabled. Page Scan enabled. 00971 00972 HCI_Command(hcibuf, 4); 00973 } 00974 00975 void BTD::hci_write_scan_disable() { 00976 hcibuf[0] = 0x1A; // HCI OCF = 1A 00977 hcibuf[1] = 0x03 << 2; // HCI OGF = 3 00978 hcibuf[2] = 0x01; // parameter length = 1 00979 hcibuf[3] = 0x00; // Inquiry Scan disabled. Page Scan disabled. 00980 00981 HCI_Command(hcibuf, 4); 00982 } 00983 00984 void BTD::hci_read_bdaddr() { 00985 hci_clear_flag(HCI_FLAG_READ_BDADDR); 00986 hcibuf[0] = 0x09; // HCI OCF = 9 00987 hcibuf[1] = 0x04 << 2; // HCI OGF = 4 00988 hcibuf[2] = 0x00; 00989 00990 HCI_Command(hcibuf, 3); 00991 } 00992 00993 void BTD::hci_read_local_version_information() { 00994 hci_clear_flag(HCI_FLAG_READ_VERSION); 00995 hcibuf[0] = 0x01; // HCI OCF = 1 00996 hcibuf[1] = 0x04 << 2; // HCI OGF = 4 00997 hcibuf[2] = 0x00; 00998 00999 HCI_Command(hcibuf, 3); 01000 } 01001 01002 void BTD::hci_accept_connection() { 01003 hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE); 01004 hcibuf[0] = 0x09; // HCI OCF = 9 01005 hcibuf[1] = 0x01 << 2; // HCI OGF = 1 01006 hcibuf[2] = 0x07; // parameter length 7 01007 hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr 01008 hcibuf[4] = disc_bdaddr[1]; 01009 hcibuf[5] = disc_bdaddr[2]; 01010 hcibuf[6] = disc_bdaddr[3]; 01011 hcibuf[7] = disc_bdaddr[4]; 01012 hcibuf[8] = disc_bdaddr[5]; 01013 hcibuf[9] = 0x00; // Switch role to master 01014 01015 HCI_Command(hcibuf, 10); 01016 } 01017 01018 void BTD::hci_remote_name() { 01019 hci_clear_flag(HCI_FLAG_REMOTE_NAME_COMPLETE); 01020 hcibuf[0] = 0x19; // HCI OCF = 19 01021 hcibuf[1] = 0x01 << 2; // HCI OGF = 1 01022 hcibuf[2] = 0x0A; // parameter length = 10 01023 hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr 01024 hcibuf[4] = disc_bdaddr[1]; 01025 hcibuf[5] = disc_bdaddr[2]; 01026 hcibuf[6] = disc_bdaddr[3]; 01027 hcibuf[7] = disc_bdaddr[4]; 01028 hcibuf[8] = disc_bdaddr[5]; 01029 hcibuf[9] = 0x01; // Page Scan Repetition Mode 01030 hcibuf[10] = 0x00; // Reserved 01031 hcibuf[11] = 0x00; // Clock offset - low byte 01032 hcibuf[12] = 0x00; // Clock offset - high byte 01033 01034 HCI_Command(hcibuf, 13); 01035 } 01036 01037 void BTD::hci_set_local_name(const char* name) { 01038 hcibuf[0] = 0x13; // HCI OCF = 13 01039 hcibuf[1] = 0x03 << 2; // HCI OGF = 3 01040 hcibuf[2] = strlen(name) + 1; // parameter length = the length of the string + end byte 01041 uint8_t i; 01042 for(i = 0; i < strlen(name); i++) 01043 hcibuf[i + 3] = name[i]; 01044 hcibuf[i + 3] = 0x00; // End of string 01045 01046 HCI_Command(hcibuf, 4 + strlen(name)); 01047 } 01048 01049 void BTD::hci_inquiry() { 01050 hci_clear_flag(HCI_FLAG_DEVICE_FOUND); 01051 hcibuf[0] = 0x01; 01052 hcibuf[1] = 0x01 << 2; // HCI OGF = 1 01053 hcibuf[2] = 0x05; // Parameter Total Length = 5 01054 hcibuf[3] = 0x33; // LAP: Genera/Unlimited Inquiry Access Code (GIAC = 0x9E8B33) - see https://www.bluetooth.org/Technical/AssignedNumbers/baseband.htm 01055 hcibuf[4] = 0x8B; 01056 hcibuf[5] = 0x9E; 01057 hcibuf[6] = 0x30; // Inquiry time = 61.44 sec (maximum) 01058 hcibuf[7] = 0x0A; // 10 number of responses 01059 01060 HCI_Command(hcibuf, 8); 01061 } 01062 01063 void BTD::hci_inquiry_cancel() { 01064 hcibuf[0] = 0x02; 01065 hcibuf[1] = 0x01 << 2; // HCI OGF = 1 01066 hcibuf[2] = 0x00; // Parameter Total Length = 0 01067 01068 HCI_Command(hcibuf, 3); 01069 } 01070 01071 void BTD::hci_connect() { 01072 hci_connect(disc_bdaddr); // Use last discovered device 01073 } 01074 01075 void BTD::hci_connect(uint8_t *bdaddr) { 01076 hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE | HCI_FLAG_CONNECT_EVENT); 01077 hcibuf[0] = 0x05; 01078 hcibuf[1] = 0x01 << 2; // HCI OGF = 1 01079 hcibuf[2] = 0x0D; // parameter Total Length = 13 01080 hcibuf[3] = bdaddr[0]; // 6 octet bdaddr (LSB) 01081 hcibuf[4] = bdaddr[1]; 01082 hcibuf[5] = bdaddr[2]; 01083 hcibuf[6] = bdaddr[3]; 01084 hcibuf[7] = bdaddr[4]; 01085 hcibuf[8] = bdaddr[5]; 01086 hcibuf[9] = 0x18; // DM1 or DH1 may be used 01087 hcibuf[10] = 0xCC; // DM3, DH3, DM5, DH5 may be used 01088 hcibuf[11] = 0x01; // Page repetition mode R1 01089 hcibuf[12] = 0x00; // Reserved 01090 hcibuf[13] = 0x00; // Clock offset 01091 hcibuf[14] = 0x00; // Invalid clock offset 01092 hcibuf[15] = 0x00; // Do not allow role switch 01093 01094 HCI_Command(hcibuf, 16); 01095 } 01096 01097 void BTD::hci_pin_code_request_reply() { 01098 hcibuf[0] = 0x0D; // HCI OCF = 0D 01099 hcibuf[1] = 0x01 << 2; // HCI OGF = 1 01100 hcibuf[2] = 0x17; // parameter length 23 01101 hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr 01102 hcibuf[4] = disc_bdaddr[1]; 01103 hcibuf[5] = disc_bdaddr[2]; 01104 hcibuf[6] = disc_bdaddr[3]; 01105 hcibuf[7] = disc_bdaddr[4]; 01106 hcibuf[8] = disc_bdaddr[5]; 01107 if(pairWithWii) { 01108 hcibuf[9] = 6; // Pin length is the length of the Bluetooth address 01109 if(pairWiiUsingSync) { 01110 #ifdef DEBUG_USB_HOST 01111 Notify(PSTR("\r\nPairing with Wii controller via SYNC"), 0x80); 01112 #endif 01113 for(uint8_t i = 0; i < 6; i++) 01114 hcibuf[10 + i] = my_bdaddr[i]; // The pin is the Bluetooth dongles Bluetooth address backwards 01115 } else { 01116 for(uint8_t i = 0; i < 6; i++) 01117 hcibuf[10 + i] = disc_bdaddr[i]; // The pin is the Wiimote's Bluetooth address backwards 01118 } 01119 for(uint8_t i = 16; i < 26; i++) 01120 hcibuf[i] = 0x00; // The rest should be 0 01121 } else { 01122 hcibuf[9] = strlen(btdPin); // Length of pin 01123 uint8_t i; 01124 for(i = 0; i < strlen(btdPin); i++) // The maximum size of the pin is 16 01125 hcibuf[i + 10] = btdPin[i]; 01126 for(; i < 16; i++) 01127 hcibuf[i + 10] = 0x00; // The rest should be 0 01128 } 01129 01130 HCI_Command(hcibuf, 26); 01131 } 01132 01133 void BTD::hci_pin_code_negative_request_reply() { 01134 hcibuf[0] = 0x0E; // HCI OCF = 0E 01135 hcibuf[1] = 0x01 << 2; // HCI OGF = 1 01136 hcibuf[2] = 0x06; // parameter length 6 01137 hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr 01138 hcibuf[4] = disc_bdaddr[1]; 01139 hcibuf[5] = disc_bdaddr[2]; 01140 hcibuf[6] = disc_bdaddr[3]; 01141 hcibuf[7] = disc_bdaddr[4]; 01142 hcibuf[8] = disc_bdaddr[5]; 01143 01144 HCI_Command(hcibuf, 9); 01145 } 01146 01147 void BTD::hci_link_key_request_negative_reply() { 01148 hcibuf[0] = 0x0C; // HCI OCF = 0C 01149 hcibuf[1] = 0x01 << 2; // HCI OGF = 1 01150 hcibuf[2] = 0x06; // parameter length 6 01151 hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr 01152 hcibuf[4] = disc_bdaddr[1]; 01153 hcibuf[5] = disc_bdaddr[2]; 01154 hcibuf[6] = disc_bdaddr[3]; 01155 hcibuf[7] = disc_bdaddr[4]; 01156 hcibuf[8] = disc_bdaddr[5]; 01157 01158 HCI_Command(hcibuf, 9); 01159 } 01160 01161 void BTD::hci_authentication_request() { 01162 hcibuf[0] = 0x11; // HCI OCF = 11 01163 hcibuf[1] = 0x01 << 2; // HCI OGF = 1 01164 hcibuf[2] = 0x02; // parameter length = 2 01165 hcibuf[3] = (uint8_t)(hci_handle & 0xFF); //connection handle - low byte 01166 hcibuf[4] = (uint8_t)((hci_handle >> 8) & 0x0F); //connection handle - high byte 01167 01168 HCI_Command(hcibuf, 5); 01169 } 01170 01171 void BTD::hci_disconnect(uint16_t handle) { // This is called by the different services 01172 hci_clear_flag(HCI_FLAG_DISCONNECT_COMPLETE); 01173 hcibuf[0] = 0x06; // HCI OCF = 6 01174 hcibuf[1] = 0x01 << 2; // HCI OGF = 1 01175 hcibuf[2] = 0x03; // parameter length = 3 01176 hcibuf[3] = (uint8_t)(handle & 0xFF); //connection handle - low byte 01177 hcibuf[4] = (uint8_t)((handle >> 8) & 0x0F); //connection handle - high byte 01178 hcibuf[5] = 0x13; // reason 01179 01180 HCI_Command(hcibuf, 6); 01181 } 01182 01183 void BTD::hci_write_class_of_device() { // See http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html 01184 hcibuf[0] = 0x24; // HCI OCF = 24 01185 hcibuf[1] = 0x03 << 2; // HCI OGF = 3 01186 hcibuf[2] = 0x03; // parameter length = 3 01187 hcibuf[3] = 0x04; // Robot 01188 hcibuf[4] = 0x08; // Toy 01189 hcibuf[5] = 0x00; 01190 01191 HCI_Command(hcibuf, 6); 01192 } 01193 /******************************************************************* 01194 * * 01195 * HCI ACL Data Packet * 01196 * * 01197 * buf[0] buf[1] buf[2] buf[3] 01198 * 0 4 8 11 12 16 24 31 MSB 01199 * .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. 01200 * | HCI Handle |PB |BC | Data Total Length | HCI ACL Data Packet 01201 * .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. 01202 * 01203 * buf[4] buf[5] buf[6] buf[7] 01204 * 0 8 16 31 MSB 01205 * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. 01206 * | Length | Channel ID | Basic L2CAP header 01207 * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. 01208 * 01209 * buf[8] buf[9] buf[10] buf[11] 01210 * 0 8 16 31 MSB 01211 * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. 01212 * | Code | Identifier | Length | Control frame (C-frame) 01213 * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. (signaling packet format) 01214 */ 01215 /************************************************************/ 01216 /* L2CAP Commands */ 01217 01218 /************************************************************/ 01219 void BTD::L2CAP_Command(uint16_t handle, uint8_t* data, uint8_t nbytes, uint8_t channelLow, uint8_t channelHigh) { 01220 uint8_t buf[8 + nbytes]; 01221 buf[0] = (uint8_t)(handle & 0xff); // HCI handle with PB,BC flag 01222 buf[1] = (uint8_t)(((handle >> 8) & 0x0f) | 0x20); 01223 buf[2] = (uint8_t)((4 + nbytes) & 0xff); // HCI ACL total data length 01224 buf[3] = (uint8_t)((4 + nbytes) >> 8); 01225 buf[4] = (uint8_t)(nbytes & 0xff); // L2CAP header: Length 01226 buf[5] = (uint8_t)(nbytes >> 8); 01227 buf[6] = channelLow; 01228 buf[7] = channelHigh; 01229 01230 for(uint16_t i = 0; i < nbytes; i++) // L2CAP C-frame 01231 buf[8 + i] = data[i]; 01232 01233 uint8_t rcode = pUsb->outTransfer(bAddress, epInfo[ BTD_DATAOUT_PIPE ].epAddr, (8 + nbytes), buf); 01234 if(rcode) { 01235 delay(100); // This small delay prevents it from overflowing if it fails 01236 #ifdef DEBUG_USB_HOST 01237 Notify(PSTR("\r\nError sending L2CAP message: 0x"), 0x80); 01238 D_PrintHex<uint8_t > (rcode, 0x80); 01239 Notify(PSTR(" - Channel ID: "), 0x80); 01240 D_PrintHex<uint8_t > (channelHigh, 0x80); 01241 Notify(PSTR(" "), 0x80); 01242 D_PrintHex<uint8_t > (channelLow, 0x80); 01243 #endif 01244 } 01245 } 01246 01247 void BTD::l2cap_connection_request(uint16_t handle, uint8_t rxid, uint8_t* scid, uint16_t psm) { 01248 l2capoutbuf[0] = L2CAP_CMD_CONNECTION_REQUEST; // Code 01249 l2capoutbuf[1] = rxid; // Identifier 01250 l2capoutbuf[2] = 0x04; // Length 01251 l2capoutbuf[3] = 0x00; 01252 l2capoutbuf[4] = (uint8_t)(psm & 0xff); // PSM 01253 l2capoutbuf[5] = (uint8_t)(psm >> 8); 01254 l2capoutbuf[6] = scid[0]; // Source CID 01255 l2capoutbuf[7] = scid[1]; 01256 01257 L2CAP_Command(handle, l2capoutbuf, 8); 01258 } 01259 01260 void BTD::l2cap_connection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid, uint8_t result) { 01261 l2capoutbuf[0] = L2CAP_CMD_CONNECTION_RESPONSE; // Code 01262 l2capoutbuf[1] = rxid; // Identifier 01263 l2capoutbuf[2] = 0x08; // Length 01264 l2capoutbuf[3] = 0x00; 01265 l2capoutbuf[4] = dcid[0]; // Destination CID 01266 l2capoutbuf[5] = dcid[1]; 01267 l2capoutbuf[6] = scid[0]; // Source CID 01268 l2capoutbuf[7] = scid[1]; 01269 l2capoutbuf[8] = result; // Result: Pending or Success 01270 l2capoutbuf[9] = 0x00; 01271 l2capoutbuf[10] = 0x00; // No further information 01272 l2capoutbuf[11] = 0x00; 01273 01274 L2CAP_Command(handle, l2capoutbuf, 12); 01275 } 01276 01277 void BTD::l2cap_config_request(uint16_t handle, uint8_t rxid, uint8_t* dcid) { 01278 l2capoutbuf[0] = L2CAP_CMD_CONFIG_REQUEST; // Code 01279 l2capoutbuf[1] = rxid; // Identifier 01280 l2capoutbuf[2] = 0x08; // Length 01281 l2capoutbuf[3] = 0x00; 01282 l2capoutbuf[4] = dcid[0]; // Destination CID 01283 l2capoutbuf[5] = dcid[1]; 01284 l2capoutbuf[6] = 0x00; // Flags 01285 l2capoutbuf[7] = 0x00; 01286 l2capoutbuf[8] = 0x01; // Config Opt: type = MTU (Maximum Transmission Unit) - Hint 01287 l2capoutbuf[9] = 0x02; // Config Opt: length 01288 l2capoutbuf[10] = 0xFF; // MTU 01289 l2capoutbuf[11] = 0xFF; 01290 01291 L2CAP_Command(handle, l2capoutbuf, 12); 01292 } 01293 01294 void BTD::l2cap_config_response(uint16_t handle, uint8_t rxid, uint8_t* scid) { 01295 l2capoutbuf[0] = L2CAP_CMD_CONFIG_RESPONSE; // Code 01296 l2capoutbuf[1] = rxid; // Identifier 01297 l2capoutbuf[2] = 0x0A; // Length 01298 l2capoutbuf[3] = 0x00; 01299 l2capoutbuf[4] = scid[0]; // Source CID 01300 l2capoutbuf[5] = scid[1]; 01301 l2capoutbuf[6] = 0x00; // Flag 01302 l2capoutbuf[7] = 0x00; 01303 l2capoutbuf[8] = 0x00; // Result 01304 l2capoutbuf[9] = 0x00; 01305 l2capoutbuf[10] = 0x01; // Config 01306 l2capoutbuf[11] = 0x02; 01307 l2capoutbuf[12] = 0xA0; 01308 l2capoutbuf[13] = 0x02; 01309 01310 L2CAP_Command(handle, l2capoutbuf, 14); 01311 } 01312 01313 void BTD::l2cap_disconnection_request(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid) { 01314 l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_REQUEST; // Code 01315 l2capoutbuf[1] = rxid; // Identifier 01316 l2capoutbuf[2] = 0x04; // Length 01317 l2capoutbuf[3] = 0x00; 01318 l2capoutbuf[4] = dcid[0]; 01319 l2capoutbuf[5] = dcid[1]; 01320 l2capoutbuf[6] = scid[0]; 01321 l2capoutbuf[7] = scid[1]; 01322 01323 L2CAP_Command(handle, l2capoutbuf, 8); 01324 } 01325 01326 void BTD::l2cap_disconnection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid) { 01327 l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_RESPONSE; // Code 01328 l2capoutbuf[1] = rxid; // Identifier 01329 l2capoutbuf[2] = 0x04; // Length 01330 l2capoutbuf[3] = 0x00; 01331 l2capoutbuf[4] = dcid[0]; 01332 l2capoutbuf[5] = dcid[1]; 01333 l2capoutbuf[6] = scid[0]; 01334 l2capoutbuf[7] = scid[1]; 01335 01336 L2CAP_Command(handle, l2capoutbuf, 8); 01337 } 01338 01339 void BTD::l2cap_information_response(uint16_t handle, uint8_t rxid, uint8_t infoTypeLow, uint8_t infoTypeHigh) { 01340 l2capoutbuf[0] = L2CAP_CMD_INFORMATION_RESPONSE; // Code 01341 l2capoutbuf[1] = rxid; // Identifier 01342 l2capoutbuf[2] = 0x08; // Length 01343 l2capoutbuf[3] = 0x00; 01344 l2capoutbuf[4] = infoTypeLow; 01345 l2capoutbuf[5] = infoTypeHigh; 01346 l2capoutbuf[6] = 0x00; // Result = success 01347 l2capoutbuf[7] = 0x00; // Result = success 01348 l2capoutbuf[8] = 0x00; 01349 l2capoutbuf[9] = 0x00; 01350 l2capoutbuf[10] = 0x00; 01351 l2capoutbuf[11] = 0x00; 01352 01353 L2CAP_Command(handle, l2capoutbuf, 12); 01354 } 01355 01356 /* PS3 Commands - only set Bluetooth address is implemented in this library */ 01357 void BTD::setBdaddr(uint8_t* bdaddr) { 01358 /* Set the internal Bluetooth address */ 01359 uint8_t buf[8]; 01360 buf[0] = 0x01; 01361 buf[1] = 0x00; 01362 01363 for(uint8_t i = 0; i < 6; i++) 01364 buf[i + 2] = bdaddr[5 - i]; // Copy into buffer, has to be written reversed, so it is MSB first 01365 01366 // 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 01367 pUsb->ctrlReq(bAddress, epInfo[BTD_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL); 01368 } 01369 01370 void BTD::setMoveBdaddr(uint8_t* bdaddr) { 01371 /* Set the internal Bluetooth address */ 01372 uint8_t buf[11]; 01373 buf[0] = 0x05; 01374 buf[7] = 0x10; 01375 buf[8] = 0x01; 01376 buf[9] = 0x02; 01377 buf[10] = 0x12; 01378 01379 for(uint8_t i = 0; i < 6; i++) 01380 buf[i + 1] = bdaddr[i]; 01381 01382 // 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 01383 pUsb->ctrlReq(bAddress, epInfo[BTD_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00, 11, 11, buf, NULL); 01384 } 01385
Generated on Thu Jul 14 2022 08:33:40 by
1.7.2