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

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

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

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