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:
- 13:327622e38551
- Parent:
- 12:57f7679dd651
diff -r 57f7679dd651 -r 327622e38551 L2CAP.cpp --- a/L2CAP.cpp Sun Jun 19 19:32:51 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,614 +0,0 @@ -/* -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" -#include "sdp.h" -#include "RFCOMM.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 - -#define TXID (++_txid?_txid:1) -//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; - _txid = 1; - //cntr_cred = 1; -} - -// 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 -int BTDevice::Accept(SocketInternal* sock, int scid, int rxid) { - L2CAPSocket* s = (L2CAPSocket*)sock; - s->scid = 0x40 + sock->ID-1; // are these reserved? - s->dcid = scid; - u16 p[4]; - p[0] = s->scid; - p[1] = scid; - p[2] = 0; //success - p[3] = 0; //no further information - Send(L2CAP_CONN_RSP,rxid,p,4); - printf("send conn_rsp with dcid=%#x and scid=%#x\n", p[0],p[1]); - sock->State = SocketState_L2CAP_Config_wait; - 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); -#ifdef HOST_CONTR_FLOW - pkts_sent++; -#endif - _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, 30.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::CommandReject(u16 reason, u16 data0, u16 data1) { - u16 p[3]; - p[0] = reason; - p[1] = data0; - p[2] = data1; - int parlen = 2; - switch (reason) { - case 0: //command not understood - break; - case 1: //MTU exceeded - parlen = 4; //return actual mtu in data - break; - case 2: //invalid CID - parlen = 6; //return local, remote cid - break; - } - return Send(L2CAP_COMMAND_REJ,TXID,p,parlen); -} - -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); -} - -void server(int socket, SocketState state, const u8* data, int len, void* userData) { - // printfBytes("Server: ", data, len); - if (state==SocketState_Open && len>0) - SDP.SDPServer(socket, state, data, len, userData); -} - -void serserver(int socket, SocketState state, const u8* data, int len, void* userData) { - printfBytes("serserver: ", data, len); - SocketHandler *h = (SocketHandler*)userData; - printf("userData refers to %s, state = %d\n", h->Name(), state); - if (state==SocketState_Open) { - if (len == 0) { //assume that the socket has just been opened and bind it to a new rfcomm server entity - printf("Calling RFCOMMManager::BindSocket\n"); - rfcomm_manager.BindSocket(socket); - } else { - printf("Calling RFCOMMManager::SerServer\n"); - rfcomm_manager.SerServer(socket, state, data, len, userData); - } - } else if (state==SocketState_L2CAP_WaitDisconnect) { - printf("Calling RFCOMMManager::SerServer\n"); - rfcomm_manager.SerServer(socket, state, data, len, userData); - } -} - -//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"); - { - int scid = LE16(data+6); - int psm = LE16(data+4); - int rxid = data[1]; - u16 p[4]; - p[0] = 0; //no dcid - p[1] = scid; - p[3] = 0; //no further information - printf(" scid=%d, psm=%d\n", scid, psm); - peer_mtu = 672; //default mtu - int s = 0; - switch (psm) { - case L2CAP_PSM_SDP: - s = Socket_Accept(SOCKET_SDP, scid, rxid, server, this);//allocate an sdp socket but use it as L2CAP - break; - case L2CAP_PSM_RFCOMM: //SOCKET_RFCOM; -#if 0 - s = Socket_Accept(SOCKET_RFCOM, scid, rxid, serserver, this);//allocate an rfcomm socket - //using L2CAP i.o. RFCOM makes little difference in processing but it also changes the handler to HCI i.o. RFCOMMManager -#else -//an RFCOMM requests comes in from a known (this) device -//the channel is not yet known - s = rfcomm_manager.FindSocket(this);//this should return 0 otherwise the remote device was asking a second rfcomm on the same device - if (s==0) { - printf("No connection to this device yet, allocate L2CAP Socket and accept\n"); - //accept the connection, even though there may be no listener??? - //have no choice because w/o acceptance no rfcomm req. - s = Socket_Accept(SOCKET_L2CAP, scid, rxid, serserver, this);//allocate an l2cap socket - //get a new l2cap socket, call HCI::Accept (fill in btdevice internals), then call BTDevice::Accept (send accept message) - //serserver is called on state changes (SocketInternal::SetState) and on received packets from the peer device to the new l2cap handle - //after sending the accept message, the devices will execute the normal l2cap connection state-machine - //ending in a call to SetState(Open) which will invoke 'serserver' for the first time -//or something like: -// s = Socket_Create(SOCKET_L2CAP, serserver, this);//allocate an l2cap socket -// Accept(GetSocketInternal(s), scid, rxid);//send accept response, this would bypass HCI::Accept() - } else { - printf("Already had an L2CAP connection on socket %d\n", s); - } -#endif - break; - default: - printf("PSM %d not supported\n", psm); - } - switch (s) { - case 0: - printf("Not a valid socket\n"); - break; - case ERR_SOCKET_TYPE_NOT_FOUND: - p[2] = 2; //psm not supported - Send(L2CAP_CONN_RSP,rxid,p,4); - break; - case ERR_SOCKET_NONE_LEFT: - p[2] = 4; //no resources available - Send(L2CAP_CONN_RSP,rxid,p,4); - break; - } - } - 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+8); - 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 if (result == 1) {//pending, stay in the present state - } 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 len = LE16(data+2); - int scid = LE16(data+4);//flags (data[6] LSB is continuation flag, data[10],[11] are the MTU - int flags = LE16(data+6); - const u8* conf = data+8; - if (flags) - printf("Warning! Continuation flag in L2CAP configuration not supported\n"); - 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 - printf("Connection should be rejected\n"); - break; - } -#if 0 - if (len > 4) - switch (data[8]) { - case 1: - peer_mtu = LE16(data+10); - printf("Peer L2CAP MTU = %d bytes\n", peer_mtu); - break; - case 2: //flush timeout - case 3: //QOS - case 4: //retrans and FC option - default: - printf("Unsupported configuration option %d, value = %#X\n", data[8], LE16(data+10)); - break; - } - else - printf("Empty config req. peer_mtu = %d\n", peer_mtu); -#else - while (conf < data+len+4) { - bool hint = conf[0] & 0x80; - switch (conf[0] & 0x7F) { - case 1: - peer_mtu = LE16(conf+2); - printf("Peer L2CAP MTU = %d bytes\n", peer_mtu); - break; - case 2: //flush timeout - case 3: //QOS - case 4: //retrans and FC option - default: - printf("Unsupported configuration option %d, value = %#X\n", conf[0], LE16(conf+2)); - break; - } - conf += conf[1]+2; - } -#endif - if (1 /* options acceptable */) { - if (flags == 0) { - 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; - ConfigureRequest(s->dcid); - s->si.State = SocketState_L2CAP_Config_wait_rsp; - 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 - printf("L2CAP config continuation, delaying response...\n"); - } else { //options not acceptable - printf("Configure failure should be indicated\n"); - 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(dcid); - if (s) { - s->si.SetState(SocketState_Closed); - DisconnectResponse(data[1], scid, dcid); - } else { - printf("request to disconnect cid %d fails, no such cid\n", dcid); - CommandReject(0, dcid, scid); - } - } - 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"); -} - -void BTDevice::ACLFwd(const u8* data, int len) { - if (l2cap_sock == 1) - Control(data, len); - else { - SocketInternal* s = (SocketInternal*)SCIDToSocket(l2cap_sock);//in fact cid in the l2cap header - if (s) - s->Recv(data,len);//forward to the sockethandler for the type - 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 -int BTDevice::ACLRecv(const u8* data, int acllen) { - //printfBytes("L2CP",data,acllen); - //cntr_cred--; - u16 handle = LE16(data); - if ((handle&0x0fff) != _handle) { - printf("unexpected handle %#x, this _handle=%#x\n", handle, _handle); - return 1; - } - //below is the ACL packet recombination engine - char pb = (handle>>12) & 3; - if (pb == 2) - segments = 1; - else - segments++; - int p = 4; //start of l2cap packet - int len = LE16(data+2); //length of l2cap pkt - while (p < acllen) - switch (contState) { - case 0://allow even for fragmented length field - plen = data[p++];//payload length lsb - contState = 1; - break; - case 1: - plen += data[p++]<<8; //payload length msb - 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 segments; //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; - return segments; - }//else stay in this state to wait for the rest - } else {//data contains (part of) next packet, never seen this happen - 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 segments;//flushed - } - break; - }//switch (and while) - return 0;//the buffers are not processed yet -}