File content as of revision 0:81ed8b6e4a8b:

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