Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed myUSBHost AvailableMemory
Dependents: mbed_TANK_Kinect myBlueUSB_ros ftusbClass
Diff: L2CAP.cpp
- Revision:
- 6:567607a9a79f
- Parent:
- 5:378c208637e3
- Child:
- 7:99068afea04d
--- a/L2CAP.cpp Sat Jun 11 19:43:00 2011 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,585 +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);
- 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);
- 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 (len > 4)
- switch (data[8]) {
- case 1:
- peer_mtu = LE16(data+10);
- printf("Peer L2CAP 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;
- 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 { //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
-}