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