Luminardo + Bluetooth dongle: to be or not to be?

Our next goal would be to establish communication between Luminardo and ELM327 OBDII Bluetooth adapter like the one shown on the picture below. The adapter allows not to interfere with wires and therefore eliminates (or at least minimises) chances of running into problems with car’s onboard electronics, however, Bluetooth link adds up an additional complexity which becomes extra burden on our shoulders. ELM327 operates wirelessly via Bluetooth link using so-called SPP interface – a Serial Port Profile which is based on ETSI 07.10 and the RFCOMM protocols effectively eliminating need for physical wires yet logically representing the well-known RS232 serial interface. It doesn’t sound particulary scary especially taking into account that USB Host 2.0 for Arduino library has an extensive support for Bluetooth profiles and SPP is one of them. However, don’t get carried away as the sketch that demonstrates usage of SPP stack won’t work for our case. To get to know why please keep reading this post.

Mini ELM327 Bluetooth OBDII Adapter

Mini ELM327 Bluetooth OBDII Adapter

There is a fundamental difference between Bluetooth devices which INITIATE connection and the ones which ACCEPT connection. Most bluetooth profiles which are currently part of USB Host 2.0 for Arduino are built as ACCEPTORS, in other words, it is Arduino board which waits for a Bluetooth-enabled device to initiate connection and establish communication. There are two exceptions, HID and Wii devices which work the other way around, however, it is not the case for SPP profile which is the subject of our particular interest. So our Luminardo won’t make an effort of starting a connection for SPP profile. Needless to say that ELM327 would do the same, it only waits for incomming connections and does not have any intention to take the initiative. The irony of this situation is highlighted by that simple fact that currenly there is no way to make two Arduino boards talking over SPP and our task would be to implement the lacking functionality.

Without falling into lengthy explanations about Bluetooth specification we just would note for now that in order to initiate connection to a remote SPP-aware device the following three steps must be done: 1) initiate and complete Bluetooth pairing, 2) connect to and inquire a remote SDP (Service discovery protocol) service to get parameters for establishing RFComm transport layer and 3) establish connection to RFComm. Right now we will make an effort to take the first step only. First of all, we need to enhance BTD.h and BTH.cpp in such a way that we would be able to specify whether Luminardo should initiate or accept pairing procedure. The modified BTD.h is given below (please scroll down and refer to the highlighted lines):

/* 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
 */

#ifndef _btd_h_
#define _btd_h_

#include "Usb.h"

//PID and VID of the Sony PS3 devices
#define PS3_VID                 0x054C  // Sony Corporation
#define PS3_PID                 0x0268  // PS3 Controller DualShock 3
#define PS3NAVIGATION_PID       0x042F  // Navigation controller
#define PS3MOVE_PID             0x03D5  // Motion controller

#define IOGEAR_GBU521_VID       0x0A5C // The IOGEAR GBU521 dongle does not presents itself correctly, so we have to check for it manually
#define IOGEAR_GBU521_PID       0x21E8

/* Bluetooth dongle data taken from descriptors */
#define BULK_MAXPKTSIZE         64 // Max size for ACL data

// Used in control endpoint header for HCI Commands
#define bmREQ_HCI_OUT USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_DEVICE
// Used in control endpoint header for HID Commands
#define bmREQ_HID_OUT USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
#define HID_REQUEST_SET_REPORT      0x09

/* Bluetooth HCI states for hci_task() */
#define HCI_INIT_STATE                  0
#define HCI_RESET_STATE                 1
#define HCI_CLASS_STATE                 2
#define HCI_BDADDR_STATE                3
#define HCI_LOCAL_VERSION_STATE         4
#define HCI_SET_NAME_STATE              5
#define HCI_CHECK_DEVICE_SERVICE        6

#define HCI_INQUIRY_STATE               7 // These three states are only used if it should pair and connect to a device
#define HCI_CONNECT_DEVICE_STATE        8
#define HCI_CONNECTED_DEVICE_STATE      9

#define HCI_SCANNING_STATE              10
#define HCI_CONNECT_IN_STATE            11
#define HCI_REMOTE_NAME_STATE           12
#define HCI_CONNECTED_STATE             13
#define HCI_DISABLE_SCAN_STATE          14
#define HCI_DONE_STATE                  15
#define HCI_DISCONNECT_STATE            16

/* HCI event flags*/
#define HCI_FLAG_CMD_COMPLETE           0x01
#define HCI_FLAG_CONNECT_COMPLETE       0x02
#define HCI_FLAG_DISCONNECT_COMPLETE    0x04
#define HCI_FLAG_REMOTE_NAME_COMPLETE   0x08
#define HCI_FLAG_INCOMING_REQUEST       0x10
#define HCI_FLAG_READ_BDADDR            0x20
#define HCI_FLAG_READ_VERSION           0x40
#define HCI_FLAG_DEVICE_FOUND           0x80
#define HCI_FLAG_CONNECT_EVENT          0x100

/* Macros for HCI event flag tests */
#define hci_check_flag(flag) (hci_event_flag & (flag))
#define hci_set_flag(flag) (hci_event_flag |= (flag))
#define hci_clear_flag(flag) (hci_event_flag &= ~(flag))

/* HCI Events managed */
#define EV_INQUIRY_COMPLETE                             0x01
#define EV_INQUIRY_RESULT                               0x02
#define EV_CONNECT_COMPLETE                             0x03
#define EV_INCOMING_CONNECT                             0x04
#define EV_DISCONNECT_COMPLETE                          0x05
#define EV_AUTHENTICATION_COMPLETE                      0x06
#define EV_REMOTE_NAME_COMPLETE                         0x07
#define EV_ENCRYPTION_CHANGE                            0x08
#define EV_CHANGE_CONNECTION_LINK                       0x09
#define EV_ROLE_CHANGED                                 0x12
#define EV_NUM_COMPLETE_PKT                             0x13
#define EV_PIN_CODE_REQUEST                             0x16
#define EV_LINK_KEY_REQUEST                             0x17
#define EV_LINK_KEY_NOTIFICATION                        0x18
#define EV_DATA_BUFFER_OVERFLOW                         0x1A
#define EV_MAX_SLOTS_CHANGE                             0x1B
#define EV_READ_REMOTE_VERSION_INFORMATION_COMPLETE     0x0C
#define EV_QOS_SETUP_COMPLETE                           0x0D
#define EV_COMMAND_COMPLETE                             0x0E
#define EV_COMMAND_STATUS                               0x0F
#define EV_LOOPBACK_COMMAND                             0x19
#define EV_PAGE_SCAN_REP_MODE                           0x20

/* Bluetooth states for the different Bluetooth drivers */
#define L2CAP_WAIT                      0
#define L2CAP_DONE                      1

/* Used for HID Control channel */
#define L2CAP_CONTROL_CONNECT_REQUEST   2
#define L2CAP_CONTROL_CONFIG_REQUEST    3
#define L2CAP_CONTROL_SUCCESS           4
#define L2CAP_CONTROL_DISCONNECT        5

/* Used for HID Interrupt channel */
#define L2CAP_INTERRUPT_SETUP           6
#define L2CAP_INTERRUPT_CONNECT_REQUEST 7
#define L2CAP_INTERRUPT_CONFIG_REQUEST  8
#define L2CAP_INTERRUPT_DISCONNECT      9

/* Used for SDP channel */
#define L2CAP_SDP_WAIT                  10
#define L2CAP_SDP_SUCCESS               11

/* Used for RFCOMM channel */
#define L2CAP_RFCOMM_WAIT               12
#define L2CAP_RFCOMM_SUCCESS            13

#define L2CAP_DISCONNECT_RESPONSE       14 // Used for both SDP and RFCOMM channel

/* Bluetooth states used by some drivers */
#define TURN_ON_LED                     17
#define PS3_ENABLE_SIXAXIS              18
#define WII_CHECK_MOTION_PLUS_STATE     19
#define WII_CHECK_EXTENSION_STATE       20
#define WII_INIT_MOTION_PLUS_STATE      21

/* L2CAP event flags for HID Control channel */
#define L2CAP_FLAG_CONNECTION_CONTROL_REQUEST           0x00000001
#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS               0x00000002
#define L2CAP_FLAG_CONTROL_CONNECTED                    0x00000004
#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE          0x00000008

/* L2CAP event flags for HID Interrupt channel */
#define L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST         0x00000010
#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS             0x00000020
#define L2CAP_FLAG_INTERRUPT_CONNECTED                  0x00000040
#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE        0x00000080

/* L2CAP event flags for SDP channel */
#define L2CAP_FLAG_CONNECTION_SDP_REQUEST               0x00000100
#define L2CAP_FLAG_CONFIG_SDP_SUCCESS                   0x00000200
#define L2CAP_FLAG_DISCONNECT_SDP_REQUEST               0x00000400

/* L2CAP event flags for RFCOMM channel */
#define L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST            0x00000800
#define L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS                0x00001000
#define L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST            0x00002000

#define L2CAP_FLAG_DISCONNECT_RESPONSE                  0x00004000

/* Macros for L2CAP event flag tests */
#define l2cap_check_flag(flag) (l2cap_event_flag & (flag))
#define l2cap_set_flag(flag) (l2cap_event_flag |= (flag))
#define l2cap_clear_flag(flag) (l2cap_event_flag &= ~(flag))

/* L2CAP signaling commands */
#define L2CAP_CMD_COMMAND_REJECT        0x01
#define L2CAP_CMD_CONNECTION_REQUEST    0x02
#define L2CAP_CMD_CONNECTION_RESPONSE   0x03
#define L2CAP_CMD_CONFIG_REQUEST        0x04
#define L2CAP_CMD_CONFIG_RESPONSE       0x05
#define L2CAP_CMD_DISCONNECT_REQUEST    0x06
#define L2CAP_CMD_DISCONNECT_RESPONSE   0x07
#define L2CAP_CMD_INFORMATION_REQUEST   0x0A
#define L2CAP_CMD_INFORMATION_RESPONSE  0x0B

// Used For Connection Response - Remember to Include High Byte
#define PENDING     0x01
#define SUCCESSFUL  0x00

/* Bluetooth L2CAP PSM - see http://www.bluetooth.org/Technical/AssignedNumbers/logical_link.htm */
#define SDP_PSM         0x01 // Service Discovery Protocol PSM Value
#define RFCOMM_PSM      0x03 // RFCOMM PSM Value
#define HID_CTRL_PSM    0x11 // HID_Control PSM Value
#define HID_INTR_PSM    0x13 // HID_Interrupt PSM Value

// Used to determine if it is a Bluetooth dongle
#define WI_SUBCLASS_RF      0x01 // RF Controller
#define WI_PROTOCOL_BT      0x01 // Bluetooth Programming Interface

#define BTD_MAX_ENDPOINTS   4
#define BTD_NUM_SERVICES    4 // Max number of Bluetooth services - if you need more than 4 simply increase this number

#define PAIR    1

/* acl_handle_ok or it's a new connection */
#if 0
#define UHS_ACL_HANDLE_OK(x, y) ((uint16_t)(x[0]) | (uint16_t)(x[1] << 8)) == (y | 0x2000U)
#else
/*
 *  Better implementation.
 *  o One place for this code, it is reused four times in the source.
 *    Perhaps it is better as a function.
 *  o This should be faster since the && operation can early exit, this means
 *    the shift would only be performed if the first byte matches.
 *  o Casting is eliminated.
 *  o How does this compare in code size? No difference. It is a free optimization.
 */
#define UHS_ACL_HANDLE_OK(x, y) ((x[0] == (y & 0xff)) && (x[1] == ((y >> 8) | 0x20)))
#endif

/** All Bluetooth services should inherit this class. */
class BluetoothService {
public:
        /**
         * Used to pass acldata to the Bluetooth service.
         * @param ACLData Pointer to the incoming acldata.
         */
        virtual void ACLData(uint8_t* ACLData);
        /** Used to run the different state machines in the Bluetooth service. */
        virtual void Run();
        /** Used to reset the Bluetooth service. */
        virtual void Reset();
        /** Used to disconnect both the L2CAP Channel and the HCI Connection for the Bluetooth service. */
        virtual void disconnect();
};

/**
 * The Bluetooth Dongle class will take care of all the USB communication
 * and then pass the data to the BluetoothService classes.
 */
class BTD : public USBDeviceConfig, public UsbConfigXtracter {
public:
        /**
         * Constructor for the BTD class.
         * @param  p   Pointer to USB class instance.
         */
        BTD(USB *p);

        /** @name USBDeviceConfig implementation */
        /**
         * Address assignment and basic initialization is done here.
         * @param  parent   Hub number.
         * @param  port     Port number on the hub.
         * @param  lowspeed Speed of the device.
         * @return          0 on success.
         */
        virtual uint8_t ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed);
        /**
         * Initialize the Bluetooth dongle.
         * @param  parent   Hub number.
         * @param  port     Port number on the hub.
         * @param  lowspeed Speed of the device.
         * @return          0 on success.
         */
        virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed);
        /**
         * Release the USB device.
         * @return 0 on success.
         */
        virtual uint8_t Release();
        /**
         * Poll the USB Input endpoints and run the state machines.
         * @return 0 on success.
         */
        virtual uint8_t Poll();

        /**
         * Get the device address.
         * @return The device address.
         */
        virtual uint8_t GetAddress() {
                return bAddress;
        };

        /**
         * Used to check if the dongle has been initialized.
         * @return True if it's ready.
         */
        virtual bool isReady() {
                return bPollEnable;
        };

        /**
         * Used by the USB core to check what this driver support.
         * @param  klass The device's USB class.
         * @return       Returns true if the device's USB class matches this driver.
         */
        virtual boolean DEVCLASSOK(uint8_t klass) {
                return (klass == USB_CLASS_WIRELESS_CTRL);
        };

        /**
         * Used by the USB core to check what this driver support.
         * Used to set the Bluetooth address into the PS3 controllers.
         * @param  vid The device's VID.
         * @param  pid The device's PID.
         * @return     Returns true if the device's VID and PID matches this driver.
         */
        virtual boolean VIDPIDOK(uint16_t vid, uint16_t pid) {
                if(vid == IOGEAR_GBU521_VID && pid == IOGEAR_GBU521_PID)
                        return true;
                if(my_bdaddr[0] != 0x00 || my_bdaddr[1] != 0x00 || my_bdaddr[2] != 0x00 || my_bdaddr[3] != 0x00 || my_bdaddr[4] != 0x00 || my_bdaddr[5] != 0x00) { // Check if Bluetooth address is set
                        if(vid == PS3_VID && (pid == PS3_PID || pid == PS3NAVIGATION_PID || pid == PS3MOVE_PID))
                                return true;
                }
                return false;
        };
        /**@}*/

        /** @name UsbConfigXtracter implementation */
        /**
         * UsbConfigXtracter implementation, used to extract endpoint information.
         * @param conf  Configuration value.
         * @param iface Interface number.
         * @param alt   Alternate setting.
         * @param proto Interface Protocol.
         * @param ep    Endpoint Descriptor.
         */
        virtual void EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *ep);
        /**@}*/

        /** Disconnects both the L2CAP Channel and the HCI Connection for all Bluetooth services. */
        void disconnect() {
                for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++)
                        if(btService[i])
                                btService[i]->disconnect();
        };

        /**
         * Register Bluetooth dongle members/services.
         * @param  pService Pointer to BluetoothService class instance.
         * @return          The service ID on success or -1 on fail.
         */
        int8_t registerServiceClass(BluetoothService *pService) {
                for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++) {
                        if(!btService[i]) {
                                btService[i] = pService;
                                return i; // Return ID
                        }
                }
                return -1; // ErrorregisterServiceClass
        };

        /** @name HCI Commands */
        /**
         * Used to send a HCI Command.
         * @param data   Data to send.
         * @param nbytes Number of bytes to send.
         */
        void HCI_Command(uint8_t* data, uint16_t nbytes);
        /** Reset the Bluetooth dongle. */
        void hci_reset();
        /** Read the Bluetooth address of the dongle. */
        void hci_read_bdaddr();
        /** Read the HCI Version of the Bluetooth dongle. */
        void hci_read_local_version_information();
        /**
         * Set the local name of the Bluetooth dongle.
         * @param name Desired name.
         */
        void hci_set_local_name(const char* name);
        /** Enable visibility to other Bluetooth devices. */
        void hci_write_scan_enable();
        /** Disable visibility to other Bluetooth devices. */
        void hci_write_scan_disable();
        /** Read the remote devices name. */
        void hci_remote_name();
        /** Accept the connection with the Bluetooth device. */
        void hci_accept_connection();
        /**
         * Disconnect the HCI connection.
         * @param handle The HCI Handle for the connection.
         */
        void hci_disconnect(uint16_t handle);
        /**
         * Respond with the pin for the connection.
         * The pin is automatically set for the Wii library,
         * but can be customized for the SPP library.
         */
        void hci_pin_code_request_reply();
        /** Respons when no pin was set. */
        void hci_pin_code_negative_request_reply();
        /**
         * Command is used to reply to a Link Key Request event from the BR/EDR Controller
         * if the Host does not have a stored Link Key for the connection.
         */
        void hci_link_key_request_negative_reply();
        /** Used to try to authenticate with the remote device. */
        void hci_authentication_request();
        /** Start a HCI inquiry. */
        void hci_inquiry();
        /** Cancel a HCI inquiry. */
        void hci_inquiry_cancel();
        /** Connect to last device communicated with. */
        void hci_connect();
        /**
         * Connect to device.
         * @param bdaddr Bluetooth address of the device.
         */
        void hci_connect(uint8_t *bdaddr);
        /** Used to a set the class of the device. */
        void hci_write_class_of_device();
        /**@}*/

        /** @name L2CAP Commands */
        /**
         * Used to send L2CAP Commands.
         * @param handle      HCI Handle.
         * @param data        Data to send.
         * @param nbytes      Number of bytes to send.
         * @param channelLow,channelHigh  Low and high byte of channel to send to.
         * If argument is omitted then the Standard L2CAP header: Channel ID (0x01) for ACL-U will be used.
         */
        void L2CAP_Command(uint16_t handle, uint8_t* data, uint8_t nbytes, uint8_t channelLow = 0x01, uint8_t channelHigh = 0x00);
        /**
         * L2CAP Connection Request.
         * @param handle HCI handle.
         * @param rxid   Identifier.
         * @param scid   Source Channel Identifier.
         * @param psm    Protocol/Service Multiplexer - see: https://www.bluetooth.org/Technical/AssignedNumbers/logical_link.htm.
         */
        void l2cap_connection_request(uint16_t handle, uint8_t rxid, uint8_t* scid, uint16_t psm);
        /**
         * L2CAP Connection Response.
         * @param handle HCI handle.
         * @param rxid   Identifier.
         * @param dcid   Destination Channel Identifier.
         * @param scid   Source Channel Identifier.
         * @param result Result - First send ::PENDING and then ::SUCCESSFUL.
         */
        void l2cap_connection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid, uint8_t result);
        /**
         * L2CAP Config Request.
         * @param handle HCI Handle.
         * @param rxid   Identifier.
         * @param dcid   Destination Channel Identifier.
         */
        void l2cap_config_request(uint16_t handle, uint8_t rxid, uint8_t* dcid);
        /**
         * L2CAP Config Response.
         * @param handle HCI Handle.
         * @param rxid   Identifier.
         * @param scid   Source Channel Identifier.
         */
        void l2cap_config_response(uint16_t handle, uint8_t rxid, uint8_t* scid);
        /**
         * L2CAP Disconnection Request.
         * @param handle HCI Handle.
         * @param rxid   Identifier.
         * @param dcid   Device Channel Identifier.
         * @param scid   Source Channel Identifier.
         */
        void l2cap_disconnection_request(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid);
        /**
         * L2CAP Disconnection Response.
         * @param handle HCI Handle.
         * @param rxid   Identifier.
         * @param dcid   Device Channel Identifier.
         * @param scid   Source Channel Identifier.
         */
        void l2cap_disconnection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid);
        /**
         * L2CAP Information Response.
         * @param handle       HCI Handle.
         * @param rxid         Identifier.
         * @param infoTypeLow,infoTypeHigh  Infotype.
         */
        void l2cap_information_response(uint16_t handle, uint8_t rxid, uint8_t infoTypeLow, uint8_t infoTypeHigh);
        /**@}*/

        /** Use this to see if it is waiting for a incoming connection. */
        bool watingForConnection;
        /** This is used by the service to know when to store the device information. */
        bool l2capConnectionClaimed;
        /** This is used by the SPP library to claim the current SDP incoming request. */
        bool sdpConnectionClaimed;
        /** This is used by the SPP library to claim the current RFCOMM incoming request. */
        bool rfcommConnectionClaimed;

        /** The name you wish to make the dongle show up as. It is set automatically by the SPP library. */
        const char* btdName;
        /** The pin you wish to make the dongle use for authentication. It is set automatically by the SPP and BTHID library. */
        const char* btdPin;

        /** The bluetooth dongles Bluetooth address. */
        uint8_t my_bdaddr[6];
        /** HCI handle for the last connection. */
        uint16_t hci_handle;
        /** Last incoming devices Bluetooth address. */
        uint8_t disc_bdaddr[6];
        /** First 30 chars of last remote name. */
        char remote_name[30];
        /**
         * The supported HCI Version read from the Bluetooth dongle.
         * Used by the PS3BT library to check the HCI Version of the Bluetooth dongle,
         * it should be at least 3 to work properly with the library.
         */
        uint8_t hci_version;

        /** Call this function to pair with a Wiimote */
        void pairWithWiimote() {
                pairWithWii = true;
                hci_state = HCI_CHECK_DEVICE_SERVICE;
        };
        /** Used to only send the ACL data to the Wiimote. */
        bool connectToWii;
        /** True if a Wiimote is connecting. */
        bool incomingWii;
        /** True when it should pair with a Wiimote. */
        bool pairWithWii;
        /** True if it's the new Wiimote with the Motion Plus Inside or a Wii U Pro Controller. */
        bool motionPlusInside;
        /** True if it's a Wii U Pro Controller. */
        bool wiiUProController;

        /** Call this function to pair with a Wiimote */
        void pairWithHID() {
                pairWithHIDDevice = true;
                hci_state = HCI_CHECK_DEVICE_SERVICE;
        };
        /** Used to only send the ACL data to the Wiimote. */
        bool connectToHIDDevice;
        /** True if a Wiimote is connecting. */
        bool incomingHIDDevice;
        /** True when it should pair with a device like a mouse or keyboard. */
        bool pairWithHIDDevice;


        bool pairWithOtherDevice;
        bool connectToOtherDevice;
        void pairWithOther() {
                pairWithOtherDevice = true;
                hci_state = HCI_CONNECT_DEVICE_STATE;
        };

        /** Remote address of Bluetooth device to connect to. */
        uint8_t remote_bdaddr[6];


        /**
         * Read the poll interval taken from the endpoint descriptors.
         * @return The poll interval in ms.
         */
        uint8_t readPollInterval() {
                return pollInterval;
        };

protected:
        /** Pointer to USB class instance. */
        USB *pUsb;
        /** Device address. */
        uint8_t bAddress;
        /** Endpoint info structure. */
        EpInfo epInfo[BTD_MAX_ENDPOINTS];

        /** Configuration number. */
        uint8_t bConfNum;
        /** Total number of endpoints in the configuration. */
        uint8_t bNumEP;
        /** Next poll time based on poll interval taken from the USB descriptor. */
        uint32_t qNextPollTime;

        /** Bluetooth dongle control endpoint. */
        static const uint8_t BTD_CONTROL_PIPE;
        /** HCI event endpoint index. */
        static const uint8_t BTD_EVENT_PIPE;
        /** ACL In endpoint index. */
        static const uint8_t BTD_DATAIN_PIPE;
        /** ACL Out endpoint index. */
        static const uint8_t BTD_DATAOUT_PIPE;

        /**
         * Used to print the USB Endpoint Descriptor.
         * @param ep_ptr Pointer to USB Endpoint Descriptor.
         */
        void PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR* ep_ptr);

private:
        void Initialize(); // Set all variables, endpoint structs etc. to default values
        BluetoothService *btService[BTD_NUM_SERVICES];

        uint16_t PID, VID; // PID and VID of device connected

        uint8_t pollInterval;
        bool bPollEnable;

        bool incomingPS4; // True if a PS4 controller is connecting
        uint8_t classOfDevice[3]; // Class of device of last device

        /* Variables used by high level HCI task */
        uint8_t hci_state; // Current state of Bluetooth HCI connection
        uint16_t hci_counter; // Counter used for Bluetooth HCI reset loops
        uint16_t hci_num_reset_loops; // This value indicate how many times it should read before trying to reset
        uint16_t hci_event_flag; // HCI flags of received Bluetooth events
        uint8_t inquiry_counter;

        uint8_t hcibuf[BULK_MAXPKTSIZE]; // General purpose buffer for HCI data
        uint8_t l2capinbuf[BULK_MAXPKTSIZE]; // General purpose buffer for L2CAP in data
        uint8_t l2capoutbuf[14]; // General purpose buffer for L2CAP out data

        /* State machines */
        void HCI_event_task(); // Poll the HCI event pipe
        void HCI_task(); // HCI state machine
        void ACL_event_task(); // ACL input pipe

        /* Used to set the Bluetooth Address internally to the PS3 Controllers */
        void setBdaddr(uint8_t* BDADDR);
        void setMoveBdaddr(uint8_t* BDADDR);
};
#endif

There are the following multiple changes that have been done on BTD.cpp as well:

/* 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
 */

#include "BTD.h"
// To enable serial debugging see "settings.h"
//#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_DATAIN_PIPE = 2;
const uint8_t BTD::BTD_DATAOUT_PIPE = 3;

BTD::BTD(USB *p) :
connectToWii(false),
pairWithWii(false),
connectToHIDDevice(false),
pairWithHIDDevice(false),
pairWithOtherDevice(false),
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
pollInterval(0),
bPollEnable(false) // Don't start polling before dongle is connected
{
        for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++)
                btService[i] = NULL;

        Initialize(); // Set all variables, endpoint structs etc. to default values

        if(pUsb) // Register in USB subsystem
                pUsb->RegisterDeviceClass(this); // Set devConfig[] entry
}

uint8_t BTD::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
        const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR);
        uint8_t buf[constBufSize];
        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
        uint8_t rcode;
        UsbDevice *p = NULL;
        EpInfo *oldep_ptr = NULL;

        Initialize(); // Set all variables, endpoint structs etc. to default values

        AddressPool &addrPool = pUsb->GetAddressPool(); // Get memory address of USB device address pool
#ifdef EXTRADEBUG
        Notify(PSTR("\r\nBTD ConfigureDevice"), 0x80);
#endif

        if(bAddress) { // Check if address has already been assigned to an instance
#ifdef DEBUG_USB_HOST
                Notify(PSTR("\r\nAddress in use"), 0x80);
#endif
                return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
        }

        p = addrPool.GetUsbDevicePtr(0); // Get pointer to pseudo device with address 0 assigned
        if(!p) {
#ifdef DEBUG_USB_HOST
                Notify(PSTR("\r\nAddress not found"), 0x80);
#endif
                return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
        }

        if(!p->epinfo) {
#ifdef DEBUG_USB_HOST
                Notify(PSTR("\r\nepinfo is null"), 0x80);
#endif
                return USB_ERROR_EPINFO_IS_NULL;
        }

        oldep_ptr = p->epinfo; // Save old pointer to EP_RECORD of address 0
        p->epinfo = epInfo; // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence
        p->lowspeed = lowspeed;
        rcode = pUsb->getDevDescr(0, 0, constBufSize, (uint8_t*)buf); // Get device descriptor - addr, ep, nbytes, data

        p->epinfo = oldep_ptr; // Restore p->epinfo

        if(rcode)
                goto FailGetDevDescr;

        bAddress = addrPool.AllocAddress(parent, false, port); // Allocate new address according to device class

        if(!bAddress) {
#ifdef DEBUG_USB_HOST
                Notify(PSTR("\r\nOut of address space"), 0x80);
#endif
                return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
        }

        epInfo[0].maxPktSize = udd->bMaxPacketSize0; // Extract Max Packet Size from device descriptor
        epInfo[1].epAddr = udd->bNumConfigurations; // Steal and abuse from epInfo structure to save memory

        VID = udd->idVendor;
        PID = udd->idProduct;

        return USB_ERROR_CONFIG_REQUIRES_ADDITIONAL_RESET;

FailGetDevDescr:
#ifdef DEBUG_USB_HOST
        NotifyFailGetDevDescr(rcode);
#endif
        if(rcode != hrJERR)
                rcode = USB_ERROR_FailGetDevDescr;
        Release();
        return rcode;
};

uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
        uint8_t rcode;
        uint8_t num_of_conf = epInfo[1].epAddr; // Number of configurations
        epInfo[1].epAddr = 0;

        AddressPool &addrPool = pUsb->GetAddressPool();
#ifdef EXTRADEBUG
        Notify(PSTR("\r\nBTD Init"), 0x80);
#endif
        UsbDevice *p = addrPool.GetUsbDevicePtr(bAddress); // Get pointer to assigned address record

        if(!p) {
#ifdef DEBUG_USB_HOST
                Notify(PSTR("\r\nAddress not found"), 0x80);
#endif
                return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
        }

        delay(300); // Assign new address to the device

        rcode = pUsb->setAddr(0, 0, bAddress); // Assign new address to the device
        if(rcode) {
#ifdef DEBUG_USB_HOST
                Notify(PSTR("\r\nsetAddr: "), 0x80);
                D_PrintHex<uint8_t > (rcode, 0x80);
#endif
                p->lowspeed = false;
                goto Fail;
        }
#ifdef EXTRADEBUG
        Notify(PSTR("\r\nAddr: "), 0x80);
        D_PrintHex<uint8_t > (bAddress, 0x80);
#endif

        p->lowspeed = false;

        p = addrPool.GetUsbDevicePtr(bAddress); // Get pointer to assigned address record
        if(!p) {
#ifdef DEBUG_USB_HOST
                Notify(PSTR("\r\nAddress not found"), 0x80);
#endif
                return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
        }

        p->lowspeed = lowspeed;

        rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); // Assign epInfo to epinfo pointer - only EP0 is known
        if(rcode)
                goto FailSetDevTblEntry;

        if(VID == PS3_VID && (PID == PS3_PID || PID == PS3NAVIGATION_PID || PID == PS3MOVE_PID)) {
                delay(100);
                rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 1); // We only need the Control endpoint, so we don't have to initialize the other endpoints of device
                if(rcode)
                        goto FailSetConfDescr;

#ifdef DEBUG_USB_HOST
                if(PID == PS3_PID || PID == PS3NAVIGATION_PID) {
                        if(PID == PS3_PID)
                                Notify(PSTR("\r\nDualshock 3 Controller Connected"), 0x80);
                        else // It must be a navigation controller
                                Notify(PSTR("\r\nNavigation Controller Connected"), 0x80);
                } else // It must be a Motion controller
                        Notify(PSTR("\r\nMotion Controller Connected"), 0x80);
#endif

                if(my_bdaddr[0] == 0x00 && my_bdaddr[1] == 0x00 && my_bdaddr[2] == 0x00 && my_bdaddr[3] == 0x00 && my_bdaddr[4] == 0x00 && my_bdaddr[5] == 0x00) {
#ifdef DEBUG_USB_HOST
                        Notify(PSTR("\r\nPlease plug in the dongle before trying to pair with the PS3 Controller\r\nor set the Bluetooth address in the constructor of the PS3BT class"), 0x80);
#endif
                } else {
                        if(PID == PS3_PID || PID == PS3NAVIGATION_PID)
                                setBdaddr(my_bdaddr); // Set internal Bluetooth address
                        else
                                setMoveBdaddr(my_bdaddr); // Set internal Bluetooth address
#ifdef DEBUG_USB_HOST
                        Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80);
                        for(int8_t i = 5; i > 0; i--) {
                                D_PrintHex<uint8_t > (my_bdaddr[i], 0x80);
                                Notify(PSTR(":"), 0x80);
                        }
                        D_PrintHex<uint8_t > (my_bdaddr[0], 0x80);
#endif
                }

                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 {
                // 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++) {
                        if(VID == IOGEAR_GBU521_VID && PID == IOGEAR_GBU521_PID) {
                                ConfigDescParser<USB_CLASS_VENDOR_SPECIFIC, WI_SUBCLASS_RF, WI_PROTOCOL_BT, CP_MASK_COMPARE_ALL> confDescrParser(this); // Needed for the IOGEAR GBU521
                                rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
                        } else {
                                ConfigDescParser<USB_CLASS_WIRELESS_CTRL, WI_SUBCLASS_RF, WI_PROTOCOL_BT, CP_MASK_COMPARE_ALL> confDescrParser(this);
                                rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
                        }
                        if(rcode) // Check error code
                                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;

                // Set Configuration Value
                rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bConfNum);
                if(rcode)
                        goto FailSetConfDescr;

                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_USB_HOST
                Notify(PSTR("\r\nBluetooth Dongle Initialized"), 0x80);
#endif
        }
        return 0; // Successful configuration

        /* Diagnostic messages */
FailSetDevTblEntry:
#ifdef DEBUG_USB_HOST
        NotifyFailSetDevTblEntry();
        goto Fail;
#endif

FailGetConfDescr:
#ifdef DEBUG_USB_HOST
        NotifyFailGetConfDescr();
        goto Fail;
#endif

FailSetConfDescr:
#ifdef DEBUG_USB_HOST
        NotifyFailSetConfDescr();
#endif
        goto Fail;

FailUnknownDevice:
#ifdef DEBUG_USB_HOST
        NotifyFailUnknownDevice(VID, PID);
#endif
        pUsb->setAddr(bAddress, 0, 0); // Reset address
        rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
Fail:
#ifdef DEBUG_USB_HOST
        Notify(PSTR("\r\nBTD Init Failed, error code: "), 0x80);
        NotifyFail(rcode);
#endif
        Release();
        return rcode;
}

void BTD::Initialize() {
        uint8_t i;
        for(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;
        }
        for(i = 0; i < BTD_NUM_SERVICES; i++) {
                if(btService[i])
                        btService[i]->Reset(); // Reset all Bluetooth services
        }

        connectToWii = false;
        incomingWii = false;
        connectToHIDDevice = false;
        connectToOtherDevice = false;  
        incomingHIDDevice = false;
        incomingPS4 = false;
        bAddress = 0; // Clear device address
        bNumEP = 1; // Must have to be reset to 1
        qNextPollTime = 0; // Reset next poll time
        pollInterval = 0;
        bPollEnable = false; // Don't start polling before dongle is connected
}

/* 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<uint8_t>(PSTR("Conf.Val"),conf);
        //ErrorMessage<uint8_t>(PSTR("Iface Num"),iface);
        //ErrorMessage<uint8_t>(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;
                epInfo[index].bmNakPower = USB_NAK_NOWAIT;
        } 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);
#endif
        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:"), 0x80);
        Notify(PSTR("\r\nLength:\t\t"), 0x80);
        D_PrintHex<uint8_t > (ep_ptr->bLength, 0x80);
        Notify(PSTR("\r\nType:\t\t"), 0x80);
        D_PrintHex<uint8_t > (ep_ptr->bDescriptorType, 0x80);
        Notify(PSTR("\r\nAddress:\t"), 0x80);
        D_PrintHex<uint8_t > (ep_ptr->bEndpointAddress, 0x80);
        Notify(PSTR("\r\nAttributes:\t"), 0x80);
        D_PrintHex<uint8_t > (ep_ptr->bmAttributes, 0x80);
        Notify(PSTR("\r\nMaxPktSize:\t"), 0x80);
        D_PrintHex<uint16_t > (ep_ptr->wMaxPacketSize, 0x80);
        Notify(PSTR("\r\nPoll Intrv:\t"), 0x80);
        D_PrintHex<uint8_t > (ep_ptr->bInterval, 0x80);
#endif
}

/* Performs a cleanup after failed Init() attempt */
uint8_t BTD::Release() {
        Initialize(); // Set all variables, endpoint structs etc. to default values
        pUsb->GetAddressPool().FreeAddress(bAddress);
        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
                HCI_task(); // HCI state machine
                ACL_event_task(); // Poll the ACL input pipe too
        }
        return 0;
}

void BTD::HCI_event_task() {
        uint16_t length = 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, &length, hcibuf); // Input on endpoint 1

        if(!rcode || rcode == hrNAK) { // Check for errors
                switch(hcibuf[0]) { // Switch on event type
                        case EV_COMMAND_COMPLETE:
                                if(!hcibuf[5]) { // Check if command succeeded
                                        hci_set_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_set_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_set_flag(HCI_FLAG_READ_BDADDR);
                                        }
                                }
                                break;

                        case EV_COMMAND_STATUS:
                                if(hcibuf[2]) { // Show status on serial if not OK
#ifdef DEBUG_USB_HOST
                                        Notify(PSTR("\r\nHCI Command Failed: "), 0x80);
                                        D_PrintHex<uint8_t > (hcibuf[2], 0x80);
#endif
                                }
                                break;

                        case EV_INQUIRY_COMPLETE:
                                if (inquiry_counter >= 5 && (pairWithWii || pairWithHIDDevice || pairWithOtherDevice)) {
                                        inquiry_counter = 0;
#ifdef DEBUG_USB_HOST
                                        if(pairWithWii)
                                                Notify(PSTR("\r\nCouldn't find Wiimote"), 0x80);
                                        else if (pairWithOtherDevice)
                                                Notify(PSTR("\r\nCouldn't find 'Other' device"), 0x80);    
                                        else
                                                Notify(PSTR("\r\nCouldn't find HID device"), 0x80);
#endif
                                        connectToWii = false;
                                        pairWithWii = false;
                                        connectToHIDDevice = false;
                                        pairWithHIDDevice = false;
                                        connectToOtherDevice = false;
                                        pairWithOtherDevice = 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: "), 0x80);
                                        Notify(hcibuf[2], 0x80);
#endif
                                        for(uint8_t i = 0; i < hcibuf[2]; i++) {
                                                uint8_t offset = 8 * hcibuf[2] + 3 * i;

                                                for(uint8_t j = 0; j < 3; j++)
                                                        classOfDevice[j] = hcibuf[j + 4 + offset];

                                                if(pairWithWii && classOfDevice[2] == 0x00 && (classOfDevice[1] & 0x05) && (classOfDevice[0] & 0x0C)) { // See http://wiibrew.org/wiki/Wiimote#SDP_information
                                                        if(classOfDevice[0] & 0x08) // Check if it's the new Wiimote with motion plus inside that was detected
                                                                motionPlusInside = true;
                                                        else
                                                                motionPlusInside = false;

                                                        for(uint8_t j = 0; j < 6; j++)
                                                                disc_bdaddr[j] = hcibuf[j + 3 + 6 * i];

                                                        hci_set_flag(HCI_FLAG_DEVICE_FOUND);
                                                        break;
                                                } else if(pairWithHIDDevice && (classOfDevice[1] & 0x05) && (classOfDevice[0] & 0xC8)) { // Check if it is a mouse, keyboard or a gamepad - see: http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html
#ifdef DEBUG_USB_HOST
                                                        if(classOfDevice[0] & 0x80)
                                                                Notify(PSTR("\r\nMouse found"), 0x80);
                                                        if(classOfDevice[0] & 0x40)
                                                                Notify(PSTR("\r\nKeyboard found"), 0x80);
                                                        if(classOfDevice[0] & 0x08)
                                                                Notify(PSTR("\r\nGamepad found"), 0x80);
#endif

                                                        for(uint8_t j = 0; j < 6; j++)
                                                                disc_bdaddr[j] = hcibuf[j + 3 + 6 * i];

                                                        hci_set_flag(HCI_FLAG_DEVICE_FOUND);
                                                }else {
#ifdef EXTRADEBUG
                                                        Notify(PSTR("\r\nClass of device: "), 0x80);
                                                        D_PrintHex<uint8_t > (classOfDevice[2], 0x80);
                                                        Notify(PSTR(" "), 0x80);
                                                        D_PrintHex<uint8_t > (classOfDevice[1], 0x80);
                                                        Notify(PSTR(" "), 0x80);
                                                        D_PrintHex<uint8_t > (classOfDevice[0], 0x80);
                                                
#endif
                                                        uint8_t discovered = true;
                                                        for (uint8_t j = 0; j < 6; j++)
                                                        {
                                                                if (hcibuf[j + 3 + 6 * i] != remote_bdaddr[j])
                                                                        discovered = false;

                                                                disc_bdaddr[j] = hcibuf[j + 3 + 6 * i];

                                                        }
                                                        if (discovered)
                                                        {
#ifdef DEBUG_USB_HOST
                                                                Notify(PSTR("\r\nDevice: "), 0x80);
                                                                for (int8_t j = 5; j > 0; j--) {
                                                                        D_PrintHex<uint8_t > (disc_bdaddr[j], 0x80);
                                                                        Notify(PSTR(":"), 0x80);
                                                                }
                                                                D_PrintHex<uint8_t > (disc_bdaddr[0], 0x80);

                                                                Notify(PSTR(" has been found"), 0x80);
#endif
                                                                hci_event_flag |= HCI_FLAG_DEVICE_FOUND;
                                                        }

                                                }

                                        }
                                }
                                break;

                        case EV_CONNECT_COMPLETE:
                                hci_set_flag(HCI_FLAG_CONNECT_EVENT);
                                if(!hcibuf[2]) { // Check if connected OK
#ifdef EXTRADEBUG
                                        Notify(PSTR("\r\nConnection established"), 0x80);
#endif
                                        hci_handle = hcibuf[3] | ((hcibuf[4] & 0x0F) << 8); // Store the handle for the ACL connection
                                        hci_set_flag(HCI_FLAG_CONNECT_COMPLETE); // Set connection complete flag
                                } else {
                                        hci_state = HCI_CHECK_DEVICE_SERVICE;
#ifdef DEBUG_USB_HOST
                                        Notify(PSTR("\r\nConnection Failed: "), 0x80);
                                        D_PrintHex<uint8_t > (hcibuf[2], 0x80);
#endif
                                }
                                break;

                        case EV_DISCONNECT_COMPLETE:
                                if(!hcibuf[2]) { // Check if disconnected OK
                                        hci_set_flag(HCI_FLAG_DISCONNECT_COMPLETE); // Set disconnect command complete flag
                                        hci_clear_flag(HCI_FLAG_CONNECT_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 < min(sizeof (remote_name), sizeof (hcibuf) - 9); i++) {
                                                remote_name[i] = hcibuf[9 + i];
                                                if(remote_name[i] == '\0') // End of string
                                                        break;
                                        }
                                        hci_set_flag(HCI_FLAG_REMOTE_NAME_COMPLETE);
                                }
                                break;

                        case EV_INCOMING_CONNECT:
                                for(uint8_t i = 0; i < 6; i++)
                                        disc_bdaddr[i] = hcibuf[i + 2];

                                for(uint8_t i = 0; i < 3; i++)
                                        classOfDevice[i] = hcibuf[i + 8];

                                if((classOfDevice[1] & 0x05) && (classOfDevice[0] & 0xC8)) { // Check if it is a mouse, keyboard or a gamepad
#ifdef DEBUG_USB_HOST
                                        if(classOfDevice[0] & 0x80)
                                                Notify(PSTR("\r\nMouse is connecting"), 0x80);
                                        if(classOfDevice[0] & 0x40)
                                                Notify(PSTR("\r\nKeyboard is connecting"), 0x80);
                                        if(classOfDevice[0] & 0x08)
                                                Notify(PSTR("\r\nGamepad is connecting"), 0x80);
#endif
                                        incomingHIDDevice = true;
                                }

#ifdef EXTRADEBUG
                                Notify(PSTR("\r\nClass of device: "), 0x80);
                                D_PrintHex<uint8_t > (classOfDevice[2], 0x80);
                                Notify(PSTR(" "), 0x80);
                                D_PrintHex<uint8_t > (classOfDevice[1], 0x80);
                                Notify(PSTR(" "), 0x80);
                                D_PrintHex<uint8_t > (classOfDevice[0], 0x80);
#endif
                                hci_set_flag(HCI_FLAG_INCOMING_REQUEST);
                                break;

                        case EV_PIN_CODE_REQUEST:
                                if(pairWithWii) {
#ifdef DEBUG_USB_HOST
                                        Notify(PSTR("\r\nPairing with wiimote"), 0x80);
#endif
                                        hci_pin_code_request_reply();
                                } else if(btdPin != NULL) {
#ifdef DEBUG_USB_HOST
                                        Notify(PSTR("\r\nBluetooth pin is set too: "), 0x80);
                                        NotifyStr(btdPin, 0x80);
#endif
                                        hci_pin_code_request_reply();
                                } else {
#ifdef DEBUG_USB_HOST
                                        Notify(PSTR("\r\nNo pin was set"), 0x80);
#endif
                                        hci_pin_code_negative_request_reply();
                                }
                                break;

                        case EV_LINK_KEY_REQUEST:
#ifdef DEBUG_USB_HOST
                                Notify(PSTR("\r\nReceived Key Request"), 0x80);
#endif
                                hci_link_key_request_negative_reply();
                                break;

                        case EV_AUTHENTICATION_COMPLETE:
                                if(pairWithWii && !connectToWii) {
#ifdef DEBUG_USB_HOST
                                        Notify(PSTR("\r\nPairing successful with Wiimote"), 0x80);
#endif
                                        connectToWii = true; // Used to indicate to the Wii service, that it should connect to this device
                                } else if(pairWithHIDDevice && !connectToHIDDevice) {
#ifdef DEBUG_USB_HOST
                                        Notify(PSTR("\r\nPairing successful with HID device"), 0x80);
#endif
                                        connectToHIDDevice = true; // Used to indicate to the BTHID service, that it should connect to this device
                                } else if (pairWithOtherDevice && !connectToOtherDevice)
                                {
                                        Notify(PSTR("\r\nPairing successful with 'Other' device"), 0x80);
                                        connectToOtherDevice = true;
                                }
                                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: "), 0x80);
                                        D_PrintHex<uint8_t > (hcibuf[0], 0x80);
                                }
                                break;
#endif
                } // Switch
        }
#ifdef EXTRADEBUG
        else {
                Notify(PSTR("\r\nHCI event error: "), 0x80);
                D_PrintHex<uint8_t > (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_check_flag(HCI_FLAG_CMD_COMPLETE)) {
                                hci_counter = 0;
#ifdef DEBUG_USB_HOST
                                Notify(PSTR("\r\nHCI Reset complete"), 0x80);
#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_USB_HOST
                                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_check_flag(HCI_FLAG_CMD_COMPLETE)) {
#ifdef DEBUG_USB_HOST
                                Notify(PSTR("\r\nWrite class of device"), 0x80);
#endif
                                hci_state = HCI_BDADDR_STATE;
                                hci_read_bdaddr();
                        }
                        break;

                case HCI_BDADDR_STATE:
                        if(hci_check_flag(HCI_FLAG_READ_BDADDR)) {
#ifdef DEBUG_USB_HOST
                                Notify(PSTR("\r\nLocal Bluetooth Address: "), 0x80);
                                for(int8_t i = 5; i > 0; i--) {
                                        D_PrintHex<uint8_t > (my_bdaddr[i], 0x80);
                                        Notify(PSTR(":"), 0x80);
                                }
                                D_PrintHex<uint8_t > (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_check_flag(HCI_FLAG_READ_VERSION)) {
                                if(btdName != NULL) {
                                        hci_set_local_name(btdName);
                                        hci_state = HCI_SET_NAME_STATE;
                                } else
                                        hci_state = HCI_CHECK_DEVICE_SERVICE;
                        }
                        break;

                case HCI_SET_NAME_STATE:
                        if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
#ifdef DEBUG_USB_HOST
                                Notify(PSTR("\r\nThe name is set to: "), 0x80);
                                NotifyStr(btdName, 0x80);
#endif
                                hci_state = HCI_CHECK_DEVICE_SERVICE;
                        }
                        break;

                case HCI_CHECK_DEVICE_SERVICE:
                        if(pairWithHIDDevice || pairWithWii) { // Check if it should try to connect to a Wiimote
#ifdef DEBUG_USB_HOST
                                if(pairWithWii)
                                        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);
                                else
                                        Notify(PSTR("\r\nPlease enable discovery of your device"), 0x80);
#endif
                                hci_inquiry();
                                hci_state = HCI_INQUIRY_STATE;
                        } else if (pairWithOtherDevice)
                        {
                                Notify(PSTR("\r\nPairing to 'Other' device with predefined address"), 0x80);
                                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_check_flag(HCI_FLAG_DEVICE_FOUND)) {
                                hci_inquiry_cancel(); // Stop inquiry
#ifdef DEBUG_USB_HOST
                                if(pairWithWii)
                                        Notify(PSTR("\r\nWiimote found"), 0x80);
                                else if (pairWithOtherDevice)
                                        ;//Notify(PSTR("\r\n'Other' device found"), 0x80);
                                else
                                        Notify(PSTR("\r\nHID device found"), 0x80);

                                if (!pairWithOtherDevice)
                                {
                                        Notify(PSTR("\r\nNow just create the instance like so:"), 0x80);
                                        if(pairWithWii)
                                                Notify(PSTR("\r\nWII Wii(&Btd);"), 0x80);
                                        else
                                                Notify(PSTR("\r\nBTHID bthid(&Btd);"), 0x80);

                                        Notify(PSTR("\r\nAnd then press any button on the "), 0x80);
                                        if(pairWithWii)
                                                Notify(PSTR("Wiimote"), 0x80);
                                        else
                                                Notify(PSTR("device"), 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_DEVICE_STATE;
                        }
                        break;

                case HCI_CONNECT_DEVICE_STATE:
                        if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
#ifdef DEBUG_USB_HOST
                                if(pairWithWii)
                                        Notify(PSTR("\r\nConnecting to Wiimote"), 0x80);
                                else if (pairWithOtherDevice)
                                        Notify(PSTR("\r\nConnecting to 'Other' device..."), 0x80);
                                else
                                        Notify(PSTR("\r\nConnecting to HID device"), 0x80);
#endif
                                hci_connect();
                                hci_state = HCI_CONNECTED_DEVICE_STATE;
                        }
                        break;

                case HCI_CONNECTED_DEVICE_STATE:
                        if(hci_check_flag(HCI_FLAG_CONNECT_EVENT)) {
                                if(hci_check_flag(HCI_FLAG_CONNECT_COMPLETE)) {
#ifdef DEBUG_USB_HOST
                                        if(pairWithWii)
                                                Notify(PSTR("\r\nConnected to Wiimote"), 0x80);
                                        else if (pairWithOtherDevice)
                                                Notify(PSTR("\r\nConnected to 'Other' device"), 0x80);
                                        else
                                                Notify(PSTR("\r\nConnected to HID device"), 0x80);
#endif
                                        hci_authentication_request(); // This will start the pairing with the Wiimote
                                        hci_state = HCI_SCANNING_STATE;
                                } else {
#ifdef DEBUG_USB_HOST
                                        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 && !connectToHIDDevice && !pairWithHIDDevice
                                && !connectToOtherDevice && !pairWithOtherDevice) {

#ifdef DEBUG_USB_HOST
                                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_check_flag(HCI_FLAG_INCOMING_REQUEST)) {
                                watingForConnection = false;
#ifdef DEBUG_USB_HOST
                                Notify(PSTR("\r\nIncoming Connection Request"), 0x80);
#endif
                                hci_remote_name();
                                hci_state = HCI_REMOTE_NAME_STATE;
                        } else if(hci_check_flag(HCI_FLAG_DISCONNECT_COMPLETE))
                                hci_state = HCI_DISCONNECT_STATE;
                        break;

                case HCI_REMOTE_NAME_STATE:
                        if(hci_check_flag(HCI_FLAG_REMOTE_NAME_COMPLETE)) {
#ifdef DEBUG_USB_HOST
                                Notify(PSTR("\r\nRemote Name: "), 0x80);
                                for(uint8_t i = 0; i < strlen(remote_name); i++)
                                        Notifyc(remote_name[i], 0x80);
#endif
                                if(strncmp((const char*)remote_name, "Nintendo", 8) == 0) {
                                        incomingWii = true;
#ifdef DEBUG_USB_HOST
                                        Notify(PSTR("\r\nWiimote is connecting"), 0x80);
#endif
                                        if(strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-TR", 22) == 0) {
#ifdef DEBUG_USB_HOST
                                                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_USB_HOST
                                                Notify(PSTR(" - Wii U Pro Controller"), 0x80);
#endif
                                                motionPlusInside = true;
                                                wiiUProController = true;
                                        } else {
                                                motionPlusInside = false;
                                                wiiUProController = false;
                                        }
                                }
                                if(classOfDevice[2] == 0 && classOfDevice[1] == 0x25 && classOfDevice[0] == 0x08 && strncmp((const char*)remote_name, "Wireless Controller", 19) == 0) {
#ifdef DEBUG_USB_HOST
                                        Notify(PSTR("\r\nPS4 controller is connecting"), 0x80);
#endif
                                        incomingPS4 = true;
                                }
                                if(pairWithWii && motionPlusInside)
                                        hci_state = HCI_CONNECT_DEVICE_STATE;
                                else {
                                        hci_accept_connection();
                                        hci_state = HCI_CONNECTED_STATE;
                                }
                        }
                        break;

                case HCI_CONNECTED_STATE:
                        if(hci_check_flag(HCI_FLAG_CONNECT_COMPLETE)) {
#ifdef DEBUG_USB_HOST
                                Notify(PSTR("\r\nConnected to Device: "), 0x80);
                                for(int8_t i = 5; i > 0; i--) {
                                        D_PrintHex<uint8_t > (disc_bdaddr[i], 0x80);
                                        Notify(PSTR(":"), 0x80);
                                }
                                D_PrintHex<uint8_t > (disc_bdaddr[0], 0x80);
#endif
                                if(incomingPS4)
                                        connectToHIDDevice = true; // We should always connect to the PS4 controller

                                // 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_check_flag(HCI_FLAG_DISCONNECT_COMPLETE)) {
#ifdef DEBUG_USB_HOST
                                Notify(PSTR("\r\nHCI Disconnected from Device"), 0x80);
#endif
                                hci_event_flag = 0; // Clear all flags

                                // Reset all buffers
                                memset(hcibuf, 0, BULK_MAXPKTSIZE);
                                memset(l2capinbuf, 0, BULK_MAXPKTSIZE);

                                connectToWii = incomingWii = pairWithWii = false;
                                connectToHIDDevice = incomingHIDDevice = pairWithHIDDevice = false;
                                pairWithOtherDevice = connectToOtherDevice = false;
                                incomingPS4 = false;

                                hci_state = HCI_SCANNING_STATE;
                        }
                        break;
                default:
                        break;
        }
}

void BTD::ACL_event_task() {
        uint16_t length = BULK_MAXPKTSIZE;
        uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_DATAIN_PIPE ].epAddr, &length, l2capinbuf); // Input on endpoint 2

        if(!rcode) { // Check for errors
                if(length > 0) { // Check if any data was read
                        for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++) {
                                if(btService[i])
                                        btService[i]->ACLData(l2capinbuf);
                        }
                }
        }
#ifdef EXTRADEBUG
        else if(rcode != hrNAK) {
                Notify(PSTR("\r\nACL data in error: "), 0x80);
                D_PrintHex<uint8_t > (rcode, 0x80);
        }
#endif
        for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++)
                if(btService[i])
                        btService[i]->Run();
}

/************************************************************/
/*                    HCI Commands                        */

/************************************************************/
void BTD::HCI_Command(uint8_t* data, uint16_t nbytes) {
        hci_clear_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_clear_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() {
        hci_clear_flag(HCI_FLAG_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() {
        hci_clear_flag(HCI_FLAG_READ_VERSION);
        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_clear_flag(HCI_FLAG_CONNECT_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_clear_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_inquiry() {
        hci_clear_flag(HCI_FLAG_DEVICE_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] = 0x00; // Parameter Total Length = 0

        HCI_Command(hcibuf, 3);
}

void BTD::hci_connect() {
        if (pairWithOtherDevice)
                hci_connect(remote_bdaddr);
        else
                hci_connect(disc_bdaddr); // Use last discovered device
}

void BTD::hci_connect(uint8_t *bdaddr) {
        hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE | HCI_FLAG_CONNECT_EVENT);
        hcibuf[0] = 0x05;
        hcibuf[1] = 0x01 << 2; // HCI OGF = 1
        hcibuf[2] = 0x0D; // parameter Total Length = 13
        hcibuf[3] = bdaddr[0]; // 6 octet bdaddr (LSB)
        hcibuf[4] = bdaddr[1];
        hcibuf[5] = bdaddr[2];
        hcibuf[6] = bdaddr[3];
        hcibuf[7] = bdaddr[4];
        hcibuf[8] = 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) {
                hcibuf[9] = 6; // Pin length is the length of the Bluetooth address
                if(wiiUProController) {
#ifdef DEBUG_USB_HOST
                        Notify(PSTR("\r\nParing with Wii U Pro Controller"), 0x80);
#endif
                        for(uint8_t i = 0; i < 6; i++)
                                hcibuf[10 + i] = my_bdaddr[i]; // The pin is the Bluetooth dongles Bluetooth address backwards
                } else {
                        for(uint8_t i = 0; i < 6; i++)
                                hcibuf[10 + i] = disc_bdaddr[i]; // The pin is the Wiimote's Bluetooth address backwards
                }
                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);
}

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);
}

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);
}

void BTD::hci_disconnect(uint16_t handle) { // This is called by the different services
        hci_clear_flag(HCI_FLAG_DISCONNECT_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 = 24
        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);
}
/*******************************************************************
 *                                                                 *
 *                        HCI ACL Data Packet                      *
 *                                                                 *
 *   buf[0]          buf[1]          buf[2]          buf[3]
 *   0       4       8    11 12      16              24            31 MSB
 *  .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
 *  |      HCI Handle       |PB |BC |       Data Total Length       |   HCI ACL Data Packet
 *  .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
 *
 *   buf[4]          buf[5]          buf[6]          buf[7]
 *   0               8               16                            31 MSB
 *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
 *  |            Length             |          Channel ID           |   Basic L2CAP header
 *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
 *
 *   buf[8]          buf[9]          buf[10]         buf[11]
 *   0               8               16                            31 MSB
 *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
 *  |     Code      |  Identifier   |            Length             |   Control frame (C-frame)
 *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.   (signaling packet format)
 */
/************************************************************/
/*                    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
#ifdef DEBUG_USB_HOST
                Notify(PSTR("\r\nError sending L2CAP message: 0x"), 0x80);
                D_PrintHex<uint8_t > (rcode, 0x80);
                Notify(PSTR(" - Channel ID: "), 0x80);
                D_PrintHex<uint8_t > (channelHigh, 0x80);
                Notify(PSTR(" "), 0x80);
                D_PrintHex<uint8_t > (channelLow, 0x80);
#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);
}

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);
}

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);
}

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);
}

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);
}

/* PS3 Commands - only set Bluetooth address is implemented in this library */
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, so it is MSB first

        // 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);
}

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);
}

Now in order to make our Luminardo to initiate pairing with remote device the original sketch provided with USB Host 2.0 library should be modified the following way (please refer to the highlighted lines in the listing below). Note, that remote Bluetooth MAC address must be reversed as shown before fed to the instance of SPP. Unfortunately, this sketch won’t be able to establish SPP communication just yet – there is still some work to be done.

/*
   Luminardo USB Host Bluetooth SPP Test 
   (based on example from TKJ Electronics)
   This example code is in the public domain.
 */
 
#include <Board.h>
#include <SPP.h>
#include <usbhub.h>

USB Usb;

BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so

//40:22:11:00:69:58 OBDII
uint8_t remote_addr[6] = {0x58, 0x69, 0x00, 0x11, 0x22, 0x40};

SPP SerialBT(&Btd, "Luminardo", "1234", true, (uint8_t*)&remote_addr);
boolean firstMessage = true;

// the setup routine runs once when you press reset:
void setup() {
  // initialize serial communication at 57600 bits per second:
  Serial.begin(57600);
  Serial.println(F("Luminardo USB Host Bluetooth SPP Test"));
  
  if (Usb.Init() == -1) {
    Serial.println(F("OSC did not start"));
    while (1); //halt
  }
  Serial.println(F("SPP Bluetooth Library Started"));
  
  Serial.print(F("Enabling VBus..."));  
  Usb.gpioWr(1);
  Serial.print(F(" enabled"));
}

// the loop routine runs over and over again forever:
void loop() {
  
  Usb.Task(); // The SPP data is actually not send until this is called, one could call SerialBT.send() directly as well

  if (SerialBT.connected) {
    if (firstMessage) {
      Serial.print(F("\r\nSerial Bluetooth connected"));
      firstMessage = false;
      SerialBT.println(F("Hello from Luminardo")); // Send welcome message
    }
    if (Serial.available())
      SerialBT.write(Serial.read());
    if (SerialBT.available())
      Serial.write(SerialBT.read());
  }
  else
    firstMessage = true;
}

There are also a few minor changes in SPP.cpp and SPP.h in constructor, please refer to Download section of this post. After compiling and running the sketch above Luminardo will produce output similar to the one below:

Luminardo USB Host Bluetooth SPP Test
SPP Bluetooth Library Started
Enabling VBus... enabled
Bluetooth Dongle Initialized
HCI Reset complete
Write class of device
Local Bluetooth Address: 00:19:0E:12:65:6A
The name is set to: Luminardo
Pairing to 'Other' device with predefined address
Device: 40:22:11:00:69:58 has been found
Connecting to 'Other' device...
Connected to 'Other' device
Received Key Request
Bluetooth pin is set too: 1234
Pairing successful with 'Other' device

Now our Luminardo is capable of pairing with SPP-aware Bluetooth devices. However, at this stage it doesn’t give us much of a practical application, it is rather a moral satisfaction. The next post will be giving an insight a little bit deeper into Bluetooth world: L2CAP layer, SDP (Service Discovery Protocol) and its pecularities, Bluetooth Core specification and how to make sense out of these things. Stay tuned!

Downloads:

1. USB Host 2.0 Changes Allowing to Initiate Pairing with any Device.