Diff: L2CAP.cpp

Revision:

0:81ed8b6e4a8b

--- /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)
+}