Diff: L2CAP.cpp

Revision:

1:0dde58e0cccf

Parent:

0:81ed8b6e4a8b

Child:

2:0118da9e5169

diff -r 81ed8b6e4a8b -r 0dde58e0cccf L2CAP.cpp
--- a/L2CAP.cpp	Mon Apr 04 16:41:03 2011 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,569 +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"
-
-#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)
-}