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