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