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)){
#if UGLY_SCARY_DEBUGGING_CODE
uint32_t i;
uint8_t buf[256];
sg_copyto(data, buf);
dbgPrintf("L2CAP TX: ");
for(i = 0; i < sg_length(data); i++) dbgPrintf(" %02X", buf[i]);
dbgPrintf("\n");
#endif
btAclDataTx(conn, 1, BT_BCAST_NONE, data);
}
else{
sg_free(data);
free(data);
}
}
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 btRfcommSend(uint16_t conn, uint16_t remChan, const uint8_t* data, uint16_t sz){
int i;
#if UGLY_SCARY_DEBUGGING_CODE
dbgPrintf("Sending RFCOMM packet:");
for(i = 0; i < sz; i++) dbgPrintf(" %02X", data[i]);
dbgPrintf("\n\n");
#endif
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);
}
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);
}
