diff --git a/BTD.cpp b/BTD.cpp index e30be250..79a4b6cd 100755 --- a/BTD.cpp +++ b/BTD.cpp @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -20,990 +20,1004 @@ //#define EXTRADEBUG // Uncomment to get even more debugging data const uint8_t BTD::BTD_CONTROL_PIPE = 0; -const uint8_t BTD::BTD_EVENT_PIPE = 1; +const uint8_t BTD::BTD_EVENT_PIPE = 1; const uint8_t BTD::BTD_DATAIN_PIPE = 2; const uint8_t BTD::BTD_DATAOUT_PIPE = 3; -BTD::BTD(USB *p): +BTD::BTD(USB *p) : pUsb(p), // Pointer to USB class instance - mandatory bAddress(0), // Device address - mandatory bNumEP(1), // If config descriptor needs to be parsed qNextPollTime(0), // Reset NextPollTime bPollEnable(false) // Don't start polling before dongle is connected { - for(uint8_t i=0; iRegisterDeviceClass(this); //set devConfig[] entry + for (uint8_t i = 0; i < BTD_MAX_ENDPOINTS; i++) { + epInfo[i].epAddr = 0; + epInfo[i].maxPktSize = (i) ? 0 : 8; + epInfo[i].epAttribs = 0; + epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER; + } + + if (pUsb) // register in USB subsystem + pUsb->RegisterDeviceClass(this); //set devConfig[] entry } uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) { - uint8_t buf[sizeof(USB_DEVICE_DESCRIPTOR)]; - uint8_t rcode; - UsbDevice *p = NULL; - EpInfo *oldep_ptr = NULL; - uint8_t num_of_conf; // number of configurations - uint16_t PID; - uint16_t VID; - - // get memory address of USB device address pool - AddressPool &addrPool = pUsb->GetAddressPool(); + uint8_t buf[sizeof (USB_DEVICE_DESCRIPTOR)]; + uint8_t rcode; + UsbDevice *p = NULL; + EpInfo *oldep_ptr = NULL; + uint8_t num_of_conf; // number of configurations + uint16_t PID; + uint16_t VID; + + // get memory address of USB device address pool + AddressPool &addrPool = pUsb->GetAddressPool(); #ifdef EXTRADEBUG - Notify(PSTR("\r\nBTD Init")); + Notify(PSTR("\r\nBTD Init"), 0x80); #endif - // check if address has already been assigned to an instance - if (bAddress) { + // check if address has already been assigned to an instance + if (bAddress) { #ifdef DEBUG - Notify(PSTR("\r\nAddress in use")); + Notify(PSTR("\r\nAddress in use"), 0x80); #endif - return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; - } - - // Get pointer to pseudo device with address 0 assigned - p = addrPool.GetUsbDevicePtr(0); - - if (!p) { + return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; + } + + // Get pointer to pseudo device with address 0 assigned + p = addrPool.GetUsbDevicePtr(0); + + if (!p) { #ifdef DEBUG - Notify(PSTR("\r\nAddress not found")); + Notify(PSTR("\r\nAddress not found"), 0x80); #endif - return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - } - - if (!p->epinfo) { + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + } + + if (!p->epinfo) { #ifdef DEBUG - Notify(PSTR("\r\nepinfo is null")); + Notify(PSTR("\r\nepinfo is null"), 0x80); +#endif + return USB_ERROR_EPINFO_IS_NULL; + } + + // Save old pointer to EP_RECORD of address 0 + oldep_ptr = p->epinfo; + + // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence + p->epinfo = epInfo; + + p->lowspeed = lowspeed; + + // Get device descriptor + rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*)buf); // Get device descriptor - addr, ep, nbytes, data + + // Restore p->epinfo + p->epinfo = oldep_ptr; + + if (rcode) + goto FailGetDevDescr; + + // Allocate new address according to device class + bAddress = addrPool.AllocAddress(parent, false, port); + + if (!bAddress) + return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; + + // Extract Max Packet Size from device descriptor + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; + + // Assign new address to the device + rcode = pUsb->setAddr(0, 0, bAddress); + if (rcode) { + p->lowspeed = false; + addrPool.FreeAddress(bAddress); + bAddress = 0; +#ifdef DEBUG + Notify(PSTR("\r\nsetAddr: "), 0x80); +#endif + PrintHex (rcode, 0x80); + return rcode; + } +#ifdef EXTRADEBUG + Notify(PSTR("\r\nAddr: "), 0x80); + PrintHex (bAddress, 0x80); #endif - return USB_ERROR_EPINFO_IS_NULL; - } - - // Save old pointer to EP_RECORD of address 0 - oldep_ptr = p->epinfo; - - // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence - p->epinfo = epInfo; - - p->lowspeed = lowspeed; - - // Get device descriptor - rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data - - // Restore p->epinfo - p->epinfo = oldep_ptr; - - if(rcode) - goto FailGetDevDescr; - - // Allocate new address according to device class - bAddress = addrPool.AllocAddress(parent, false, port); - - if (!bAddress) - return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; - - // Extract Max Packet Size from device descriptor - epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; - - // Assign new address to the device - rcode = pUsb->setAddr( 0, 0, bAddress ); - if (rcode) { p->lowspeed = false; - addrPool.FreeAddress(bAddress); - bAddress = 0; -#ifdef DEBUG - Notify(PSTR("\r\nsetAddr: ")); -#endif - PrintHex(rcode); - return rcode; - } -#ifdef EXTRADEBUG - Notify(PSTR("\r\nAddr: ")); - PrintHex(bAddress); -#endif - p->lowspeed = false; - - //get pointer to assigned address record - p = addrPool.GetUsbDevicePtr(bAddress); - if (!p) - return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - - p->lowspeed = lowspeed; - - // Assign epInfo to epinfo pointer - only EP0 is known - rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); - if (rcode) - goto FailSetDevTblEntry; - VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor; - PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct; - - if(VID == PS3_VID && (PID == PS3_PID || PID == PS3NAVIGATION_PID || PID == PS3MOVE_PID)) { - /* We only need the Control endpoint, so we don't have to initialize the other endpoints of device */ - rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 1); - if( rcode ) - goto FailSetConf; - - if(PID == PS3_PID || PID == PS3NAVIGATION_PID) { -#ifdef DEBUG - if(PID == PS3_PID) - Notify(PSTR("\r\nDualshock 3 Controller Connected")); - else // must be a navigation controller - Notify(PSTR("\r\nNavigation Controller Connected")); -#endif - /* Set internal bluetooth address */ - setBdaddr(my_bdaddr); - } - else { // must be a Motion controller -#ifdef DEBUG - Notify(PSTR("\r\nMotion Controller Connected")); -#endif - setMoveBdaddr(my_bdaddr); - } - rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 0); // Reset configuration value - pUsb->setAddr(bAddress, 0, 0); // Reset address - Release(); // Release device - return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; // return - } - else { - num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; - // check if attached device is a Bluetooth dongle and fill endpoint data structure - // first interface in the configuration must have Bluetooth assigned Class/Subclass/Protocol - // and 3 endpoints - interrupt-IN, bulk-IN, bulk-OUT, - // not necessarily in this order - for (uint8_t i=0; i confDescrParser(this); - rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser); - if(rcode) - goto FailGetConfDescr; - if(bNumEP >= BTD_MAX_ENDPOINTS) // All endpoints extracted - break; - } - - if (bNumEP < BTD_MAX_ENDPOINTS) - goto FailUnknownDevice; - - // Assign epInfo to epinfo pointer - this time all 3 endpoins - rcode = pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo); - if(rcode) - goto FailSetDevTblEntry; - - delay(200); // Give time for address change - - // Set Configuration Value - rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bConfNum); - if(rcode) - goto FailSetConf; - - hci_num_reset_loops = 100; // only loop 100 times before trying to send the hci reset command - hci_counter = 0; - hci_state = HCI_INIT_STATE; - watingForConnection = false; - bPollEnable = true; + //get pointer to assigned address record + p = addrPool.GetUsbDevicePtr(bAddress); + if (!p) + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + + p->lowspeed = lowspeed; + + // Assign epInfo to epinfo pointer - only EP0 is known + rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); + if (rcode) + goto FailSetDevTblEntry; + VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor; + PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct; + + if (VID == PS3_VID && (PID == PS3_PID || PID == PS3NAVIGATION_PID || PID == PS3MOVE_PID)) { + /* We only need the Control endpoint, so we don't have to initialize the other endpoints of device */ + rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 1); + if (rcode) + goto FailSetConf; + + if (PID == PS3_PID || PID == PS3NAVIGATION_PID) { +#ifdef DEBUG + if (PID == PS3_PID) + Notify(PSTR("\r\nDualshock 3 Controller Connected"), 0x80); + else // must be a navigation controller + Notify(PSTR("\r\nNavigation Controller Connected"), 0x80); +#endif + /* Set internal bluetooth address */ + setBdaddr(my_bdaddr); + } else { // must be a Motion controller +#ifdef DEBUG + Notify(PSTR("\r\nMotion Controller Connected"), 0x80); +#endif + setMoveBdaddr(my_bdaddr); + } + rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 0); // Reset configuration value + pUsb->setAddr(bAddress, 0, 0); // Reset address + Release(); // Release device + return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; // return + } else { + num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; + + // check if attached device is a Bluetooth dongle and fill endpoint data structure + // first interface in the configuration must have Bluetooth assigned Class/Subclass/Protocol + // and 3 endpoints - interrupt-IN, bulk-IN, bulk-OUT, + // not necessarily in this order + for (uint8_t i = 0; i < num_of_conf; i++) { + ConfigDescParser confDescrParser(this); + rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser); + if (rcode) + goto FailGetConfDescr; + if (bNumEP >= BTD_MAX_ENDPOINTS) // All endpoints extracted + break; + } + + if (bNumEP < BTD_MAX_ENDPOINTS) + goto FailUnknownDevice; + + // Assign epInfo to epinfo pointer - this time all 3 endpoins + rcode = pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo); + if (rcode) + goto FailSetDevTblEntry; + + delay(200); // Give time for address change + + // Set Configuration Value + rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bConfNum); + if (rcode) + goto FailSetConf; + + hci_num_reset_loops = 100; // only loop 100 times before trying to send the hci reset command + hci_counter = 0; + hci_state = HCI_INIT_STATE; + watingForConnection = false; + bPollEnable = true; #ifdef DEBUG - Notify(PSTR("\r\nBluetooth Dongle Initialized")); + Notify(PSTR("\r\nBluetooth Dongle Initialized"), 0x80); #endif - } - return 0; // Successful configuration - - /* diagnostic messages */ + } + return 0; // Successful configuration + + /* diagnostic messages */ FailGetDevDescr: #ifdef DEBUG - Notify(PSTR("\r\ngetDevDescr")); + Notify(PSTR("\r\ngetDevDescr"), 0x80); #endif - goto Fail; + goto Fail; FailSetDevTblEntry: #ifdef DEBUG - Notify(PSTR("\r\nsetDevTblEn")); + Notify(PSTR("\r\nsetDevTblEn"), 0x80); #endif - goto Fail; + goto Fail; FailGetConfDescr: #ifdef DEBUG - Notify(PSTR("\r\ngetConf")); + Notify(PSTR("\r\ngetConf"), 0x80); #endif - goto Fail; + goto Fail; FailSetConf: #ifdef DEBUG - Notify(PSTR("\r\nsetConf")); + Notify(PSTR("\r\nsetConf"), 0x80); #endif - goto Fail; + goto Fail; FailUnknownDevice: #ifdef DEBUG - Notify(PSTR("\r\nUnknown Device Connected - VID: ")); - PrintHex(VID); - Notify(PSTR(" PID: ")); - PrintHex(PID); + Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80); + PrintHex (VID, 0x80); + Notify(PSTR(" PID: "), 0x80); + PrintHex (PID, 0x80); #endif - pUsb->setAddr(bAddress, 0, 0); // Reset address - rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; - goto Fail; + pUsb->setAddr(bAddress, 0, 0); // Reset address + rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; + goto Fail; Fail: #ifdef DEBUG - Notify(PSTR("\r\nBTD Init Failed, error code: ")); - Serial.print(rcode); -#endif - Release(); - return rcode; + Notify(PSTR("\r\nBTD Init Failed, error code: "), 0x80); + Serial.print(rcode); +#endif + Release(); + return rcode; } + /* Extracts interrupt-IN, bulk-IN, bulk-OUT endpoint information from config descriptor */ void BTD::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *pep) { - //ErrorMessage(PSTR("Conf.Val"),conf); - //ErrorMessage(PSTR("Iface Num"),iface); - //ErrorMessage(PSTR("Alt.Set"),alt); - - if(alt) // wrong interface - by BT spec, no alt setting - return; - - bConfNum = conf; - uint8_t index; - - if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80) // Interrupt In endpoint found - index = BTD_EVENT_PIPE; - - else { - if ((pep->bmAttributes & 0x02) == 2) // bulk endpoint found - index = ((pep->bEndpointAddress & 0x80) == 0x80) ? BTD_DATAIN_PIPE : BTD_DATAOUT_PIPE; - else - return; - } - - // Fill the rest of endpoint data structure - epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F); - epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize; + //ErrorMessage(PSTR("Conf.Val"),conf); + //ErrorMessage(PSTR("Iface Num"),iface); + //ErrorMessage(PSTR("Alt.Set"),alt); + + if (alt) // wrong interface - by BT spec, no alt setting + return; + + bConfNum = conf; + uint8_t index; + + if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80) // Interrupt In endpoint found + index = BTD_EVENT_PIPE; + + else { + if ((pep->bmAttributes & 0x02) == 2) // bulk endpoint found + index = ((pep->bEndpointAddress & 0x80) == 0x80) ? BTD_DATAIN_PIPE : BTD_DATAOUT_PIPE; + else + return; + } + + // Fill the rest of endpoint data structure + epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F); + epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize; #ifdef EXTRADEBUG - PrintEndpointDescriptor(pep); + PrintEndpointDescriptor(pep); #endif - if(pollInterval < pep->bInterval) // Set the polling interval as the largest polling interval obtained from endpoints - pollInterval = pep->bInterval; - bNumEP++; + if (pollInterval < pep->bInterval) // Set the polling interval as the largest polling interval obtained from endpoints + pollInterval = pep->bInterval; + bNumEP++; } + void BTD::PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR* ep_ptr) { #ifdef EXTRADEBUG - Notify(PSTR("\r\nEndpoint descriptor:")); - Notify(PSTR("\r\nLength:\t\t")); - PrintHex(ep_ptr->bLength); - Notify(PSTR("\r\nType:\t\t")); - PrintHex(ep_ptr->bDescriptorType); - Notify(PSTR("\r\nAddress:\t")); - PrintHex(ep_ptr->bEndpointAddress); - Notify(PSTR("\r\nAttributes:\t")); - PrintHex(ep_ptr->bmAttributes); - Notify(PSTR("\r\nMaxPktSize:\t")); - PrintHex(ep_ptr->wMaxPacketSize); - Notify(PSTR("\r\nPoll Intrv:\t")); - PrintHex(ep_ptr->bInterval); + Notify(PSTR("\r\nEndpoint descriptor:"), 0x80); + Notify(PSTR("\r\nLength:\t\t"), 0x80); + PrintHex (ep_ptr->bLength, 0x80); + Notify(PSTR("\r\nType:\t\t"), 0x80); + PrintHex (ep_ptr->bDescriptorType, 0x80); + Notify(PSTR("\r\nAddress:\t"), 0x80); + PrintHex (ep_ptr->bEndpointAddress, 0x80); + Notify(PSTR("\r\nAttributes:\t"), 0x80); + PrintHex (ep_ptr->bmAttributes, 0x80); + Notify(PSTR("\r\nMaxPktSize:\t"), 0x80); + PrintHex (ep_ptr->wMaxPacketSize, 0x80); + Notify(PSTR("\r\nPoll Intrv:\t"), 0x80); + PrintHex (ep_ptr->bInterval, 0x80); #endif } /* Performs a cleanup after failed Init() attempt */ uint8_t BTD::Release() { - for (uint8_t i=0; iReset(); // Reset all Bluetooth services - pUsb->GetAddressPool().FreeAddress(bAddress); - bAddress = 0; - bPollEnable = false; - bNumEP = 1; // must have to be reset to 1 - return 0; + for (uint8_t i = 0; i < BTD_NUMSERVICES; i++) + if (btService[i]) + btService[i]->Reset(); // Reset all Bluetooth services + pUsb->GetAddressPool().FreeAddress(bAddress); + bAddress = 0; + bPollEnable = false; + bNumEP = 1; // must have to be reset to 1 + return 0; } + uint8_t BTD::Poll() { - if (!bPollEnable) - return 0; - if (qNextPollTime <= millis()) { // Don't poll if shorter than polling interval - qNextPollTime = millis() + pollInterval; // Set new poll time - HCI_event_task(); // poll the HCI event pipe - ACL_event_task(); // start polling the ACL input pipe too, though discard data until connected - } - return 0; + if (!bPollEnable) + return 0; + if (qNextPollTime <= millis()) { // Don't poll if shorter than polling interval + qNextPollTime = millis() + pollInterval; // Set new poll time + HCI_event_task(); // poll the HCI event pipe + ACL_event_task(); // start polling the ACL input pipe too, though discard data until connected + } + return 0; } void BTD::HCI_event_task() { - /* check the event pipe*/ - uint16_t MAX_BUFFER_SIZE = BULK_MAXPKTSIZE; // Request more than 16 bytes anyway, the inTransfer routine will take care of this - uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_EVENT_PIPE ].epAddr, &MAX_BUFFER_SIZE, hcibuf); // input on endpoint 1 - if(!rcode || rcode == hrNAK) // Check for errors - { - switch (hcibuf[0]) //switch on event type + /* check the event pipe*/ + uint16_t MAX_BUFFER_SIZE = BULK_MAXPKTSIZE; // Request more than 16 bytes anyway, the inTransfer routine will take care of this + uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_EVENT_PIPE ].epAddr, &MAX_BUFFER_SIZE, hcibuf); // input on endpoint 1 + if (!rcode || rcode == hrNAK) // Check for errors { - case EV_COMMAND_COMPLETE: - if (!hcibuf[5]) { // Check if command succeeded - hci_event_flag |= HCI_FLAG_CMD_COMPLETE; // set command complete flag - if((hcibuf[3] == 0x01) && (hcibuf[4] == 0x10)) { // parameters from read local version information - hci_version = hcibuf[6]; // Used to check if it supports 2.0+EDR - see http://www.bluetooth.org/Technical/AssignedNumbers/hci.htm - hci_event_flag |= HCI_FLAG_READ_VERSION; - } else if((hcibuf[3] == 0x09) && (hcibuf[4] == 0x10)) { // parameters from read local bluetooth address - for (uint8_t i = 0; i < 6; i++) - my_bdaddr[i] = hcibuf[6 + i]; - hci_event_flag |= HCI_FLAG_READ_BDADDR; - } - } - break; - - case EV_COMMAND_STATUS: - if(hcibuf[2]) { // show status on serial if not OK + switch (hcibuf[0]) //switch on event type + { + case EV_COMMAND_COMPLETE: + if (!hcibuf[5]) { // Check if command succeeded + hci_event_flag |= HCI_FLAG_CMD_COMPLETE; // set command complete flag + if ((hcibuf[3] == 0x01) && (hcibuf[4] == 0x10)) { // parameters from read local version information + hci_version = hcibuf[6]; // Used to check if it supports 2.0+EDR - see http://www.bluetooth.org/Technical/AssignedNumbers/hci.htm + hci_event_flag |= HCI_FLAG_READ_VERSION; + } else if ((hcibuf[3] == 0x09) && (hcibuf[4] == 0x10)) { // parameters from read local bluetooth address + for (uint8_t i = 0; i < 6; i++) + my_bdaddr[i] = hcibuf[6 + i]; + hci_event_flag |= HCI_FLAG_READ_BDADDR; + } + } + break; + + case EV_COMMAND_STATUS: + if (hcibuf[2]) { // show status on serial if not OK #ifdef DEBUG - Notify(PSTR("\r\nHCI Command Failed: ")); - PrintHex(hcibuf[2]); - Notify(PSTR(" ")); - PrintHex(hcibuf[4]); - Notify(PSTR(" ")); - PrintHex(hcibuf[5]); + Notify(PSTR("\r\nHCI Command Failed: "), 0x80); + PrintHex (hcibuf[2], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (hcibuf[4], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (hcibuf[5], 0x80); #endif - } - break; - - case EV_INQUIRY_COMPLETE: - if(inquiry_counter >= 5) { - inquiry_counter = 0; + } + break; + + case EV_INQUIRY_COMPLETE: + if (inquiry_counter >= 5) { + inquiry_counter = 0; #ifdef DEBUG - Notify(PSTR("\r\nCouldn't find Wiimote")); + Notify(PSTR("\r\nCouldn't find Wiimote"), 0x80); #endif - connectToWii = false; - pairWithWii = false; - hci_state = HCI_SCANNING_STATE; - } - inquiry_counter++; - break; - - case EV_INQUIRY_RESULT: - if (hcibuf[2]) { // Check that there is more than zero responses + connectToWii = false; + pairWithWii = false; + hci_state = HCI_SCANNING_STATE; + } + inquiry_counter++; + break; + + case EV_INQUIRY_RESULT: + if (hcibuf[2]) { // Check that there is more than zero responses #ifdef EXTRADEBUG - Notify(PSTR("\r\nNumber of responses: ")); - Serial.print(hcibuf[2]); + Notify(PSTR("\r\nNumber of responses: "), 0x80); + Serial.print(hcibuf[2]); #endif - for(uint8_t i = 0; i < hcibuf[2]; i++) { - if((hcibuf[4+8*hcibuf[2]+3*i] == 0x04 && hcibuf[5+8*hcibuf[2]+3*i] == 0x25 && hcibuf[6+8*hcibuf[2]+3*i] == 0x00) || (hcibuf[4+8*hcibuf[2]+3*i] == 0x08 && hcibuf[5+8*hcibuf[2]+3*i] == 0x05 && hcibuf[6+8*hcibuf[2]+3*i] == 0x00)) { // See http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html and http://wiibrew.org/wiki/Wiimote#SDP_information - if(hcibuf[4+8*hcibuf[2]+3*i] == 0x08) // Check if it's the new Wiimote with motion plus inside that was detected - motionPlusInside = true; - else - motionPlusInside = false; - disc_bdaddr[0] = hcibuf[3+6*i]; - disc_bdaddr[1] = hcibuf[4+6*i]; - disc_bdaddr[2] = hcibuf[5+6*i]; - disc_bdaddr[3] = hcibuf[6+6*i]; - disc_bdaddr[4] = hcibuf[7+6*i]; - disc_bdaddr[5] = hcibuf[8+6*i]; - hci_event_flag |= HCI_FLAG_WII_FOUND; - break; - } + for (uint8_t i = 0; i < hcibuf[2]; i++) { + if ((hcibuf[4 + 8 * hcibuf[2] + 3 * i] == 0x04 && hcibuf[5 + 8 * hcibuf[2] + 3 * i] == 0x25 && hcibuf[6 + 8 * hcibuf[2] + 3 * i] == 0x00) || (hcibuf[4 + 8 * hcibuf[2] + 3 * i] == 0x08 && hcibuf[5 + 8 * hcibuf[2] + 3 * i] == 0x05 && hcibuf[6 + 8 * hcibuf[2] + 3 * i] == 0x00)) { // See http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html and http://wiibrew.org/wiki/Wiimote#SDP_information + if (hcibuf[4 + 8 * hcibuf[2] + 3 * i] == 0x08) // Check if it's the new Wiimote with motion plus inside that was detected + motionPlusInside = true; + else + motionPlusInside = false; + disc_bdaddr[0] = hcibuf[3 + 6 * i]; + disc_bdaddr[1] = hcibuf[4 + 6 * i]; + disc_bdaddr[2] = hcibuf[5 + 6 * i]; + disc_bdaddr[3] = hcibuf[6 + 6 * i]; + disc_bdaddr[4] = hcibuf[7 + 6 * i]; + disc_bdaddr[5] = hcibuf[8 + 6 * i]; + hci_event_flag |= HCI_FLAG_WII_FOUND; + break; + } #ifdef EXTRADEBUG - else { - Notify(PSTR("\r\nClass of device: ")); - PrintHex(hcibuf[6+8*hcibuf[2]+3*i]); - Notify(PSTR(" ")); - PrintHex(hcibuf[5+8*hcibuf[2]+3*i]); - Notify(PSTR(" ")); - PrintHex(hcibuf[4+8*hcibuf[2]+3*i]); - } + else { + Notify(PSTR("\r\nClass of device: "), 0x80); + PrintHex (hcibuf[6 + 8 * hcibuf[2] + 3 * i], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (hcibuf[5 + 8 * hcibuf[2] + 3 * i], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (hcibuf[4 + 8 * hcibuf[2] + 3 * i], 0x80); + } #endif - } - } - break; - - case EV_CONNECT_COMPLETE: - hci_event_flag |= HCI_FLAG_CONNECT_EVENT; - if (!hcibuf[2]) { // check if connected OK - hci_handle = hcibuf[3] | ((hcibuf[4] & 0x0F) << 8); // store the handle for the ACL connection - hci_event_flag |= HCI_FLAG_CONN_COMPLETE; // set connection complete flag - } + } + } + break; + + case EV_CONNECT_COMPLETE: + hci_event_flag |= HCI_FLAG_CONNECT_EVENT; + if (!hcibuf[2]) { // check if connected OK + hci_handle = hcibuf[3] | ((hcibuf[4] & 0x0F) << 8); // store the handle for the ACL connection + hci_event_flag |= HCI_FLAG_CONN_COMPLETE; // set connection complete flag + } #ifdef EXTRADEBUG - else { - Notify(PSTR("\r\nConnection Failed")); - } + else { + Notify(PSTR("\r\nConnection Failed"), 0x80); + } #endif - break; - - case EV_DISCONNECT_COMPLETE: - if (!hcibuf[2]) { // check if disconnected OK - hci_event_flag |= HCI_FLAG_DISCONN_COMPLETE; // set disconnect command complete flag - hci_event_flag &= ~HCI_FLAG_CONN_COMPLETE; // clear connection complete flag - } - break; - - case EV_REMOTE_NAME_COMPLETE: - if (!hcibuf[2]) { // check if reading is OK - for (uint8_t i = 0; i < 30; i++) - remote_name[i] = hcibuf[9 + i]; //store first 30 bytes - hci_event_flag |= HCI_FLAG_REMOTE_NAME_COMPLETE; - } - break; - - case EV_INCOMING_CONNECT: - disc_bdaddr[0] = hcibuf[2]; - disc_bdaddr[1] = hcibuf[3]; - disc_bdaddr[2] = hcibuf[4]; - disc_bdaddr[3] = hcibuf[5]; - disc_bdaddr[4] = hcibuf[6]; - disc_bdaddr[5] = hcibuf[7]; - hci_event_flag |= HCI_FLAG_INCOMING_REQUEST; - break; - - case EV_PIN_CODE_REQUEST: - if(pairWithWii) { + break; + + case EV_DISCONNECT_COMPLETE: + if (!hcibuf[2]) { // check if disconnected OK + hci_event_flag |= HCI_FLAG_DISCONN_COMPLETE; // set disconnect command complete flag + hci_event_flag &= ~HCI_FLAG_CONN_COMPLETE; // clear connection complete flag + } + break; + + case EV_REMOTE_NAME_COMPLETE: + if (!hcibuf[2]) { // check if reading is OK + for (uint8_t i = 0; i < 30; i++) + remote_name[i] = hcibuf[9 + i]; //store first 30 bytes + hci_event_flag |= HCI_FLAG_REMOTE_NAME_COMPLETE; + } + break; + + case EV_INCOMING_CONNECT: + disc_bdaddr[0] = hcibuf[2]; + disc_bdaddr[1] = hcibuf[3]; + disc_bdaddr[2] = hcibuf[4]; + disc_bdaddr[3] = hcibuf[5]; + disc_bdaddr[4] = hcibuf[6]; + disc_bdaddr[5] = hcibuf[7]; + hci_event_flag |= HCI_FLAG_INCOMING_REQUEST; + break; + + case EV_PIN_CODE_REQUEST: + if (pairWithWii) { #ifdef DEBUG - Notify(PSTR("\r\nPairing with wiimote")); + Notify(PSTR("\r\nPairing with wiimote"), 0x80); #endif - hci_pin_code_request_reply(); - } - else if(btdPin != NULL) { + hci_pin_code_request_reply(); + } else if (btdPin != NULL) { #ifdef DEBUG - Notify(PSTR("\r\nBluetooth pin is set too: ")); - Serial.print(btdPin); + Notify(PSTR("\r\nBluetooth pin is set too: "), 0x80); + Serial.print(btdPin); #endif - hci_pin_code_request_reply(); - } - else { + hci_pin_code_request_reply(); + } else { #ifdef DEBUG - Notify(PSTR("\r\nNo pin was set")); + Notify(PSTR("\r\nNo pin was set"), 0x80); #endif - hci_pin_code_negative_request_reply(); - } - break; - - case EV_LINK_KEY_REQUEST: + hci_pin_code_negative_request_reply(); + } + break; + + case EV_LINK_KEY_REQUEST: #ifdef DEBUG - Notify(PSTR("\r\nReceived Key Request")); + Notify(PSTR("\r\nReceived Key Request"), 0x80); #endif - hci_link_key_request_negative_reply(); - break; - - case EV_AUTHENTICATION_COMPLETE: - if(pairWithWii && !connectToWii) { + hci_link_key_request_negative_reply(); + break; + + case EV_AUTHENTICATION_COMPLETE: + if (pairWithWii && !connectToWii) { #ifdef DEBUG - Notify(PSTR("\r\nPairing successful")); + Notify(PSTR("\r\nPairing successful"), 0x80); #endif - connectToWii = true; // Only send the ACL data to the Wii service - } - break; - /* We will just ignore the following events */ - case EV_NUM_COMPLETE_PKT: - case EV_ROLE_CHANGED: - case EV_PAGE_SCAN_REP_MODE: - case EV_LOOPBACK_COMMAND: - case EV_DATA_BUFFER_OVERFLOW: - case EV_CHANGE_CONNECTION_LINK: - case EV_MAX_SLOTS_CHANGE: - case EV_QOS_SETUP_COMPLETE: - case EV_LINK_KEY_NOTIFICATION: - case EV_ENCRYPTION_CHANGE: - case EV_READ_REMOTE_VERSION_INFORMATION_COMPLETE: - break; + connectToWii = true; // Only send the ACL data to the Wii service + } + break; + /* We will just ignore the following events */ + case EV_NUM_COMPLETE_PKT: + case EV_ROLE_CHANGED: + case EV_PAGE_SCAN_REP_MODE: + case EV_LOOPBACK_COMMAND: + case EV_DATA_BUFFER_OVERFLOW: + case EV_CHANGE_CONNECTION_LINK: + case EV_MAX_SLOTS_CHANGE: + case EV_QOS_SETUP_COMPLETE: + case EV_LINK_KEY_NOTIFICATION: + case EV_ENCRYPTION_CHANGE: + case EV_READ_REMOTE_VERSION_INFORMATION_COMPLETE: + break; #ifdef EXTRADEBUG - default: - if(hcibuf[0] != 0x00) { - Notify(PSTR("\r\nUnmanaged HCI Event: ")); - PrintHex(hcibuf[0]); - } - break; + default: + if (hcibuf[0] != 0x00) { + Notify(PSTR("\r\nUnmanaged HCI Event: "), 0x80); + PrintHex (hcibuf[0], 0x80); + } + break; #endif - } // switch - HCI_task(); - } + } // switch + HCI_task(); + } #ifdef EXTRADEBUG - else { - Notify(PSTR("\r\nHCI event error: ")); - PrintHex(rcode); - } + else { + Notify(PSTR("\r\nHCI event error: "), 0x80); + PrintHex (rcode, 0x80); + } #endif } /* Poll Bluetooth and print result */ void BTD::HCI_task() { - switch (hci_state){ - case HCI_INIT_STATE: - hci_counter++; - if (hci_counter > hci_num_reset_loops) { // wait until we have looped x times to clear any old events - hci_reset(); - hci_state = HCI_RESET_STATE; - hci_counter = 0; - } - break; - - case HCI_RESET_STATE: - hci_counter++; - if (hci_cmd_complete) { - hci_counter = 0; -#ifdef DEBUG - Notify(PSTR("\r\nHCI Reset complete")); -#endif - hci_state = HCI_CLASS_STATE; - hci_write_class_of_device(); - } - else if (hci_counter > hci_num_reset_loops) { - hci_num_reset_loops *= 10; - if(hci_num_reset_loops > 2000) - hci_num_reset_loops = 2000; -#ifdef DEBUG - Notify(PSTR("\r\nNo response to HCI Reset")); -#endif - hci_state = HCI_INIT_STATE; - hci_counter = 0; - } - break; - - case HCI_CLASS_STATE: - if(hci_cmd_complete) { -#ifdef DEBUG - Notify(PSTR("\r\nWrite class of device")); -#endif - hci_state = HCI_BDADDR_STATE; - hci_read_bdaddr(); - } - break; - - case HCI_BDADDR_STATE: - if (hci_read_bdaddr_complete) { -#ifdef DEBUG - Notify(PSTR("\r\nLocal Bluetooth Address: ")); - for(int8_t i = 5; i > 0;i--) { - PrintHex(my_bdaddr[i]); - Notify(PSTR(":")); - } - PrintHex(my_bdaddr[0]); -#endif - hci_read_local_version_information(); - hci_state = HCI_LOCAL_VERSION_STATE; - } - break; - - case HCI_LOCAL_VERSION_STATE: // The local version is used by the PS3BT class - if (hci_read_version_complete) { - if(btdName != NULL) { - hci_set_local_name(btdName); - hci_state = HCI_SET_NAME_STATE; - } else - hci_state = HCI_CHECK_WII_SERVICE; - } - break; - - case HCI_SET_NAME_STATE: - if (hci_cmd_complete) { -#ifdef DEBUG - Notify(PSTR("\r\nThe name is set to: ")); - Serial.print(btdName); -#endif - hci_state = HCI_CHECK_WII_SERVICE; - } - break; - - case HCI_CHECK_WII_SERVICE: - if(pairWithWii) { // Check if it should try to connect to a wiimote -#ifdef DEBUG - Notify(PSTR("\r\nStarting inquiry\r\nPress 1 & 2 on the Wiimote\r\nOr press sync if you are using a Wii U Pro Controller")); -#endif - hci_inquiry(); - hci_state = HCI_INQUIRY_STATE; - } - else - hci_state = HCI_SCANNING_STATE; // Don't try to connect to a Wiimote - break; - - case HCI_INQUIRY_STATE: - if(hci_wii_found) { - hci_inquiry_cancel(); // Stop inquiry -#ifdef DEBUG - Notify(PSTR("\r\nWiimote found")); - Notify(PSTR("\r\nNow just create the instance like so:")); - Notify(PSTR("\r\nWII Wii(&Btd);")); - Notify(PSTR("\r\nAnd then press any button on the Wiimote")); -#endif - if(motionPlusInside) { - hci_remote_name(); // We need to know the name to distinguish between a Wiimote and a Wii U Pro Controller - hci_state = HCI_REMOTE_NAME_STATE; - } else - hci_state = HCI_CONNECT_WII_STATE; - } - break; - - case HCI_CONNECT_WII_STATE: - if(hci_cmd_complete) { -#ifdef DEBUG - Notify(PSTR("\r\nConnecting to Wiimote")); -#endif - hci_connect(); - hci_state = HCI_CONNECTED_WII_STATE; - } - break; - - case HCI_CONNECTED_WII_STATE: - if(hci_connect_event) { - if(hci_connect_complete) { -#ifdef DEBUG - Notify(PSTR("\r\nConnected to Wiimote")); -#endif - hci_authentication_request(); // This will start the pairing with the wiimote - hci_state = HCI_SCANNING_STATE; - } else { -#ifdef DEBUG - Notify(PSTR("\r\nTrying to connect one more time...")); -#endif - hci_connect(); // Try to connect one more time - } - } - break; - - case HCI_SCANNING_STATE: - if(!connectToWii && !pairWithWii) { -#ifdef DEBUG - Notify(PSTR("\r\nWait For Incoming Connection Request")); -#endif - hci_write_scan_enable(); - watingForConnection = true; - hci_state = HCI_CONNECT_IN_STATE; - } - break; - - case HCI_CONNECT_IN_STATE: - if(hci_incoming_connect_request) { - watingForConnection = false; -#ifdef DEBUG - Notify(PSTR("\r\nIncoming Connection Request")); -#endif - hci_remote_name(); - hci_state = HCI_REMOTE_NAME_STATE; - } else if (hci_disconnect_complete) - hci_state = HCI_DISCONNECT_STATE; - break; - - case HCI_REMOTE_NAME_STATE: - if(hci_remote_name_complete) { -#ifdef DEBUG - Notify(PSTR("\r\nRemote Name: ")); - for (uint8_t i = 0; i < 30; i++) { - if(remote_name[i] == NULL) + switch (hci_state) { + case HCI_INIT_STATE: + hci_counter++; + if (hci_counter > hci_num_reset_loops) { // wait until we have looped x times to clear any old events + hci_reset(); + hci_state = HCI_RESET_STATE; + hci_counter = 0; + } break; - Serial.write(remote_name[i]); - } -#endif - if(strncmp((const char*)remote_name, "Nintendo", 8) == 0) { -#ifdef DEBUG - Notify(PSTR("\r\nWiimote is connecting")); -#endif - if(strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-TR", 22) == 0) { -#ifdef DEBUG - Notify(PSTR(" with Motion Plus Inside")); -#endif - motionPlusInside = true; - } - else if(strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-UC", 22) == 0) { -#ifdef DEBUG - Notify(PSTR(" - Wii U Pro Controller")); -#endif - motionPlusInside = true; - wiiUProController = true; - } else { - motionPlusInside = false; - wiiUProController = false; - } - incomingWii = true; - } - if(pairWithWii && motionPlusInside) - hci_state = HCI_CONNECT_WII_STATE; - else { - hci_accept_connection(); - hci_state = HCI_CONNECTED_STATE; - } - } - break; - - case HCI_CONNECTED_STATE: - if (hci_connect_complete) { -#ifdef DEBUG - Notify(PSTR("\r\nConnected to Device: ")); - for(int8_t i = 5; i>0;i--) { - PrintHex(disc_bdaddr[i]); - Notify(PSTR(":")); - } - PrintHex(disc_bdaddr[0]); -#endif - // Clear these flags for a new connection - l2capConnectionClaimed = false; - sdpConnectionClaimed = false; - rfcommConnectionClaimed = false; - - hci_event_flag = 0; - hci_state = HCI_DONE_STATE; - } - break; - case HCI_DONE_STATE: - hci_counter++; - if (hci_counter > 1000) { // Wait until we have looped 1000 times to make sure that the L2CAP connection has been started - hci_counter = 0; - hci_state = HCI_SCANNING_STATE; - } - break; - - case HCI_DISCONNECT_STATE: - if (hci_disconnect_complete) { + case HCI_RESET_STATE: + hci_counter++; + if (hci_cmd_complete) { + hci_counter = 0; #ifdef DEBUG - Notify(PSTR("\r\nHCI Disconnected from Device")); + Notify(PSTR("\r\nHCI Reset complete"), 0x80); #endif - hci_event_flag = 0; // Clear all flags - - // Reset all buffers - for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) - hcibuf[i] = 0; - for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) - l2capinbuf[i] = 0; - - hci_state = HCI_SCANNING_STATE; - } - break; - default: - break; - } + hci_state = HCI_CLASS_STATE; + hci_write_class_of_device(); + } else if (hci_counter > hci_num_reset_loops) { + hci_num_reset_loops *= 10; + if (hci_num_reset_loops > 2000) + hci_num_reset_loops = 2000; +#ifdef DEBUG + Notify(PSTR("\r\nNo response to HCI Reset"), 0x80); +#endif + hci_state = HCI_INIT_STATE; + hci_counter = 0; + } + break; + + case HCI_CLASS_STATE: + if (hci_cmd_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nWrite class of device"), 0x80); +#endif + hci_state = HCI_BDADDR_STATE; + hci_read_bdaddr(); + } + break; + + case HCI_BDADDR_STATE: + if (hci_read_bdaddr_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nLocal Bluetooth Address: "), 0x80); + for (int8_t i = 5; i > 0; i--) { + PrintHex (my_bdaddr[i], 0x80); + Notify(PSTR(":"), 0x80); + } + PrintHex (my_bdaddr[0], 0x80); +#endif + hci_read_local_version_information(); + hci_state = HCI_LOCAL_VERSION_STATE; + } + break; + + case HCI_LOCAL_VERSION_STATE: // The local version is used by the PS3BT class + if (hci_read_version_complete) { + if (btdName != NULL) { + hci_set_local_name(btdName); + hci_state = HCI_SET_NAME_STATE; + } else + hci_state = HCI_CHECK_WII_SERVICE; + } + break; + + case HCI_SET_NAME_STATE: + if (hci_cmd_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nThe name is set to: "), 0x80); + Serial.print(btdName); +#endif + hci_state = HCI_CHECK_WII_SERVICE; + } + break; + + case HCI_CHECK_WII_SERVICE: + if (pairWithWii) { // Check if it should try to connect to a wiimote +#ifdef DEBUG + Notify(PSTR("\r\nStarting inquiry\r\nPress 1 & 2 on the Wiimote\r\nOr press sync if you are using a Wii U Pro Controller"), 0x80); +#endif + hci_inquiry(); + hci_state = HCI_INQUIRY_STATE; + } else + hci_state = HCI_SCANNING_STATE; // Don't try to connect to a Wiimote + break; + + case HCI_INQUIRY_STATE: + if (hci_wii_found) { + hci_inquiry_cancel(); // Stop inquiry +#ifdef DEBUG + Notify(PSTR("\r\nWiimote found"), 0x80); + Notify(PSTR("\r\nNow just create the instance like so:"), 0x80); + Notify(PSTR("\r\nWII Wii(&Btd);"), 0x80); + Notify(PSTR("\r\nAnd then press any button on the Wiimote"), 0x80); +#endif + if (motionPlusInside) { + hci_remote_name(); // We need to know the name to distinguish between a Wiimote and a Wii U Pro Controller + hci_state = HCI_REMOTE_NAME_STATE; + } else + hci_state = HCI_CONNECT_WII_STATE; + } + break; + + case HCI_CONNECT_WII_STATE: + if (hci_cmd_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nConnecting to Wiimote"), 0x80); +#endif + hci_connect(); + hci_state = HCI_CONNECTED_WII_STATE; + } + break; + + case HCI_CONNECTED_WII_STATE: + if (hci_connect_event) { + if (hci_connect_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nConnected to Wiimote"), 0x80); +#endif + hci_authentication_request(); // This will start the pairing with the wiimote + hci_state = HCI_SCANNING_STATE; + } else { +#ifdef DEBUG + Notify(PSTR("\r\nTrying to connect one more time..."), 0x80); +#endif + hci_connect(); // Try to connect one more time + } + } + break; + + case HCI_SCANNING_STATE: + if (!connectToWii && !pairWithWii) { +#ifdef DEBUG + Notify(PSTR("\r\nWait For Incoming Connection Request"), 0x80); +#endif + hci_write_scan_enable(); + watingForConnection = true; + hci_state = HCI_CONNECT_IN_STATE; + } + break; + + case HCI_CONNECT_IN_STATE: + if (hci_incoming_connect_request) { + watingForConnection = false; +#ifdef DEBUG + Notify(PSTR("\r\nIncoming Connection Request"), 0x80); +#endif + hci_remote_name(); + hci_state = HCI_REMOTE_NAME_STATE; + } else if (hci_disconnect_complete) + hci_state = HCI_DISCONNECT_STATE; + break; + + case HCI_REMOTE_NAME_STATE: + if (hci_remote_name_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nRemote Name: "), 0x80); + for (uint8_t i = 0; i < 30; i++) { + if (remote_name[i] == NULL) + break; + Serial.write(remote_name[i]); + } +#endif + if (strncmp((const char*)remote_name, "Nintendo", 8) == 0) { +#ifdef DEBUG + Notify(PSTR("\r\nWiimote is connecting"), 0x80); +#endif + if (strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-TR", 22) == 0) { +#ifdef DEBUG + Notify(PSTR(" with Motion Plus Inside"), 0x80); +#endif + motionPlusInside = true; + } else if (strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-UC", 22) == 0) { +#ifdef DEBUG + Notify(PSTR(" - Wii U Pro Controller"), 0x80); +#endif + motionPlusInside = true; + wiiUProController = true; + } else { + motionPlusInside = false; + wiiUProController = false; + } + incomingWii = true; + } + if (pairWithWii && motionPlusInside) + hci_state = HCI_CONNECT_WII_STATE; + else { + hci_accept_connection(); + hci_state = HCI_CONNECTED_STATE; + } + } + break; + + case HCI_CONNECTED_STATE: + if (hci_connect_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nConnected to Device: "), 0x80); + for (int8_t i = 5; i > 0; i--) { + PrintHex (disc_bdaddr[i], 0x80); + Notify(PSTR(":"), 0x80); + } + PrintHex (disc_bdaddr[0], 0x80); +#endif + // Clear these flags for a new connection + l2capConnectionClaimed = false; + sdpConnectionClaimed = false; + rfcommConnectionClaimed = false; + + hci_event_flag = 0; + hci_state = HCI_DONE_STATE; + } + break; + + case HCI_DONE_STATE: + hci_counter++; + if (hci_counter > 1000) { // Wait until we have looped 1000 times to make sure that the L2CAP connection has been started + hci_counter = 0; + hci_state = HCI_SCANNING_STATE; + } + break; + + case HCI_DISCONNECT_STATE: + if (hci_disconnect_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nHCI Disconnected from Device"), 0x80); +#endif + hci_event_flag = 0; // Clear all flags + + // Reset all buffers + for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) + hcibuf[i] = 0; + for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) + l2capinbuf[i] = 0; + + hci_state = HCI_SCANNING_STATE; + } + break; + default: + break; + } } void BTD::ACL_event_task() { - uint16_t MAX_BUFFER_SIZE = BULK_MAXPKTSIZE; - uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_DATAIN_PIPE ].epAddr, &MAX_BUFFER_SIZE, l2capinbuf); // input on endpoint 2 - if(!rcode) { // Check for errors - for (uint8_t i=0; iACLData(l2capinbuf); - } + uint16_t MAX_BUFFER_SIZE = BULK_MAXPKTSIZE; + uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_DATAIN_PIPE ].epAddr, &MAX_BUFFER_SIZE, l2capinbuf); // input on endpoint 2 + if (!rcode) { // Check for errors + for (uint8_t i = 0; i < BTD_NUMSERVICES; i++) + if (btService[i]) + btService[i]->ACLData(l2capinbuf); + } #ifdef EXTRADEBUG - else if (rcode != hrNAK) { - Notify(PSTR("\r\nACL data in error: ")); - PrintHex(rcode); - } + else if (rcode != hrNAK) { + Notify(PSTR("\r\nACL data in error: "), 0x80); + PrintHex (rcode, 0x80); + } #endif - for (uint8_t i=0; iRun(); + for (uint8_t i = 0; i < BTD_NUMSERVICES; i++) + if (btService[i]) + btService[i]->Run(); } /************************************************************/ /* HCI Commands */ + /************************************************************/ void BTD::HCI_Command(uint8_t* data, uint16_t nbytes) { - hci_event_flag &= ~HCI_FLAG_CMD_COMPLETE; - pUsb->ctrlReq(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bmREQ_HCI_OUT, 0x00, 0x00, 0x00 ,0x00, nbytes, nbytes, data, NULL); + hci_event_flag &= ~HCI_FLAG_CMD_COMPLETE; + pUsb->ctrlReq(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bmREQ_HCI_OUT, 0x00, 0x00, 0x00, 0x00, nbytes, nbytes, data, NULL); } -void BTD::hci_reset() { - hci_event_flag = 0; // Clear all the flags - hcibuf[0] = 0x03; // HCI OCF = 3 - hcibuf[1] = 0x03 << 2; // HCI OGF = 3 - hcibuf[2] = 0x00; - HCI_Command(hcibuf, 3); -} -void BTD::hci_write_scan_enable() { - hci_event_flag &= ~HCI_FLAG_INCOMING_REQUEST; - hcibuf[0] = 0x1A; // HCI OCF = 1A - hcibuf[1] = 0x03 << 2; // HCI OGF = 3 - hcibuf[2] = 0x01; // parameter length = 1 - if(btdName != NULL) - hcibuf[3] = 0x03; // Inquiry Scan enabled. Page Scan enabled. - else - hcibuf[3] = 0x02; // Inquiry Scan disabled. Page Scan enabled. - HCI_Command(hcibuf, 4); -} -void BTD::hci_write_scan_disable() { - hcibuf[0] = 0x1A; // HCI OCF = 1A - hcibuf[1] = 0x03 << 2; // HCI OGF = 3 - hcibuf[2] = 0x01; // parameter length = 1 - hcibuf[3] = 0x00; // Inquiry Scan disabled. Page Scan disabled. - HCI_Command(hcibuf, 4); -} -void BTD::hci_read_bdaddr() { - hcibuf[0] = 0x09; // HCI OCF = 9 - hcibuf[1] = 0x04 << 2; // HCI OGF = 4 - hcibuf[2] = 0x00; - HCI_Command(hcibuf, 3); -} -void BTD::hci_read_local_version_information() { - hcibuf[0] = 0x01; // HCI OCF = 1 - hcibuf[1] = 0x04 << 2; // HCI OGF = 4 - hcibuf[2] = 0x00; - HCI_Command(hcibuf, 3); -} -void BTD::hci_accept_connection() { - hci_event_flag &= ~HCI_FLAG_CONN_COMPLETE; - hcibuf[0] = 0x09; // HCI OCF = 9 - hcibuf[1] = 0x01 << 2; // HCI OGF = 1 - hcibuf[2] = 0x07; // parameter length 7 - hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr - hcibuf[4] = disc_bdaddr[1]; - hcibuf[5] = disc_bdaddr[2]; - hcibuf[6] = disc_bdaddr[3]; - hcibuf[7] = disc_bdaddr[4]; - hcibuf[8] = disc_bdaddr[5]; - hcibuf[9] = 0x00; //switch role to master - - HCI_Command(hcibuf, 10); -} -void BTD::hci_remote_name() { - hci_event_flag &= ~HCI_FLAG_REMOTE_NAME_COMPLETE; - hcibuf[0] = 0x19; // HCI OCF = 19 - hcibuf[1] = 0x01 << 2; // HCI OGF = 1 - hcibuf[2] = 0x0A; // parameter length = 10 - hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr - hcibuf[4] = disc_bdaddr[1]; - hcibuf[5] = disc_bdaddr[2]; - hcibuf[6] = disc_bdaddr[3]; - hcibuf[7] = disc_bdaddr[4]; - hcibuf[8] = disc_bdaddr[5]; - hcibuf[9] = 0x01; //Page Scan Repetition Mode - hcibuf[10] = 0x00; //Reserved - hcibuf[11] = 0x00; //Clock offset - low byte - hcibuf[12] = 0x00; //Clock offset - high byte - - HCI_Command(hcibuf, 13); -} -void BTD::hci_set_local_name(const char* name) { - hcibuf[0] = 0x13; // HCI OCF = 13 - hcibuf[1] = 0x03 << 2; // HCI OGF = 3 - hcibuf[2] = strlen(name)+1; // parameter length = the length of the string + end byte - uint8_t i; - for(i = 0; i < strlen(name); i++) - hcibuf[i+3] = name[i]; - hcibuf[i+3] = 0x00; // End of string - HCI_Command(hcibuf, 4+strlen(name)); +void BTD::hci_reset() { + hci_event_flag = 0; // Clear all the flags + hcibuf[0] = 0x03; // HCI OCF = 3 + hcibuf[1] = 0x03 << 2; // HCI OGF = 3 + hcibuf[2] = 0x00; + HCI_Command(hcibuf, 3); } -void BTD::hci_inquiry() { - hci_event_flag &= ~HCI_FLAG_WII_FOUND; - hcibuf[0] = 0x01; - hcibuf[1] = 0x01 << 2; // HCI OGF = 1 - hcibuf[2] = 0x05; // Parameter Total Length = 5 - hcibuf[3] = 0x33; // LAP: Genera/Unlimited Inquiry Access Code (GIAC = 0x9E8B33) - see https://www.bluetooth.org/Technical/AssignedNumbers/baseband.htm - hcibuf[4] = 0x8B; - hcibuf[5] = 0x9E; - hcibuf[6] = 0x30; // Inquiry time = 61.44 sec (maximum) - hcibuf[7] = 0x0A; // 10 number of responses - - HCI_Command(hcibuf, 8); + +void BTD::hci_write_scan_enable() { + hci_event_flag &= ~HCI_FLAG_INCOMING_REQUEST; + hcibuf[0] = 0x1A; // HCI OCF = 1A + hcibuf[1] = 0x03 << 2; // HCI OGF = 3 + hcibuf[2] = 0x01; // parameter length = 1 + if (btdName != NULL) + hcibuf[3] = 0x03; // Inquiry Scan enabled. Page Scan enabled. + else + hcibuf[3] = 0x02; // Inquiry Scan disabled. Page Scan enabled. + HCI_Command(hcibuf, 4); } -void BTD::hci_inquiry_cancel() { - hcibuf[0] = 0x02; - hcibuf[1] = 0x01 << 2; // HCI OGF = 1 - hcibuf[2] = 0x0; // Parameter Total Length = 0 - - HCI_Command(hcibuf, 3); + +void BTD::hci_write_scan_disable() { + hcibuf[0] = 0x1A; // HCI OCF = 1A + hcibuf[1] = 0x03 << 2; // HCI OGF = 3 + hcibuf[2] = 0x01; // parameter length = 1 + hcibuf[3] = 0x00; // Inquiry Scan disabled. Page Scan disabled. + HCI_Command(hcibuf, 4); } -void BTD::hci_connect() { - hci_event_flag &= ~(HCI_FLAG_CONN_COMPLETE | HCI_FLAG_CONNECT_EVENT); - hcibuf[0] = 0x05; - hcibuf[1] = 0x01 << 2; // HCI OGF = 1 - hcibuf[2] = 0x0D; // parameter Total Length = 13 - hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr - hcibuf[4] = disc_bdaddr[1]; - hcibuf[5] = disc_bdaddr[2]; - hcibuf[6] = disc_bdaddr[3]; - hcibuf[7] = disc_bdaddr[4]; - hcibuf[8] = disc_bdaddr[5]; - hcibuf[9] = 0x18; // DM1 or DH1 may be used - hcibuf[10] = 0xCC; // DM3, DH3, DM5, DH5 may be used - hcibuf[11] = 0x01; // Page repetition mode R1 - hcibuf[12] = 0x00; // Reserved - hcibuf[13] = 0x00; // Clock offset - hcibuf[14] = 0x00; // Invalid clock offset - hcibuf[15] = 0x00; // Do not allow role switch - - HCI_Command(hcibuf, 16); + +void BTD::hci_read_bdaddr() { + hcibuf[0] = 0x09; // HCI OCF = 9 + hcibuf[1] = 0x04 << 2; // HCI OGF = 4 + hcibuf[2] = 0x00; + HCI_Command(hcibuf, 3); } -void BTD::hci_pin_code_request_reply() { - hcibuf[0] = 0x0D; // HCI OCF = 0D - hcibuf[1] = 0x01 << 2; // HCI OGF = 1 - hcibuf[2] = 0x17; // parameter length 23 - hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr - hcibuf[4] = disc_bdaddr[1]; - hcibuf[5] = disc_bdaddr[2]; - hcibuf[6] = disc_bdaddr[3]; - hcibuf[7] = disc_bdaddr[4]; - hcibuf[8] = disc_bdaddr[5]; - if(pairWithWii && !wiiUProController) { - hcibuf[9] = 6; // Pin length is the length of the bt address - hcibuf[10] = disc_bdaddr[0]; // The pin is the Wiimotes bt address backwards - hcibuf[11] = disc_bdaddr[1]; - hcibuf[12] = disc_bdaddr[2]; - hcibuf[13] = disc_bdaddr[3]; - hcibuf[14] = disc_bdaddr[4]; - hcibuf[15] = disc_bdaddr[5]; - for(uint8_t i = 16; i < 26; i++) - hcibuf[i] = 0x00; // The rest should be 0 - } else if(pairWithWii && wiiUProController) { -#ifdef DEBUG - Notify(PSTR("\r\nParing with Wii U Pro Controller")); -#endif - hcibuf[9] = 6; // Pin length is the length of the bt address - hcibuf[10] = my_bdaddr[0]; // The pin is the Bluetooth dongles bt address backwards - hcibuf[11] = my_bdaddr[1]; - hcibuf[12] = my_bdaddr[2]; - hcibuf[13] = my_bdaddr[3]; - hcibuf[14] = my_bdaddr[4]; - hcibuf[15] = my_bdaddr[5]; - for(uint8_t i = 16; i < 26; i++) - hcibuf[i] = 0x00; // The rest should be 0 - } else { - hcibuf[9] = strlen(btdPin); // Length of pin + +void BTD::hci_read_local_version_information() { + hcibuf[0] = 0x01; // HCI OCF = 1 + hcibuf[1] = 0x04 << 2; // HCI OGF = 4 + hcibuf[2] = 0x00; + HCI_Command(hcibuf, 3); +} + +void BTD::hci_accept_connection() { + hci_event_flag &= ~HCI_FLAG_CONN_COMPLETE; + hcibuf[0] = 0x09; // HCI OCF = 9 + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x07; // parameter length 7 + hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr + hcibuf[4] = disc_bdaddr[1]; + hcibuf[5] = disc_bdaddr[2]; + hcibuf[6] = disc_bdaddr[3]; + hcibuf[7] = disc_bdaddr[4]; + hcibuf[8] = disc_bdaddr[5]; + hcibuf[9] = 0x00; //switch role to master + + HCI_Command(hcibuf, 10); +} + +void BTD::hci_remote_name() { + hci_event_flag &= ~HCI_FLAG_REMOTE_NAME_COMPLETE; + hcibuf[0] = 0x19; // HCI OCF = 19 + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x0A; // parameter length = 10 + hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr + hcibuf[4] = disc_bdaddr[1]; + hcibuf[5] = disc_bdaddr[2]; + hcibuf[6] = disc_bdaddr[3]; + hcibuf[7] = disc_bdaddr[4]; + hcibuf[8] = disc_bdaddr[5]; + hcibuf[9] = 0x01; //Page Scan Repetition Mode + hcibuf[10] = 0x00; //Reserved + hcibuf[11] = 0x00; //Clock offset - low byte + hcibuf[12] = 0x00; //Clock offset - high byte + + HCI_Command(hcibuf, 13); +} + +void BTD::hci_set_local_name(const char* name) { + hcibuf[0] = 0x13; // HCI OCF = 13 + hcibuf[1] = 0x03 << 2; // HCI OGF = 3 + hcibuf[2] = strlen(name) + 1; // parameter length = the length of the string + end byte uint8_t i; - for(i = 0; i < strlen(btdPin); i++) // The maximum size of the pin is 16 - hcibuf[i+10] = btdPin[i]; - for(;i < 16; i++) - hcibuf[i+10] = 0x00; // The rest should be 0 - } - - HCI_Command(hcibuf, 26); + for (i = 0; i < strlen(name); i++) + hcibuf[i + 3] = name[i]; + hcibuf[i + 3] = 0x00; // End of string + + HCI_Command(hcibuf, 4 + strlen(name)); } + +void BTD::hci_inquiry() { + hci_event_flag &= ~HCI_FLAG_WII_FOUND; + hcibuf[0] = 0x01; + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x05; // Parameter Total Length = 5 + hcibuf[3] = 0x33; // LAP: Genera/Unlimited Inquiry Access Code (GIAC = 0x9E8B33) - see https://www.bluetooth.org/Technical/AssignedNumbers/baseband.htm + hcibuf[4] = 0x8B; + hcibuf[5] = 0x9E; + hcibuf[6] = 0x30; // Inquiry time = 61.44 sec (maximum) + hcibuf[7] = 0x0A; // 10 number of responses + + HCI_Command(hcibuf, 8); +} + +void BTD::hci_inquiry_cancel() { + hcibuf[0] = 0x02; + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x0; // Parameter Total Length = 0 + + HCI_Command(hcibuf, 3); +} + +void BTD::hci_connect() { + hci_event_flag &= ~(HCI_FLAG_CONN_COMPLETE | HCI_FLAG_CONNECT_EVENT); + hcibuf[0] = 0x05; + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x0D; // parameter Total Length = 13 + hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr + hcibuf[4] = disc_bdaddr[1]; + hcibuf[5] = disc_bdaddr[2]; + hcibuf[6] = disc_bdaddr[3]; + hcibuf[7] = disc_bdaddr[4]; + hcibuf[8] = disc_bdaddr[5]; + hcibuf[9] = 0x18; // DM1 or DH1 may be used + hcibuf[10] = 0xCC; // DM3, DH3, DM5, DH5 may be used + hcibuf[11] = 0x01; // Page repetition mode R1 + hcibuf[12] = 0x00; // Reserved + hcibuf[13] = 0x00; // Clock offset + hcibuf[14] = 0x00; // Invalid clock offset + hcibuf[15] = 0x00; // Do not allow role switch + + HCI_Command(hcibuf, 16); +} + +void BTD::hci_pin_code_request_reply() { + hcibuf[0] = 0x0D; // HCI OCF = 0D + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x17; // parameter length 23 + hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr + hcibuf[4] = disc_bdaddr[1]; + hcibuf[5] = disc_bdaddr[2]; + hcibuf[6] = disc_bdaddr[3]; + hcibuf[7] = disc_bdaddr[4]; + hcibuf[8] = disc_bdaddr[5]; + if (pairWithWii && !wiiUProController) { + hcibuf[9] = 6; // Pin length is the length of the bt address + hcibuf[10] = disc_bdaddr[0]; // The pin is the Wiimotes bt address backwards + hcibuf[11] = disc_bdaddr[1]; + hcibuf[12] = disc_bdaddr[2]; + hcibuf[13] = disc_bdaddr[3]; + hcibuf[14] = disc_bdaddr[4]; + hcibuf[15] = disc_bdaddr[5]; + for (uint8_t i = 16; i < 26; i++) + hcibuf[i] = 0x00; // The rest should be 0 + } else if (pairWithWii && wiiUProController) { +#ifdef DEBUG + Notify(PSTR("\r\nParing with Wii U Pro Controller"), 0x80); +#endif + hcibuf[9] = 6; // Pin length is the length of the bt address + hcibuf[10] = my_bdaddr[0]; // The pin is the Bluetooth dongles bt address backwards + hcibuf[11] = my_bdaddr[1]; + hcibuf[12] = my_bdaddr[2]; + hcibuf[13] = my_bdaddr[3]; + hcibuf[14] = my_bdaddr[4]; + hcibuf[15] = my_bdaddr[5]; + for (uint8_t i = 16; i < 26; i++) + hcibuf[i] = 0x00; // The rest should be 0 + } else { + hcibuf[9] = strlen(btdPin); // Length of pin + uint8_t i; + for (i = 0; i < strlen(btdPin); i++) // The maximum size of the pin is 16 + hcibuf[i + 10] = btdPin[i]; + for (; i < 16; i++) + hcibuf[i + 10] = 0x00; // The rest should be 0 + } + + HCI_Command(hcibuf, 26); +} + void BTD::hci_pin_code_negative_request_reply() { - hcibuf[0] = 0x0E; // HCI OCF = 0E - hcibuf[1] = 0x01 << 2; // HCI OGF = 1 - hcibuf[2] = 0x06; // parameter length 6 - hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr - hcibuf[4] = disc_bdaddr[1]; - hcibuf[5] = disc_bdaddr[2]; - hcibuf[6] = disc_bdaddr[3]; - hcibuf[7] = disc_bdaddr[4]; - hcibuf[8] = disc_bdaddr[5]; - - HCI_Command(hcibuf, 9); + hcibuf[0] = 0x0E; // HCI OCF = 0E + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x06; // parameter length 6 + hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr + hcibuf[4] = disc_bdaddr[1]; + hcibuf[5] = disc_bdaddr[2]; + hcibuf[6] = disc_bdaddr[3]; + hcibuf[7] = disc_bdaddr[4]; + hcibuf[8] = disc_bdaddr[5]; + + HCI_Command(hcibuf, 9); } + void BTD::hci_link_key_request_negative_reply() { - hcibuf[0] = 0x0C; // HCI OCF = 0C - hcibuf[1] = 0x01 << 2; // HCI OGF = 1 - hcibuf[2] = 0x06; // parameter length 6 - hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr - hcibuf[4] = disc_bdaddr[1]; - hcibuf[5] = disc_bdaddr[2]; - hcibuf[6] = disc_bdaddr[3]; - hcibuf[7] = disc_bdaddr[4]; - hcibuf[8] = disc_bdaddr[5]; - - HCI_Command(hcibuf, 9); + hcibuf[0] = 0x0C; // HCI OCF = 0C + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x06; // parameter length 6 + hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr + hcibuf[4] = disc_bdaddr[1]; + hcibuf[5] = disc_bdaddr[2]; + hcibuf[6] = disc_bdaddr[3]; + hcibuf[7] = disc_bdaddr[4]; + hcibuf[8] = disc_bdaddr[5]; + + HCI_Command(hcibuf, 9); } + void BTD::hci_authentication_request() { - hcibuf[0] = 0x11; // HCI OCF = 11 - hcibuf[1] = 0x01 << 2; // HCI OGF = 1 - hcibuf[2] = 0x02; // parameter length = 2 - hcibuf[3] = (uint8_t)(hci_handle & 0xFF);//connection handle - low byte - hcibuf[4] = (uint8_t)((hci_handle >> 8) & 0x0F);//connection handle - high byte - - HCI_Command(hcibuf, 5); + hcibuf[0] = 0x11; // HCI OCF = 11 + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x02; // parameter length = 2 + hcibuf[3] = (uint8_t)(hci_handle & 0xFF); //connection handle - low byte + hcibuf[4] = (uint8_t)((hci_handle >> 8) & 0x0F); //connection handle - high byte + + HCI_Command(hcibuf, 5); } + void BTD::hci_disconnect(uint16_t handle) { // This is called by the different services - hci_event_flag &= ~HCI_FLAG_DISCONN_COMPLETE; - hcibuf[0] = 0x06; // HCI OCF = 6 - hcibuf[1] = 0x01 << 2; // HCI OGF = 1 - hcibuf[2] = 0x03; // parameter length = 3 - hcibuf[3] = (uint8_t)(handle & 0xFF);//connection handle - low byte - hcibuf[4] = (uint8_t)((handle >> 8) & 0x0F);//connection handle - high byte - hcibuf[5] = 0x13; // reason - - HCI_Command(hcibuf, 6); + hci_event_flag &= ~HCI_FLAG_DISCONN_COMPLETE; + hcibuf[0] = 0x06; // HCI OCF = 6 + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x03; // parameter length = 3 + hcibuf[3] = (uint8_t)(handle & 0xFF); //connection handle - low byte + hcibuf[4] = (uint8_t)((handle >> 8) & 0x0F); //connection handle - high byte + hcibuf[5] = 0x13; // reason + + HCI_Command(hcibuf, 6); } + void BTD::hci_write_class_of_device() { // See http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html - hcibuf[0] = 0x24; // HCI OCF = 3 - hcibuf[1] = 0x03 << 2; // HCI OGF = 3 - hcibuf[2] = 0x03; // parameter length = 3 - hcibuf[3] = 0x04; // Robot - hcibuf[4] = 0x08; // Toy - hcibuf[5] = 0x00; - HCI_Command(hcibuf, 6); + hcibuf[0] = 0x24; // HCI OCF = 3 + hcibuf[1] = 0x03 << 2; // HCI OGF = 3 + hcibuf[2] = 0x03; // parameter length = 3 + hcibuf[3] = 0x04; // Robot + hcibuf[4] = 0x08; // Toy + hcibuf[5] = 0x00; + HCI_Command(hcibuf, 6); } /******************************************************************* * * @@ -1029,174 +1043,183 @@ void BTD::hci_write_class_of_device() { // See http://bluetooth-pentest.narod.ru */ /************************************************************/ /* L2CAP Commands */ + /************************************************************/ void BTD::L2CAP_Command(uint16_t handle, uint8_t* data, uint8_t nbytes, uint8_t channelLow, uint8_t channelHigh) { - uint8_t buf[8+nbytes]; - buf[0] = (uint8_t)(handle & 0xff); // HCI handle with PB,BC flag - buf[1] = (uint8_t)(((handle >> 8) & 0x0f) | 0x20); - buf[2] = (uint8_t)((4 + nbytes) & 0xff); // HCI ACL total data length - buf[3] = (uint8_t)((4 + nbytes) >> 8); - buf[4] = (uint8_t)(nbytes & 0xff); // L2CAP header: Length - buf[5] = (uint8_t)(nbytes >> 8); - buf[6] = channelLow; - buf[7] = channelHigh; - - for (uint16_t i = 0; i < nbytes; i++) // L2CAP C-frame - buf[8 + i] = data[i]; - - uint8_t rcode = pUsb->outTransfer(bAddress, epInfo[ BTD_DATAOUT_PIPE ].epAddr, (8 + nbytes), buf); - if(rcode) { - delay(100); // This small delay prevents it from overflowing if it fails + uint8_t buf[8 + nbytes]; + buf[0] = (uint8_t)(handle & 0xff); // HCI handle with PB,BC flag + buf[1] = (uint8_t)(((handle >> 8) & 0x0f) | 0x20); + buf[2] = (uint8_t)((4 + nbytes) & 0xff); // HCI ACL total data length + buf[3] = (uint8_t)((4 + nbytes) >> 8); + buf[4] = (uint8_t)(nbytes & 0xff); // L2CAP header: Length + buf[5] = (uint8_t)(nbytes >> 8); + buf[6] = channelLow; + buf[7] = channelHigh; + + for (uint16_t i = 0; i < nbytes; i++) // L2CAP C-frame + buf[8 + i] = data[i]; + + uint8_t rcode = pUsb->outTransfer(bAddress, epInfo[ BTD_DATAOUT_PIPE ].epAddr, (8 + nbytes), buf); + if (rcode) { + delay(100); // This small delay prevents it from overflowing if it fails #ifdef DEBUG - Notify(PSTR("\r\nError sending L2CAP message: 0x")); - PrintHex(rcode); - Notify(PSTR(" - Channel ID: ")); - Serial.print(channelHigh); - Notify(PSTR(" ")); - Serial.print(channelLow); -#endif - } + Notify(PSTR("\r\nError sending L2CAP message: 0x"), 0x80); + PrintHex (rcode, 0x80); + Notify(PSTR(" - Channel ID: "), 0x80); + Serial.print(channelHigh); + Notify(PSTR(" "), 0x80); + Serial.print(channelLow); +#endif + } } + void BTD::l2cap_connection_request(uint16_t handle, uint8_t rxid, uint8_t* scid, uint16_t psm) { - l2capoutbuf[0] = L2CAP_CMD_CONNECTION_REQUEST; // Code - l2capoutbuf[1] = rxid; // Identifier - l2capoutbuf[2] = 0x04; // Length - l2capoutbuf[3] = 0x00; - l2capoutbuf[4] = (uint8_t)(psm & 0xff); // PSM - l2capoutbuf[5] = (uint8_t)(psm >> 8); - l2capoutbuf[6] = scid[0]; // Source CID - l2capoutbuf[7] = scid[1]; - - L2CAP_Command(handle, l2capoutbuf, 8); + l2capoutbuf[0] = L2CAP_CMD_CONNECTION_REQUEST; // Code + l2capoutbuf[1] = rxid; // Identifier + l2capoutbuf[2] = 0x04; // Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = (uint8_t)(psm & 0xff); // PSM + l2capoutbuf[5] = (uint8_t)(psm >> 8); + l2capoutbuf[6] = scid[0]; // Source CID + l2capoutbuf[7] = scid[1]; + + L2CAP_Command(handle, l2capoutbuf, 8); } -void BTD::l2cap_connection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid, uint8_t result) { - l2capoutbuf[0] = L2CAP_CMD_CONNECTION_RESPONSE; // Code - l2capoutbuf[1] = rxid; // Identifier - l2capoutbuf[2] = 0x08; // Length - l2capoutbuf[3] = 0x00; - l2capoutbuf[4] = dcid[0]; // Destination CID - l2capoutbuf[5] = dcid[1]; - l2capoutbuf[6] = scid[0]; // Source CID - l2capoutbuf[7] = scid[1]; - l2capoutbuf[8] = result; // Result: Pending or Success - l2capoutbuf[9] = 0x00; - l2capoutbuf[10] = 0x00; // No further information - l2capoutbuf[11] = 0x00; - - L2CAP_Command(handle, l2capoutbuf, 12); -} + +void BTD::l2cap_connection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid, uint8_t result) { + l2capoutbuf[0] = L2CAP_CMD_CONNECTION_RESPONSE; // Code + l2capoutbuf[1] = rxid; // Identifier + l2capoutbuf[2] = 0x08; // Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = dcid[0]; // Destination CID + l2capoutbuf[5] = dcid[1]; + l2capoutbuf[6] = scid[0]; // Source CID + l2capoutbuf[7] = scid[1]; + l2capoutbuf[8] = result; // Result: Pending or Success + l2capoutbuf[9] = 0x00; + l2capoutbuf[10] = 0x00; // No further information + l2capoutbuf[11] = 0x00; + + L2CAP_Command(handle, l2capoutbuf, 12); +} + void BTD::l2cap_config_request(uint16_t handle, uint8_t rxid, uint8_t* dcid) { - l2capoutbuf[0] = L2CAP_CMD_CONFIG_REQUEST; // Code - l2capoutbuf[1] = rxid; // Identifier - l2capoutbuf[2] = 0x08; // Length - l2capoutbuf[3] = 0x00; - l2capoutbuf[4] = dcid[0]; // Destination CID - l2capoutbuf[5] = dcid[1]; - l2capoutbuf[6] = 0x00; // Flags - l2capoutbuf[7] = 0x00; - l2capoutbuf[8] = 0x01; // Config Opt: type = MTU (Maximum Transmission Unit) - Hint - l2capoutbuf[9] = 0x02; // Config Opt: length - l2capoutbuf[10] = 0xFF; // MTU - l2capoutbuf[11] = 0xFF; - - L2CAP_Command(handle, l2capoutbuf, 12); + l2capoutbuf[0] = L2CAP_CMD_CONFIG_REQUEST; // Code + l2capoutbuf[1] = rxid; // Identifier + l2capoutbuf[2] = 0x08; // Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = dcid[0]; // Destination CID + l2capoutbuf[5] = dcid[1]; + l2capoutbuf[6] = 0x00; // Flags + l2capoutbuf[7] = 0x00; + l2capoutbuf[8] = 0x01; // Config Opt: type = MTU (Maximum Transmission Unit) - Hint + l2capoutbuf[9] = 0x02; // Config Opt: length + l2capoutbuf[10] = 0xFF; // MTU + l2capoutbuf[11] = 0xFF; + + L2CAP_Command(handle, l2capoutbuf, 12); } -void BTD::l2cap_config_response(uint16_t handle, uint8_t rxid, uint8_t* scid) { - l2capoutbuf[0] = L2CAP_CMD_CONFIG_RESPONSE; // Code - l2capoutbuf[1] = rxid; // Identifier - l2capoutbuf[2] = 0x0A; // Length - l2capoutbuf[3] = 0x00; - l2capoutbuf[4] = scid[0]; // Source CID - l2capoutbuf[5] = scid[1]; - l2capoutbuf[6] = 0x00; // Flag - l2capoutbuf[7] = 0x00; - l2capoutbuf[8] = 0x00; // Result - l2capoutbuf[9] = 0x00; - l2capoutbuf[10] = 0x01; // Config - l2capoutbuf[11] = 0x02; - l2capoutbuf[12] = 0xA0; - l2capoutbuf[13] = 0x02; - - L2CAP_Command(handle, l2capoutbuf, 14); + +void BTD::l2cap_config_response(uint16_t handle, uint8_t rxid, uint8_t* scid) { + l2capoutbuf[0] = L2CAP_CMD_CONFIG_RESPONSE; // Code + l2capoutbuf[1] = rxid; // Identifier + l2capoutbuf[2] = 0x0A; // Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = scid[0]; // Source CID + l2capoutbuf[5] = scid[1]; + l2capoutbuf[6] = 0x00; // Flag + l2capoutbuf[7] = 0x00; + l2capoutbuf[8] = 0x00; // Result + l2capoutbuf[9] = 0x00; + l2capoutbuf[10] = 0x01; // Config + l2capoutbuf[11] = 0x02; + l2capoutbuf[12] = 0xA0; + l2capoutbuf[13] = 0x02; + + L2CAP_Command(handle, l2capoutbuf, 14); } + void BTD::l2cap_disconnection_request(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid) { - l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_REQUEST; // Code - l2capoutbuf[1] = rxid; // Identifier - l2capoutbuf[2] = 0x04; // Length - l2capoutbuf[3] = 0x00; - l2capoutbuf[4] = dcid[0]; - l2capoutbuf[5] = dcid[1]; - l2capoutbuf[6] = scid[0]; - l2capoutbuf[7] = scid[1]; - L2CAP_Command(handle, l2capoutbuf, 8); + l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_REQUEST; // Code + l2capoutbuf[1] = rxid; // Identifier + l2capoutbuf[2] = 0x04; // Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = dcid[0]; + l2capoutbuf[5] = dcid[1]; + l2capoutbuf[6] = scid[0]; + l2capoutbuf[7] = scid[1]; + L2CAP_Command(handle, l2capoutbuf, 8); } + void BTD::l2cap_disconnection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid) { - l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_RESPONSE; // Code - l2capoutbuf[1] = rxid; // Identifier - l2capoutbuf[2] = 0x04; // Length - l2capoutbuf[3] = 0x00; - l2capoutbuf[4] = dcid[0]; - l2capoutbuf[5] = dcid[1]; - l2capoutbuf[6] = scid[0]; - l2capoutbuf[7] = scid[1]; - L2CAP_Command(handle, l2capoutbuf, 8); + l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_RESPONSE; // Code + l2capoutbuf[1] = rxid; // Identifier + l2capoutbuf[2] = 0x04; // Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = dcid[0]; + l2capoutbuf[5] = dcid[1]; + l2capoutbuf[6] = scid[0]; + l2capoutbuf[7] = scid[1]; + L2CAP_Command(handle, l2capoutbuf, 8); } + void BTD::l2cap_information_response(uint16_t handle, uint8_t rxid, uint8_t infoTypeLow, uint8_t infoTypeHigh) { - l2capoutbuf[0] = L2CAP_CMD_INFORMATION_RESPONSE; // Code - l2capoutbuf[1] = rxid; // Identifier - l2capoutbuf[2] = 0x08; // Length - l2capoutbuf[3] = 0x00; - l2capoutbuf[4] = infoTypeLow; - l2capoutbuf[5] = infoTypeHigh; - l2capoutbuf[6] = 0x00; // Result = success - l2capoutbuf[7] = 0x00; // Result = success - l2capoutbuf[8] = 0x00; - l2capoutbuf[9] = 0x00; - l2capoutbuf[10] = 0x00; - l2capoutbuf[11] = 0x00; - L2CAP_Command(handle, l2capoutbuf, 12); + l2capoutbuf[0] = L2CAP_CMD_INFORMATION_RESPONSE; // Code + l2capoutbuf[1] = rxid; // Identifier + l2capoutbuf[2] = 0x08; // Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = infoTypeLow; + l2capoutbuf[5] = infoTypeHigh; + l2capoutbuf[6] = 0x00; // Result = success + l2capoutbuf[7] = 0x00; // Result = success + l2capoutbuf[8] = 0x00; + l2capoutbuf[9] = 0x00; + l2capoutbuf[10] = 0x00; + l2capoutbuf[11] = 0x00; + L2CAP_Command(handle, l2capoutbuf, 12); } /* PS3 Commands - only set Bluetooth address is implemented */ void BTD::setBdaddr(uint8_t* BDADDR) { - /* Set the internal bluetooth address */ - uint8_t buf[8]; - buf[0] = 0x01; - buf[1] = 0x00; - for (uint8_t i = 0; i < 6; i++) - buf[i+2] = BDADDR[5 - i];//Copy into buffer, has to be written reversed - - //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) - pUsb->ctrlReq(bAddress,epInfo[BTD_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL); + /* Set the internal bluetooth address */ + uint8_t buf[8]; + buf[0] = 0x01; + buf[1] = 0x00; + for (uint8_t i = 0; i < 6; i++) + buf[i + 2] = BDADDR[5 - i]; //Copy into buffer, has to be written reversed + + //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) + pUsb->ctrlReq(bAddress, epInfo[BTD_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL); #ifdef DEBUG - Notify(PSTR("\r\nBluetooth Address was set to: ")); - for(int8_t i = 5; i > 0; i--) { - PrintHex(my_bdaddr[i]); - Notify(PSTR(":")); - } - PrintHex(my_bdaddr[0]); + Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80); + for (int8_t i = 5; i > 0; i--) { + PrintHex (my_bdaddr[i], 0x80); + Notify(PSTR(":"), 0x80); + } + PrintHex (my_bdaddr[0], 0x80); #endif } + void BTD::setMoveBdaddr(uint8_t* BDADDR) { - /* Set the internal bluetooth address */ - uint8_t buf[11]; - buf[0] = 0x05; - buf[7] = 0x10; - buf[8] = 0x01; - buf[9] = 0x02; - buf[10] = 0x12; - - for (uint8_t i = 0; i < 6; i++) - buf[i + 1] = BDADDR[i]; - - //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) - pUsb->ctrlReq(bAddress,epInfo[BTD_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00,11,11, buf, NULL); + /* Set the internal bluetooth address */ + uint8_t buf[11]; + buf[0] = 0x05; + buf[7] = 0x10; + buf[8] = 0x01; + buf[9] = 0x02; + buf[10] = 0x12; + + for (uint8_t i = 0; i < 6; i++) + buf[i + 1] = BDADDR[i]; + + //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) + pUsb->ctrlReq(bAddress, epInfo[BTD_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00, 11, 11, buf, NULL); #ifdef DEBUG - Notify(PSTR("\r\nBluetooth Address was set to: ")); - for(int8_t i = 5; i > 0; i--) { - PrintHex(my_bdaddr[i]); - Notify(PSTR(":")); - } - PrintHex(my_bdaddr[0]); + Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80); + for (int8_t i = 5; i > 0; i--) { + PrintHex (my_bdaddr[i], 0x80); + Notify(PSTR(":"), 0x80); + } + PrintHex (my_bdaddr[0], 0x80); #endif } \ No newline at end of file diff --git a/PS3BT.cpp b/PS3BT.cpp index cf35a7f4..87214ab3 100644 --- a/PS3BT.cpp +++ b/PS3BT.cpp @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -21,15 +21,15 @@ //#define PRINTREPORT // Uncomment to print the report send by the PS3 Controllers const uint8_t OUTPUT_REPORT_BUFFER[] PROGMEM = { - 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, - 0xff, 0x27, 0x10, 0x00, 0x32, - 0xff, 0x27, 0x10, 0x00, 0x32, - 0xff, 0x27, 0x10, 0x00, 0x32, - 0xff, 0x27, 0x10, 0x00, 0x32, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + 0x00, 0x00, 0x00, 0x00, 0x00, + 0xff, 0x27, 0x10, 0x00, 0x32, + 0xff, 0x27, 0x10, 0x00, 0x32, + 0xff, 0x27, 0x10, 0x00, 0x32, + 0xff, 0x27, 0x10, 0x00, 0x32, + 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; PS3BT::PS3BT(BTD *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0): @@ -37,27 +37,27 @@ pBtd(p) // pointer to USB class instance - mandatory { if (pBtd) pBtd->registerServiceClass(this); // Register it as a Bluetooth service - + pBtd->my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead pBtd->my_bdaddr[4] = btadr4; pBtd->my_bdaddr[3] = btadr3; pBtd->my_bdaddr[2] = btadr2; pBtd->my_bdaddr[1] = btadr1; pBtd->my_bdaddr[0] = btadr0; - + HIDBuffer[0] = 0x52;// HID BT Set_report (0x50) | Report Type (Output 0x02) HIDBuffer[1] = 0x01;// Report ID - + //Needed for PS3 Move Controller commands to work via bluetooth HIDMoveBuffer[0] = 0xA2;// HID BT DATA_request (0xA0) | Report Type (Output 0x02) HIDMoveBuffer[1] = 0x02;// Report ID - + /* Set device cid for the control and intterrupt channelse - LSB */ control_dcid[0] = 0x40;//0x0040 control_dcid[1] = 0x00; interrupt_dcid[0] = 0x41;//0x0041 interrupt_dcid[1] = 0x00; - + Reset(); } bool PS3BT::getButtonPress(Button b) { @@ -76,7 +76,7 @@ uint8_t PS3BT::getAnalogButton(Button a) { } uint8_t PS3BT::getAnalogHat(AnalogHat a) { if (l2capinbuf == NULL) - return 0; + return 0; return (uint8_t)(l2capinbuf[(uint8_t)a+15]); } int16_t PS3BT::getSensor(Sensor a) { @@ -93,7 +93,7 @@ int16_t PS3BT::getSensor(Sensor a) { else if (a == mZmove || a == tempMove) // The tempearature is also 12 bits long return ((l2capinbuf[(uint16_t)a] << 4) | ((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4)); else // aXmove, aYmove, aZmove, gXmove, gYmove and gZmove - return (l2capinbuf[(uint16_t)a] | (l2capinbuf[(uint16_t)a + 1] << 8)); + return (l2capinbuf[(uint16_t)a] | (l2capinbuf[(uint16_t)a + 1] << 8)); } else return 0; } @@ -101,7 +101,7 @@ double PS3BT::getAngle(Angle a) { double accXval; double accYval; double accZval; - + if(PS3Connected) { // Data for the Kionix KXPC4 used in the DualShock 3 const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) @@ -115,7 +115,7 @@ double PS3BT::getAngle(Angle a) { accYval = (int16_t)(getSensor(aYmove)-zeroG); accZval = (int16_t)(getSensor(aZmove)-zeroG); } - + // Convert to 360 degrees resolution // atan2 outputs the value of -π to π (radians) // We are then converting it to 0 to 2π and then to degrees @@ -157,15 +157,15 @@ double PS3BT::get9DOFValues(Sensor a) { // Thanks to Manfred Piendl } String PS3BT::getTemperature() { if(PS3MoveConnected) { - int16_t input = getSensor(tempMove); - + int16_t input = getSensor(tempMove); + String output = String(input/100); output += "."; if(input%100 < 10) output += "0"; output += String(input%100); - - return output; + + return output; } } bool PS3BT::getStatus(Status c) { @@ -178,14 +178,14 @@ bool PS3BT::getStatus(Status c) { String PS3BT::getStatusString() { if (PS3Connected || PS3NavigationConnected) { char statusOutput[100]; - + strcpy(statusOutput,"ConnectionStatus: "); - + if (getStatus(Plugged)) strcat(statusOutput,"Plugged"); else if (getStatus(Unplugged)) strcat(statusOutput,"Unplugged"); else strcat(statusOutput,"Error"); - - + + strcat(statusOutput," - PowerRating: "); if (getStatus(Charging)) strcat(statusOutput,"Charging"); else if (getStatus(NotCharging)) strcat(statusOutput,"Not Charging"); @@ -195,23 +195,23 @@ String PS3BT::getStatusString() { else if (getStatus(High)) strcat(statusOutput,"High"); else if (getStatus(Full)) strcat(statusOutput,"Full"); else strcat(statusOutput,"Error"); - + strcat(statusOutput," - WirelessStatus: "); - + if (getStatus(CableRumble)) strcat(statusOutput,"Cable - Rumble is on"); else if (getStatus(Cable)) strcat(statusOutput,"Cable - Rumble is off"); else if (getStatus(BluetoothRumble)) strcat(statusOutput,"Bluetooth - Rumble is on"); else if (getStatus(Bluetooth)) strcat(statusOutput,"Bluetooth - Rumble is off"); else strcat(statusOutput,"Error"); - + return statusOutput; - + } else if(PS3MoveConnected) { char statusOutput[50]; - + strcpy(statusOutput,"PowerRating: "); - + if (getStatus(MoveCharging)) strcat(statusOutput,"Charging"); else if (getStatus(MoveNotCharging)) strcat(statusOutput,"Not Charging"); else if (getStatus(MoveShutdown)) strcat(statusOutput,"Shutdown"); @@ -220,7 +220,7 @@ String PS3BT::getStatusString() { else if (getStatus(MoveHigh)) strcat(statusOutput,"High"); else if (getStatus(MoveFull)) strcat(statusOutput,"Full"); else strcat(statusOutput,"Error"); - + return statusOutput; } } @@ -231,10 +231,10 @@ void PS3BT::Reset() { activeConnection = false; l2cap_event_flag = 0; // Reset flags l2cap_state = L2CAP_WAIT; - + // Needed for PS3 Dualshock Controller commands to work via bluetooth for (uint8_t i = 0; i < OUTPUT_REPORT_BUFFER_SIZE; i++) - HIDBuffer[i + 2] = pgm_read_byte(&OUTPUT_REPORT_BUFFER[i]); // First two bytes reserved for report type and ID + HIDBuffer[i + 2] = pgm_read_byte(&OUTPUT_REPORT_BUFFER[i]); // First two bytes reserved for report type and ID } void PS3BT::disconnect() { // Use this void to disconnect any of the controllers @@ -256,11 +256,11 @@ void PS3BT::ACLData(uint8_t* ACLData) { remote_name[i] = pBtd->remote_name[i]; // Store the remote name for the connection #ifdef DEBUG if(pBtd->hci_version < 3) { // Check the HCI Version of the Bluetooth dongle - Notify(PSTR("\r\nYour dongle may not support reading the analog buttons, sensors and status\r\nYour HCI Version is: ")); + Notify(PSTR("\r\nYour dongle may not support reading the analog buttons, sensors and status\r\nYour HCI Version is: "), 0x80); Serial.print(pBtd->hci_version); - Notify(PSTR("\r\nBut should be at least 3\r\nThis means that it doesn't support Bluetooth Version 2.0+EDR")); + Notify(PSTR("\r\nBut should be at least 3\r\nThis means that it doesn't support Bluetooth Version 2.0+EDR"), 0x80); } -#endif +#endif } } } @@ -270,34 +270,34 @@ void PS3BT::ACLData(uint8_t* ACLData) { if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) { #ifdef DEBUG - Notify(PSTR("\r\nL2CAP Command Rejected - Reason: ")); - PrintHex(l2capinbuf[13]); + Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80); + PrintHex(l2capinbuf[13], 0x80); Serial.print(" "); - PrintHex(l2capinbuf[12]); + PrintHex(l2capinbuf[12], 0x80); Serial.print(" Data: "); - PrintHex(l2capinbuf[17]); + PrintHex(l2capinbuf[17], 0x80); Serial.print(" "); - PrintHex(l2capinbuf[16]); - Serial.print(" "); - PrintHex(l2capinbuf[15]); + PrintHex(l2capinbuf[16], 0x80); Serial.print(" "); - PrintHex(l2capinbuf[14]); + PrintHex(l2capinbuf[15], 0x80); + Serial.print(" "); + PrintHex(l2capinbuf[14], 0x80); #endif } - else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { + else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { #ifdef EXTRADEBUG - Notify(PSTR("\r\nL2CAP Connection Request - PSM: ")); - PrintHex(l2capinbuf[13]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[12]); - Notify(PSTR(" SCID: ")); - PrintHex(l2capinbuf[15]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[14]); - Notify(PSTR(" Identifier: ")); - PrintHex(l2capinbuf[9]); + Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80); + PrintHex(l2capinbuf[13], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[12], 0x80); + Notify(PSTR(" SCID: "), 0x80); + PrintHex(l2capinbuf[15], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[14], 0x80); + Notify(PSTR(" Identifier: "), 0x80); + PrintHex(l2capinbuf[9], 0x80); #endif - if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) { + if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) { identifier = l2capinbuf[9]; control_scid[0] = l2capinbuf[14]; control_scid[1] = l2capinbuf[15]; @@ -325,7 +325,7 @@ void PS3BT::ACLData(uint8_t* ACLData) { else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) { if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { //Serial.print("\r\nHID Control Configuration Request"); - identifier = l2capinbuf[9]; + identifier = l2capinbuf[9]; l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_REQUEST; } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { @@ -333,11 +333,11 @@ void PS3BT::ACLData(uint8_t* ACLData) { identifier = l2capinbuf[9]; l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_REQUEST; } - } + } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) { if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnect Request: Control Channel")); + Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80); #endif identifier = l2capinbuf[9]; pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid); @@ -345,7 +345,7 @@ void PS3BT::ACLData(uint8_t* ACLData) { } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnect Request: Interrupt Channel")); + Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80); #endif identifier = l2capinbuf[9]; pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid); @@ -353,12 +353,12 @@ void PS3BT::ACLData(uint8_t* ACLData) { } } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) { - if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) { + if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) { //Serial.print("\r\nDisconnect Response: Control Channel"); identifier = l2capinbuf[9]; l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE; } - else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) { + else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) { //Serial.print("\r\nDisconnect Response: Interrupt Channel"); identifier = l2capinbuf[9]; l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE; @@ -366,8 +366,8 @@ void PS3BT::ACLData(uint8_t* ACLData) { } #ifdef EXTRADEBUG else { - Notify(PSTR("\r\nL2CAP Unknown Signaling Command: ")); - PrintHex(l2capinbuf[8]); + Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80); + PrintHex(l2capinbuf[8], 0x80); } #endif } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt @@ -379,18 +379,18 @@ void PS3BT::ACLData(uint8_t* ACLData) { ButtonState = (uint32_t)(l2capinbuf[11] | ((uint16_t)l2capinbuf[12] << 8) | ((uint32_t)l2capinbuf[13] << 16)); else if(PS3MoveConnected) ButtonState = (uint32_t)(l2capinbuf[10] | ((uint16_t)l2capinbuf[11] << 8) | ((uint32_t)l2capinbuf[12] << 16)); - - //Notify(PSTR("\r\nButtonState"); - //PrintHex(ButtonState); - + + //Notify(PSTR("\r\nButtonState", 0x80); + //PrintHex(ButtonState, 0x80); + if(ButtonState != OldButtonState) { ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable OldButtonState = ButtonState; } - + #ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers for(uint8_t i = 10; i < 58;i++) { - PrintHex(l2capinbuf[i]); + PrintHex(l2capinbuf[i], 0x80); Serial.print(" "); } Serial.println(); @@ -406,7 +406,7 @@ void PS3BT::L2CAP_task() { case L2CAP_WAIT: if (l2cap_connection_request_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Control Incoming Connection Request")); + Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80); #endif pBtd->l2cap_connection_response(hci_handle,identifier, control_dcid, control_scid, PENDING); delay(1); @@ -420,17 +420,17 @@ void PS3BT::L2CAP_task() { case L2CAP_CONTROL_REQUEST: if (l2cap_config_request_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Control Configuration Request")); + Notify(PSTR("\r\nHID Control Configuration Request"), 0x80); #endif pBtd->l2cap_config_response(hci_handle,identifier, control_scid); l2cap_state = L2CAP_CONTROL_SUCCESS; } break; - + case L2CAP_CONTROL_SUCCESS: if (l2cap_config_success_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Control Successfully Configured")); + Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80); #endif l2cap_state = L2CAP_INTERRUPT_SETUP; } @@ -438,7 +438,7 @@ void PS3BT::L2CAP_task() { case L2CAP_INTERRUPT_SETUP: if (l2cap_connection_request_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Interrupt Incoming Connection Request")); + Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80); #endif pBtd->l2cap_connection_response(hci_handle,identifier, interrupt_dcid, interrupt_scid, PENDING); delay(1); @@ -446,14 +446,14 @@ void PS3BT::L2CAP_task() { identifier++; delay(1); pBtd->l2cap_config_request(hci_handle,identifier, interrupt_scid); - + l2cap_state = L2CAP_INTERRUPT_REQUEST; } break; case L2CAP_INTERRUPT_REQUEST: if (l2cap_config_request_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Interrupt Configuration Request")); + Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80); #endif pBtd->l2cap_config_response(hci_handle,identifier, interrupt_scid); l2cap_state = L2CAP_INTERRUPT_SUCCESS; @@ -462,14 +462,14 @@ void PS3BT::L2CAP_task() { case L2CAP_INTERRUPT_SUCCESS: if (l2cap_config_success_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Interrupt Successfully Configured")); + Notify(PSTR("\r\nHID Interrupt Successfully Configured"), 0x80); #endif if(remote_name[0] == 'M') { // First letter in Motion Controller ('M') for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) // Reset l2cap in buffer as it sometimes read it as a button has been pressed l2capinbuf[i] = 0; ButtonState = 0; OldButtonState = 0; - + l2cap_state = L2CAP_HID_PS3_LED; } else l2cap_state = L2CAP_HID_ENABLE_SIXAXIS; @@ -478,22 +478,22 @@ void PS3BT::L2CAP_task() { break; /* These states are handled in Run() */ - + case L2CAP_INTERRUPT_DISCONNECT: if (l2cap_disconnect_response_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnected Interrupt Channel")); + Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80); #endif identifier++; - pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid); + pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid); l2cap_state = L2CAP_CONTROL_DISCONNECT; } break; - + case L2CAP_CONTROL_DISCONNECT: if (l2cap_disconnect_response_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnected Control Channel")); + Notify(PSTR("\r\nDisconnected Control Channel"), 0x80); #endif pBtd->hci_disconnect(hci_handle); hci_handle = -1; // Reset handle @@ -501,7 +501,7 @@ void PS3BT::L2CAP_task() { l2cap_state = L2CAP_WAIT; } break; - } + } } void PS3BT::Run() { switch (l2cap_state) { @@ -511,7 +511,7 @@ void PS3BT::Run() { l2capinbuf[i] = 0; ButtonState = 0; OldButtonState = 0; - + enable_sixaxis(); for (uint8_t i = 15; i < 19; i++) l2capinbuf[i] = 0x7F; // Set the analog joystick values to center position @@ -519,33 +519,33 @@ void PS3BT::Run() { timer = millis(); } break; - + case L2CAP_HID_PS3_LED: if(millis() - timer > 1000) { // loop 1 second before sending the command if (remote_name[0] == 'P') { // First letter in PLAYSTATION(R)3 Controller ('P') setLedOn(LED1); #ifdef DEBUG - Notify(PSTR("\r\nDualshock 3 Controller Enabled\r\n")); + Notify(PSTR("\r\nDualshock 3 Controller Enabled\r\n"), 0x80); #endif PS3Connected = true; } else if (remote_name[0] == 'N') { // First letter in Navigation Controller ('N') setLedOn(LED1); // This just turns LED constantly on, on the Navigation controller #ifdef DEBUG - Notify(PSTR("\r\nNavigation Controller Enabled\r\n")); + Notify(PSTR("\r\nNavigation Controller Enabled\r\n"), 0x80); #endif PS3NavigationConnected = true; } else if(remote_name[0] == 'M') { // First letter in Motion Controller ('M') moveSetBulb(Red); timerBulbRumble = millis(); #ifdef DEBUG - Notify(PSTR("\r\nMotion Controller Enabled\r\n")); + Notify(PSTR("\r\nMotion Controller Enabled\r\n"), 0x80); #endif PS3MoveConnected = true; } l2cap_state = L2CAP_DONE; } break; - + case L2CAP_DONE: if (PS3MoveConnected) { //The Bulb and rumble values, has to be send at aproximatly every 5th second for it to stay on if (millis() - timerBulbRumble > 4000) { //Send at least every 4th second @@ -562,8 +562,8 @@ void PS3BT::Run() { /************************************************************/ //Playstation Sixaxis Dualshock and Navigation Controller commands -void PS3BT::HID_Command(uint8_t* data, uint8_t nbytes) { - if (millis() - timerHID <= 250)// Check if is has been more than 250ms since last command +void PS3BT::HID_Command(uint8_t* data, uint8_t nbytes) { + if (millis() - timerHID <= 250)// Check if is has been more than 250ms since last command delay((uint32_t)(250 - (millis() - timerHID)));//There have to be a delay between commands pBtd->L2CAP_Command(hci_handle,data,nbytes,control_scid[0],control_scid[1]); // Both the Navigation and Dualshock controller sends data via the control channel timerHID = millis(); @@ -571,7 +571,7 @@ void PS3BT::HID_Command(uint8_t* data, uint8_t nbytes) { void PS3BT::setAllOff() { for (uint8_t i = 0; i < OUTPUT_REPORT_BUFFER_SIZE; i++) HIDBuffer[i + 2] = pgm_read_byte(&OUTPUT_REPORT_BUFFER[i]);//First two bytes reserved for report type and ID - + HID_Command(HIDBuffer, HID_BUFFERSIZE); } void PS3BT::setRumbleOff() { @@ -579,7 +579,7 @@ void PS3BT::setRumbleOff() { HIDBuffer[4] = 0x00;//low mode off HIDBuffer[5] = 0x00; HIDBuffer[6] = 0x00;//high mode off - + HID_Command(HIDBuffer, HID_BUFFERSIZE); } void PS3BT::setRumbleOn(Rumble mode) { @@ -591,7 +591,7 @@ void PS3BT::setRumbleOn(Rumble mode) { */ if ((mode & 0x30) > 0) { HIDBuffer[3] = 0xfe; - HIDBuffer[5] = 0xfe; + HIDBuffer[5] = 0xfe; if (mode == RumbleHigh) { HIDBuffer[4] = 0;//low mode off HIDBuffer[6] = 0xff;//high mode on @@ -599,7 +599,7 @@ void PS3BT::setRumbleOn(Rumble mode) { else { HIDBuffer[4] = 0xff;//low mode on HIDBuffer[6] = 0;//high mode off - } + } HID_Command(HIDBuffer, HID_BUFFERSIZE); } } @@ -623,7 +623,7 @@ void PS3BT::enable_sixaxis() { //Command used to enable the Dualshock 3 and Navi cmd_buf[3] = 0x03; cmd_buf[4] = 0x00; cmd_buf[5] = 0x00; - + HID_Command(cmd_buf, 6); } @@ -635,12 +635,12 @@ void PS3BT::HIDMove_Command(uint8_t* data,uint8_t nbytes) { timerHID = millis(); } void PS3BT::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { //Use this to set the Color using RGB values - //set the Bulb's values into the write buffer + //set the Bulb's values into the write buffer HIDMoveBuffer[3] = r; HIDMoveBuffer[4] = g; HIDMoveBuffer[5] = b; - - HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); + + HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); } void PS3BT::moveSetBulb(Colors color) { //Use this to set the Color using the predefined colors in enum moveSetBulb((uint8_t)(color >> 16),(uint8_t)(color >> 8),(uint8_t)(color)); @@ -648,10 +648,10 @@ void PS3BT::moveSetBulb(Colors color) { //Use this to set the Color using the pr void PS3BT::moveSetRumble(uint8_t rumble) { #ifdef DEBUG if(rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100) - Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%")); + Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80); #endif //set the rumble value into the write buffer HIDMoveBuffer[7] = rumble; - - HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); + + HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); } \ No newline at end of file diff --git a/PS3USB.cpp b/PS3USB.cpp index 57c2e2b9..13d835db 100644 --- a/PS3USB.cpp +++ b/PS3USB.cpp @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -21,20 +21,20 @@ //#define PRINTREPORT // Uncomment to print the report send by the PS3 Controllers const uint8_t PS3_REPORT_BUFFER[] PROGMEM = { - 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, - 0xff, 0x27, 0x10, 0x00, 0x32, - 0xff, 0x27, 0x10, 0x00, 0x32, - 0xff, 0x27, 0x10, 0x00, 0x32, - 0xff, 0x27, 0x10, 0x00, 0x32, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + 0x00, 0x00, 0x00, 0x00, 0x00, + 0xff, 0x27, 0x10, 0x00, 0x32, + 0xff, 0x27, 0x10, 0x00, 0x32, + 0xff, 0x27, 0x10, 0x00, 0x32, + 0xff, 0x27, 0x10, 0x00, 0x32, + 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; const uint8_t MOVE_REPORT_BUFFER[] PROGMEM = { - 0x02, 0x00, // Always 0x02, 0x00, - 0x00, 0x00, 0x00, // r, g, b, - 0x00, // Always 0x00, + 0x02, 0x00, // Always 0x02, 0x00, + 0x00, 0x00, 0x00, // r, g, b, + 0x00, // Always 0x00, 0x00 // Rumble }; @@ -46,13 +46,13 @@ bPollEnable(false) // don't start polling before dongle is connected for(uint8_t i=0; iRegisterDeviceClass(this); //set devConfig[] entry - + my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead my_bdaddr[4] = btadr4; my_bdaddr[3] = btadr3; @@ -68,68 +68,68 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) { EpInfo *oldep_ptr = NULL; uint16_t PID; uint16_t VID; - + // get memory address of USB device address pool - AddressPool &addrPool = pUsb->GetAddressPool(); + AddressPool &addrPool = pUsb->GetAddressPool(); #ifdef EXTRADEBUG - Notify(PSTR("\r\nPS3USB Init")); + Notify(PSTR("\r\nPS3USB Init"), 0x80); #endif // check if address has already been assigned to an instance if (bAddress) { #ifdef DEBUG - Notify(PSTR("\r\nAddress in use")); + Notify(PSTR("\r\nAddress in use"), 0x80); #endif return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; } - + // Get pointer to pseudo device with address 0 assigned p = addrPool.GetUsbDevicePtr(0); - - if (!p) { + + if (!p) { #ifdef DEBUG - Notify(PSTR("\r\nAddress not found")); + Notify(PSTR("\r\nAddress not found"), 0x80); #endif return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; } - + if (!p->epinfo) { #ifdef DEBUG - Notify(PSTR("\r\nepinfo is null")); + Notify(PSTR("\r\nepinfo is null"), 0x80); #endif return USB_ERROR_EPINFO_IS_NULL; } - + // Save old pointer to EP_RECORD of address 0 oldep_ptr = p->epinfo; - + // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence p->epinfo = epInfo; - + p->lowspeed = lowspeed; - + // Get device descriptor rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data // Restore p->epinfo p->epinfo = oldep_ptr; - + if(rcode) goto FailGetDevDescr; - + VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor; PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct; - + if(VID != PS3_VID || (PID != PS3_PID && PID != PS3NAVIGATION_PID && PID != PS3MOVE_PID)) - goto FailUnknownDevice; - + goto FailUnknownDevice; + // Allocate new address according to device class bAddress = addrPool.AllocAddress(parent, false, port); - + if (!bAddress) return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; - + // Extract Max Packet Size from device descriptor - epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; - + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; + // Assign new address to the device rcode = pUsb->setAddr( 0, 0, bAddress ); if (rcode) { @@ -137,34 +137,34 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) { addrPool.FreeAddress(bAddress); bAddress = 0; #ifdef DEBUG - Notify(PSTR("\r\nsetAddr: ")); + Notify(PSTR("\r\nsetAddr: "), 0x80); #endif - PrintHex(rcode); + PrintHex(rcode, 0x80); return rcode; } #ifdef EXTRADEBUG - Notify(PSTR("\r\nAddr: ")); - PrintHex(bAddress); + Notify(PSTR("\r\nAddr: "), 0x80); + PrintHex(bAddress, 0x80); #endif p->lowspeed = false; - + //get pointer to assigned address record p = addrPool.GetUsbDevicePtr(bAddress); - if (!p) + if (!p) return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - - p->lowspeed = lowspeed; - + + p->lowspeed = lowspeed; + // Assign epInfo to epinfo pointer - only EP0 is known rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); if (rcode) goto FailSetDevTblEntry; - - + + /* The application will work in reduced host mode, so we can save program and data memory space. After verifying the PID and VID we will use known values for the configuration values for device, interface, endpoints and HID for the PS3 Controllers */ - + /* Initialize data structures for endpoints of device */ epInfo[ PS3_OUTPUT_PIPE ].epAddr = 0x02; // PS3 output endpoint epInfo[ PS3_OUTPUT_PIPE ].epAttribs = EP_INTERRUPT; @@ -178,26 +178,26 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) { epInfo[ PS3_INPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE; epInfo[ PS3_INPUT_PIPE ].bmSndToggle = bmSNDTOG0; epInfo[ PS3_INPUT_PIPE ].bmRcvToggle = bmRCVTOG0; - + rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo); if( rcode ) goto FailSetDevTblEntry; - + delay(200);//Give time for address change - + rcode = pUsb->setConf(bAddress, epInfo[ PS3_CONTROL_PIPE ].epAddr, 1); if( rcode ) goto FailSetConf; - + if(PID == PS3_PID || PID == PS3NAVIGATION_PID) { if(PID == PS3_PID) { #ifdef DEBUG - Notify(PSTR("\r\nDualshock 3 Controller Connected")); -#endif + Notify(PSTR("\r\nDualshock 3 Controller Connected"), 0x80); +#endif PS3Connected = true; } else { // must be a navigation controller #ifdef DEBUG - Notify(PSTR("\r\nNavigation Controller Connected")); + Notify(PSTR("\r\nNavigation Controller Connected"), 0x80); #endif PS3NavigationConnected = true; } @@ -205,62 +205,62 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) { setBdaddr(my_bdaddr); enable_sixaxis(); setLedOn(LED1); - + // Needed for PS3 Dualshock and Navigation commands to work for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++) writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); - + for (uint8_t i = 6; i < 10; i++) readBuf[i] = 0x7F; // Set the analog joystick values to center position } else { // must be a Motion controller #ifdef DEBUG - Notify(PSTR("\r\nMotion Controller Connected")); -#endif + Notify(PSTR("\r\nMotion Controller Connected"), 0x80); +#endif PS3MoveConnected = true; setMoveBdaddr(my_bdaddr); // Set internal bluetooth address moveSetBulb(Red); - + // Needed for Move commands to work for (uint8_t i = 0; i < MOVE_REPORT_BUFFER_SIZE; i++) writeBuf[i] = pgm_read_byte(&MOVE_REPORT_BUFFER[i]); } bPollEnable = true; - Notify(PSTR("\r\n")); + Notify(PSTR("\r\n"), 0x80); timer = millis(); return 0; // successful configuration - - /* diagnostic messages */ + + /* diagnostic messages */ FailGetDevDescr: #ifdef DEBUG - Notify(PSTR("\r\ngetDevDescr:")); + Notify(PSTR("\r\ngetDevDescr:"), 0x80); #endif - goto Fail; + goto Fail; FailSetDevTblEntry: #ifdef DEBUG - Notify(PSTR("\r\nsetDevTblEn:")); + Notify(PSTR("\r\nsetDevTblEn:"), 0x80); #endif goto Fail; FailSetConf: #ifdef DEBUG - Notify(PSTR("\r\nsetConf:")); + Notify(PSTR("\r\nsetConf:"), 0x80); #endif - goto Fail; + goto Fail; FailUnknownDevice: #ifdef DEBUG - Notify(PSTR("\r\nUnknown Device Connected - VID: ")); - PrintHex(VID); - Notify(PSTR(" PID: ")); - PrintHex(PID); + Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80); + PrintHex(VID, 0x80); + Notify(PSTR(" PID: "), 0x80); + PrintHex(PID, 0x80); #endif rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; goto Fail; Fail: #ifdef DEBUG - Notify(PSTR("\r\nPS3 Init Failed, error code: ")); + Notify(PSTR("\r\nPS3 Init Failed, error code: "), 0x80); Serial.print(rcode,HEX); -#endif +#endif Release(); return rcode; } @@ -270,15 +270,15 @@ uint8_t PS3USB::Release() { PS3Connected = false; PS3MoveConnected = false; PS3NavigationConnected = false; - pUsb->GetAddressPool().FreeAddress(bAddress); + pUsb->GetAddressPool().FreeAddress(bAddress); bAddress = 0; bPollEnable = false; return 0; } -uint8_t PS3USB::Poll() { +uint8_t PS3USB::Poll() { if (!bPollEnable) return 0; - + if(PS3Connected || PS3NavigationConnected) { uint16_t BUFFER_SIZE = EP_MAXPKTSIZE; pUsb->inTransfer(bAddress, epInfo[ PS3_INPUT_PIPE ].epAddr, &BUFFER_SIZE, readBuf); // input on endpoint 1 @@ -289,25 +289,25 @@ uint8_t PS3USB::Poll() { #endif } } - else if(PS3MoveConnected) { // One can only set the color of the bulb, set the rumble, set and get the bluetooth address and calibrate the magnetometer via USB + else if(PS3MoveConnected) { // One can only set the color of the bulb, set the rumble, set and get the bluetooth address and calibrate the magnetometer via USB if (millis() - timer > 4000) // Send at least every 4th second { Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); // The Bulb and rumble values, has to be written again and again, for it to stay turned on timer = millis(); - } - } + } + } return 0; } -void PS3USB::readReport() { +void PS3USB::readReport() { if (readBuf == NULL) - return; + return; ButtonState = (uint32_t)(readBuf[2] | ((uint16_t)readBuf[3] << 8) | ((uint32_t)readBuf[4] << 16)); - - //Notify(PSTR("\r\nButtonState"); - //PrintHex(ButtonState); - + + //Notify(PSTR("\r\nButtonState", 0x80); + //PrintHex(ButtonState, 0x80); + if(ButtonState != OldButtonState) { ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable OldButtonState = ButtonState; @@ -319,9 +319,9 @@ void PS3USB::printReport() { //Uncomment "#define PRINTREPORT" to print the repo if (readBuf == NULL) return; for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE;i++) { - PrintHex(readBuf[i]); + PrintHex(readBuf[i], 0x80); Serial.print(" "); - } + } Serial.println(); #endif } @@ -342,7 +342,7 @@ uint8_t PS3USB::getAnalogButton(Button a) { } uint8_t PS3USB::getAnalogHat(AnalogHat a) { if (readBuf == NULL) - return 0; + return 0; return (uint8_t)(readBuf[((uint8_t)a+6)]); } uint16_t PS3USB::getSensor(Sensor a) { @@ -350,22 +350,22 @@ uint16_t PS3USB::getSensor(Sensor a) { return 0; return ((readBuf[((uint16_t)a)-9] << 8) | readBuf[((uint16_t)a + 1)-9]); } -double PS3USB::getAngle(Angle a) { +double PS3USB::getAngle(Angle a) { if(PS3Connected) { double accXval; double accYval; double accZval; - + // Data for the Kionix KXPC4 used in the DualShock 3 const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) accXval = -((double)getSensor(aX)-zeroG); accYval = -((double)getSensor(aY)-zeroG); accZval = -((double)getSensor(aZ)-zeroG); - + // Convert to 360 degrees resolution // atan2 outputs the value of -π to π (radians) - // We are then converting it to 0 to 2π and then to degrees - if (a == Pitch) { + // We are then converting it to 0 to 2π and then to degrees + if (a == Pitch) { double angle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG; return angle; } else { @@ -373,7 +373,7 @@ double PS3USB::getAngle(Angle a) { return angle; } } else - return 0; + return 0; } bool PS3USB::getStatus(Status c) { if (readBuf == NULL) @@ -385,16 +385,16 @@ bool PS3USB::getStatus(Status c) { String PS3USB::getStatusString() { if (PS3Connected || PS3NavigationConnected) { char statusOutput[100]; - + strcpy(statusOutput,"ConnectionStatus: "); - + if (getStatus(Plugged)) strcat(statusOutput,"Plugged"); else if (getStatus(Unplugged)) strcat(statusOutput,"Unplugged"); else strcat(statusOutput,"Error"); - - + + strcat(statusOutput," - PowerRating: "); - + if (getStatus(Charging)) strcat(statusOutput,"Charging"); else if (getStatus(NotCharging)) strcat(statusOutput,"Not Charging"); else if (getStatus(Shutdown)) strcat(statusOutput,"Shutdown"); @@ -403,16 +403,16 @@ String PS3USB::getStatusString() { else if (getStatus(High)) strcat(statusOutput,"High"); else if (getStatus(Full)) strcat(statusOutput,"Full"); else strcat(statusOutput,"Error"); - + strcat(statusOutput," - WirelessStatus: "); - + if (getStatus(CableRumble)) strcat(statusOutput,"Cable - Rumble is on"); else if (getStatus(Cable)) strcat(statusOutput,"Cable - Rumble is off"); else if (getStatus(BluetoothRumble)) strcat(statusOutput,"Bluetooth - Rumble is on"); else if (getStatus(Bluetooth)) strcat(statusOutput,"Bluetooth - Rumble is off"); else strcat(statusOutput,"Error"); - - return statusOutput; + + return statusOutput; } } @@ -424,7 +424,7 @@ void PS3USB::PS3_Command(uint8_t* data, uint16_t nbytes) { void PS3USB::setAllOff() { for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++) writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); // Reset buffer - + PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); } void PS3USB::setRumbleOff() { @@ -432,7 +432,7 @@ void PS3USB::setRumbleOff() { writeBuf[2] = 0x00;//low mode off writeBuf[3] = 0x00; writeBuf[4] = 0x00;//high mode off - + PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); } void PS3USB::setRumbleOn(Rumble mode) { @@ -444,7 +444,7 @@ void PS3USB::setRumbleOn(Rumble mode) { */ if ((mode & 0x30) > 0) { writeBuf[1] = 0xfe; - writeBuf[3] = 0xfe; + writeBuf[3] = 0xfe; if (mode == RumbleHigh) { writeBuf[2] = 0;//low mode off writeBuf[4] = 0xff;//high mode on @@ -468,23 +468,23 @@ void PS3USB::setLedToggle(LED a) { writeBuf[9] ^= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1); PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); } -void PS3USB::setBdaddr(uint8_t* BDADDR) { - /* Set the internal bluetooth address */ - uint8_t buf[8]; +void PS3USB::setBdaddr(uint8_t* BDADDR) { + /* Set the internal bluetooth address */ + uint8_t buf[8]; buf[0] = 0x01; buf[1] = 0x00; for (uint8_t i = 0; i < 6; i++) buf[i+2] = BDADDR[5 - i];//Copy into buffer, has to be written reversed - + //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) - pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL); + pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL); #ifdef DEBUG - Notify(PSTR("\r\nBluetooth Address was set to: ")); + Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80); for(int8_t i = 5; i > 0; i--) { - PrintHex(my_bdaddr[i]); + PrintHex(my_bdaddr[i], 0x80); Serial.print(":"); } - PrintHex(my_bdaddr[0]); + PrintHex(my_bdaddr[0], 0x80); #endif return; } @@ -494,7 +494,7 @@ void PS3USB::enable_sixaxis() { //Command used to enable the Dualshock 3 and Nav cmd_buf[1] = 0x0c; cmd_buf[2] = 0x00; cmd_buf[3] = 0x00; - + //bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0xF4), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data) pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF4, 0x03, 0x00, 4, 4, cmd_buf, NULL); } @@ -502,15 +502,15 @@ void PS3USB::enable_sixaxis() { //Command used to enable the Dualshock 3 and Nav /* Playstation Move Controller commands */ void PS3USB::Move_Command(uint8_t* data, uint16_t nbytes) { pUsb->outTransfer(bAddress, epInfo[ PS3_OUTPUT_PIPE ].epAddr, nbytes, data); -} +} void PS3USB::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { //Use this to set the Color using RGB values - // set the Bulb's values into the write buffer + // set the Bulb's values into the write buffer writeBuf[2] = r; writeBuf[3] = g; writeBuf[4] = b; - - Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); + + Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); } void PS3USB::moveSetBulb(Colors color) { //Use this to set the Color using the predefined colors in "enums.h" moveSetBulb((uint8_t)(color >> 16),(uint8_t)(color >> 8),(uint8_t)(color)); @@ -518,34 +518,34 @@ void PS3USB::moveSetBulb(Colors color) { //Use this to set the Color using the p void PS3USB::moveSetRumble(uint8_t rumble) { #ifdef DEBUG if(rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100) - Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%")); + Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80); #endif //set the rumble value into the write buffer writeBuf[6] = rumble; - - Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); + + Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); } void PS3USB::setMoveBdaddr(uint8_t* BDADDR) { - /* Set the internal bluetooth address */ + /* Set the internal bluetooth address */ uint8_t buf[11]; buf[0] = 0x05; buf[7] = 0x10; buf[8] = 0x01; buf[9] = 0x02; - buf[10] = 0x12; - + buf[10] = 0x12; + for (uint8_t i = 0; i < 6; i++) buf[i + 1] = BDADDR[i]; - + //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) - pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00,11,11, buf, NULL); + pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00,11,11, buf, NULL); #ifdef DEBUG - Notify(PSTR("\r\nBluetooth Address was set to: ")); + Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80); for(int8_t i = 5; i > 0; i--) { - PrintHex(my_bdaddr[i]); + PrintHex(my_bdaddr[i], 0x80); Serial.print(":"); } - PrintHex(my_bdaddr[0]); + PrintHex(my_bdaddr[0], 0x80); #endif return; } \ No newline at end of file diff --git a/SPP.cpp b/SPP.cpp index 62920555..538d2193 100644 --- a/SPP.cpp +++ b/SPP.cpp @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -47,16 +47,16 @@ pBtd(p) // Pointer to BTD class instance - mandatory { if (pBtd) pBtd->registerServiceClass(this); // Register it as a Bluetooth service - + pBtd->btdName = name; pBtd->btdPin = pin; - + /* Set device cid for the SDP and RFCOMM channelse */ sdp_dcid[0] = 0x50; // 0x0050 sdp_dcid[1] = 0x00; rfcomm_dcid[0] = 0x51; // 0x0051 rfcomm_dcid[1] = 0x00; - + Reset(); } void SPP::Reset() { @@ -65,7 +65,7 @@ void SPP::Reset() { SDPConnected = false; l2cap_sdp_state = L2CAP_SDP_WAIT; l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; - l2cap_event_flag = 0; + l2cap_event_flag = 0; } void SPP::disconnect(){ connected = false; @@ -96,31 +96,31 @@ void SPP::ACLData(uint8_t* l2capinbuf) { if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) { #ifdef DEBUG - Notify(PSTR("\r\nL2CAP Command Rejected - Reason: ")); - PrintHex(l2capinbuf[13]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[12]); - Notify(PSTR(" Data: ")); - PrintHex(l2capinbuf[17]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[16]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[15]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[14]); + Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80); + PrintHex(l2capinbuf[13], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[12], 0x80); + Notify(PSTR(" Data: "), 0x80); + PrintHex(l2capinbuf[17], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[16], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[15], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[14], 0x80); #endif } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { #ifdef EXTRADEBUG - Notify(PSTR("\r\nL2CAP Connection Request - PSM: ")); - PrintHex(l2capinbuf[13]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[12]); - Notify(PSTR(" SCID: ")); - PrintHex(l2capinbuf[15]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[14]); - Notify(PSTR(" Identifier: ")); - PrintHex(l2capinbuf[9]); + Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80); + PrintHex(l2capinbuf[13], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[12], 0x80); + Notify(PSTR(" SCID: "), 0x80); + PrintHex(l2capinbuf[15], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[14], 0x80); + Notify(PSTR(" Identifier: "), 0x80); + PrintHex(l2capinbuf[9], 0x80); #endif if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM) { // It doesn't matter if it receives another reqeust, since it waits for the channel to disconnect in the L2CAP_SDP_DONE state, and the l2cap_event_flag will be cleared if so identifier = l2capinbuf[9]; @@ -157,11 +157,11 @@ void SPP::ACLData(uint8_t* l2capinbuf) { } } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) { if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) { - //Notify(PSTR("\r\nDisconnect Request: SDP Channel")); + //Notify(PSTR("\r\nDisconnect Request: SDP Channel"), 0x80); identifier = l2capinbuf[9]; l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_SDP_REQUEST; } else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) { - //Notify(PSTR("\r\nDisconnect Request: RFCOMM Channel")); + //Notify(PSTR("\r\nDisconnect Request: RFCOMM Channel"), 0x80); identifier = l2capinbuf[9]; l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST; } @@ -177,15 +177,15 @@ void SPP::ACLData(uint8_t* l2capinbuf) { } } else if (l2capinbuf[8] == L2CAP_CMD_INFORMATION_REQUEST) { #ifdef DEBUG - Notify(PSTR("\r\nInformation request")); + Notify(PSTR("\r\nInformation request"), 0x80); #endif identifier = l2capinbuf[9]; pBtd->l2cap_information_response(hci_handle,identifier,l2capinbuf[12],l2capinbuf[13]); } #ifdef EXTRADEBUG else { - Notify(PSTR("\r\nL2CAP Unknown Signaling Command: ")); - PrintHex(l2capinbuf[8]); + Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80); + PrintHex(l2capinbuf[8], 0x80); } #endif } else if (l2capinbuf[6] == sdp_dcid[0] && l2capinbuf[7] == sdp_dcid[1]) { // SDP @@ -221,25 +221,25 @@ void SPP::ACLData(uint8_t* l2capinbuf) { rfcommCommandResponse = l2capinbuf[8] & 0x02; rfcommChannelType = l2capinbuf[9] & 0xEF; rfcommPfBit = l2capinbuf[9] & 0x10; - + if(rfcommChannel>>3 != 0x00) rfcommChannelConnection = rfcommChannel; - + #ifdef EXTRADEBUG - Notify(PSTR("\r\nRFCOMM Channel: ")); + Notify(PSTR("\r\nRFCOMM Channel: "), 0x80); Serial.print(rfcommChannel>>3,HEX); - Notify(PSTR(" Direction: ")); + Notify(PSTR(" Direction: "), 0x80); Serial.print(rfcommDirection>>2,HEX); - Notify(PSTR(" CommandResponse: ")); + Notify(PSTR(" CommandResponse: "), 0x80); Serial.print(rfcommCommandResponse>>1,HEX); - Notify(PSTR(" ChannelType: ")); + Notify(PSTR(" ChannelType: "), 0x80); Serial.print(rfcommChannelType,HEX); - Notify(PSTR(" PF_BIT: ")); + Notify(PSTR(" PF_BIT: "), 0x80); Serial.print(rfcommPfBit,HEX); #endif if (rfcommChannelType == RFCOMM_DISC) { #ifdef DEBUG - Notify(PSTR("\r\nReceived Disconnect RFCOMM Command on channel: ")); + Notify(PSTR("\r\nReceived Disconnect RFCOMM Command on channel: "), 0x80); Serial.print(rfcommChannel>>3,HEX); #endif connected = false; @@ -256,10 +256,10 @@ void SPP::ACLData(uint8_t* l2capinbuf) { rfcommAvailable += length; } #ifdef EXTRADEBUG - Notify(PSTR("\r\nRFCOMM Data Available: ")); + Notify(PSTR("\r\nRFCOMM Data Available: "), 0x80); Serial.print(rfcommAvailable); if (offset) { - Notify(PSTR(" - Credit: 0x")); + Notify(PSTR(" - Credit: 0x"), 0x80); Serial.print(l2capinbuf[11],HEX); } #endif @@ -269,7 +269,7 @@ void SPP::ACLData(uint8_t* l2capinbuf) { #endif } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command #ifdef DEBUG - Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command")); + Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80); #endif rfcommbuf[0] = BT_RFCOMM_RPN_RSP; // Command rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1 @@ -284,7 +284,7 @@ void SPP::ACLData(uint8_t* l2capinbuf) { sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x0A); // UIH Remote Port Negotiation Response } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command #ifdef DEBUG - Notify(PSTR("\r\nSend UIH Modem Status Response")); + Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80); #endif rfcommbuf[0] = BT_RFCOMM_MSC_RSP; // UIH Modem Status Response rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1 @@ -295,12 +295,12 @@ void SPP::ACLData(uint8_t* l2capinbuf) { } else { if(rfcommChannelType == RFCOMM_SABM) { // SABM Command - this is sent twice: once for channel 0 and then for the channel to establish #ifdef DEBUG - Notify(PSTR("\r\nReceived SABM Command")); -#endif + Notify(PSTR("\r\nReceived SABM Command"), 0x80); +#endif sendRfcomm(rfcommChannel,rfcommDirection,rfcommCommandResponse,RFCOMM_UA,rfcommPfBit,rfcommbuf,0x00); // UA Command } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_PN_CMD) { // UIH Parameter Negotiation Command #ifdef DEBUG - Notify(PSTR("\r\nReceived UIH Parameter Negotiation Command")); + Notify(PSTR("\r\nReceived UIH Parameter Negotiation Command"), 0x80); #endif rfcommbuf[0] = BT_RFCOMM_PN_RSP; // UIH Parameter Negotiation Response rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1 @@ -315,28 +315,28 @@ void SPP::ACLData(uint8_t* l2capinbuf) { sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x0A); } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command #ifdef DEBUG - Notify(PSTR("\r\nSend UIH Modem Status Response")); + Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80); #endif rfcommbuf[0] = BT_RFCOMM_MSC_RSP; // UIH Modem Status Response rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1 rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3) rfcommbuf[3] = l2capinbuf[14]; sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x04); - + delay(1); #ifdef DEBUG - Notify(PSTR("\r\nSend UIH Modem Status Command")); + Notify(PSTR("\r\nSend UIH Modem Status Command"), 0x80); #endif rfcommbuf[0] = BT_RFCOMM_MSC_CMD; // UIH Modem Status Command rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1 rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3) rfcommbuf[3] = 0x8D; // Can receive frames (YES), Ready to Communicate (YES), Ready to Receive (YES), Incomig Call (NO), Data is Value (YES) - + sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x04); } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_RSP) { // UIH Modem Status Response if(!creditSent) { #ifdef DEBUG - Notify(PSTR("\r\nSend UIH Command with credit")); + Notify(PSTR("\r\nSend UIH Command with credit"), 0x80); #endif sendRfcommCredit(rfcommChannelConnection,rfcommDirection,0,RFCOMM_UIH,0x10,sizeof(rfcommDataBuffer)); // Send credit creditSent = true; @@ -345,11 +345,11 @@ void SPP::ACLData(uint8_t* l2capinbuf) { } } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[10] == 0x01) { // UIH Command with credit #ifdef DEBUG - Notify(PSTR("\r\nReceived UIH Command with credit")); -#endif + Notify(PSTR("\r\nReceived UIH Command with credit"), 0x80); +#endif } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command #ifdef DEBUG - Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command")); + Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80); #endif rfcommbuf[0] = BT_RFCOMM_RPN_RSP; // Command rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1 @@ -363,28 +363,28 @@ void SPP::ACLData(uint8_t* l2capinbuf) { rfcommbuf[9] = l2capinbuf[20]; // Number of Frames sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x0A); // UIH Remote Port Negotiation Response #ifdef DEBUG - Notify(PSTR("\r\nRFCOMM Connection is now established\r\n")); -#endif + Notify(PSTR("\r\nRFCOMM Connection is now established\r\n"), 0x80); +#endif waitForLastCommand = false; creditSent = false; connected = true; // The RFCOMM channel is now established } #ifdef DEBUG else if(rfcommChannelType != RFCOMM_DISC) { - Notify(PSTR("\r\nUnsupported RFCOMM Data - ChannelType: ")); - PrintHex(rfcommChannelType); - Notify(PSTR(" Command: ")); - PrintHex(l2capinbuf[11]); + Notify(PSTR("\r\nUnsupported RFCOMM Data - ChannelType: "), 0x80); + PrintHex(rfcommChannelType, 0x80); + Notify(PSTR(" Command: "), 0x80); + PrintHex(l2capinbuf[11], 0x80); } -#endif +#endif } } #ifdef EXTRADEBUG else { - Notify(PSTR("\r\nUnsupported L2CAP Data - Channel ID: ")); - PrintHex(l2capinbuf[7]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[6]); + Notify(PSTR("\r\nUnsupported L2CAP Data - Channel ID: "), 0x80); + PrintHex(l2capinbuf[7], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[6], 0x80); } #endif SDP_task(); @@ -394,12 +394,12 @@ void SPP::ACLData(uint8_t* l2capinbuf) { void SPP::Run() { if(waitForLastCommand && (millis() - timer) > 100) { // We will only wait 100ms and see if the UIH Remote Port Negotiation Command is send, as some deviced don't send it #ifdef DEBUG - Notify(PSTR("\r\nRFCOMM Connection is now established - Automatic\r\n")); + Notify(PSTR("\r\nRFCOMM Connection is now established - Automatic\r\n"), 0x80); #endif creditSent = false; waitForLastCommand = false; connected = true; // The RFCOMM channel is now established - } + } } void SPP::SDP_task() { switch (l2cap_sdp_state) @@ -408,24 +408,24 @@ void SPP::SDP_task() { if (l2cap_connection_request_sdp_flag) { l2cap_event_flag &= ~L2CAP_FLAG_CONNECTION_SDP_REQUEST; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nSDP Incoming Connection Request")); + Notify(PSTR("\r\nSDP Incoming Connection Request"), 0x80); #endif pBtd->l2cap_connection_response(hci_handle,identifier, sdp_dcid, sdp_scid, PENDING); delay(1); - pBtd->l2cap_connection_response(hci_handle,identifier, sdp_dcid, sdp_scid, SUCCESSFUL); + pBtd->l2cap_connection_response(hci_handle,identifier, sdp_dcid, sdp_scid, SUCCESSFUL); identifier++; delay(1); - pBtd->l2cap_config_request(hci_handle,identifier, sdp_scid); - l2cap_sdp_state = L2CAP_SDP_REQUEST; - } + pBtd->l2cap_config_request(hci_handle,identifier, sdp_scid); + l2cap_sdp_state = L2CAP_SDP_REQUEST; + } break; case L2CAP_SDP_REQUEST: if (l2cap_config_request_sdp_flag) { l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_SDP_REQUEST; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nSDP Configuration Request")); -#endif - pBtd->l2cap_config_response(hci_handle,identifier, sdp_scid); + Notify(PSTR("\r\nSDP Configuration Request"), 0x80); +#endif + pBtd->l2cap_config_response(hci_handle,identifier, sdp_scid); l2cap_sdp_state = L2CAP_SDP_SUCCESS; } break; @@ -433,7 +433,7 @@ void SPP::SDP_task() { if (l2cap_config_success_sdp_flag) { l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_SDP_SUCCESS; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nSDP Successfully Configured")); + Notify(PSTR("\r\nSDP Successfully Configured"), 0x80); #endif firstMessage = true; // Reset bool SDPConnected = true; @@ -445,9 +445,9 @@ void SPP::SDP_task() { l2cap_event_flag &= ~L2CAP_FLAG_DISCONNECT_SDP_REQUEST; // Clear flag SDPConnected = false; #ifdef DEBUG - Notify(PSTR("\r\nDisconnected SDP Channel")); + Notify(PSTR("\r\nDisconnected SDP Channel"), 0x80); #endif - pBtd->l2cap_disconnection_response(hci_handle,identifier,sdp_dcid,sdp_scid); + pBtd->l2cap_disconnection_response(hci_handle,identifier,sdp_dcid,sdp_scid); l2cap_sdp_state = L2CAP_SDP_WAIT; } else if(l2cap_connection_request_sdp_flag) l2cap_rfcomm_state = L2CAP_SDP_WAIT; @@ -455,7 +455,7 @@ void SPP::SDP_task() { case L2CAP_DISCONNECT_RESPONSE: // This is for both disconnection response from the RFCOMM and SDP channel if they were connected if (l2cap_disconnect_response_flag) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnected L2CAP Connection")); + Notify(PSTR("\r\nDisconnected L2CAP Connection"), 0x80); #endif RFCOMMConnected = false; SDPConnected = false; @@ -465,35 +465,35 @@ void SPP::SDP_task() { l2cap_sdp_state = L2CAP_SDP_WAIT; l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; } - break; + break; } } void SPP::RFCOMM_task() -{ +{ switch (l2cap_rfcomm_state) { - case L2CAP_RFCOMM_WAIT: + case L2CAP_RFCOMM_WAIT: if(l2cap_connection_request_rfcomm_flag) { l2cap_event_flag &= ~L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nRFCOMM Incoming Connection Request")); + Notify(PSTR("\r\nRFCOMM Incoming Connection Request"), 0x80); #endif - pBtd->l2cap_connection_response(hci_handle,identifier, rfcomm_dcid, rfcomm_scid, PENDING); + pBtd->l2cap_connection_response(hci_handle,identifier, rfcomm_dcid, rfcomm_scid, PENDING); delay(1); - pBtd->l2cap_connection_response(hci_handle,identifier, rfcomm_dcid, rfcomm_scid, SUCCESSFUL); + pBtd->l2cap_connection_response(hci_handle,identifier, rfcomm_dcid, rfcomm_scid, SUCCESSFUL); identifier++; delay(1); - pBtd->l2cap_config_request(hci_handle,identifier, rfcomm_scid); - l2cap_rfcomm_state = L2CAP_RFCOMM_REQUEST; - } - break; + pBtd->l2cap_config_request(hci_handle,identifier, rfcomm_scid); + l2cap_rfcomm_state = L2CAP_RFCOMM_REQUEST; + } + break; case L2CAP_RFCOMM_REQUEST: if (l2cap_config_request_rfcomm_flag) { l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_RFCOMM_REQUEST; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nRFCOMM Configuration Request")); + Notify(PSTR("\r\nRFCOMM Configuration Request"), 0x80); #endif - pBtd->l2cap_config_response(hci_handle,identifier, rfcomm_scid); + pBtd->l2cap_config_response(hci_handle,identifier, rfcomm_scid); l2cap_rfcomm_state = L2CAP_RFCOMM_SUCCESS; } break; @@ -501,27 +501,27 @@ void SPP::RFCOMM_task() if (l2cap_config_success_rfcomm_flag) { l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nRFCOMM Successfully Configured")); -#endif + Notify(PSTR("\r\nRFCOMM Successfully Configured"), 0x80); +#endif rfcommAvailable = 0; // Reset number of bytes available bytesRead = 0; // Reset number of bytes received RFCOMMConnected = true; l2cap_rfcomm_state = L2CAP_RFCOMM_DONE; } - break; + break; case L2CAP_RFCOMM_DONE: if(l2cap_disconnect_request_rfcomm_flag) { l2cap_event_flag &= ~L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST; // Clear flag RFCOMMConnected = false; connected = false; #ifdef DEBUG - Notify(PSTR("\r\nDisconnected RFCOMM Channel")); + Notify(PSTR("\r\nDisconnected RFCOMM Channel"), 0x80); #endif pBtd->l2cap_disconnection_response(hci_handle,identifier,rfcomm_dcid,rfcomm_scid); l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; } else if(l2cap_connection_request_rfcomm_flag) l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; - break; + break; } } /************************************************************/ @@ -538,12 +538,12 @@ void SPP::serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLo l2capoutbuf[4] = 0x05; // Parameter Length l2capoutbuf[5] = 0x00; // AttributeListsByteCount l2capoutbuf[6] = 0x02; // AttributeListsByteCount - + /* Attribute ID/Value Sequence: */ l2capoutbuf[7] = 0x35; l2capoutbuf[8] = 0x00; l2capoutbuf[9] = 0x00; - + SDP_Command(l2capoutbuf,10); } void SPP::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow) { @@ -561,7 +561,7 @@ void SPP::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLo l2capoutbuf[9] = 0x3C; l2capoutbuf[10] = 0x36; l2capoutbuf[11] = 0x00; - + l2capoutbuf[12] = 0x39; l2capoutbuf[13] = 0x09; l2capoutbuf[14] = 0x00; @@ -578,7 +578,7 @@ void SPP::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLo l2capoutbuf[25] = 0x03; l2capoutbuf[26] = 0x19; l2capoutbuf[27] = 0x11; - + l2capoutbuf[28] = 0x01; l2capoutbuf[29] = 0x09; l2capoutbuf[30] = 0x00; @@ -595,13 +595,13 @@ void SPP::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLo l2capoutbuf[41] = 0x19; l2capoutbuf[42] = 0x00; l2capoutbuf[43] = 0x03; - + l2capoutbuf[44] = 0x08; l2capoutbuf[45] = 0x02; // Two extra bytes l2capoutbuf[46] = 0x00; // 25 (0x19) more bytes to come - l2capoutbuf[47] = 0x19; - - SDP_Command(l2capoutbuf,48); + l2capoutbuf[47] = 0x19; + + SDP_Command(l2capoutbuf,48); } void SPP::serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow) { l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU; @@ -611,14 +611,14 @@ void SPP::serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLo l2capoutbuf[4] = 0x1C; // Parameter Length l2capoutbuf[5] = 0x00; // AttributeListsByteCount l2capoutbuf[6] = 0x19; // AttributeListsByteCount - + /* Attribute ID/Value Sequence: */ l2capoutbuf[7] = 0x01; l2capoutbuf[8] = 0x09; l2capoutbuf[9] = 0x00; l2capoutbuf[10] = 0x06; l2capoutbuf[11] = 0x35; - + l2capoutbuf[12] = 0x09; l2capoutbuf[13] = 0x09; l2capoutbuf[14] = 0x65; @@ -634,7 +634,7 @@ void SPP::serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLo l2capoutbuf[24] = 0x00; l2capoutbuf[25] = 0x25; - l2capoutbuf[26] = 0x05; // Name length + l2capoutbuf[26] = 0x05; // Name length l2capoutbuf[27] = 'T'; l2capoutbuf[28] = 'K'; l2capoutbuf[29] = 'J'; @@ -648,7 +648,7 @@ void SPP::l2capResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow) { serialPortResponse1(transactionIDHigh,transactionIDLow); // These has to send all the supported functions, since it only supports virtual serialport it just sends the message again } void SPP::l2capResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow) { - serialPortResponse2(transactionIDHigh,transactionIDLow); // Same data as serialPortResponse2 + serialPortResponse2(transactionIDHigh,transactionIDLow); // Same data as serialPortResponse2 } /************************************************************/ /* RFCOMM Commands */ @@ -666,26 +666,26 @@ void SPP::sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t cha l2capoutbuf[i+3] = data[i]; l2capoutbuf[i+3] = calcFcs(l2capoutbuf); #ifdef EXTRADEBUG - Notify(PSTR(" - RFCOMM Data: ")); + Notify(PSTR(" - RFCOMM Data: "), 0x80); for(i = 0; i < length+4; i++) { Serial.print(l2capoutbuf[i],HEX); - Notify(PSTR(" ")); + Notify(PSTR(" "), 0x80); } -#endif +#endif RFCOMM_Command(l2capoutbuf,length+4); -} +} void SPP::sendRfcommCredit(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t credit) { l2capoutbuf[0] = channel | direction | CR | extendAddress; // RFCOMM Address l2capoutbuf[1] = channelType | pfBit; // RFCOMM Control l2capoutbuf[2] = 0x01; // Length = 0 l2capoutbuf[3] = credit; // Credit - l2capoutbuf[4] = calcFcs(l2capoutbuf); + l2capoutbuf[4] = calcFcs(l2capoutbuf); #ifdef EXTRADEBUG - Notify(PSTR(" - RFCOMM Credit Data: ")); + Notify(PSTR(" - RFCOMM Credit Data: "), 0x80); for(uint8_t i = 0; i < 5; i++) { Serial.print(l2capoutbuf[i],HEX); - Notify(PSTR(" ")); + Notify(PSTR(" "), 0x80); } #endif RFCOMM_Command(l2capoutbuf,5); @@ -714,11 +714,11 @@ void SPP::print(const String &str) { l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress;; // RFCOMM Address l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control l2capoutbuf[2] = length << 1 | 1; // Length - uint8_t i = 0; + uint8_t i = 0; for(; i < length; i++) l2capoutbuf[i+3] = str[i]; l2capoutbuf[i+3] = calcFcs(l2capoutbuf); - + RFCOMM_Command(l2capoutbuf,length+4); } void SPP::print(const char* str) { @@ -728,13 +728,13 @@ void SPP::print(const char* str) { if(length > (sizeof(l2capoutbuf)-4)) length = sizeof(l2capoutbuf)-4; l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress;; // RFCOMM Address - l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control + l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control l2capoutbuf[2] = length << 1 | 1; // Length uint8_t i = 0; for(; i < length; i++) - l2capoutbuf[i+3] = str[i]; - l2capoutbuf[i+3] = calcFcs(l2capoutbuf); - + l2capoutbuf[i+3] = str[i]; + l2capoutbuf[i+3] = calcFcs(l2capoutbuf); + RFCOMM_Command(l2capoutbuf,length+4); } void SPP::print(uint8_t* array, uint8_t length) { @@ -749,7 +749,7 @@ void SPP::print(uint8_t* array, uint8_t length) { for(; i < length; i++) l2capoutbuf[i+3] = array[i]; l2capoutbuf[i+3] = calcFcs(l2capoutbuf); - + RFCOMM_Command(l2capoutbuf,length+4); } void SPP::println(const String &str) { @@ -783,10 +783,10 @@ void SPP::printFlashString(const __FlashStringHelper *ifsh, bool newline) { size++; } uint8_t buf[size+2]; // Add two extra in case it needs to print a newline and carriage return - + for(uint8_t i = 0; i < size; i++) buf[i] = pgm_read_byte(p++); - + if(newline) { buf[size] = '\r'; buf[size+1] = '\n'; @@ -868,13 +868,13 @@ void SPP::doubleToString(double input, char* output, uint8_t digits) { } else strcpy(output,""); - + // Round correctly double rounding = 0.5; for (uint8_t i=0; iregisterServiceClass(this); // Register it as a Bluetooth service - + pBtd->pairWithWii = pair; - + HIDBuffer[0] = 0xA2;// HID BT DATA_request (0xA0) | Report Type (Output 0x02) - + /* Set device cid for the control and intterrupt channelse - LSB */ control_dcid[0] = 0x60;//0x0060 control_dcid[1] = 0x00; interrupt_dcid[0] = 0x61;//0x0061 interrupt_dcid[1] = 0x00; - + Reset(); } void WII::Reset() { @@ -117,10 +117,10 @@ void WII::Reset() { void WII::disconnect() { // Use this void to disconnect any of the controllers if(motionPlusConnected && !pBtd->motionPlusInside) { // Disable the Motion Plus extension -#ifdef DEBUG - Notify(PSTR("\r\nDeactivating Motion Plus")); +#ifdef DEBUG + Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80); #endif - initExtension1(); // This will disable the Motion Plus extension + initExtension1(); // This will disable the Motion Plus extension } //First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection pBtd->l2cap_disconnection_request(hci_handle,0x0A, interrupt_scid, interrupt_dcid); @@ -144,18 +144,18 @@ void WII::ACLData(uint8_t* l2capinbuf) { if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) { #ifdef DEBUG - Notify(PSTR("\r\nL2CAP Command Rejected - Reason: ")); - PrintHex(l2capinbuf[13]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[12]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[17]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[16]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[15]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[14]); + Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80); + PrintHex(l2capinbuf[13], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[12], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[17], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[16], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[15], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[14], 0x80); #endif } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) { @@ -175,19 +175,19 @@ void WII::ACLData(uint8_t* l2capinbuf) { l2cap_event_flag |= L2CAP_FLAG_INTERRUPT_CONNECTED; } } - } + } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { #ifdef EXTRADEBUG - Notify(PSTR("\r\nL2CAP Connection Request - PSM: ")); - PrintHex(l2capinbuf[13]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[12]); - Notify(PSTR(" SCID: ")); - PrintHex(l2capinbuf[15]); - Notify(PSTR(" ")); - PrintHex(l2capinbuf[14]); - Notify(PSTR(" Identifier: ")); - PrintHex(l2capinbuf[9]); + Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80); + PrintHex(l2capinbuf[13], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[12], 0x80); + Notify(PSTR(" SCID: "), 0x80); + PrintHex(l2capinbuf[15], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex(l2capinbuf[14], 0x80); + Notify(PSTR(" Identifier: "), 0x80); + PrintHex(l2capinbuf[9], 0x80); #endif if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) { identifier = l2capinbuf[9]; @@ -211,7 +211,7 @@ void WII::ACLData(uint8_t* l2capinbuf) { } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { //Serial.print("\r\nHID Interrupt Configuration Complete"); - identifier = l2capinbuf[9]; + identifier = l2capinbuf[9]; l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS; } } @@ -229,18 +229,18 @@ void WII::ACLData(uint8_t* l2capinbuf) { else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) { if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnect Request: Control Channel")); + Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80); #endif identifier = l2capinbuf[9]; - pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid); + pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid); Reset(); } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnect Request: Interrupt Channel")); + Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80); #endif identifier = l2capinbuf[9]; - pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid); + pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid); Reset(); } } @@ -259,8 +259,8 @@ void WII::ACLData(uint8_t* l2capinbuf) { #ifdef EXTRADEBUG else { identifier = l2capinbuf[9]; - Notify(PSTR("\r\nL2CAP Unknown Signaling Command: ")); - PrintHex(l2capinbuf[8]); + Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80); + PrintHex(l2capinbuf[8], 0x80); } #endif } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt @@ -285,11 +285,11 @@ void WII::ACLData(uint8_t* l2capinbuf) { else if(!unknownExtensionConnected) ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); #ifdef PRINTREPORT - Notify(PSTR("ButtonState: ")); - PrintHex(ButtonState); - Notify(PSTR("\r\n")); + Notify(PSTR("ButtonState: "), 0x80); + PrintHex(ButtonState, 0x80); + Notify(PSTR("\r\n"), 0x80); #endif - if(ButtonState != OldButtonState) { + if(ButtonState != OldButtonState) { ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable OldButtonState = ButtonState; } @@ -297,7 +297,7 @@ void WII::ACLData(uint8_t* l2capinbuf) { if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5))-500; accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4))-500; - accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500; + accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500; wiimotePitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; wiimoteRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; } @@ -307,27 +307,27 @@ void WII::ACLData(uint8_t* l2capinbuf) { batteryLevel = l2capinbuf[15]; // Update battery level if(l2capinbuf[12] & 0x01) { #ifdef DEBUG - Notify(PSTR("\r\nWARNING: Battery is nearly empty")); -#endif + Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80); +#endif } if(l2capinbuf[12] & 0x02) { // Check if a extension is connected #ifdef DEBUG if(!unknownExtensionConnected) - Notify(PSTR("\r\nExtension connected")); + Notify(PSTR("\r\nExtension connected"), 0x80); #endif unknownExtensionConnected = true; #ifdef WIICAMERA if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera -#endif +#endif setReportMode(false,0x35); // Also read the extension } else { #ifdef DEBUG - Notify(PSTR("\r\nExtension disconnected")); + Notify(PSTR("\r\nExtension disconnected"), 0x80); #endif if(motionPlusConnected) { #ifdef DEBUG - Notify(PSTR(" - from Motion Plus")); + Notify(PSTR(" - from Motion Plus"), 0x80); #endif l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED; if(!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false @@ -336,82 +336,82 @@ void WII::ACLData(uint8_t* l2capinbuf) { } else if(nunchuckConnected) { #ifdef DEBUG - Notify(PSTR(" - Nunchuck")); + Notify(PSTR(" - Nunchuck"), 0x80); #endif nunchuckConnected = false; // It must be the Nunchuck controller then l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED; setLedStatus(); setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer } else { - setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer + setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer } } break; case 0x21: // Read Memory Data if((l2capinbuf[12] & 0x0F) == 0) { // No error // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers - if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { + if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { #ifdef DEBUG - Notify(PSTR("\r\nNunchuck connected")); + Notify(PSTR("\r\nNunchuck connected"), 0x80); #endif - l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED; + l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED; } else if(l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) { #ifdef DEBUG - Notify(PSTR("\r\nMotion Plus connected")); + Notify(PSTR("\r\nMotion Plus connected"), 0x80); #endif l2cap_event_flag |= WII_FLAG_MOTION_PLUS_CONNECTED; } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) { #ifdef DEBUG - Notify(PSTR("\r\nMotion Plus activated in normal mode")); + Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80); #endif motionPlusConnected = true; } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) { #ifdef DEBUG - Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode")); + Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80); #endif activateNunchuck = false; motionPlusConnected = true; nunchuckConnected = true; } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && (l2capinbuf[19] == 0x00 || l2capinbuf[19] == 0x04 || l2capinbuf[19] == 0x05 || l2capinbuf[19] == 0x07) && l2capinbuf[20] == 0x05) { #ifdef DEBUG - Notify(PSTR("\r\nInactive Wii Motion Plus")); - Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension")); + Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80); + Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"), 0x80); #endif stateCounter = 300; // Skip the rest in "L2CAP_CHECK_MOTION_PLUS_STATE" } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) { #ifdef DEBUG - Notify(PSTR("\r\nWii U Pro Controller connected")); + Notify(PSTR("\r\nWii U Pro Controller connected"), 0x80); #endif wiiUProControllerConnected = true; } #ifdef DEBUG else { - Notify(PSTR("\r\nUnknown Device: ")); - PrintHex(l2capinbuf[13]); - PrintHex(l2capinbuf[14]); - Notify(PSTR("\r\nData: ")); + Notify(PSTR("\r\nUnknown Device: "), 0x80); + PrintHex(l2capinbuf[13], 0x80); + PrintHex(l2capinbuf[14], 0x80); + Notify(PSTR("\r\nData: "), 0x80); for(uint8_t i = 0; i < ((l2capinbuf[12] >> 4)+1); i++) { // bit 4-7 is the length-1 - PrintHex(l2capinbuf[15+i]); - Notify(PSTR(" ")); + PrintHex(l2capinbuf[15+i], 0x80); + Notify(PSTR(" "), 0x80); } } #endif } #ifdef EXTRADEBUG else { - Notify(PSTR("\r\nReport Error: ")); - PrintHex(l2capinbuf[13]); - PrintHex(l2capinbuf[14]); + Notify(PSTR("\r\nReport Error: "), 0x80); + PrintHex(l2capinbuf[13], 0x80); + PrintHex(l2capinbuf[14], 0x80); } #endif break; case 0x22: // Acknowledge output report, return function result #ifdef DEBUG if(l2capinbuf[13] != 0x00) { // Check if there is an error - Notify(PSTR("\r\nCommand failed: ")); - PrintHex(l2capinbuf[12]); + Notify(PSTR("\r\nCommand failed: "), 0x80); + PrintHex(l2capinbuf[12], 0x80); } -#endif +#endif break; case 0x30: // Core buttons - (a1) 30 BB BB break; @@ -425,7 +425,7 @@ void WII::ACLData(uint8_t* l2capinbuf) { pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus data available roll = wiimoteRoll; #ifdef WIICAMERA - // Read the IR data + // Read the IR data IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position IR_object_y1 = (l2capinbuf[16] | ((uint16_t)(l2capinbuf[17] & 0xC0) << 2)); // y position IR_object_s1 = (l2capinbuf[17] & 0x0F); // size value, 0-15 @@ -447,7 +447,7 @@ void WII::ACLData(uint8_t* l2capinbuf) { break; /* 0x3e and 0x3f both give unknown report types when report mode is 0x3e or 0x3f with mode number 0x05 */ case 0x3E: // Core Buttons with Accelerometer and 32 IR bytes - // (a1) 31 BB BB AA AA AA II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II + // (a1) 31 BB BB AA AA AA II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II // corresponds to output report mode 0x3e /**** for reading in full mode: DOES NOT WORK YET ****/ @@ -456,7 +456,7 @@ void WII::ACLData(uint8_t* l2capinbuf) { IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4)); IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2)); IR_object_s1 = (l2capinbuf[15] & 0x0F); - */ + */ break; case 0x3F: /* @@ -466,7 +466,7 @@ void WII::ACLData(uint8_t* l2capinbuf) { */ break; case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes - // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE + // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE if(motionPlusConnected) { if(l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension if(motionValuesReset) { // We will only use the values when the gyro value has been set @@ -477,7 +477,7 @@ void WII::ACLData(uint8_t* l2capinbuf) { yawGyroSpeed = (double)gyroYawRaw/((double)gyroYawZero/yawGyroScale); rollGyroSpeed = -(double)gyroRollRaw/((double)gyroRollZero/rollGyroScale); // We invert these values so they will fit the acc values pitchGyroSpeed = (double)gyroPitchRaw/((double)gyroPitchZero/pitchGyroScale); - + /* The onboard gyro has two ranges for slow and fast mode */ if(!(l2capinbuf[18] & 0x02)) // Check if fast more is used yawGyroSpeed *= 4.545; @@ -485,10 +485,10 @@ void WII::ACLData(uint8_t* l2capinbuf) { pitchGyroSpeed *= 4.545; if(!(l2capinbuf[19] & 0x02)) // Check if fast more is used rollGyroSpeed *= 4.545; - + pitch = (0.93*(pitch+(pitchGyroSpeed*(double)(micros()-timer)/1000000)))+(0.07*wiimotePitch); // Use a complimentary filter to calculate the angle roll = (0.93*(roll+(rollGyroSpeed*(double)(micros()-timer)/1000000)))+(0.07*wiimoteRoll); - + gyroYaw += (yawGyroSpeed*((double)(micros()-timer)/1000000)); gyroRoll += (rollGyroSpeed*((double)(micros()-timer)/1000000)); gyroPitch += (pitchGyroSpeed*((double)(micros()-timer)/1000000)); @@ -503,20 +503,20 @@ void WII::ACLData(uint8_t* l2capinbuf) { Serial.print(gyroPitch); */ /* - Serial.print("\twiimoteRoll: "); + Serial.print("\twiimoteRoll: "); Serial.print(wiimoteRoll); Serial.print("\twiimotePitch: "); Serial.print(wiimotePitch); - */ + */ } else { if((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values #ifdef DEBUG - Notify(PSTR("\r\nThe gyro values has been reset")); + Notify(PSTR("\r\nThe gyro values has been reset"), 0x80); #endif gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)); gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)); gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)); - + rollGyroScale = 500; // You might need to adjust these pitchGyroScale = 400; yawGyroScale = 415; @@ -524,10 +524,10 @@ void WII::ACLData(uint8_t* l2capinbuf) { gyroYaw = 0; gyroRoll = 0; gyroPitch = 0; - + motionValuesReset = true; timer = micros(); - } + } } } else { if(nunchuckConnected) { @@ -539,14 +539,14 @@ void WII::ACLData(uint8_t* l2capinbuf) { nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; } - //else if(classicControllerConnected) { } + //else if(classicControllerConnected) { } } if(l2capinbuf[19] & 0x01) { if(!extensionConnected) { extensionConnected = true; unknownExtensionConnected = true; #ifdef DEBUG - Notify(PSTR("\r\nExtension connected to Motion Plus")); + Notify(PSTR("\r\nExtension connected to Motion Plus"), 0x80); #endif } } @@ -555,12 +555,12 @@ void WII::ACLData(uint8_t* l2capinbuf) { extensionConnected = false; unknownExtensionConnected = true; #ifdef DEBUG - Notify(PSTR("\r\nExtension disconnected from Motion Plus")); + Notify(PSTR("\r\nExtension disconnected from Motion Plus"), 0x80); #endif nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent } } - + } else if(nunchuckConnected) { hatValues[HatX] = l2capinbuf[15]; hatValues[HatY] = l2capinbuf[16]; @@ -569,7 +569,7 @@ void WII::ACLData(uint8_t* l2capinbuf) { accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6))-416; nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; - + pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected roll = wiimoteRoll; } else if(wiiUProControllerConnected) { @@ -581,11 +581,11 @@ void WII::ACLData(uint8_t* l2capinbuf) { break; #ifdef DEBUG default: - Notify(PSTR("\r\nUnknown Report type: ")); + Notify(PSTR("\r\nUnknown Report type: "), 0x80); Serial.print(l2capinbuf[9],HEX); break; #endif - } + } } } } @@ -598,16 +598,16 @@ void WII::L2CAP_task() { case L2CAP_CONTROL_SUCCESS: if (l2cap_config_success_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Control Successfully Configured")); + Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80); #endif l2cap_state = L2CAP_INTERRUPT_SETUP; } break; - + case L2CAP_INTERRUPT_SETUP: if (l2cap_connection_request_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Interrupt Incoming Connection Request")); + Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80); #endif pBtd->l2cap_connection_response(hci_handle,identifier, interrupt_dcid, interrupt_scid, PENDING); delay(1); @@ -615,7 +615,7 @@ void WII::L2CAP_task() { identifier++; delay(1); pBtd->l2cap_config_request(hci_handle,identifier, interrupt_scid); - + l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST; } break; @@ -624,40 +624,40 @@ void WII::L2CAP_task() { case L2CAP_CONTROL_CONNECT_REQUEST: if (l2cap_connected_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nSend HID Control Config Request")); + Notify(PSTR("\r\nSend HID Control Config Request"), 0x80); #endif identifier++; pBtd->l2cap_config_request(hci_handle, identifier, control_scid); l2cap_state = L2CAP_CONTROL_CONFIG_REQUEST; } break; - + case L2CAP_CONTROL_CONFIG_REQUEST: if(l2cap_config_success_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nSend HID Interrupt Connection Request")); + Notify(PSTR("\r\nSend HID Interrupt Connection Request"), 0x80); #endif identifier++; pBtd->l2cap_connection_request(hci_handle,identifier,interrupt_dcid,HID_INTR_PSM); l2cap_state = L2CAP_INTERRUPT_CONNECT_REQUEST; } break; - + case L2CAP_INTERRUPT_CONNECT_REQUEST: if(l2cap_connected_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nSend HID Interrupt Config Request")); + Notify(PSTR("\r\nSend HID Interrupt Config Request"), 0x80); #endif identifier++; pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid); l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST; } break; - + case L2CAP_INTERRUPT_CONFIG_REQUEST: if(l2cap_config_success_interrupt_flag) { // Now the HID channels is established #ifdef DEBUG - Notify(PSTR("\r\nHID Channels Established")); + Notify(PSTR("\r\nHID Channels Established"), 0x80); #endif pBtd->connectToWii = false; pBtd->pairWithWii = false; @@ -666,24 +666,24 @@ void WII::L2CAP_task() { l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE; } break; - - /* The next states are in run() */ - + + /* The next states are in run() */ + case L2CAP_INTERRUPT_DISCONNECT: if (l2cap_disconnect_response_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnected Interrupt Channel")); + Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80); #endif identifier++; - pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid); + pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid); l2cap_state = L2CAP_CONTROL_DISCONNECT; } break; - + case L2CAP_CONTROL_DISCONNECT: if (l2cap_disconnect_response_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnected Control Channel")); + Notify(PSTR("\r\nDisconnected Control Channel"), 0x80); #endif pBtd->hci_disconnect(hci_handle); hci_handle = -1; // Reset handle @@ -691,7 +691,7 @@ void WII::L2CAP_task() { l2cap_state = L2CAP_WAIT; } break; - } + } } void WII::Run() { switch (l2cap_state) { @@ -700,16 +700,16 @@ void WII::Run() { pBtd->l2capConnectionClaimed = true; activeConnection = true; #ifdef DEBUG - Notify(PSTR("\r\nSend HID Control Connection Request")); + Notify(PSTR("\r\nSend HID Control Connection Request"), 0x80); #endif - hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection + hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection l2cap_event_flag = 0; // Reset flags identifier = 0; pBtd->l2cap_connection_request(hci_handle,identifier,control_dcid,HID_CTRL_PSM); - l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST; + l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST; } else if (l2cap_connection_request_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Control Incoming Connection Request")); + Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80); #endif pBtd->l2cap_connection_response(hci_handle,identifier, control_dcid, control_scid, PENDING); delay(1); @@ -719,12 +719,12 @@ void WII::Run() { pBtd->l2cap_config_request(hci_handle,identifier, control_scid); l2cap_state = L2CAP_CONTROL_SUCCESS; } - break; - + break; + case L2CAP_CHECK_MOTION_PLUS_STATE: #ifdef DEBUG if(stateCounter == 0) // Only print onnce - Notify(PSTR("\r\nChecking if a Motion Plus is connected")); + Notify(PSTR("\r\nChecking if a Motion Plus is connected"), 0x80); #endif stateCounter++; if(stateCounter%200 == 0) @@ -733,28 +733,28 @@ void WII::Run() { stateCounter = 0; l2cap_state = L2CAP_INIT_MOTION_PLUS_STATE; timer = micros(); - + if(unknownExtensionConnected) { #ifdef DEBUG - Notify(PSTR("\r\nA extension is also connected")); + Notify(PSTR("\r\nA extension is also connected"), 0x80); #endif activateNunchuck = true; // For we will just set this to true as this the only extension supported so far } - + } else if(stateCounter == 601) { // We will try three times to check for the motion plus -#ifdef DEBUG - Notify(PSTR("\r\nNo Motion Plus was detected")); +#ifdef DEBUG + Notify(PSTR("\r\nNo Motion Plus was detected"), 0x80); #endif stateCounter = 0; l2cap_state = L2CAP_CHECK_EXTENSION_STATE; } break; - + case L2CAP_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port #ifdef DEBUG if(stateCounter == 0) // Only print onnce - Notify(PSTR("\r\nChecking if there is any extension connected")); + Notify(PSTR("\r\nChecking if there is any extension connected"), 0x80); #endif stateCounter++; // We use this counter as there has to be a short delay between the commands if(stateCounter == 1) @@ -774,7 +774,7 @@ void WII::Run() { l2cap_state = L2CAP_LED_STATE; } break; - + case L2CAP_INIT_MOTION_PLUS_STATE: stateCounter++; if(stateCounter == 1) @@ -784,24 +784,24 @@ void WII::Run() { else if(stateCounter == 200) readExtensionType(); // Check if it has been activated else if(stateCounter == 300) { - stateCounter = 0; + stateCounter = 0; unknownExtensionConnected = false; // The motion plus will send a status report when it's activated, we will set this to false so it doesn't reinitialize the Motion Plus l2cap_state = L2CAP_LED_STATE; } break; - + case L2CAP_LED_STATE: if(nunchuck_connected_flag) nunchuckConnected = true; setLedStatus(); l2cap_state = L2CAP_DONE; break; - + case L2CAP_DONE: if(unknownExtensionConnected) { #ifdef DEBUG if(stateCounter == 0) // Only print once - Notify(PSTR("\r\nChecking extension port")); + Notify(PSTR("\r\nChecking extension port"), 0x80); #endif stateCounter++; // We will use this counter as there has to be a short delay between the commands if(stateCounter == 50) @@ -818,7 +818,7 @@ void WII::Run() { else if(stateCounter == 250) { if(nunchuck_connected_flag) { #ifdef DEBUG - Notify(PSTR("\r\nNunchuck was reconnected")); + Notify(PSTR("\r\nNunchuck was reconnected"), 0x80); #endif activateNunchuck = true; nunchuckConnected = true; @@ -829,7 +829,7 @@ void WII::Run() { else if (stateCounter == 300) { if(motionPlusConnected) { #ifdef DEBUG - Notify(PSTR("\r\nReactivating the Motion Plus")); + Notify(PSTR("\r\nReactivating the Motion Plus"), 0x80); #endif initMotionPlus(); } else @@ -867,7 +867,7 @@ void WII::setAllOff() { void WII::setRumbleOff() { HIDBuffer[1] = 0x11; HIDBuffer[2] &= ~0x01; // Bit 0 control the rumble - HID_Command(HIDBuffer, 3); + HID_Command(HIDBuffer, 3); } void WII::setRumbleOn() { HIDBuffer[1] = 0x11; @@ -963,12 +963,12 @@ void WII::activateMotionPlus() { uint8_t buf[1]; if(pBtd->wiiUProController) { #ifdef DEBUG - Notify(PSTR("\r\nActivating Wii U Pro Controller")); + Notify(PSTR("\r\nActivating Wii U Pro Controller"), 0x80); #endif buf[0] = 0x00; // It seems like you can send anything but 0x04, 0x05, and 0x07 } else if(activateNunchuck) { #ifdef DEBUG - Notify(PSTR("\r\nActivating Motion Plus in pass-through mode")); + Notify(PSTR("\r\nActivating Motion Plus in pass-through mode"), 0x80); #endif buf[0] = 0x05; // Activate nunchuck pass-through mode } @@ -976,7 +976,7 @@ void WII::activateMotionPlus() { //buf[0] = 0x07; else { #ifdef DEBUG - Notify(PSTR("\r\nActivating Motion Plus in normal mode")); + Notify(PSTR("\r\nActivating Motion Plus in normal mode"), 0x80); #endif buf[0] = 0x04; // Don't use any extension } @@ -989,13 +989,13 @@ void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) { if(EEPROM) cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Read from EEPROM else - cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Read from memory + cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Read from memory cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16); cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8); cmd_buf[5] = (uint8_t)(offset & 0xFF); cmd_buf[6] = (uint8_t)((size & 0xFF00) >> 8); cmd_buf[7] = (uint8_t)(size & 0xFF); - + HID_Command(cmd_buf,8); } void WII::readExtensionType() { @@ -1014,7 +1014,7 @@ void WII::checkMotionPresent() { bool WII::getButtonPress(Button b) { // Return true when a button is pressed if(wiiUProControllerConnected) - return (ButtonState & pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b])); + return (ButtonState & pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b])); else return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b])); } @@ -1057,69 +1057,69 @@ uint16_t WII::getAnalogHat(AnalogHat a) { #ifdef WIICAMERA void WII::IRinitialize(){ // Turns on and initialises the IR camera - + enableIRCamera1(); #ifdef DEBUG - Notify(PSTR("\r\nEnable IR Camera1 Complete")); + Notify(PSTR("\r\nEnable IR Camera1 Complete"), 0x80); #endif delay(80); - + enableIRCamera2(); #ifdef DEBUG - Notify(PSTR("\r\nEnable IR Camera2 Complete")); + Notify(PSTR("\r\nEnable IR Camera2 Complete"), 0x80); #endif delay(80); - + write0x08Value(); #ifdef DEBUG - Notify(PSTR("\r\nWrote hex number 0x08")); + Notify(PSTR("\r\nWrote hex number 0x08"), 0x80); #endif delay(80); - + writeSensitivityBlock1(); #ifdef DEBUG - Notify(PSTR("\r\nWrote Sensitivity Block 1")); + Notify(PSTR("\r\nWrote Sensitivity Block 1"), 0x80); #endif delay(80); - + writeSensitivityBlock2(); #ifdef DEBUG - Notify(PSTR("\r\nWrote Sensitivity Block 2")); + Notify(PSTR("\r\nWrote Sensitivity Block 2"), 0x80); #endif delay(80); uint8_t mode_num = 0x03; setWiiModeNumber(mode_num); // Change input for whatever mode you want i.e. 0x01, 0x03, or 0x05 #ifdef DEBUG - Notify(PSTR("\r\nSet Wii Mode Number To 0x")); - PrintHex(mode_num); + Notify(PSTR("\r\nSet Wii Mode Number To 0x"), 0x80); + PrintHex(mode_num, 0x80); #endif delay(80); - + write0x08Value(); #ifdef DEBUG - Notify(PSTR("\r\nWrote Hex Number 0x08")); + Notify(PSTR("\r\nWrote Hex Number 0x08"), 0x80); #endif delay(80); setReportMode(false, 0x33); //setReportMode(false, 0x3f); // For full reporting mode, doesn't work yet #ifdef DEBUG - Notify(PSTR("\r\nSet Report Mode to 0x33")); + Notify(PSTR("\r\nSet Report Mode to 0x33"), 0x80); #endif delay(80); - + statusRequest(); // Used to update wiiState - call isIRCameraEnabled() afterwards to check if it actually worked #ifdef DEBUG - Notify(PSTR("\r\nIR Initialized")); -#endif + Notify(PSTR("\r\nIR Initialized"), 0x80); +#endif } void WII::enableIRCamera1(){ uint8_t cmd_buf[3]; cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) cmd_buf[1] = 0x13; // Output report 13 - cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2 + cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2 HID_Command(cmd_buf, 3); } @@ -1127,7 +1127,7 @@ void WII::enableIRCamera2(){ uint8_t cmd_buf[3]; cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) cmd_buf[1] = 0x1A; // Output report 1A - cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2 + cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2 HID_Command(cmd_buf, 3); } @@ -1162,4 +1162,4 @@ void WII::write0x08Value(){ void WII::setWiiModeNumber(uint8_t mode_number){ // mode_number in hex i.e. 0x03 for extended mode writeData(0xb00033,1,&mode_number); } -#endif \ No newline at end of file +#endif diff --git a/XBOXRECV.cpp b/XBOXRECV.cpp index e4d0627a..ab0df9ef 100644 --- a/XBOXRECV.cpp +++ b/XBOXRECV.cpp @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -29,10 +29,10 @@ bPollEnable(false) { // don't start polling before dongle is connected for(uint8_t i=0; iRegisterDeviceClass(this); //set devConfig[] entry } @@ -44,74 +44,74 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) { EpInfo *oldep_ptr = NULL; uint16_t PID; uint16_t VID; - + // get memory address of USB device address pool - AddressPool &addrPool = pUsb->GetAddressPool(); + AddressPool &addrPool = pUsb->GetAddressPool(); #ifdef EXTRADEBUG - Notify(PSTR("\r\nXBOXRECV Init")); + Notify(PSTR("\r\nXBOXRECV Init"), 0x80); #endif // check if address has already been assigned to an instance if (bAddress) { #ifdef DEBUG - Notify(PSTR("\r\nAddress in use")); + Notify(PSTR("\r\nAddress in use"), 0x80); #endif return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; } - + // Get pointer to pseudo device with address 0 assigned p = addrPool.GetUsbDevicePtr(0); - - if (!p) { + + if (!p) { #ifdef DEBUG - Notify(PSTR("\r\nAddress not found")); + Notify(PSTR("\r\nAddress not found"), 0x80); #endif return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; } - + if (!p->epinfo) { #ifdef DEBUG - Notify(PSTR("\r\nepinfo is null")); + Notify(PSTR("\r\nepinfo is null"), 0x80); #endif return USB_ERROR_EPINFO_IS_NULL; } - + // Save old pointer to EP_RECORD of address 0 oldep_ptr = p->epinfo; - + // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence p->epinfo = epInfo; - + p->lowspeed = lowspeed; - + // Get device descriptor rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data // Restore p->epinfo p->epinfo = oldep_ptr; - + if(rcode) goto FailGetDevDescr; - + VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor; PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct; - + if(VID != XBOX_VID && VID != MADCATZ_VID) // We just check if it's a xbox receiver using the Vendor ID goto FailUnknownDevice; else if(PID != XBOX_WIRELESS_RECEIVER_PID && PID != XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID) { #ifdef DEBUG - Notify(PSTR("\r\nYou'll need a wireless receiver for this libary to work")); + Notify(PSTR("\r\nYou'll need a wireless receiver for this libary to work"), 0x80); #endif goto FailUnknownDevice; - } - + } + // Allocate new address according to device class bAddress = addrPool.AllocAddress(parent, false, port); - + if (!bAddress) return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; - + // Extract Max Packet Size from device descriptor - epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; - + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; + // Assign new address to the device rcode = pUsb->setAddr( 0, 0, bAddress ); if (rcode) { @@ -119,33 +119,33 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) { addrPool.FreeAddress(bAddress); bAddress = 0; #ifdef DEBUG - Notify(PSTR("\r\nsetAddr: ")); + Notify(PSTR("\r\nsetAddr: "), 0x80); #endif - PrintHex(rcode); + PrintHex(rcode, 0x80); return rcode; } #ifdef EXTRADEBUG - Notify(PSTR("\r\nAddr: ")); - PrintHex(bAddress); + Notify(PSTR("\r\nAddr: "), 0x80); + PrintHex(bAddress, 0x80); #endif p->lowspeed = false; - + //get pointer to assigned address record p = addrPool.GetUsbDevicePtr(bAddress); - if (!p) + if (!p) return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - - p->lowspeed = lowspeed; - + + p->lowspeed = lowspeed; + // Assign epInfo to epinfo pointer - only EP0 is known rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); if (rcode) goto FailSetDevTblEntry; - + /* The application will work in reduced host mode, so we can save program and data memory space. After verifying the VID we will use known values for the configuration values for device, interface, endpoints and HID for the XBOX360 Wireless receiver */ - + /* Initialize data structures for endpoints of device */ epInfo[ XBOX_INPUT_PIPE_1 ].epAddr = 0x01; // XBOX 360 report endpoint - poll interval 1ms epInfo[ XBOX_INPUT_PIPE_1 ].epAttribs = EP_INTERRUPT; @@ -159,7 +159,7 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) { epInfo[ XBOX_OUTPUT_PIPE_1 ].maxPktSize = EP_MAXPKTSIZE; epInfo[ XBOX_OUTPUT_PIPE_1 ].bmSndToggle = bmSNDTOG0; epInfo[ XBOX_OUTPUT_PIPE_1 ].bmRcvToggle = bmRCVTOG0; - + epInfo[ XBOX_INPUT_PIPE_2 ].epAddr = 0x03; // XBOX 360 report endpoint - poll interval 1ms epInfo[ XBOX_INPUT_PIPE_2 ].epAttribs = EP_INTERRUPT; epInfo[ XBOX_INPUT_PIPE_2 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints @@ -172,7 +172,7 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) { epInfo[ XBOX_OUTPUT_PIPE_2 ].maxPktSize = EP_MAXPKTSIZE; epInfo[ XBOX_OUTPUT_PIPE_2 ].bmSndToggle = bmSNDTOG0; epInfo[ XBOX_OUTPUT_PIPE_2 ].bmRcvToggle = bmRCVTOG0; - + epInfo[ XBOX_INPUT_PIPE_3 ].epAddr = 0x05; // XBOX 360 report endpoint - poll interval 1ms epInfo[ XBOX_INPUT_PIPE_3 ].epAttribs = EP_INTERRUPT; epInfo[ XBOX_INPUT_PIPE_3 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints @@ -185,7 +185,7 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) { epInfo[ XBOX_OUTPUT_PIPE_3 ].maxPktSize = EP_MAXPKTSIZE; epInfo[ XBOX_OUTPUT_PIPE_3 ].bmSndToggle = bmSNDTOG0; epInfo[ XBOX_OUTPUT_PIPE_3 ].bmRcvToggle = bmRCVTOG0; - + epInfo[ XBOX_INPUT_PIPE_4 ].epAddr = 0x07; // XBOX 360 report endpoint - poll interval 1ms epInfo[ XBOX_INPUT_PIPE_4 ].epAttribs = EP_INTERRUPT; epInfo[ XBOX_INPUT_PIPE_4 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints @@ -198,54 +198,54 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) { epInfo[ XBOX_OUTPUT_PIPE_4 ].maxPktSize = EP_MAXPKTSIZE; epInfo[ XBOX_OUTPUT_PIPE_4 ].bmSndToggle = bmSNDTOG0; epInfo[ XBOX_OUTPUT_PIPE_4 ].bmRcvToggle = bmRCVTOG0; - + rcode = pUsb->setEpInfoEntry(bAddress, 9, epInfo); if( rcode ) goto FailSetDevTblEntry; - + delay(200);//Give time for address change - + rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1); if( rcode ) - goto FailSetConf; + goto FailSetConf; #ifdef DEBUG - Notify(PSTR("\r\nXbox Wireless Receiver Connected\r\n")); + Notify(PSTR("\r\nXbox Wireless Receiver Connected\r\n"), 0x80); #endif XboxReceiverConnected = true; bPollEnable = true; return 0; // successful configuration - - /* diagnostic messages */ + + /* diagnostic messages */ FailGetDevDescr: #ifdef DEBUG - Notify(PSTR("\r\ngetDevDescr:")); + Notify(PSTR("\r\ngetDevDescr:"), 0x80); #endif - goto Fail; + goto Fail; FailSetDevTblEntry: #ifdef DEBUG - Notify(PSTR("\r\nsetDevTblEn:")); + Notify(PSTR("\r\nsetDevTblEn:"), 0x80); #endif goto Fail; FailSetConf: #ifdef DEBUG - Notify(PSTR("\r\nsetConf:")); + Notify(PSTR("\r\nsetConf:"), 0x80); #endif - goto Fail; + goto Fail; FailUnknownDevice: #ifdef DEBUG - Notify(PSTR("\r\nUnknown Device Connected - VID: ")); - PrintHex(VID); - Notify(PSTR(" PID: ")); - PrintHex(PID); + Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80); + PrintHex(VID, 0x80); + Notify(PSTR(" PID: "), 0x80); + PrintHex(PID, 0x80); #endif rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; goto Fail; Fail: #ifdef DEBUG - Notify(PSTR("\r\nXbox 360 Init Failed, error code: ")); + Notify(PSTR("\r\nXbox 360 Init Failed, error code: "), 0x80); Serial.print(rcode,HEX); -#endif +#endif Release(); return rcode; } @@ -255,12 +255,12 @@ uint8_t XBOXRECV::Release() { XboxReceiverConnected = false; for(uint8_t i=0;i<4;i++) Xbox360Connected[i] = 0x00; - pUsb->GetAddressPool().FreeAddress(bAddress); + pUsb->GetAddressPool().FreeAddress(bAddress); bAddress = 0; bPollEnable = false; return 0; } -uint8_t XBOXRECV::Poll() { +uint8_t XBOXRECV::Poll() { if (!bPollEnable) return 0; if(!timer || ((millis() - timer) > 3000)) { // Run checkStatus every 3 seconds @@ -269,7 +269,7 @@ uint8_t XBOXRECV::Poll() { } uint8_t inputPipe; uint16_t bufferSize; - for(uint8_t i=0;i<4;i++) { + for(uint8_t i=0;i<4;i++) { switch (i) { case 0: inputPipe = XBOX_INPUT_PIPE_1; break; case 1: inputPipe = XBOX_INPUT_PIPE_2; break; @@ -280,14 +280,14 @@ uint8_t XBOXRECV::Poll() { pUsb->inTransfer(bAddress, epInfo[ inputPipe ].epAddr, &bufferSize, readBuf); if(bufferSize > 0) { // The number of received bytes #ifdef EXTRADEBUG - Notify(PSTR("Bytes Received: ")); + Notify(PSTR("Bytes Received: "), 0x80); Serial.print(bufferSize); - Notify(PSTR("\r\n")); + Notify(PSTR("\r\n"), 0x80); #endif readReport(i); #ifdef PRINTREPORT printReport(i,bufferSize); // Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller -#endif +#endif } } return 0; @@ -300,7 +300,7 @@ void XBOXRECV::readReport(uint8_t controller) { if(readBuf[0] == 0x08 && readBuf[1] != Xbox360Connected[controller]) { Xbox360Connected[controller] = readBuf[1]; #ifdef DEBUG - Notify(PSTR("Controller ")); + Notify(PSTR("Controller "), 0x80); Serial.print(controller); #endif if(Xbox360Connected[controller]) { @@ -310,9 +310,9 @@ void XBOXRECV::readReport(uint8_t controller) { case 0x80: str = PSTR(" as controller\r\n"); break; case 0x40: str = PSTR(" as headset\r\n"); break; case 0xC0: str = PSTR(" as controller+headset\r\n"); break; - } - Notify(PSTR(": connected")); - Notify(str); + } + Notify(PSTR(": connected"), 0x80); + Notify(str, 0x80); #endif LED led; switch (controller) { @@ -325,7 +325,7 @@ void XBOXRECV::readReport(uint8_t controller) { } #ifdef DEBUG else - Notify(PSTR(": disconnected\r\n")); + Notify(PSTR(": disconnected\r\n"), 0x80); #endif return; } @@ -336,21 +336,21 @@ void XBOXRECV::readReport(uint8_t controller) { } if(readBuf[1] != 0x01) // Check if it's the correct report - the receiver also sends different status reports return; - + // A controller must be connected if it's sending data if(!Xbox360Connected[controller]) Xbox360Connected[controller] |= 0x80; ButtonState[controller] = (uint32_t)(readBuf[9] | ((uint16_t)readBuf[8] << 8) | ((uint32_t)readBuf[7] << 16) | ((uint32_t)readBuf[6] << 24)); - + hatValue[controller][LeftHatX] = (int16_t)(((uint16_t)readBuf[11] << 8) | readBuf[10]); hatValue[controller][LeftHatY] = (int16_t)(((uint16_t)readBuf[13] << 8) | readBuf[12]); hatValue[controller][RightHatX] = (int16_t)(((uint16_t)readBuf[15] << 8) | readBuf[14]); hatValue[controller][RightHatY] = (int16_t)(((uint16_t)readBuf[17] << 8) | readBuf[16]); - - //Notify(PSTR("\r\nButtonState: ")); - //PrintHex(ButtonState[controller]); - + + //Notify(PSTR("\r\nButtonState: "), 0x80); + //PrintHex(ButtonState[controller], 0x80); + if(ButtonState[controller] != OldButtonState[controller]) { buttonStateChanged[controller] = true; ButtonClickState[controller] = (ButtonState[controller] >> 16) & ((~OldButtonState[controller]) >> 16); // Update click state variable, but don't include the two trigger buttons L2 and R2 @@ -366,11 +366,11 @@ void XBOXRECV::printReport(uint8_t controller, uint8_t nBytes) { //Uncomment "#d #ifdef PRINTREPORT if (readBuf == NULL) return; - Notify(PSTR("Controller ")); + Notify(PSTR("Controller "), 0x80); Serial.print(controller); - Notify(PSTR(": ")); + Notify(PSTR(": "), 0x80); for(uint8_t i = 0; i < nBytes;i++) { - PrintHex(readBuf[i]); + PrintHex(readBuf[i], 0x80); Serial.print(" "); } Serial.println(); @@ -433,8 +433,8 @@ ControllerStatus Breakdown uint8_t XBOXRECV::getBatteryLevel(uint8_t controller) { uint8_t batteryLevel = ((controllerStatus[controller] & 0x00C0) >> 6) * 33; if(batteryLevel == 99) - batteryLevel = 100; - return batteryLevel; + batteryLevel = 100; + return batteryLevel; } void XBOXRECV::XboxCommand(uint8_t controller, uint8_t* data, uint16_t nbytes) { @@ -449,7 +449,7 @@ void XBOXRECV::XboxCommand(uint8_t controller, uint8_t* data, uint16_t nbytes) { rcode = pUsb->outTransfer(bAddress, epInfo[ outputPipe ].epAddr, nbytes, data); #ifdef EXTRADEBUG if(rcode) - Notify(PSTR("Error sending Xbox message\r\n")); + Notify(PSTR("Error sending Xbox message\r\n"), 0x80); #endif } void XBOXRECV::setLedRaw(uint8_t controller, uint8_t value) { @@ -457,7 +457,7 @@ void XBOXRECV::setLedRaw(uint8_t controller, uint8_t value) { writeBuf[1] = 0x00; writeBuf[2] = 0x08; writeBuf[3] = value | 0x40; - + XboxCommand(controller, writeBuf, 4); } void XBOXRECV::setLedOn(uint8_t controller, LED led) { @@ -470,12 +470,12 @@ void XBOXRECV::setLedBlink(uint8_t controller, LED led) { void XBOXRECV::setLedMode(uint8_t controller, LEDMode ledMode) { // This function is used to do some speciel LED stuff the controller supports setLedRaw(controller,(uint8_t)ledMode); } -/* PC runs this at interval of approx 2 seconds +/* PC runs this at interval of approx 2 seconds Thanks to BusHound from Perisoft.net for the Windows USB Analysis output Found by timstamp.co.uk */ void XBOXRECV::checkStatus() { - if(!bPollEnable) + if(!bPollEnable) return; // Get controller info writeBuf[0] = 0x08; @@ -504,6 +504,6 @@ void XBOXRECV::setRumbleOn(uint8_t controller, uint8_t lValue, uint8_t rValue) { writeBuf[4] = 0x00; writeBuf[5] = lValue; // big weight writeBuf[6] = rValue; // small weight - + XboxCommand(controller, writeBuf, 7); -} \ No newline at end of file +} diff --git a/XBOXUSB.cpp b/XBOXUSB.cpp index c18d056f..93047227 100644 --- a/XBOXUSB.cpp +++ b/XBOXUSB.cpp @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -27,10 +27,10 @@ bPollEnable(false) { // don't start polling before dongle is connected for(uint8_t i=0; iRegisterDeviceClass(this); //set devConfig[] entry } @@ -42,80 +42,80 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) { EpInfo *oldep_ptr = NULL; uint16_t PID; uint16_t VID; - + // get memory address of USB device address pool - AddressPool &addrPool = pUsb->GetAddressPool(); + AddressPool &addrPool = pUsb->GetAddressPool(); #ifdef EXTRADEBUG - Notify(PSTR("\r\nXBOXUSB Init")); + Notify(PSTR("\r\nXBOXUSB Init"), 0x80); #endif // check if address has already been assigned to an instance if (bAddress) { #ifdef DEBUG - Notify(PSTR("\r\nAddress in use")); + Notify(PSTR("\r\nAddress in use"), 0x80); #endif return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; } - + // Get pointer to pseudo device with address 0 assigned p = addrPool.GetUsbDevicePtr(0); - - if (!p) { + + if (!p) { #ifdef DEBUG - Notify(PSTR("\r\nAddress not found")); + Notify(PSTR("\r\nAddress not found"), 0x80); #endif return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; } - + if (!p->epinfo) { #ifdef DEBUG - Notify(PSTR("\r\nepinfo is null")); + Notify(PSTR("\r\nepinfo is null"), 0x80); #endif return USB_ERROR_EPINFO_IS_NULL; } - + // Save old pointer to EP_RECORD of address 0 oldep_ptr = p->epinfo; - + // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence p->epinfo = epInfo; - + p->lowspeed = lowspeed; - + // Get device descriptor rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data // Restore p->epinfo p->epinfo = oldep_ptr; - + if(rcode) goto FailGetDevDescr; - + VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor; PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct; - + if(VID != XBOX_VID && VID != MADCATZ_VID && VID != JOYTECH_VID) // We just check if it's a xbox controller using the Vendor ID goto FailUnknownDevice; if(PID == XBOX_WIRELESS_PID) { #ifdef DEBUG - Notify(PSTR("\r\nYou have plugged in a wireless Xbox 360 controller - it doesn't support USB communication")); + Notify(PSTR("\r\nYou have plugged in a wireless Xbox 360 controller - it doesn't support USB communication"), 0x80); #endif goto FailUnknownDevice; } else if(PID == XBOX_WIRELESS_RECEIVER_PID || PID == XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID) { #ifdef DEBUG - Notify(PSTR("\r\nThis library only supports Xbox 360 controllers via USB")); + Notify(PSTR("\r\nThis library only supports Xbox 360 controllers via USB"), 0x80); #endif goto FailUnknownDevice; - } - + } + // Allocate new address according to device class bAddress = addrPool.AllocAddress(parent, false, port); - + if (!bAddress) return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; - + // Extract Max Packet Size from device descriptor - epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; - + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; + // Assign new address to the device rcode = pUsb->setAddr( 0, 0, bAddress ); if (rcode) { @@ -123,33 +123,33 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) { addrPool.FreeAddress(bAddress); bAddress = 0; #ifdef DEBUG - Notify(PSTR("\r\nsetAddr: ")); + Notify(PSTR("\r\nsetAddr: "), 0x80); #endif - PrintHex(rcode); + PrintHex(rcode, 0x80); return rcode; } #ifdef EXTRADEBUG - Notify(PSTR("\r\nAddr: ")); - PrintHex(bAddress); + Notify(PSTR("\r\nAddr: "), 0x80); + PrintHex(bAddress, 0x80); #endif p->lowspeed = false; - + //get pointer to assigned address record p = addrPool.GetUsbDevicePtr(bAddress); - if (!p) + if (!p) return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - - p->lowspeed = lowspeed; - + + p->lowspeed = lowspeed; + // Assign epInfo to epinfo pointer - only EP0 is known rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); if (rcode) goto FailSetDevTblEntry; - + /* The application will work in reduced host mode, so we can save program and data memory space. After verifying the VID we will use known values for the configuration values for device, interface, endpoints and HID for the XBOX360 Controllers */ - + /* Initialize data structures for endpoints of device */ epInfo[ XBOX_INPUT_PIPE ].epAddr = 0x01; // XBOX 360 report endpoint epInfo[ XBOX_INPUT_PIPE ].epAttribs = EP_INTERRUPT; @@ -163,55 +163,55 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) { epInfo[ XBOX_OUTPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE; epInfo[ XBOX_OUTPUT_PIPE ].bmSndToggle = bmSNDTOG0; epInfo[ XBOX_OUTPUT_PIPE ].bmRcvToggle = bmRCVTOG0; - + rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo); if( rcode ) goto FailSetDevTblEntry; - + delay(200);//Give time for address change - + rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1); if( rcode ) - goto FailSetConf; + goto FailSetConf; #ifdef DEBUG - Notify(PSTR("\r\nXbox 360 Controller Connected\r\n")); -#endif + Notify(PSTR("\r\nXbox 360 Controller Connected\r\n"), 0x80); +#endif setLedOn(LED1); Xbox360Connected = true; bPollEnable = true; return 0; // successful configuration - - /* diagnostic messages */ + + /* diagnostic messages */ FailGetDevDescr: #ifdef DEBUG - Notify(PSTR("\r\ngetDevDescr:")); + Notify(PSTR("\r\ngetDevDescr:"), 0x80); #endif - goto Fail; + goto Fail; FailSetDevTblEntry: #ifdef DEBUG - Notify(PSTR("\r\nsetDevTblEn:")); + Notify(PSTR("\r\nsetDevTblEn:"), 0x80); #endif goto Fail; FailSetConf: #ifdef DEBUG - Notify(PSTR("\r\nsetConf:")); + Notify(PSTR("\r\nsetConf:"), 0x80); #endif - goto Fail; + goto Fail; FailUnknownDevice: #ifdef DEBUG - Notify(PSTR("\r\nUnknown Device Connected - VID: ")); - PrintHex(VID); - Notify(PSTR(" PID: ")); - PrintHex(PID); + Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80); + PrintHex(VID, 0x80); + Notify(PSTR(" PID: "), 0x80); + PrintHex(PID, 0x80); #endif rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; goto Fail; Fail: #ifdef DEBUG - Notify(PSTR("\r\nXbox 360 Init Failed, error code: ")); + Notify(PSTR("\r\nXbox 360 Init Failed, error code: "), 0x80); Serial.print(rcode,HEX); -#endif +#endif Release(); return rcode; } @@ -219,12 +219,12 @@ Fail: /* Performs a cleanup after failed Init() attempt */ uint8_t XBOXUSB::Release() { Xbox360Connected = false; - pUsb->GetAddressPool().FreeAddress(bAddress); + pUsb->GetAddressPool().FreeAddress(bAddress); bAddress = 0; bPollEnable = false; return 0; } -uint8_t XBOXUSB::Poll() { +uint8_t XBOXUSB::Poll() { if (!bPollEnable) return 0; uint16_t BUFFER_SIZE = EP_MAXPKTSIZE; @@ -236,7 +236,7 @@ uint8_t XBOXUSB::Poll() { return 0; } -void XBOXUSB::readReport() { +void XBOXUSB::readReport() { if (readBuf == NULL) return; if(readBuf[0] != 0x00 || readBuf[1] != 0x14) { // Check if it's the correct report - the controller also sends different status reports @@ -244,15 +244,15 @@ void XBOXUSB::readReport() { } ButtonState = (uint32_t)(readBuf[5] | ((uint16_t)readBuf[4] << 8) | ((uint32_t)readBuf[3] << 16) | ((uint32_t)readBuf[2] << 24)); - + hatValue[LeftHatX] = (int16_t)(((uint16_t)readBuf[7] << 8) | readBuf[6]); hatValue[LeftHatY] = (int16_t)(((uint16_t)readBuf[9] << 8) | readBuf[8]); hatValue[RightHatX] = (int16_t)(((uint16_t)readBuf[11] << 8) | readBuf[10]); hatValue[RightHatY] = (int16_t)(((uint16_t)readBuf[13] << 8) | readBuf[12]); - - //Notify(PSTR("\r\nButtonState")); - //PrintHex(ButtonState); - + + //Notify(PSTR("\r\nButtonState"), 0x80); + //PrintHex(ButtonState, 0x80); + if(ButtonState != OldButtonState) { ButtonClickState = (ButtonState >> 16) & ((~OldButtonState) >> 16); // Update click state variable, but don't include the two trigger buttons L2 and R2 if(((uint8_t)OldButtonState) == 0 && ((uint8_t)ButtonState) != 0) // The L2 and R2 buttons are special as they are analog buttons @@ -261,18 +261,18 @@ void XBOXUSB::readReport() { L2Clicked = true; OldButtonState = ButtonState; } -} +} void XBOXUSB::printReport() { //Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller #ifdef PRINTREPORT if (readBuf == NULL) return; for(uint8_t i = 0; i < XBOX_REPORT_BUFFER_SIZE;i++) { - PrintHex(readBuf[i]); + PrintHex(readBuf[i], 0x80); Serial.print(" "); - } + } Serial.println(); -#endif +#endif } uint8_t XBOXUSB::getButtonPress(Button b) { @@ -315,7 +315,7 @@ void XBOXUSB::setLedRaw(uint8_t value) { writeBuf[0] = 0x01; writeBuf[1] = 0x03; writeBuf[2] = value; - + XboxCommand(writeBuf, 3); } void XBOXUSB::setLedOn(LED led) { @@ -337,6 +337,6 @@ void XBOXUSB::setRumbleOn(uint8_t lValue, uint8_t rValue) { writeBuf[5] = 0x00; writeBuf[6] = 0x00; writeBuf[7] = 0x00; - + XboxCommand(writeBuf, 8); -} \ No newline at end of file +}