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);
}
void l2capAclLinkDataRx(uint16_t conn, char first, const uint8_t* data, uint16_t size){
uint16_t chan, len;
unsigned i;
char freeIt = 0;
#if UGLY_SCARY_DEBUGGING_CODE
dbgPrintf("L2CAP data:");
for(chan = 0; chan < size; chan++) dbgPrintf(" %02X", data[chan]);
dbgPrintf("\n\n");
#endif
if(first){
len = getLE16(data + 0);
chan = getLE16(data + 2);
data += 4;
size -= 4;
if(size >= len){
if(size > len) dbgPrintf("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){
dbgPrintf("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){
dbgPrintf("L2CAP: not enough buffer slots to buffer incomplete frame. Dropping\n");
}
else{
uint8_t* ptr = malloc(size);
if(!ptr){
dbgPrintf("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){
dbgPrintf("L2CAP: unexpected 'non-first' frame for conn %u. Dropping.\n", conn);
return;
}
if(size > gIncomingPieces[i].lenNeed){
dbgPrintf("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){
dbgPrintf("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) dbgPrintf("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)) dbgPrintf("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) dbgPrintf("L2CAP: data for nonexistent connection %u.%u\n", conn, chan);
else c->service->descr.serviceRx(c->serviceInstance, data, len);
}
if(freeIt) free(data);
}
int StreamBase::get16(void)
{
return bigendian() ? getBE16() : getLE16();
}
