Advertisement
Guest User

Untitled

a guest
Aug 29th, 2012
92
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 41.60 KB | None | 0 0
  1. /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
  2.  
  3.  This software may be distributed and modified under the terms of the GNU
  4.  General Public License version 2 (GPL2) as published by the Free Software
  5.  Foundation and appearing in the file GPL2.TXT included in the packaging of
  6.  this file. Please note that GPL2 Section 2[b] requires that all works based
  7.  on this software must also be made publicly available under the terms of
  8.  the GPL2 ("Copyleft").
  9.  
  10.  Contact information
  11.  -------------------
  12.  
  13.  Kristian Lauszus, TKJ Electronics
  14.  Web      :  http://www.tkjelectronics.com
  15.  e-mail   :  kristianl@tkjelectronics.com
  16.  */
  17.  
  18. #include "BTD.h"
  19. #define DEBUG // Uncomment to print data for debugging
  20. #define EXTRADEBUG // Uncomment to get even more debugging data
  21.  
  22. const uint8_t BTD::BTD_EVENT_PIPE  = 1;
  23. const uint8_t BTD::BTD_DATAIN_PIPE = 2;
  24. const uint8_t BTD::BTD_DATAOUT_PIPE = 3;
  25.  
  26. BTD::BTD(USB *p):
  27. pUsb(p), // Pointer to USB class instance - mandatory
  28. bAddress(0), // Device address - mandatory
  29. bNumEP(1), // If config descriptor needs to be parsed
  30. qNextPollTime(0), // Reset NextPollTime
  31. bPollEnable(false) // Don't start polling before dongle is connected
  32. {
  33.     for(uint8_t i=0; i<BTD_MAX_ENDPOINTS; i++) {
  34.         epInfo[i].epAddr        = 0;
  35.         epInfo[i].maxPktSize    = (i) ? 0 : 8;
  36.         epInfo[i].epAttribs     = 0;        
  37.         epInfo[i].bmNakPower    = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
  38.     }
  39.    
  40.     if (pUsb) // register in USB subsystem
  41.         pUsb->RegisterDeviceClass(this); //set devConfig[] entry
  42.    
  43.     wiiServiceID = -1;
  44. }
  45.  
  46. uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
  47.     uint8_t buf[sizeof(USB_DEVICE_DESCRIPTOR)];
  48.     uint8_t rcode;
  49.     UsbDevice *p = NULL;
  50.     EpInfo *oldep_ptr = NULL;  
  51.     uint8_t num_of_conf; // number of configurations
  52.     uint16_t PID;
  53.     uint16_t VID;
  54.    
  55.     // get memory address of USB device address pool
  56.     AddressPool &addrPool = pUsb->GetAddressPool();    
  57. #ifdef EXTRADEBUG
  58.     Notify(PSTR("\r\nBTD Init"));
  59. #endif
  60.     // check if address has already been assigned to an instance
  61.     if (bAddress) {
  62. #ifdef DEBUG
  63.         Notify(PSTR("\r\nAddress in use"));
  64. #endif
  65.         return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
  66.     }
  67.    
  68.     // Get pointer to pseudo device with address 0 assigned
  69.     p = addrPool.GetUsbDevicePtr(0);
  70.    
  71.     if (!p) {        
  72. #ifdef DEBUG
  73.         Notify(PSTR("\r\nAddress not found"));
  74. #endif
  75.         return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
  76.     }
  77.    
  78.     if (!p->epinfo) {
  79. #ifdef DEBUG
  80.         Notify(PSTR("\r\nepinfo is null"));
  81. #endif
  82.         return USB_ERROR_EPINFO_IS_NULL;
  83.     }
  84.    
  85.     // Save old pointer to EP_RECORD of address 0
  86.     oldep_ptr = p->epinfo;
  87.    
  88.     // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence
  89.     p->epinfo = epInfo;
  90.    
  91.     p->lowspeed = lowspeed;
  92.    
  93.     // Get device descriptor
  94.     rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data
  95.    
  96.     // Restore p->epinfo
  97.     p->epinfo = oldep_ptr;
  98.    
  99.     if(rcode)
  100.         goto FailGetDevDescr;
  101.    
  102.     // Allocate new address according to device class
  103.     bAddress = addrPool.AllocAddress(parent, false, port);
  104.    
  105.     if (!bAddress)
  106.         return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
  107.    
  108.     // Extract Max Packet Size from device descriptor
  109.     epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
  110.    
  111.     // Assign new address to the device
  112.     rcode = pUsb->setAddr( 0, 0, bAddress );
  113.     if (rcode) {
  114.         p->lowspeed = false;
  115.         addrPool.FreeAddress(bAddress);
  116.         bAddress = 0;
  117. #ifdef DEBUG
  118.         Notify(PSTR("\r\nsetAddr: "));
  119. #endif
  120.         PrintHex<uint8_t>(rcode);
  121.         return rcode;
  122.     }
  123. #ifdef EXTRADEBUG
  124.     Notify(PSTR("\r\nAddr: "));
  125.     PrintHex<uint8_t>(bAddress);
  126. #endif
  127.     p->lowspeed = false;
  128.    
  129.     //get pointer to assigned address record
  130.     p = addrPool.GetUsbDevicePtr(bAddress);
  131.     if (!p)
  132.         return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
  133.    
  134.     p->lowspeed = lowspeed;        
  135.    
  136.     // Assign epInfo to epinfo pointer - only EP0 is known
  137.     rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
  138.     if (rcode)
  139.         goto FailSetDevTblEntry;
  140.     VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor;
  141.     PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct;
  142.    
  143.     if(VID == PS3_VID && (PID == PS3_PID ||  PID == PS3NAVIGATION_PID || PID == PS3MOVE_PID)) {
  144.         /* We only need the Control endpoint, so we don't have to initialize the other endpoints of device */                
  145.         rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 1);
  146.         if( rcode )
  147.             goto FailSetConf;
  148.        
  149.         if(PID == PS3_PID || PID == PS3NAVIGATION_PID) {
  150. #ifdef DEBUG
  151.             if(PID == PS3_PID)
  152.                 Notify(PSTR("\r\nDualshock 3 Controller Connected"));
  153.             else // must be a navigation controller
  154.                 Notify(PSTR("\r\nNavigation Controller Connected"));
  155. #endif
  156.             /* Set internal bluetooth address */
  157.             setBdaddr(my_bdaddr);
  158.         }
  159.         else { // must be a Motion controller
  160. #ifdef DEBUG
  161.             Notify(PSTR("\r\nMotion Controller Connected"));
  162. #endif
  163.             setMoveBdaddr(my_bdaddr);
  164.         }
  165.         rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 0); // Reset configuration value
  166.         pUsb->setAddr(bAddress, 0, 0); // Reset address
  167.         Release(); // Release device
  168.         return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; // return
  169.     }
  170.     else {
  171.         num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations;
  172.  
  173.         // check if attached device is a Bluetooth dongle and fill endpoint data structure
  174.         // first interface in the configuration must have Bluetooth assigned Class/Subclass/Protocol
  175.         // and 3 endpoints - interrupt-IN, bulk-IN, bulk-OUT,
  176.         // not necessarily in this order
  177.         for (uint8_t i=0; i<num_of_conf; i++) {
  178.             ConfigDescParser<USB_CLASS_WIRELESS_CTRL, WI_SUBCLASS_RF, WI_PROTOCOL_BT, CP_MASK_COMPARE_ALL> confDescrParser(this);
  179.             rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
  180.             if(rcode)
  181.                 goto FailGetConfDescr;
  182.             if(bNumEP >= BTD_MAX_ENDPOINTS) // All endpoints extracted
  183.                 break;
  184.         }
  185.    
  186.         if (bNumEP < BTD_MAX_ENDPOINTS)
  187.             goto FailUnknownDevice;
  188.        
  189.         // Assign epInfo to epinfo pointer - this time all 3 endpoins
  190.         rcode = pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo);
  191.         if(rcode)
  192.             goto FailSetDevTblEntry;
  193.        
  194.         delay(200); // Give time for address change
  195.    
  196.         // Set Configuration Value
  197.         rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bConfNum);
  198.         if(rcode)
  199.             goto FailSetConf;
  200.    
  201.         hci_num_reset_loops = 100; // only loop 100 times before trying to send the hci reset command
  202.         hci_counter = 0;
  203.         hci_state = HCI_INIT_STATE;
  204.         watingForConnection = false;
  205.         bPollEnable = true;
  206.  
  207. #ifdef DEBUG
  208.         Notify(PSTR("\r\nBluetooth Dongle Initialized"));
  209. #endif
  210.     }
  211.     return 0; // Successful configuration
  212.    
  213.     /* diagnostic messages */  
  214. FailGetDevDescr:
  215. #ifdef DEBUG
  216.     Notify(PSTR("\r\ngetDevDescr"));
  217. #endif
  218.     goto Fail;    
  219. FailSetDevTblEntry:
  220. #ifdef DEBUG
  221.     Notify(PSTR("\r\nsetDevTblEn"));
  222. #endif
  223.     goto Fail;
  224. FailGetConfDescr:
  225. #ifdef DEBUG
  226.     Notify(PSTR("\r\ngetConf"));
  227. #endif
  228.     goto Fail;
  229. FailSetConf:
  230. #ifdef DEBUG
  231.     Notify(PSTR("\r\nsetConf"));
  232. #endif
  233.     goto Fail;
  234. FailUnknownDevice:
  235. #ifdef DEBUG
  236.     Notify(PSTR("\r\nUnknown Device Connected - VID: "));
  237.     PrintHex<uint16_t>(VID);
  238.     Notify(PSTR(" PID: "));
  239.     PrintHex<uint16_t>(PID);
  240. #endif
  241.     pUsb->setAddr(bAddress, 0, 0); // Reset address
  242.     rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
  243.     goto Fail;
  244. Fail:
  245. #ifdef DEBUG
  246.     Notify(PSTR("\r\nBTD Init Failed, error code: "));
  247.     Serial.print(rcode);                    
  248. #endif    
  249.     Release();
  250.     return rcode;
  251. }
  252. /* Extracts interrupt-IN, bulk-IN, bulk-OUT endpoint information from config descriptor */
  253. void BTD::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *pep) {
  254.     //ErrorMessage<uint8_t>(PSTR("Conf.Val"),conf);
  255.     //ErrorMessage<uint8_t>(PSTR("Iface Num"),iface);
  256.     //ErrorMessage<uint8_t>(PSTR("Alt.Set"),alt);
  257.    
  258.     if(alt) // wrong interface - by BT spec, no alt setting
  259.         return;
  260.    
  261.     bConfNum = conf;    
  262.     uint8_t index;
  263.    
  264.     if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80) // Interrupt In endpoint found
  265.         index = BTD_EVENT_PIPE;
  266.    
  267.     else {
  268.         if ((pep->bmAttributes & 0x02) == 2) // bulk endpoint found
  269.             index = ((pep->bEndpointAddress & 0x80) == 0x80) ? BTD_DATAIN_PIPE : BTD_DATAOUT_PIPE;
  270.         else
  271.             return;
  272.     }
  273.    
  274.     // Fill the rest of endpoint data structure  
  275.     epInfo[index].epAddr        = (pep->bEndpointAddress & 0x0F);
  276.     epInfo[index].maxPktSize    = (uint8_t)pep->wMaxPacketSize;  
  277. #ifdef EXTRADEBUG
  278.     PrintEndpointDescriptor(pep);    
  279. #endif
  280.     if(pollInterval < pep->bInterval) // Set the polling interval as the largest polling interval obtained from endpoints
  281.         pollInterval = pep->bInterval;  
  282.     bNumEP++;
  283. }
  284. void BTD::PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR* ep_ptr) {
  285.     Notify(PSTR("\r\nEndpoint descriptor:"));
  286.     Notify(PSTR("\r\nLength:\t\t"));
  287.     PrintHex<uint8_t>(ep_ptr->bLength);
  288.     Notify(PSTR("\r\nType:\t\t"));
  289.     PrintHex<uint8_t>(ep_ptr->bDescriptorType);
  290.     Notify(PSTR("\r\nAddress:\t"));
  291.     PrintHex<uint8_t>(ep_ptr->bEndpointAddress);
  292.     Notify(PSTR("\r\nAttributes:\t"));
  293.     PrintHex<uint8_t>(ep_ptr->bmAttributes);
  294.     Notify(PSTR("\r\nMaxPktSize:\t"));
  295.     PrintHex<uint16_t>(ep_ptr->wMaxPacketSize);
  296.     Notify(PSTR("\r\nPoll Intrv:\t"));
  297.     PrintHex<uint8_t>(ep_ptr->bInterval);
  298. }
  299.  
  300. /* Performs a cleanup after failed Init() attempt */
  301. uint8_t BTD::Release() {
  302.     for (uint8_t i=0; i<BTD_NUMSERVICES; i++)
  303.         if (btService[i])
  304.             btService[i]->Reset(); // Reset all Bluetooth services
  305.     pUsb->GetAddressPool().FreeAddress(bAddress);    
  306.     bAddress = 0;
  307.     bPollEnable = false;
  308.     bNumEP = 1; // must have to be reset to 1  
  309.     return 0;
  310. }
  311. uint8_t BTD::Poll() {
  312.     if (!bPollEnable)
  313.         return 0;
  314.     if (qNextPollTime <= millis()) { // Don't poll if shorter than polling interval
  315.         qNextPollTime = millis() + pollInterval; // Set new poll time
  316.         HCI_event_task(); // poll the HCI event pipe
  317.         ACL_event_task(); // start polling the ACL input pipe too, though discard data until connected        
  318.     }    
  319.     return 0;
  320. }
  321.  
  322. void BTD::HCI_event_task() {
  323.     /* check the event pipe*/    
  324.     uint16_t MAX_BUFFER_SIZE = BULK_MAXPKTSIZE; // Request more than 16 bytes anyway, the inTransfer routine will take care of this
  325.     uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_EVENT_PIPE ].epAddr, &MAX_BUFFER_SIZE, hcibuf); // input on endpoint 1
  326.     if(!rcode || rcode == hrNAK) // Check for errors
  327.     {
  328.         switch (hcibuf[0]) //switch on event type
  329.         {
  330.             case EV_COMMAND_COMPLETE:                
  331.                 if (!hcibuf[5]) { // Check if command succeeded
  332.                     hci_event_flag |= HCI_FLAG_CMD_COMPLETE; // set command complete flag
  333.                     if((hcibuf[3] == 0x01) && (hcibuf[4] == 0x10)) { // parameters from read local version information
  334.                         hci_version = hcibuf[6]; // Used to check if it supports 2.0+EDR - see http://www.bluetooth.org/Technical/AssignedNumbers/hci.htm
  335.                         hci_event_flag |= HCI_FLAG_READ_VERSION;
  336.                     } else if((hcibuf[3] == 0x09) && (hcibuf[4] == 0x10)) { // parameters from read local bluetooth address
  337.                         for (uint8_t i = 0; i < 6; i++)
  338.                             my_bdaddr[i] = hcibuf[6 + i];
  339.                         hci_event_flag |= HCI_FLAG_READ_BDADDR;
  340.                     }
  341.                 }
  342.                 break;
  343.                
  344.             case EV_COMMAND_STATUS:
  345.                 if(hcibuf[2]) { // show status on serial if not OK
  346. #ifdef DEBUG
  347.                     Notify(PSTR("\r\nHCI Command Failed: "));
  348.                     PrintHex<uint8_t>(hcibuf[2]);
  349.                     Notify(PSTR(" "));
  350.                     PrintHex<uint8_t>(hcibuf[4]);
  351.                     Notify(PSTR(" "));
  352.                     PrintHex<uint8_t>(hcibuf[5]);                    
  353. #endif
  354.                 }
  355.                 break;
  356.                
  357.             case EV_INQUIRY_COMPLETE: // We don't use this for anything
  358.                 break;
  359.                
  360.             case EV_INQUIRY_RESULT:
  361.                 if (hcibuf[2]) { // Check that there is more than zero responses
  362. #ifdef EXTRADEBUG
  363.                     Notify(PSTR("\r\nNumber of responses: "));
  364.                     Serial.print(hcibuf[2]);
  365. #endif
  366.                     for(uint8_t i = 0; i < hcibuf[2]; i++) {
  367.                         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) { // See http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html
  368.                             disc_bdaddr[0] = hcibuf[3+6*i];
  369.                             disc_bdaddr[1] = hcibuf[4+6*i];
  370.                             disc_bdaddr[2] = hcibuf[5+6*i];
  371.                             disc_bdaddr[3] = hcibuf[6+6*i];
  372.                             disc_bdaddr[4] = hcibuf[7+6*i];
  373.                             disc_bdaddr[5] = hcibuf[8+6*i];
  374.                             hci_event_flag |= HCI_FLAG_WII_FOUND;
  375.                             break;
  376.                         }
  377. #ifdef EXTRADEBUG
  378.                         else {
  379.                             Notify(PSTR("\r\nClass of device: "));
  380.                             PrintHex<uint8_t>(hcibuf[6+8*hcibuf[2]+3*i]);
  381.                             Notify(PSTR(" "));
  382.                             PrintHex<uint8_t>(hcibuf[5+8*hcibuf[2]+3*i]);
  383.                             Notify(PSTR(" "));
  384.                             PrintHex<uint8_t>(hcibuf[4+8*hcibuf[2]+3*i]);
  385.                         }
  386. #endif
  387.                     }
  388.                 }
  389.                 break;
  390.                
  391.             case EV_CONNECT_COMPLETE:
  392.                 hci_event_flag |= HCI_FLAG_CONNECT_EVENT;
  393.                 if (!hcibuf[2]) { // check if connected OK
  394.                     hci_handle = hcibuf[3] | ((hcibuf[4] & 0x0F) << 8); // store the handle for the ACL connection
  395.                     hci_event_flag |= HCI_FLAG_CONN_COMPLETE; // set connection complete flag
  396.                 }
  397. #ifdef EXTRADEBUG
  398.                 else {
  399.                     Notify(PSTR("\r\nConnection Failed"));
  400.                 }
  401. #endif
  402.                 break;
  403.                
  404.             case EV_DISCONNECT_COMPLETE:            
  405.                 if (!hcibuf[2]) { // check if disconnected OK
  406.                     hci_event_flag |= HCI_FLAG_DISCONN_COMPLETE; // set disconnect command complete flag
  407.                     hci_event_flag &= ~HCI_FLAG_CONN_COMPLETE; // clear connection complete flag
  408.                 }
  409.                 break;                              
  410.                
  411.             case EV_REMOTE_NAME_COMPLETE:
  412.                 if (!hcibuf[2]) { // check if reading is OK
  413.                     for (uint8_t i = 0; i < 30; i++)
  414.                         remote_name[i] = hcibuf[9 + i];  //store first 30 bytes
  415.                     hci_event_flag |= HCI_FLAG_REMOTE_NAME_COMPLETE;
  416.                 }
  417.                 break;
  418.                
  419.             case EV_INCOMING_CONNECT:
  420.                 disc_bdaddr[0] = hcibuf[2];
  421.                 disc_bdaddr[1] = hcibuf[3];
  422.                 disc_bdaddr[2] = hcibuf[4];
  423.                 disc_bdaddr[3] = hcibuf[5];
  424.                 disc_bdaddr[4] = hcibuf[6];
  425.                 disc_bdaddr[5] = hcibuf[7];
  426.                 hci_event_flag |= HCI_FLAG_INCOMING_REQUEST;
  427.                 break;
  428.                
  429.             case EV_PIN_CODE_REQUEST:
  430.                 if(btdPin != NULL) {
  431. #ifdef DEBUG
  432.                     Notify(PSTR("\r\nBluetooth pin is set too: "));
  433.                     Serial.print(btdPin);
  434. #endif
  435.                     hci_pin_code_request_reply(btdPin);
  436.                 }
  437.                 else {
  438. #ifdef DEBUG
  439.                     Notify(PSTR("\r\nNo pin was set"));
  440. #endif
  441.                     hci_pin_code_negative_request_reply();
  442.                 }
  443.                 break;
  444.                
  445.             case EV_LINK_KEY_REQUEST:
  446. #ifdef DEBUG
  447.                 Notify(PSTR("\r\nReceived Key Request"));
  448. #endif
  449.                 hci_link_key_request_negative_reply();
  450.                 break;
  451.                
  452.             /* We will just ignore the following events */                
  453.             case EV_NUM_COMPLETE_PKT:                
  454.             case EV_ROLE_CHANGED:                
  455.             case EV_PAGE_SCAN_REP_MODE:                
  456.             case EV_LOOPBACK_COMMAND:                
  457.             case EV_DATA_BUFFER_OVERFLOW:                
  458.             case EV_CHANGE_CONNECTION_LINK:                
  459.             case EV_AUTHENTICATION_COMPLETE:                
  460.             case EV_MAX_SLOTS_CHANGE:
  461.             case EV_QOS_SETUP_COMPLETE:
  462.             case EV_LINK_KEY_NOTIFICATION:
  463.             case EV_ENCRYPTION_CHANGE:
  464.             case EV_READ_REMOTE_VERSION_INFORMATION_COMPLETE:
  465.                 break;
  466. #ifdef EXTRADEBUG                
  467.             default:
  468.                 if(hcibuf[0] != 0x00) {
  469.                     Notify(PSTR("\r\nUnmanaged HCI Event: "));
  470.                     PrintHex<uint8_t>(hcibuf[0]);
  471.                 }
  472.                 break;
  473. #endif
  474.         } // switch
  475.         HCI_task();
  476.     }
  477. #ifdef EXTRADEBUG
  478.     else {
  479.         Notify(PSTR("\r\nHCI event error: "));
  480.         PrintHex<uint8_t>(rcode);
  481.     }
  482. #endif
  483. }
  484.  
  485. /* Poll Bluetooth and print result */
  486. void BTD::HCI_task() {
  487.     switch (hci_state){
  488.         case HCI_INIT_STATE:
  489.             hci_counter++;
  490.             if (hci_counter > hci_num_reset_loops) { // wait until we have looped x times to clear any old events
  491.                 hci_reset();
  492.                 hci_state = HCI_RESET_STATE;
  493.                 hci_counter = 0;
  494.             }
  495.             break;
  496.            
  497.         case HCI_RESET_STATE:
  498.             hci_counter++;
  499.             if (hci_cmd_complete) {
  500. #ifdef DEBUG
  501.                 Notify(PSTR("\r\nHCI Reset complete"));
  502. #endif
  503.                 hci_state = HCI_BDADDR_STATE;
  504.                 hci_read_bdaddr();
  505.             }
  506.             else if (hci_counter > hci_num_reset_loops) {
  507.                 hci_num_reset_loops *= 10;
  508.                 if(hci_num_reset_loops > 2000)
  509.                     hci_num_reset_loops = 2000;
  510. #ifdef DEBUG
  511.                 Notify(PSTR("\r\nNo response to HCI Reset"));
  512. #endif
  513.                 hci_state = HCI_INIT_STATE;
  514.                 hci_counter = 0;
  515.             }
  516.             break;
  517.            
  518.         case HCI_BDADDR_STATE:
  519.             if (hci_read_bdaddr_complete) {
  520. #ifdef DEBUG
  521.                 Notify(PSTR("\r\nLocal Bluetooth Address: "));
  522.                 for(int8_t i = 5; i > 0;i--) {
  523.                     PrintHex<uint8_t>(my_bdaddr[i]);
  524.                     Notify(PSTR(":"));
  525.                 }      
  526.                 PrintHex<uint8_t>(my_bdaddr[0]);
  527. #endif
  528.                 hci_read_local_version_information();
  529.                 hci_state = HCI_LOCAL_VERSION_STATE;                
  530.             }
  531.             break;
  532.            
  533.         case HCI_LOCAL_VERSION_STATE: // The local version is used by the PS3BT class
  534.             if (hci_read_version_complete) {
  535.                 if(btdName != NULL) {
  536.                     hci_set_local_name(btdName);
  537.                     hci_state = HCI_SET_NAME_STATE;
  538.                 } else
  539.                     hci_state = HCI_CHECK_WII_SERVICE;                    
  540.             }
  541.             break;
  542.            
  543.         case HCI_SET_NAME_STATE:
  544.             if (hci_cmd_complete) {
  545. #ifdef DEBUG              
  546.                 Notify(PSTR("\r\nThe name is set to: "));
  547.                 Serial.print(btdName);
  548. #endif
  549.                 hci_state = HCI_CHECK_WII_SERVICE;
  550.             }
  551.             break;
  552.            
  553.         case HCI_CHECK_WII_SERVICE:
  554.             if(wiiServiceID != -1) { // Check if it should try to connect to a wiimote
  555.                 if(disc_bdaddr[5] == 0 && disc_bdaddr[4] == 0 && disc_bdaddr[3] == 0 && disc_bdaddr[2] == 0 && disc_bdaddr[1] == 0 && disc_bdaddr[0] == 0) {
  556. #ifdef DEBUG
  557.                     Notify(PSTR("\r\nStarting inquiry\r\nPress 1 & 2 on the Wiimote"));
  558. #endif
  559.                     hci_inquiry();
  560.                     hci_state = HCI_INQUIRY_STATE;
  561.                 }
  562.                 else
  563.                     hci_state = HCI_CONNECT_WII_STATE;
  564.             }
  565.             else
  566.                 hci_state = HCI_SCANNING_STATE; // Don't try to connect to a Wiimote
  567.             break;
  568.            
  569.         case HCI_INQUIRY_STATE:
  570.             if(hci_wii_found) {
  571.                 hci_inquiry_cancel(); // Stop inquiry
  572. #ifdef DEBUG
  573.                 Notify(PSTR("\r\nWiimote found"));
  574.                 Notify(PSTR("\r\nCreate the instance like so to connect automatically:"));
  575.                 Notify(PSTR("\r\nWII Wii(&Btd,"));
  576.                 for(int8_t i = 5; i>0;i--) {
  577.                     Notify(PSTR("0x"));
  578.                     PrintHex<uint8_t>(disc_bdaddr[i]);
  579.                     Notify(PSTR(","));
  580.                 }
  581.                 Notify(PSTR("0x"));
  582.                 PrintHex<uint8_t>(disc_bdaddr[0]);
  583.                 Notify(PSTR(");"));
  584. #endif                                
  585.                 hci_state = HCI_CONNECT_WII_STATE;
  586.             }
  587.             break;
  588.            
  589.         case HCI_CONNECT_WII_STATE:
  590.             if(!hci_wii_found || hci_cmd_complete) {
  591. #ifdef DEBUG
  592.                 Notify(PSTR("\r\nConnecting to Wiimote"));
  593. #endif            
  594.                 hci_connect();
  595.                 hci_state = HCI_CONNECTED_WII_STATE;
  596.             }
  597.             break;
  598.            
  599.         case HCI_CONNECTED_WII_STATE:
  600.             if(hci_connect_event) {
  601.                 if(hci_connect_complete) {
  602. #ifdef DEBUG
  603.                     Notify(PSTR("\r\nConnected to Wiimote"));
  604. #endif
  605.                     connectToWii = true; // Only send the ACL data to the Wii service
  606.                     hci_state = HCI_SCANNING_STATE;
  607.                 } else {
  608. #ifdef DEBUG
  609.                     Notify(PSTR("\r\nTrying to connect one more time..."));
  610. #endif
  611.                     hci_connect(); // Try to connect one more time
  612.                 }
  613.             }
  614.             break;
  615.            
  616.         case HCI_SCANNING_STATE:
  617.             if(!connectToWii) {
  618. #ifdef DEBUG
  619.                 Notify(PSTR("\r\nWait For Incoming Connection Request"));
  620. #endif            
  621.                 hci_write_scan_enable();
  622.                 watingForConnection = true;
  623.                 hci_state = HCI_CONNECT_IN_STATE;
  624.             }
  625.             break;
  626.            
  627.         case HCI_CONNECT_IN_STATE:
  628.             if(hci_incoming_connect_request) {
  629.                 watingForConnection = false;
  630. #ifdef DEBUG
  631.                 Notify(PSTR("\r\nIncoming Connection Request"));
  632. #endif                
  633.                 hci_remote_name();
  634.                 hci_state = HCI_REMOTE_NAME_STATE;
  635.             } else if (hci_disconnect_complete)
  636.                 hci_state = HCI_DISCONNECT_STATE;
  637.             break;    
  638.            
  639.         case HCI_REMOTE_NAME_STATE:
  640.             if(hci_remote_name_complete) {
  641. #ifdef DEBUG
  642.                 Notify(PSTR("\r\nRemote Name: "));
  643.                 for (uint8_t i = 0; i < 30; i++) {
  644.                     if(remote_name[i] == NULL)
  645.                         break;
  646.                     Serial.write(remote_name[i]);  
  647.                 }            
  648. #endif
  649.                 hci_accept_connection();
  650.                 hci_state = HCI_CONNECTED_STATE;                                
  651.             }      
  652.             break;
  653.            
  654.         case HCI_CONNECTED_STATE:
  655.             if (hci_connect_complete) {    
  656. #ifdef DEBUG
  657.                 Notify(PSTR("\r\nConnected to Device: "));
  658.                 for(int8_t i = 5; i>0;i--) {
  659.                     PrintHex<uint8_t>(disc_bdaddr[i]);
  660.                     Notify(PSTR(":"));
  661.                 }      
  662.                 PrintHex<uint8_t>(disc_bdaddr[0]);
  663. #endif
  664.                 hci_write_scan_disable();
  665.                 hci_state = HCI_DISABLE_SCAN_STATE;
  666.             }
  667.             break;
  668.            
  669.         case HCI_DISABLE_SCAN_STATE:
  670.             if (hci_cmd_complete) {                    
  671. #ifdef DEBUG
  672.                 Notify(PSTR("\r\nScan Disabled"));
  673. #endif
  674.                 hci_event_flag = 0;
  675.                 hci_state = HCI_DONE_STATE;
  676.             }
  677.             break;
  678.            
  679.         case HCI_DONE_STATE:
  680.             hci_counter++;
  681.             if (hci_counter > 250) { // Wait until we have looped 250 times to make sure that the L2CAP connection has been started
  682.                 hci_state = HCI_SCANNING_STATE;
  683.                 l2capConnectionClaimed = false;
  684.             }            
  685.             break;
  686.            
  687.         case HCI_DISCONNECT_STATE:
  688.             if (hci_disconnect_complete) {
  689. #ifdef DEBUG
  690.                 Notify(PSTR("\r\nHCI Disconnected from Device"));
  691. #endif
  692.                 hci_event_flag = 0; // Clear all flags
  693.                
  694.                 // Reset all buffers                        
  695.                 for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++)
  696.                     hcibuf[i] = 0;        
  697.                 for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++)
  698.                     l2capinbuf[i] = 0;
  699.                                                        
  700.                 hci_state = HCI_SCANNING_STATE;
  701.             }
  702.             break;
  703.         default:
  704.             break;
  705.     }
  706. }
  707.  
  708. void BTD::ACL_event_task() {
  709.     uint16_t MAX_BUFFER_SIZE = BULK_MAXPKTSIZE;
  710.     uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_DATAIN_PIPE ].epAddr, &MAX_BUFFER_SIZE, l2capinbuf); // input on endpoint 2  
  711.     if(!rcode) { // Check for errors
  712.         if(connectToWii) // Only send the data to the Wii service
  713.             btService[wiiServiceID]->ACLData(l2capinbuf);
  714.         else {
  715.             for (uint8_t i=0; i<BTD_NUMSERVICES; i++)
  716.                 if (btService[i])
  717.                     btService[i]->ACLData(l2capinbuf);
  718.         }
  719.     }
  720. #ifdef EXTRADEBUG
  721.     else if (rcode != hrNAK) {
  722.         Notify(PSTR("\r\nACL data in error: "));
  723.         PrintHex<uint8_t>(rcode);
  724.     }
  725. #endif
  726.     for (uint8_t i=0; i<BTD_NUMSERVICES; i++)
  727.         if (btService[i])
  728.             btService[i]->Run();
  729. }
  730.  
  731. /************************************************************/
  732. /*                    HCI Commands                        */
  733. /************************************************************/
  734. void BTD::HCI_Command(uint8_t* data, uint16_t nbytes) {
  735.     hci_event_flag &= ~HCI_FLAG_CMD_COMPLETE;
  736.     pUsb->ctrlReq(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bmREQ_HCI_OUT, 0x00, 0x00, 0x00 ,0x00, nbytes, nbytes, data, NULL);    
  737. }
  738. void BTD::hci_reset() {
  739.     hci_event_flag = 0; // Clear all the flags
  740.     hcibuf[0] = 0x03; // HCI OCF = 3
  741.     hcibuf[1] = 0x03 << 2; // HCI OGF = 3
  742.     hcibuf[2] = 0x00;
  743.     HCI_Command(hcibuf, 3);
  744. }
  745. void BTD::hci_write_scan_enable() {
  746.     hci_event_flag &= ~HCI_FLAG_INCOMING_REQUEST;
  747.     hcibuf[0] = 0x1A; // HCI OCF = 1A
  748.     hcibuf[1] = 0x03 << 2; // HCI OGF = 3
  749.     hcibuf[2] = 0x01; // parameter length = 1
  750.     if(btdName != NULL)
  751.         hcibuf[3] = 0x03; // Inquiry Scan enabled. Page Scan enabled.
  752.     else
  753.         hcibuf[3] = 0x02; // Inquiry Scan disabled. Page Scan enabled.
  754.     HCI_Command(hcibuf, 4);
  755. }
  756. void BTD::hci_write_scan_disable() {
  757.     hcibuf[0] = 0x1A; // HCI OCF = 1A
  758.     hcibuf[1] = 0x03 << 2; // HCI OGF = 3
  759.     hcibuf[2] = 0x01; // parameter length = 1
  760.     hcibuf[3] = 0x00; // Inquiry Scan disabled. Page Scan disabled.
  761.     HCI_Command(hcibuf, 4);
  762. }
  763. void BTD::hci_read_bdaddr() {  
  764.     hcibuf[0] = 0x09; // HCI OCF = 9
  765.     hcibuf[1] = 0x04 << 2; // HCI OGF = 4
  766.     hcibuf[2] = 0x00;
  767.     HCI_Command(hcibuf, 3);
  768. }
  769. void BTD::hci_read_local_version_information() {
  770.     hcibuf[0] = 0x01; // HCI OCF = 1
  771.     hcibuf[1] = 0x04 << 2; // HCI OGF = 4
  772.     hcibuf[2] = 0x00;
  773.     HCI_Command(hcibuf, 3);
  774. }
  775. void BTD::hci_accept_connection() {
  776.     hci_event_flag &= ~HCI_FLAG_CONN_COMPLETE;
  777.     hcibuf[0] = 0x09; // HCI OCF = 9
  778.     hcibuf[1] = 0x01 << 2; // HCI OGF = 1
  779.     hcibuf[2] = 0x07; // parameter length 7
  780.     hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
  781.     hcibuf[4] = disc_bdaddr[1];
  782.     hcibuf[5] = disc_bdaddr[2];
  783.     hcibuf[6] = disc_bdaddr[3];
  784.     hcibuf[7] = disc_bdaddr[4];
  785.     hcibuf[8] = disc_bdaddr[5];
  786.     hcibuf[9] = 0x00; //switch role to master
  787.    
  788.     HCI_Command(hcibuf, 10);
  789. }
  790. void BTD::hci_remote_name() {
  791.     hci_event_flag &= ~HCI_FLAG_REMOTE_NAME_COMPLETE;
  792.     hcibuf[0] = 0x19; // HCI OCF = 19
  793.     hcibuf[1] = 0x01 << 2; // HCI OGF = 1
  794.     hcibuf[2] = 0x0A; // parameter length = 10
  795.     hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
  796.     hcibuf[4] = disc_bdaddr[1];
  797.     hcibuf[5] = disc_bdaddr[2];
  798.     hcibuf[6] = disc_bdaddr[3];
  799.     hcibuf[7] = disc_bdaddr[4];
  800.     hcibuf[8] = disc_bdaddr[5];
  801.     hcibuf[9] = 0x01; //Page Scan Repetition Mode
  802.     hcibuf[10] = 0x00; //Reserved
  803.     hcibuf[11] = 0x00; //Clock offset - low byte
  804.     hcibuf[12] = 0x00; //Clock offset - high byte
  805.    
  806.     HCI_Command(hcibuf, 13);
  807. }
  808. void BTD::hci_set_local_name(const char* name) {
  809.     hcibuf[0] = 0x13; // HCI OCF = 13
  810.     hcibuf[1] = 0x03 << 2; // HCI OGF = 3
  811.     hcibuf[2] = strlen(name)+1; // parameter length = the length of the string + end byte
  812.     uint8_t i;
  813.     for(i = 0; i < strlen(name); i++)
  814.         hcibuf[i+3] = name[i];
  815.     hcibuf[i+3] = 0x00; // End of string
  816.  
  817.     HCI_Command(hcibuf, 4+strlen(name));
  818. }
  819. void BTD::hci_inquiry() {
  820.     hci_event_flag &= ~HCI_FLAG_WII_FOUND;
  821.     hcibuf[0] = 0x01;
  822.     hcibuf[1] = 0x01 << 2; // HCI OGF = 1
  823.     hcibuf[2] = 0x05;  // Parameter Total Length = 5
  824.     hcibuf[3] = 0x33;  // LAP: Genera/Unlimited Inquiry Access Code (GIAC = 0x9E8B33) - see https://www.bluetooth.org/Technical/AssignedNumbers/baseband.htm
  825.     hcibuf[4] = 0x8B;
  826.     hcibuf[5] = 0x9E;
  827.     hcibuf[6] = 0x0A;  // Inquiry time = 12.8 sec
  828.     hcibuf[7] = 0x03;  // 3 number of responses
  829.    
  830.     HCI_Command(hcibuf, 8);
  831. }
  832. void BTD::hci_inquiry_cancel() {
  833.     hcibuf[0] = 0x02;
  834.     hcibuf[1] = 0x01 << 2; // HCI OGF = 1
  835.     hcibuf[2] = 0x0;   // Parameter Total Length = 0
  836.    
  837.     HCI_Command(hcibuf, 3);
  838. }
  839. void BTD::hci_connect() {
  840.     hci_event_flag &= ~(HCI_FLAG_CONN_COMPLETE | HCI_FLAG_CONNECT_EVENT);
  841.     hcibuf[0] = 0x05;
  842.     hcibuf[1] = 0x01 << 2; // HCI OGF = 1
  843.     hcibuf[2] = 0x0D;  // parameter Total Length = 13
  844.     hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
  845.     hcibuf[4] = disc_bdaddr[1];
  846.     hcibuf[5] = disc_bdaddr[2];
  847.     hcibuf[6] = disc_bdaddr[3];
  848.     hcibuf[7] = disc_bdaddr[4];
  849.     hcibuf[8] = disc_bdaddr[5];
  850.     hcibuf[9] = 0x18; // DM1 or DH1 may be used
  851.     hcibuf[10] = 0xCC; // DM3, DH3, DM5, DH5 may be used
  852.     hcibuf[11] = 0x01; // Page repetition mode R1
  853.     hcibuf[12] = 0x00; // Reserved
  854.     hcibuf[13] = 0x00; // Clock offset
  855.     hcibuf[14] = 0x00; // Invalid clock offset
  856.     hcibuf[15] = 0x00; // Do not allow role switch
  857.    
  858.     HCI_Command(hcibuf, 16);
  859. }
  860. void BTD::hci_pin_code_request_reply(const char* key) {
  861.     hcibuf[0] = 0x0D; // HCI OCF = 0D
  862.     hcibuf[1] = 0x01 << 2; // HCI OGF = 1
  863.     hcibuf[2] = 0x17; // parameter length 23
  864.     hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
  865.     hcibuf[4] = disc_bdaddr[1];
  866.     hcibuf[5] = disc_bdaddr[2];
  867.     hcibuf[6] = disc_bdaddr[3];
  868.     hcibuf[7] = disc_bdaddr[4];
  869.     hcibuf[8] = disc_bdaddr[5];
  870.     hcibuf[9] = strlen(key); // Length of key
  871.     uint8_t i;
  872.     for(i = 0; i < strlen(key); i++) // The maximum size of the key is 16
  873.         hcibuf[i+10] = key[i];
  874.     for(;i < 16; i++)
  875.         hcibuf[i+10] = 0x00; // The rest should be 0
  876.    
  877.     HCI_Command(hcibuf, 26);
  878. }
  879. void BTD::hci_pin_code_negative_request_reply() {
  880.     hcibuf[0] = 0x0E; // HCI OCF = 0E
  881.     hcibuf[1] = 0x01 << 2; // HCI OGF = 1
  882.     hcibuf[2] = 0x06; // parameter length 6
  883.     hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
  884.     hcibuf[4] = disc_bdaddr[1];
  885.     hcibuf[5] = disc_bdaddr[2];
  886.     hcibuf[6] = disc_bdaddr[3];
  887.     hcibuf[7] = disc_bdaddr[4];
  888.     hcibuf[8] = disc_bdaddr[5];
  889.    
  890.     HCI_Command(hcibuf, 9);    
  891. }
  892. void BTD::hci_link_key_request_negative_reply() {
  893.     hcibuf[0] = 0x0C; // HCI OCF = 0C
  894.     hcibuf[1] = 0x01 << 2; // HCI OGF = 1
  895.     hcibuf[2] = 0x06; // parameter length 6
  896.     hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
  897.     hcibuf[4] = disc_bdaddr[1];
  898.     hcibuf[5] = disc_bdaddr[2];
  899.     hcibuf[6] = disc_bdaddr[3];
  900.     hcibuf[7] = disc_bdaddr[4];
  901.     hcibuf[8] = disc_bdaddr[5];
  902.    
  903.     HCI_Command(hcibuf, 9);    
  904. }
  905. void BTD::hci_disconnect(uint16_t handle) { // This is called by the different services
  906.     hci_event_flag &= ~HCI_FLAG_DISCONN_COMPLETE;
  907.     hcibuf[0] = 0x06; // HCI OCF = 6
  908.     hcibuf[1] = 0x01 << 2; // HCI OGF = 1
  909.     hcibuf[2] = 0x03; // parameter length = 3
  910.     hcibuf[3] = (uint8_t)(handle & 0xFF);//connection handle - low byte
  911.     hcibuf[4] = (uint8_t)((handle >> 8) & 0x0F);//connection handle - high byte
  912.     hcibuf[5] = 0x13; // reason
  913.    
  914.     HCI_Command(hcibuf, 6);
  915. }
  916. /*******************************************************************
  917.  *                                                                 *
  918.  *                        HCI ACL Data Packet                      *
  919.  *                                                                 *
  920.  *   buf[0]          buf[1]          buf[2]          buf[3]
  921.  *   0       4       8    11 12      16              24            31 MSB
  922.  *  .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
  923.  *  |      HCI Handle       |PB |BC |       Data Total Length       |   HCI ACL Data Packet
  924.  *  .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
  925.  *
  926.  *   buf[4]          buf[5]          buf[6]          buf[7]
  927.  *   0               8               16                            31 MSB
  928.  *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
  929.  *  |            Length             |          Channel ID           |   Basic L2CAP header
  930.  *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
  931.  *
  932.  *   buf[8]          buf[9]          buf[10]         buf[11]
  933.  *   0               8               16                            31 MSB
  934.  *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
  935.  *  |     Code      |  Identifier   |            Length             |   Control frame (C-frame)
  936.  *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.   (signaling packet format)
  937.  */
  938. /************************************************************/
  939. /*                    L2CAP Commands                        */
  940. /************************************************************/
  941. void BTD::L2CAP_Command(uint16_t handle, uint8_t* data, uint8_t nbytes, uint8_t channelLow, uint8_t channelHigh) {
  942.     uint8_t buf[8+nbytes];
  943.     buf[0] = (uint8_t)(handle & 0xff); // HCI handle with PB,BC flag
  944.     buf[1] = (uint8_t)(((handle >> 8) & 0x0f) | 0x20);
  945.     buf[2] = (uint8_t)((4 + nbytes) & 0xff); // HCI ACL total data length
  946.     buf[3] = (uint8_t)((4 + nbytes) >> 8);
  947.     buf[4] = (uint8_t)(nbytes & 0xff); // L2CAP header: Length
  948.     buf[5] = (uint8_t)(nbytes >> 8);
  949.     buf[6] = channelLow;
  950.     buf[7] = channelHigh;
  951.    
  952.     for (uint16_t i = 0; i < nbytes; i++) // L2CAP C-frame
  953.         buf[8 + i] = data[i];        
  954.    
  955.     uint8_t rcode = pUsb->outTransfer(bAddress, epInfo[ BTD_DATAOUT_PIPE ].epAddr, (8 + nbytes), buf);
  956.     if(rcode) {
  957.         delay(100); // This small delay prevents it from overflowing if it fails
  958. #ifdef DEBUG
  959.         Notify(PSTR("\r\nError sending L2CAP message: 0x"));
  960.         PrintHex<uint8_t>(rcode);
  961.         Notify(PSTR(" - Channel ID: "));
  962.         Serial.print(channelHigh);
  963.         Notify(PSTR(" "));
  964.         Serial.print(channelLow);
  965. #endif        
  966.     }
  967. }
  968. void BTD::l2cap_connection_request(uint16_t handle, uint8_t rxid, uint8_t* scid, uint16_t psm) {
  969.     l2capoutbuf[0] = L2CAP_CMD_CONNECTION_REQUEST;  // Code
  970.     l2capoutbuf[1] = rxid; // Identifier
  971.     l2capoutbuf[2] = 0x04; // Length
  972.     l2capoutbuf[3] = 0x00;
  973.     l2capoutbuf[4] = (uint8_t)(psm & 0xff); // PSM
  974.     l2capoutbuf[5] = (uint8_t)(psm >> 8);
  975.     l2capoutbuf[6] = scid[0]; // Source CID
  976.     l2capoutbuf[7] = scid[1];
  977.    
  978.     L2CAP_Command(handle, l2capoutbuf, 8);
  979. }
  980. void BTD::l2cap_connection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid, uint8_t result) {            
  981.     l2capoutbuf[0] = L2CAP_CMD_CONNECTION_RESPONSE; // Code
  982.     l2capoutbuf[1] = rxid; // Identifier
  983.     l2capoutbuf[2] = 0x08; // Length
  984.     l2capoutbuf[3] = 0x00;
  985.     l2capoutbuf[4] = dcid[0]; // Destination CID
  986.     l2capoutbuf[5] = dcid[1];
  987.     l2capoutbuf[6] = scid[0]; // Source CID
  988.     l2capoutbuf[7] = scid[1];
  989.     l2capoutbuf[8] = result; // Result: Pending or Success
  990.     l2capoutbuf[9] = 0x00;
  991.     l2capoutbuf[10] = 0x00; // No further information
  992.     l2capoutbuf[11] = 0x00;
  993.    
  994.     L2CAP_Command(handle, l2capoutbuf, 12);
  995. }        
  996. void BTD::l2cap_config_request(uint16_t handle, uint8_t rxid, uint8_t* dcid) {
  997.     l2capoutbuf[0] = L2CAP_CMD_CONFIG_REQUEST; // Code
  998.     l2capoutbuf[1] = rxid; // Identifier
  999.     l2capoutbuf[2] = 0x08; // Length
  1000.     l2capoutbuf[3] = 0x00;
  1001.     l2capoutbuf[4] = dcid[0]; // Destination CID
  1002.     l2capoutbuf[5] = dcid[1];
  1003.     l2capoutbuf[6] = 0x00; // Flags
  1004.     l2capoutbuf[7] = 0x00;
  1005.     l2capoutbuf[8] = 0x01; // Config Opt: type = MTU (Maximum Transmission Unit) - Hint
  1006.     l2capoutbuf[9] = 0x02; // Config Opt: length
  1007.     l2capoutbuf[10] = 0xFF; // MTU
  1008.     l2capoutbuf[11] = 0xFF;
  1009.    
  1010.     L2CAP_Command(handle, l2capoutbuf, 12);
  1011. }
  1012. void BTD::l2cap_config_response(uint16_t handle, uint8_t rxid, uint8_t* scid) {            
  1013.     l2capoutbuf[0] = L2CAP_CMD_CONFIG_RESPONSE; // Code
  1014.     l2capoutbuf[1] = rxid; // Identifier
  1015.     l2capoutbuf[2] = 0x0A; // Length
  1016.     l2capoutbuf[3] = 0x00;
  1017.     l2capoutbuf[4] = scid[0]; // Source CID
  1018.     l2capoutbuf[5] = scid[1];
  1019.     l2capoutbuf[6] = 0x00; // Flag
  1020.     l2capoutbuf[7] = 0x00;
  1021.     l2capoutbuf[8] = 0x00; // Result
  1022.     l2capoutbuf[9] = 0x00;    
  1023.     l2capoutbuf[10] = 0x01; // Config
  1024.     l2capoutbuf[11] = 0x02;
  1025.     l2capoutbuf[12] = 0xA0;
  1026.     l2capoutbuf[13] = 0x02;
  1027.    
  1028.     L2CAP_Command(handle, l2capoutbuf, 14);
  1029. }
  1030. void BTD::l2cap_disconnection_request(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid) {
  1031.     l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_REQUEST; // Code
  1032.     l2capoutbuf[1] = rxid; // Identifier
  1033.     l2capoutbuf[2] = 0x04; // Length
  1034.     l2capoutbuf[3] = 0x00;
  1035.     l2capoutbuf[4] = dcid[0];
  1036.     l2capoutbuf[5] = dcid[1];
  1037.     l2capoutbuf[6] = scid[0];
  1038.     l2capoutbuf[7] = scid[1];
  1039.     L2CAP_Command(handle, l2capoutbuf, 8);
  1040. }
  1041. void BTD::l2cap_disconnection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid) {
  1042.     l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_RESPONSE; // Code
  1043.     l2capoutbuf[1] = rxid; // Identifier
  1044.     l2capoutbuf[2] = 0x04; // Length
  1045.     l2capoutbuf[3] = 0x00;
  1046.     l2capoutbuf[4] = dcid[0];
  1047.     l2capoutbuf[5] = dcid[1];
  1048.     l2capoutbuf[6] = scid[0];
  1049.     l2capoutbuf[7] = scid[1];
  1050.     L2CAP_Command(handle, l2capoutbuf, 8);
  1051. }
  1052. void BTD::l2cap_information_response(uint16_t handle, uint8_t rxid, uint8_t infoTypeLow, uint8_t infoTypeHigh) {
  1053.     l2capoutbuf[0] = L2CAP_CMD_INFORMATION_RESPONSE; // Code
  1054.     l2capoutbuf[1] = rxid; // Identifier
  1055.     l2capoutbuf[2] = 0x08; // Length
  1056.     l2capoutbuf[3] = 0x00;
  1057.     l2capoutbuf[4] = infoTypeLow;
  1058.     l2capoutbuf[5] = infoTypeHigh;
  1059.     l2capoutbuf[6] = 0x00; // Result = success
  1060.     l2capoutbuf[7] = 0x00; // Result = success
  1061.     l2capoutbuf[8] = 0x00;
  1062.     l2capoutbuf[9] = 0x00;
  1063.     l2capoutbuf[10] = 0x00;
  1064.     l2capoutbuf[11] = 0x00;
  1065.     L2CAP_Command(handle, l2capoutbuf, 12);
  1066. }
  1067.  
  1068. /* PS3 Commands - only set Bluetooth address is implemented */
  1069. void BTD::setBdaddr(uint8_t* BDADDR) {
  1070.     /* Set the internal bluetooth address */
  1071.     uint8_t buf[8];
  1072.     buf[0] = 0x01;
  1073.     buf[1] = 0x00;
  1074.     for (uint8_t i = 0; i < 6; i++)
  1075.         buf[i+2] = BDADDR[5 - i];//Copy into buffer, has to be written reversed
  1076.    
  1077.     //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)
  1078.     pUsb->ctrlReq(bAddress,epInfo[BTD_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL);
  1079. #ifdef DEBUG
  1080.     Notify(PSTR("\r\nBluetooth Address was set to: "));
  1081.     for(int8_t i = 5; i > 0; i--) {
  1082.         PrintHex<uint8_t>(my_bdaddr[i]);
  1083.         Notify(PSTR(":"));
  1084.     }
  1085.     PrintHex<uint8_t>(my_bdaddr[0]);
  1086. #endif
  1087. }
  1088. void BTD::setMoveBdaddr(uint8_t* BDADDR) {
  1089.     /* Set the internal bluetooth address */
  1090.     uint8_t buf[11];
  1091.     buf[0] = 0x05;
  1092.     buf[7] = 0x10;
  1093.     buf[8] = 0x01;
  1094.     buf[9] = 0x02;
  1095.     buf[10] = 0x12;
  1096.    
  1097.     for (uint8_t i = 0; i < 6; i++)
  1098.         buf[i + 1] = BDADDR[i];
  1099.    
  1100.     //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)
  1101.     pUsb->ctrlReq(bAddress,epInfo[BTD_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00,11,11, buf, NULL);
  1102. #ifdef DEBUG
  1103.     Notify(PSTR("\r\nBluetooth Address was set to: "));
  1104.     for(int8_t i = 5; i > 0; i--) {
  1105.         PrintHex<uint8_t>(my_bdaddr[i]);
  1106.         Notify(PSTR(":"));
  1107.     }
  1108.     PrintHex<uint8_t>(my_bdaddr[0]);
  1109. #endif
  1110. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement