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 l2capConnectionCloseEx(Connection* c, Connection* prev, char sendDiscPacket){
uint8_t discCmd[6];
if(c->service->descr.serviceInstanceFree) c->service->descr.serviceInstanceFree(c->serviceInstance);
if(sendDiscPacket){
discCmd[0] = L2CAP_CMD_DISC_REQ;
discCmd[1] = gL2capID++;
putLE16(discCmd + 2, c->remChan);
putLE16(discCmd + 4, c->chan);
l2capSendControlRawBuf(c->conn, discCmd, 6);
}
if(prev) prev->next = c->next;
else connections = c->next;
free(c);
}
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;
dbgPrintf("L2CAP: control packet too small\n");
}
else{
cmd = *data++;
id = *data++;
len = getLE16(data);
data += 2;
size -= 4;
if(len != size){
dbgPrintf("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){
dbgPrintf("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)){
dbgPrintf("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){
dbgPrintf("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){
dbgPrintf("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){
dbgPrintf("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){
dbgPrintf("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){
dbgPrintf("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){
dbgPrintf("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:{
dbgPrintf("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);
}
