Test version of BlueUSB stack. Includes SDP and RFCOMM. As Client it allows to connect to my fischertechnik TX Controller. As Server it echo\\\\\\\'s characters to Putty. PIN=1234
Dependencies: mbed myUSBHost AvailableMemory
Dependents: mbed_TANK_Kinect myBlueUSB_ros ftusbClass
Diff: L2CAP.cpp
- Revision:
- 0:81ed8b6e4a8b
diff -r 000000000000 -r 81ed8b6e4a8b L2CAP.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/L2CAP.cpp Mon Apr 04 16:41:03 2011 +0000 @@ -0,0 +1,569 @@ +/* +Copyright (c) 2010 Peter Barrett + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + + +#include <stdio.h> +#include <stdlib.h> +#include <stdio.h> +#include <string.h> + +#include "Utils.h" +#include "hci.h" +#include "HCITransportUSB.h" + +#define L2CAP_COMMAND_REJ 0x01 +#define L2CAP_CONN_REQ 0x02 +#define L2CAP_CONN_RSP 0x03 +#define L2CAP_CONF_REQ 0x04 +#define L2CAP_CONF_RSP 0x05 +#define L2CAP_DISCONN_REQ 0x06 +#define L2CAP_DISCONN_RSP 0x07 +#define L2CAP_ECHO_REQ 0x08 +#define L2CAP_ECHO_RSP 0x09 +#define L2CAP_INFO_REQ 0x0a +#define L2CAP_INFO_RSP 0x0b + +//template <class T> T min(T a, T b) { return a<b ? a : b;} + +/* L2CAP command codes */ +const char* L2CAP_ComandCodeStr(int c) { + switch (c) { + case L2CAP_COMMAND_REJ: + return "L2CAP_COMMAND_REJ"; + case L2CAP_CONN_REQ: + return "L2CAP_CONN_REQ"; + case L2CAP_CONN_RSP: + return "L2CAP_CONN_RSP"; + case L2CAP_CONF_REQ: + return "L2CAP_CONF_REQ"; + case L2CAP_CONF_RSP: + return "L2CAP_CONF_RSP"; + case L2CAP_DISCONN_REQ: + return "L2CAP_DISCONN_REQ"; + case L2CAP_DISCONN_RSP: + return "L2CAP_DISCONN_RSP"; + case L2CAP_ECHO_REQ: + return "L2CAP_ECHO_REQ"; + case L2CAP_ECHO_RSP: + return "L2CAP_ECHO_RSP"; + case L2CAP_INFO_REQ: + return "L2CAP_INFO_REQ"; + case L2CAP_INFO_RSP: + return "L2CAP_INFO_RSP"; + } + return "unknown"; +} + +#define OFFSET 8 //means the buffer also has space for the l2cap/hci headers and need not be allocated and copied +//#define OFFSET 0 //means the buffer only has space for the payload which need to be copied +#if OFFSET == 0 +#define L2CAPBUFSIZE 128 +#else +#define L2CAPBUFSIZE 0 +#endif + +typedef struct { + u16 handle; + u16 length; // total + u16 l2capLength; // length -4 + u16 cid; // Signaling packet CID = 1 + u8 data[L2CAPBUFSIZE]; // Largest thing to send!!! todo +} L2CAPData; + +// +void BTDevice::Init() { + memset(&_info,0,sizeof(inquiry_info)); + _handle = 0; + _name[0] = 0; + _state = 0; +} + +// virtual SocketHandler +int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr) { + L2CAPSocket* s = (L2CAPSocket*)sock; + L2CAPAddr* a = (L2CAPAddr*)addr; + s->scid = 0x40 + sock->ID-1; // are these reserved? + s->dcid = 0; + Connect(s->scid,a->psm); + sock->State = SocketState_L2CAP_WaitConnectRsp; + contState = 0; + return sock->ID; +} + +// virtual SocketHandler, called from HCI which is ABOVE L2CAP +int BTDevice::Send(SocketInternal* sock, const u8* data, int len) { + L2CAPSocket* s = (L2CAPSocket*)sock; +#if OFFSET == 8 //sizeof L2CAPData header + L2CAPData &d = *const_cast<L2CAPData*>((L2CAPData*)data); +#else + L2CAPData d; +#endif + if (len > peer_mtu) {//mtu concerns the l2cap mtu, because we use basic mode we cannot segment + printf("MTU (%d) for outgoing packet (%d) exceeded\n", peer_mtu, len); + return 0; + } + d.handle = _handle | 0x2000; + d.length = 4 + len - OFFSET; + d.l2capLength = len - OFFSET; + d.cid = s->dcid; + printf("cid=%d: ", d.cid); + printfBytes("sending: ", data, len); + +#if OFFSET == 0 + if (len > L2CAPBUFSIZE) + return -1; + memcpy(d.data,data,len); + return Send((u8*)&d,len+8); +#else + return Send(data, len); +#endif +} + +// virtual SocketHandler +int BTDevice::Close(SocketInternal* sock) { + printf("L2CAP close %d\n",sock->ID); + sock->State = SocketState_L2CAP_WaitDisconnect; + L2CAPSocket* s = (L2CAPSocket*)sock; + return Disconnect(s->scid,s->dcid); +} + +L2CAPSocket* BTDevice::SCIDToSocket(int scid) { + return (L2CAPSocket*)GetSocketInternal(scid-0x40+1); +} + +int BTDevice::Send(const u8* data, int len) {//printfBytes("Transport : ", data, len); + _transport->ACLSend(data,len); + return 0; +} + +void BTDevice::repeat_cmd() { +printf("Cmd on handle %#x timed out, resending txid=%d\n", _handle, last_req.id); + Send ((u8*)&last_req, last_req.length+4);//danger! interrupt context, Send is not reentrant + //optionally set new larger timeout +} + +int BTDevice::Send(u8 c, u8 id, u16* params, int count) { + L2CAPCmd cmd; + cmd.handle = _handle | 0x2000; + cmd.length = 8 + count*2; + + cmd.l2capLength = cmd.length-4; + cmd.cid = 1; // Signaling packet + + cmd.cmd = c; + cmd.id = id; + cmd.cmdLength = count*2; + for (int i = 0; i < count; i++) + cmd.params[i] = params[i]; + if ((c & 1) == 0) { //this is a request + last_req = cmd; + rtx.attach(this, &BTDevice::repeat_cmd, 5.0); + printf("Starting timeout for %#x, txid=%d\n", _handle, id); + } + return Send((u8*)&cmd,cmd.length+4); +} + +int BTDevice::Connect(int scid, int psm) { + u16 p[2]; + p[0] = psm; + p[1] = scid; + return Send(L2CAP_CONN_REQ,_txid++,p,2); +} + +int BTDevice::Disconnect(int scid, int dcid) { + u16 p[2]; + p[0] = dcid; + p[1] = scid; + return Send(L2CAP_DISCONN_REQ,_txid++,p,2); +} + +int BTDevice::ConfigureRequest(int dcid) { + u16 p[4]; + p[0] = dcid; + p[1] = 0; + p[2] = 0x0201; // Options + p[3] = min(0x02A0, MAX_ACL_SIZE); // my receiving MTU 672 + return Send(L2CAP_CONF_REQ,_txid++,p,4); +} + +int BTDevice::ConfigureResponse(u8 rxid, int dcid) { + u16 p[3]; + p[0] = dcid; //source cid + p[1] = 0; //flags (no continuation) + p[2] = 0; //result (success) + return Send(L2CAP_CONF_RSP,rxid,p,3); +} + +int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid) { + u16 p[2]; + p[0] = dcid; + p[1] = scid; + return Send(L2CAP_DISCONN_RSP,rxid,p,2); +} +#if 0 +//handle16, length16, lengthL2CAP16, cid16, code8, tid8, lengthData16 +// 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 +void BTDevice::Control(const u8* data, int len) { //control channel receive + printf("\x1B[%dm", 31); + int cc = data[8];//command code + printf(L2CAP_ComandCodeStr(cc)); + //int result = LE16(data+16);//conn_rsp, and conf_resp only + //printf(" Result %d\n",result); + switch (cc) { + case L2CAP_COMMAND_REJ://bad command, eg. MTU, check (reason) + printf(" rejection reason=%d\n", LE16(data+12)); + break; + case L2CAP_CONN_REQ://incoming connection request, not expected but should reply with proper rejection (or accept) + //when a connection is accepted a new socket must be opened + break; + // Response to our initial connect from Remote + case L2CAP_CONN_RSP: { + int dcid = LE16(data+12); + int scid = LE16(data+14); + L2CAPSocket* s = SCIDToSocket(scid); + int result = LE16(data+16); + printf("Result=%d, Status = %d\n", result, LE16(data+18)); + if (s->si.State != SocketState_L2CAP_WaitConnectRsp) { + printf("Unexpected event ignored\n"); + break; + } + if (result == 0) { + if (s) { + s->si.State = SocketState_L2CAP_Config_wait; + s->dcid = dcid; + ConfigureRequest(dcid); + s->si.State = SocketState_L2CAP_Config_wait_reqrsp; + printf("Sent ConfigureRequest, state=WAIT_CONFIG_REQ-RSP\n"); + } + } else { + s->si.SetState(SocketState_Closed); + printf("Connect failed?\n"); + } + } + break; + + case L2CAP_CONF_RSP: { + int result = LE16(data+16); + printf("Result=%d, datalen=%d, %smore conf to follow\n", result, LE16(data+10), LE16(data+14)?"":"No "); + //should parse the config + printfBytes("CONF RSP:", data+8, LE16(data+10)+4); + int scid = LE16(data+12); + SocketInternal* s = (SocketInternal*)SCIDToSocket(scid); + if (s == 0) break; + if (s->State != SocketState_L2CAP_Config_wait_reqrsp && s->State != SocketState_L2CAP_Config_wait_rsp) { + printf("Unexpected event ignored\n"); + break; + } + if (result == 0) { //configuration acceptable + if (s->State == SocketState_L2CAP_Config_wait_reqrsp) { + s->State = SocketState_L2CAP_Config_wait_req; + printf("State=WAIT_CONFIG_REQ\n"); + } else { + ConfigureResponse(data[9],((L2CAPSocket*)s)->dcid);//data[9]==txid + printf("Sent ConfigureResponse, state=Open\n"); + s->SetState(SocketState_Open); + } + } else { + printf("Renegotiate configuration\n"); + } + } + break; + + case L2CAP_CONF_REQ: { + int scid = LE16(data+12);//flags (data[14] LSB is continuation flag, data[18],[19] are the MTU + L2CAPSocket* s = SCIDToSocket(scid); + printfBytes("CONF REQ: ", data+8, LE16(data+10)+4);//data+16 contains option type 1-4 1=MTU, 2=flush timeout, 3=QoS, 4=FCM + if (s == 0) break; + if (s->si.State == SocketState_Closed || + s->si.State == SocketState_L2CAP_WaitConnectRsp || + s->si.State == SocketState_L2CAP_WaitDisconnect) { + //Send Reject command + break; + } + switch (data[16]) { + case 1: + peer_mtu = LE16(data+18); + printf("MTU = %d bytes\n", peer_mtu); + break; + default: + printf("Unsupported configuration option %d, value = %#X\n", data[16], LE16(data+18)); + break; + } + if (1 /* options acceptable */) { + printf("Sending ConfigureResponse, old state=%d ", s->si.State); + ConfigureResponse(data[9],s->dcid);//data[9]==txid, success + switch (s->si.State) { + case SocketState_L2CAP_Config_wait: + s->si.State = SocketState_L2CAP_Config_wait_send; + break; + case SocketState_L2CAP_Config_wait_req: + ((SocketInternal*)s)->SetState(SocketState_Open); + break; + case SocketState_L2CAP_Config_wait_rsp: + break; + case SocketState_L2CAP_Config_wait_reqrsp: + s->si.State = SocketState_L2CAP_Config_wait_rsp; + break; + } + printf("new state=%d\n", s->si.State); + } else { //options not acceptable + ConfigureResponse(data[9],s->dcid);//indicates success but should indicate fail + } + } + break; + case L2CAP_DISCONN_REQ: { + int dcid = LE16(data+12); + int scid = LE16(data+14); + L2CAPSocket* s = SCIDToSocket(scid); + s->si.SetState(SocketState_Closed); + DisconnectResponse(data[9], scid, dcid); + } + break; + case L2CAP_DISCONN_RSP: { + int scid = LE16(data+14); + L2CAPSocket* s = SCIDToSocket(scid); + if (s->si.State == SocketState_L2CAP_WaitDisconnect) + s->si.SetState(SocketState_Closed); + } + break; + } + printf("\x1b[0m"); +} +#else +//code8, tid8, lengthData16 +// 0, 1, 2, 3 +void BTDevice::Control(const u8* data, int len) { //control channel receive + printf("\x1B[%dm", 31); + int cc = data[0];//command code + if (cc & 1) { //it is a response or a reject + rtx.detach(); //kill the timeout + printf("timeout cancelled for handle %#x, txid=%d\n", _handle, data[1]); + } + printf(L2CAP_ComandCodeStr(cc)); + switch (cc) { + case L2CAP_COMMAND_REJ://bad command, eg. MTU, check (reason) + printf(" rejection reason=%d\n", LE16(data+4)); + break; + case L2CAP_CONN_REQ://incoming connection request, not expected but should reply with proper rejection (or accept) + //when a connection is accepted a new socket must be opened + printf("Remote side requested a connection\n"); + break; + // Response to our initial connect from Remote + case L2CAP_CONN_RSP: { + int dcid = LE16(data+4); + int scid = LE16(data+6); + L2CAPSocket* s = SCIDToSocket(scid); + int result = LE16(data+10); + printf(" Result=%d, Status = %d\n", result, LE16(data+10)); + if (s->si.State != SocketState_L2CAP_WaitConnectRsp) { + printf("Unexpected event ignored\n"); + break; + } + if (result == 0) { + if (s) { + s->si.State = SocketState_L2CAP_Config_wait; + s->dcid = dcid; + ConfigureRequest(dcid); + s->si.State = SocketState_L2CAP_Config_wait_reqrsp; + printf("Sent ConfigureRequest, state=WAIT_CONFIG_REQ_RSP\n"); + } + } else { + s->si.SetState(SocketState_Closed); + printf("Connect failed?\n"); + } + } + break; + + case L2CAP_CONF_RSP: { + int result = LE16(data+8); + printf("Result=%d, datalen=%d, %smore conf to follow\n", result, LE16(data+2), LE16(data+6)?"":"No "); + //should parse the config + printfBytes("CONF RSP:", data, LE16(data+2)+4); + int scid = LE16(data+4); + SocketInternal* s = (SocketInternal*)SCIDToSocket(scid); + if (s == 0) break; + if (s->State != SocketState_L2CAP_Config_wait_reqrsp && s->State != SocketState_L2CAP_Config_wait_rsp) { + printf("Unexpected event ignored\n"); + break; + } + if (result == 0) { //configuration acceptable + if (s->State == SocketState_L2CAP_Config_wait_reqrsp) { + s->State = SocketState_L2CAP_Config_wait_req; + printf("State=WAIT_CONFIG_REQ\n"); + } else { + ConfigureResponse(data[1],((L2CAPSocket*)s)->dcid);//data[1]==txid + printf("Sent ConfigureResponse, state=Open\n"); + s->SetState(SocketState_Open); + } + } else { + printf("Renegotiate configuration\n"); + } + } + break; + + case L2CAP_CONF_REQ: { + int scid = LE16(data+4);//flags (data[6] LSB is continuation flag, data[10],[11] are the MTU + L2CAPSocket* s = SCIDToSocket(scid); + printfBytes("CONF REQ: ", data, LE16(data+2)+4);//data+8 contains option type 1-4 1=MTU, 2=flush timeout, 3=QoS, 4=FCM + if (s == 0) break; + if (s->si.State == SocketState_Closed || + s->si.State == SocketState_L2CAP_WaitConnectRsp || + s->si.State == SocketState_L2CAP_WaitDisconnect) { + //Send Reject command + break; + } + switch (data[8]) { + case 1: + peer_mtu = LE16(data+10); + printf("MTU = %d bytes\n", peer_mtu); + break; + default: + printf("Unsupported configuration option %d, value = %#X\n", data[8], LE16(data+10)); + break; + } + if (1 /* options acceptable */) { + printf("Sending ConfigureResponse, old state=%d ", s->si.State); + ConfigureResponse(data[1],s->dcid);//data[1]==txid, success + switch (s->si.State) { + case SocketState_L2CAP_Config_wait: + s->si.State = SocketState_L2CAP_Config_wait_send; + break; + case SocketState_L2CAP_Config_wait_req: + ((SocketInternal*)s)->SetState(SocketState_Open); + break; + case SocketState_L2CAP_Config_wait_rsp: + break; + case SocketState_L2CAP_Config_wait_reqrsp: + s->si.State = SocketState_L2CAP_Config_wait_rsp; + break; + } + printf("new state=%d\n", s->si.State); + } else { //options not acceptable + ConfigureResponse(data[1],s->dcid);//indicates success but should indicate fail + } + } + break; + case L2CAP_DISCONN_REQ: { + int dcid = LE16(data+4); + int scid = LE16(data+6); + L2CAPSocket* s = SCIDToSocket(scid); + s->si.SetState(SocketState_Closed); + DisconnectResponse(data[1], scid, dcid); + } + break; + case L2CAP_DISCONN_RSP: { + int scid = LE16(data+6); + L2CAPSocket* s = SCIDToSocket(scid); + if (s->si.State == SocketState_L2CAP_WaitDisconnect) + s->si.SetState(SocketState_Closed); + } + break; + default: printf("Unsupported L2CAP message %d\n", cc); + } + printf("\x1b[0m"); +} +#endif + +void BTDevice::ACLFwd(const u8* data, int len) { + if (l2cap_sock == 1) { + //printf("cannot handle segmented ACL control packets\n"); + Control(data, len); + return; + } + SocketInternal* s = (SocketInternal*)SCIDToSocket(l2cap_sock); + if (s) + s->Recv(data,len); + else + printf("Bad event cid %d\n",l2cap_sock); +} +//sometimes acl packets are segmented, in that case the l2cap payload length does not correspond to the acl pkt length +//and the l2cap packet length. L2CAP works in basic mode and cannot be segmented hence the l2cap pkt size corresponds to +//the acl pkt size +void BTDevice::ACLRecv(const u8* data, int acllen) { + printfBytes("L2CP",data,acllen); + u16 handle = LE16(data); + if ((handle&0x0fff) != _handle) { + printf("unexpected handle %#x, this _handle=%#x\n", handle, _handle); + return; + } + char pb = (handle>>12) & 3; + int p = 4; //start of l2cap packet + int len = LE16(data+2); //length of l2cap pkt + while (p < len) + switch (contState) { + case 0: + plen = data[p++]; + contState = 1; + break; + case 1: + plen += data[p++]<<8; + if (pb == 2 && plen == acllen-8) {//normal case, l2cap pkt is contained completely in this hci pkt + l2cap_sock = data[p] + (data[p+1]<<8); + contState = 0; + ACLFwd(data+8, plen); //forward the packet in its original buffer + return; //all data was dealt with + } else { //packet is segmented + printf("ACL packet is segmented\n"); + contState = 2; + contBuf = new unsigned char[plen];//allocate recombination buffer + contPos = 0; + } + break; + case 2: + l2cap_sock = data[p++]; + contState = 3; + break; + case 3: + l2cap_sock += data[p++]<<8; + contState = 4; + break; + case 4: //data, recombine segmented ACL (not l2cap!) frames + if (contPos < plen) {//buffer not yet full + int datalen = acllen - p; //data in this incoming pkt + int remcap = plen - contPos; //remaining capacity in the recombination buffer + if (datalen <= remcap) { + memcpy(contBuf+contPos, data+p, datalen); + contPos += datalen; + p = acllen;//end of data, stop the while loop + if (contPos == plen) {//buffer is full now + printfBytes("Recombined packet is:", contBuf, plen); + ACLFwd(contBuf, plen); //forward the recombination buffer + delete[] contBuf;//and free the buffer + contState = 0; + }//else stay in this state to wait for the rest + } else {//data contains (part of) next packet + memcpy(contBuf+contPos, data+p, plen-contPos);//this packet is complete + p += plen-contPos; + printfBytes("Recombined packet is:", contBuf, plen); + printfBytes("Next packet starts with:", data+p, acllen-p); + ACLFwd(contBuf, plen); //forward the recombination buffer + delete[] contBuf;//and free the buffer + contState = 0; //continue with the next packet + } + } else { + printf("Cannot append to buffer (size=%d, pos=%d, datalen = %d)\n", plen, contPos, len-p); + contState = 0; + return; + } + break; + }//switch (and while) +}