void l2capServiceTx(uint16_t conn, uint16_t remChan, sg_buf* data){
uint8_t hdr[4];
uint16_t len = sg_length(data);
putLE16(hdr + 0, len);
putLE16(hdr + 2, remChan);
if(sg_add_front(data, hdr, 4, SG_FLAG_MAKE_A_COPY)){
uint32_t i;
uint8_t buf[256];
sg_copyto(data, buf);
SIOPrintString("L2CAP TX: ");
for(i = 0; i < sg_length(data); i++)
{
SIOPutHex(buf[i]);
SIOPutChar(' ');
}
SIOPrintString("\r\n");
btAclDataTx(conn, 1, BT_BCAST_NONE, data);
}
else{
sg_free(data);
free(data);
}
}
static void btConnEndF(void* userData, uint16_t conn, uint8_t reason){
SIOPrintString("BT connection with handle ");
SIOPutDec(conn);
SIOPrintString(" down for reason ");
SIOPutDec(reason);
SIOPrintString("\r\n");
l2capAclLinkDown(conn);
}
static char btVerboseScanCbkF(void* userData, BtDiscoveryResult* dr){
if(bt_drF) return bt_drF(dr->mac, dr->PSRM, dr->PSPM, dr->PSM, dr->co, dr->dc);
SIOPrintString("BT: no callback for scan makes the scan useless, no?");
return 0;
}
static char btConnReqF(void* userData, const uint8_t* mac, uint32_t devClass, uint8_t linkType){ //return 1 to accept
if(bt_crF) return bt_crF(mac, devClass, linkType);
SIOPrintString("BT connection request: %s connection from %02x:%02x:%02x:%02x:%02x:%02x (class %06X) -> accepted due to lack of handler\n", linkType ? "ACL" : "SCO", mac[5], mac[4], mac[3], mac[2], mac[1], mac[0], devClass);
return 1;
}
static char btLinkKeyRequest(void* userData, const uint8_t* mac, uint8_t* buf){
if(bt_krF) return bt_krF(mac, buf);
SIOPrintString("BT Link key request from %02x:%02x:%02x:%02x:%02x:%02x -> denied due to lack of handler", mac[5], mac[4], mac[3], mac[2], mac[1], mac[0]);
return 0;
}
static uint8_t btPinRequestF(void* userData, const uint8_t* mac, uint8_t* buf){ //fill buff with PIN code, return num bytes used (16 max) return 0 to decline
if(bt_prF) return bt_prF(mac, buf);
SIOPrintString("BT PIN request from %02x:%02x:%02x:%02x:%02x:%02x -> '0000' due to lack of handler", mac[5], mac[4], mac[3], mac[2], mac[1], mac[0]);
buf[0] = buf[1] = buf[2] = buf[3] = '0';
return 4;
}
BOOL USBHostBluetoothInit ( BYTE address, DWORD flags, BYTE clientDriverID )
{
BYTE *pDesc;
// Initialize state
gc_DevData.rxEvtLength = 0;
gc_DevData.rxAclLength = 0;
gc_DevData.flags.val = 0;
// Save device the address, VID, & PID
gc_DevData.ID.deviceAddress = address;
pDesc = USBHostGetDeviceDescriptor(address);
pDesc += 8;
gc_DevData.ID.vid = (WORD)*pDesc; pDesc++;
gc_DevData.ID.vid |= ((WORD)*pDesc) << 8; pDesc++;
gc_DevData.ID.pid = (WORD)*pDesc; pDesc++;
gc_DevData.ID.pid |= ((WORD)*pDesc) << 8; pDesc++;
// Save the Client Driver ID
gc_DevData.clientDriverID = clientDriverID;
#ifdef USBHOSTBT_DEBUG
SIOPrintString( "GEN: USB Generic Client Initalized: flags=0x" );
SIOPutHex( flags );
SIOPrintString( " address=" );
SIOPutDec( address );
SIOPrintString( " VID=0x" );
SIOPutHex( gc_DevData.ID.vid >> 8 );
SIOPutHex( gc_DevData.ID.vid & 0xFF );
SIOPrintString( " PID=0x" );
SIOPutHex( gc_DevData.ID.pid >> 8 );
SIOPutHex( gc_DevData.ID.pid & 0xFF );
SIOPrintString( "\r\n" );
#endif
// Generic Client Driver Init Complete.
gc_DevData.flags.initialized = 1;
// Notify that application that we've been attached to a device.
USB_HOST_APP_EVENT_HANDLER(address, EVENT_BLUETOOTH_ATTACH, &(gc_DevData.ID), sizeof(BLUETOOTH_DEVICE_ID) );
return TRUE;
} // USBHostBluetoothInit
static void btRfcommTxData(RfcommInstanceState* state, uint8_t dlci, sg_buf* buf){
uint8_t data[5] = {(dlci << BT_RFCOMM_DLCI_SHIFT) | BT_RFCOMM_DLCI_MSK_EA, BT_RFCOMM_CMD_UIH};
uint32_t dataLen = sg_length(buf);
uint8_t hdrLen = 3;
if(dataLen >= 128){ //need 2 length bytes
SIOPrintString("RFCOMM Pack. too long > 128\r\n");
data[2] = dataLen << 1;
data[3] = dataLen >> 7;
hdrLen++;
} else data[2] = (dataLen << 1) | 1;
static void btRfcommSend(uint16_t conn, uint16_t remChan, const uint8_t* data, uint16_t sz){
int i;
SIOPrintString("Sending RFCOMM packet:");
for(i = 0; i < sz; i++)
{
SIOPutHex(data[i]);
SIOPutChar(' ');
}
SIOPrintString("\r\n");
sg_buf* buf = sg_alloc();
if(!buf) return;
if(sg_add_front(buf, data, sz, SG_FLAG_MAKE_A_COPY)){
l2capServiceTx(conn, remChan, buf);
return;
}
sg_free(buf);
free(buf);
}
void l2capAclLinkDataRx(uint16_t conn, char first, const uint8_t* data, uint16_t size){
uint16_t chan, len;
unsigned i;
char freeIt = 0;
SIOPrintString("L2CAP data:");
for(chan = 0; chan < size; chan++)
{
SIOPutHex(data[chan]);
SIOPutChar(' ');
}
SIOPrintString("\r\n");
if(first){
len = getLE16(data + 0);
chan = getLE16(data + 2);
data += 4;
size -= 4;
if(size >= len){
if(size > len)
{
SIOPrintString("L2CAP: ACL provided likely invalid L2CAP packet (ACL.len=%u L2CAP.len=%u)\n", size, len);
}
}
else{
for(i = 0; i < L2CAP_MAX_PIECED_MESSAGES; i++){
if(gIncomingPieces[i].conn == conn){
SIOPrintString("L2CAP: conn %d: 'first' frame while another incomplete already buffered. Dropping the old one.\n", conn);
free(gIncomingPieces[i].buf);
gIncomingPieces[i].conn = 0;
break;
}
}
if(i == L2CAP_MAX_PIECED_MESSAGES) for(i = 0; i < L2CAP_MAX_PIECED_MESSAGES && gIncomingPieces[i].conn; i++);
if(i == L2CAP_MAX_PIECED_MESSAGES){
SIOPrintString("L2CAP: not enough buffer slots to buffer incomplete frame. Dropping\n");
}
else{
uint8_t* ptr = malloc(size);
if(!ptr){
SIOPrintString("L2CAP: cannot allocate partial frame buffer. Dropping\n");
return;
}
memcpy(ptr, data, size);
gIncomingPieces[i].buf = ptr;
gIncomingPieces[i].lenGot = size;
gIncomingPieces[i].lenNeed = len - size;
gIncomingPieces[i].chan = chan;
gIncomingPieces[i].conn = conn;
}
return;
}
}
else{
uint8_t* ptr;
for(i = 0; i < L2CAP_MAX_PIECED_MESSAGES && gIncomingPieces[i].conn != conn; i++);
if(i == L2CAP_MAX_PIECED_MESSAGES){
SIOPrintString("L2CAP: unexpected 'non-first' frame for conn %u. Dropping.\n", conn);
return;
}
if(size > gIncomingPieces[i].lenNeed){
SIOPrintString("L2CAP: 'non-first' frame too large. Need %u bytes, got %u. Dropping.\n", gIncomingPieces[i].lenNeed, size);
return;
}
ptr = realloc(gIncomingPieces[i].buf, gIncomingPieces[i].lenGot + size);
if(!ptr){
SIOPrintString("L2CAP: failed to resize buffer for partial frame receive. Droping\n");
free(gIncomingPieces[i].buf);
gIncomingPieces[i].conn = 0;
return;
}
memcpy(ptr + gIncomingPieces[i].lenGot, data, size);
gIncomingPieces[i].buf = ptr;
gIncomingPieces[i].lenGot += size;
gIncomingPieces[i].lenNeed -= size;
if(gIncomingPieces[i].lenNeed) return; //data still not complete
gIncomingPieces[i].conn = 0;
chan = gIncomingPieces[i].chan;
len = gIncomingPieces[i].lenGot;
data = ptr;
freeIt = 1;
}
if(chan == 0)
{
SIOPrintString("L2CAP: data on connection %u.0\n", conn);
}
else if(chan == 1) l2capHandleControlChannel(conn, data, len);
else if(chan == 2){ //connectionless
uint16_t PSM = getLE16(data + 0);
data += 2;
len -= 2;
Service* s = services;
while(s && s->PSM != PSM) s = s->next;
if(!s || !(s->descr.flags & L2CAP_FLAG_SUPPORT_CONNECTIONLESS)) SIOPrintString("L2CAP: connectionless data on %u.2 for unknown PSM 0x%04X\n", conn, PSM);
else s->descr.serviceRx(NULL, data, len);
}
else{ //conection-oriented
Connection* c = l2capFindConnection(conn, chan, NULL);
if(!c)
{
SIOPrintString("L2CAP: data for nonexistent connection %u.%u\n", conn, chan);
}
else c->service->descr.serviceRx(c->serviceInstance, data, len);
}
if(freeIt) free(data);
}
static void l2capHandleControlChannel(uint16_t conn, const uint8_t* data, uint16_t size){
char rejectCommand = 0;
uint16_t rejectReason = L2CAP_REJECT_REASON_WTF;
Service* s = services;
Connection* c;
uint8_t cmd, id;
uint8_t buf[16];
uint16_t chan, remChan, len;
if(!gL2capID) gL2capID++;
if(size < 2){
rejectCommand = 1;
SIOPrintString("L2CAP: control packet too small\n");
}
else{
cmd = *data++;
id = *data++;
len = getLE16(data);
data += 2;
size -= 4;
if(len != size){
SIOPrintString("L2CAP (control packet internal sizes mismatch (%u != %u)\n", len, size);
rejectCommand = 1;
}
else switch(cmd){
case L2CAP_CMD_CONN_REQ:{
uint16_t PSM;
//get some request data
if(size != 4){
SIOPrintString("L2CAP: ConnectionRequest packet size is wrong(%u)\n", size);
rejectCommand = 1;
break;
}
PSM = getLE16(data + 0);
remChan = getLE16(data + 2);
//init the reply
buf[0] = L2CAP_CMD_CONN_RESP;
buf[1] = id;
putLE16(buf + 2, 8); //length
putLE16(buf + 4, 0); //DCID
putLE16(buf + 6, remChan); //SCID
putLE16(buf + 10, 0); //no further information
//find the service
while(s && s->PSM != PSM) s = s->next;
if(!s || !(s->descr.flags & L2CAP_FLAG_SUPPORT_CONNECTIONS)){
SIOPrintString("L2CAP: rejecting conection to unknown PSM 0x%04X\n", PSM);
putLE16(buf + 8, L2CAP_CONN_FAIL_NO_SUCH_PSM);
}
else{
void* instance = NULL;
chan = 0;
c = malloc(sizeof(Connection));
if(c) chan = l2capFindFreeLocalChannel(conn);
if(chan) instance = s->descr.serviceInstanceAllocate(conn, chan, remChan);
if(instance){
putLE16(buf + 4, chan);
putLE16(buf + 8, L2CAP_CONN_SUCCESS);
c->service = s;
c->serviceInstance = instance;
c->conn = conn;
c->chan = chan;
c->remChan = remChan;
c->next = connections;
connections = c;
}
else{
putLE16(buf + 8, L2CAP_CONN_FAIL_RESOURCES);
if(c) free(c);
}
}
size = 12;
break;
}
case L2CAP_CMD_CONFIG_REQ:{
uint16_t flags;
//get some request data
if(size < 4){
SIOPrintString("L2CAP: ConfigurationRequest packet size is wrong(%u)\n", size);
rejectCommand = 1;
break;
}
chan = getLE16(data + 0);
flags = getLE16(data + 2);
if(flags & 1){ //flags continue - we do not support that
size = 0;
break;
}
size -= 4;
data += 4;
//locate the connection at hand
c = l2capFindConnection(conn, chan, NULL);
if(!c){
SIOPrintString("L2CAP: ConfigurationRequest for an unknown channel %u.%u\n", conn, chan);
rejectCommand = 1;
break;
}
chan = c->remChan;
//reply with just our rx-MTU
buf[0] = L2CAP_CMD_CONFIG_RESP;
buf[1] = id;
putLE16(buf + 2, 10); //length
putLE16(buf + 4, c->remChan); //SCID
putLE16(buf + 6, 0); //flags
putLE16(buf + 8, 0); //success
buf[10] = L2CAP_OPTION_MTU; //mtu_property.type
buf[11] = 2; //mtu_property.len
putLE16(buf + 12, BT_RX_BUF_SZ - 8); //mtu value
l2capSendControlRawBuf(conn, buf, 14); //send it
//we need to send such a packet there too
buf[0] = L2CAP_CMD_CONFIG_REQ; //configuration request
buf[1] = gL2capID++;
putLE16(buf + 2, 4); //length
putLE16(buf + 4, c->remChan); //SCID
putLE16(buf + 6, 0); //flags
size = 8;
break;
}
case L2CAP_CMD_CONFIG_RESP:{
//we do nothing here - perhaps we should?
size = 0;
break;
}
case L2CAP_CMD_DISC_REQ:{
Connection* p;
//get some request data
if(size != 4){
SIOPrintString("L2CAP: DisconnectionRequest packet size is wrong(%u)\n", size);
rejectCommand = 1;
break;
}
chan = getLE16(data + 0);
remChan = getLE16(data + 2);
c = l2capFindConnection(conn, chan, &p);
//handle some likely error cases
if(!c){
SIOPrintString("L2CAP: DisconnectionRequest for an unknown channel %u.%u\n", conn, chan);
rejectReason = L2CAP_REJECT_REASON_INVALID_CID;
rejectCommand = 1; //reject as per spec
break;
}
if(c->remChan != remChan){
SIOPrintString("L2CAP: DisconnectionRequest for an unmatched channel on %u.%u (%u != %u)\n", conn, chan, c->remChan, remChan);
size = 0; //drop as per spec
break;
}
//perform needed cleanup
l2capConnectionCloseEx(c, p, 0);
//send the reply
buf[0] = L2CAP_CMD_DISC_RESP; //disconnection response
buf[1] = id; //copied as required
putLE16(buf + 2, 4); //length
putLE16(buf + 4, chan); //DCID
putLE16(buf + 6, remChan); //SCID
size = 8;
break;
}
case L2CAP_CMD_DISC_RESP:{
//nothing to do - we did cleanup when we requested the connection closure...
size = 0;
break;
}
case L2CAP_CMD_ECHO_REQ:{
buf[0] = L2CAP_CMD_ECHO_RESP; //ping response
buf[1] = id; //copied as required
putLE16(buf + 2, 0); //0-length replies are ok
size = 4;
break;
}
case L2CAP_CMD_INFO_REQ:{
uint16_t info;
//get some request data
if(size != 2){
SIOPrintString("L2CAP: InformationRequest packet size is wrong(%u)\n", size);
rejectCommand = 1;
break;
}
info = getLE16(data + 0);
buf[0] = L2CAP_CMD_INFO_RESP; //information response
buf[1] = id; //copied as required
putLE16(buf + 4, info); //info type
if(info == 1){ //connectionless mtu
putLE16(buf + 6, 0); //success
putLE16(buf + 8, BT_RX_BUF_SZ - 8);
putLE16(buf + 2, 6); //length
size = 10;
}
else if(info == 2){ //extended features
putLE16(buf + 6, 0); //success
putLE16(buf + 8, 0);
putLE16(buf + 10, 0);
putLE16(buf + 2, 8); //length
size = 12;
}
else{ //whatever this request is, we do not support it
putLE16(buf + 6, 1); //info type not supported
putLE16(buf + 2, 4); //length
size = 8;
}
break;
}
default:{
SIOPrintString("L2CAP: unexpected command 0x%02X recieved\n", cmd);
size = 0;
}
}
}
if(rejectCommand){
buf[0] = L2CAP_CMD_REJECT; //disconnection response
buf[1] = id; //copied as required
putLE16(buf + 2, 4); //length
putLE16(buf + 4, rejectReason); //rejection reason
size = 6;
}
if(size) l2capSendControlRawBuf(conn, buf, size);
}
BOOL USBHostBluetoothEventHandler ( BYTE address, USB_EVENT event, void *data, DWORD size )
{
// Make sure it was for our device
if ( address != gc_DevData.ID.deviceAddress)
{
return FALSE;
}
// Handle specific events.
switch (event)
{
case EVENT_DETACH:
// Notify that application that the device has been detached.
USB_HOST_APP_EVENT_HANDLER(gc_DevData.ID.deviceAddress, EVENT_BLUETOOTH_DETACH, &gc_DevData.ID.deviceAddress, sizeof(BYTE) );
gc_DevData.flags.val = 0;
gc_DevData.ID.deviceAddress = 0;
#ifdef USBHOSTBT_DEBUG
SIOPrintString( "USB Host Bluetooth Device Detached: address=%d\n", address );
#endif
return TRUE;
case EVENT_TRANSFER:
if ( (data != NULL) && (size == sizeof(HOST_TRANSFER_DATA)) )
{
DWORD dataCount = ((HOST_TRANSFER_DATA *)data)->dataCount;
if ( ((HOST_TRANSFER_DATA *)data)->bEndpointAddress == (USB_IN_EP|USB_EP1) ) //GVG
{
//SIOPrintString( "E\n" );
gc_DevData.flags.rxEvtBusy = 0;
gc_DevData.rxEvtLength = dataCount;
if(!dataCount) return FALSE;
USB_HOST_APP_EVENT_HANDLER(gc_DevData.ID.deviceAddress, EVENT_BLUETOOTH_RX1_DONE, &dataCount, sizeof(DWORD) );
}
else if ( ((HOST_TRANSFER_DATA *)data)->bEndpointAddress == (USB_IN_EP|USB_EP2) ) //GVG
{
//if(!dataCount) return FALSE;
gc_DevData.flags.rxAclBusy = 0;
gc_DevData.rxAclLength = dataCount;
USB_HOST_APP_EVENT_HANDLER(gc_DevData.ID.deviceAddress, EVENT_BLUETOOTH_RX2_DONE, &dataCount, sizeof(DWORD) );
}
else if ( ((HOST_TRANSFER_DATA *)data)->bEndpointAddress == (USB_OUT_EP|USB_EP2) ) //GVG
{
gc_DevData.flags.txAclBusy = 0;
USB_HOST_APP_EVENT_HANDLER(gc_DevData.ID.deviceAddress, EVENT_BLUETOOTH_TX2_DONE, &dataCount, sizeof(DWORD) );
}
else
{
return FALSE;
}
return TRUE;
}
return FALSE;
case EVENT_SUSPEND:
case EVENT_RESUME:
case EVENT_BUS_ERROR:
default:
break;
}
return FALSE;
} // USBHostBluetoothEventHandler
static void btConnStartF(void* userData, uint16_t conn, const uint8_t* mac, uint8_t linkType, uint8_t encrMode){
SIOPrintString("BT %s connection up with handle %d to %02x:%02x:%02x:%02x:%02x:%02x encryption type %d\n",
linkType ? "ACL" : "SCO", conn, mac[5], mac[4], mac[3], mac[2], mac[1], mac[0], encrMode);
l2capAclLinkUp(conn);
}
