File content as of revision 0:7b556109fd46:

/*
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"

#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


 /* 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";
 }

typedef struct
{
    u16    handle;
    u16    length;            // total
    u16    l2capLength;    // length -4
    u16    cid;            // Signaling packet CID = 1
    u8  data[64];       // Largest thing to send!!! todo
} L2CAPData;

typedef struct
{
    u16    handle;
    u16    length;            // total
    u16    l2capLength;    // length -4
    u16    cid;            // Signaling packet CID = 1

    // Payload
    u8    cmd;            //
    u8    id;
    u16    cmdLength;        // total-8
    u16 params[4];      // Params
} L2CAPCmd;

void printf(const BD_ADDR* addr);

//
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)
{
    printf("Call to BTDevice Open \r\n");
    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);
    return sock->ID;
}

// virtual SocketHandler
int BTDevice::Create(SocketInternal* sock, SocketAddrHdr* addr)
{
    printf("Call to BTDevice Create \r\n");
    L2CAPSocket* s = (L2CAPSocket*)sock;
    L2CAPAddr* a = (L2CAPAddr*)addr;

    //Connect(s->scid,a->psm);
    return sock->ID;
}

int BTDevice::Accept(SocketInternal* sock, SocketAddrHdr* addr)
{
    printf("Call to BTDevice Accept \r\n");
    L2CAPSocket* s = (L2CAPSocket*)sock;
    L2CAPAddr* a = (L2CAPAddr*)addr;
    
    
    printf("ID = %d scid = %d dcid = %d \r\n",sock->ID, s->scid, s->dcid); 
     
    return sock->ID;
}




// virtual SocketHandler
int BTDevice::Send(SocketInternal* sock, const u8* data, int len)
{
    printf("Call to BTDevice Send \r\n");
    L2CAPData d;
    L2CAPSocket* s = (L2CAPSocket*)sock;

    d.handle = _handle | 0x2000;
    d.length = 4 + len;
    d.l2capLength = len;
    d.cid = s->dcid;

    if (len > 64)
        return -1;
    memcpy(d.data,data,len);
    return Send((u8*)&d,len+8);
}

// virtual SocketHandler
int BTDevice::Close(SocketInternal* sock)
{
    printf("L2CAP close %d\r\n",sock->ID);
    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("L2CP send: ",data,len);
     _transport->ACLSend(data,len);
     return 0;
}

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];
    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] = 0x02A0;  // 672
    return Send(L2CAP_CONF_REQ,_txid++,p,4);
}

int BTDevice::ConfigureResponse(u8 rxid, int dcid)
{
    u16 p[3];
    p[0] = dcid;
    p[1] = 0;
    p[2] = 0;
    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);
}

int BTDevice::AcceptResponse(u8 rxid, int scid, int dcid)
{
    printf("Connection accepted \r\n");
    u16 p[4];
    p[0] = scid;
    p[1] = dcid;
    p[2] = L2CAP_CONN_SUCCESS;
    p[3] = 0;

    return Send(L2CAP_CONN_RSP,rxid,p,4);

}

int BTDevice::RefuseResponse(u8 rxid)
{
    printf("Connection refused \r\n");
    u16 p[2];
    p[0] = L2CAP_CONN_REF_PSM;
    p[1] = 0;
    
    return Send(L2CAP_CONN_RSP,rxid,p,2);
}


//int BTDevice::InUse(int psm)
//{
//    for (int i = 0; i < MAX_PORTS; i++){
//                 printf("Listen Q %d = %d \r\n",i, _listen[i]);
//                 if ( _listen[i] == psm ) {
//                        printf("We are listening on port %d \r\n",psm); //in use
//                        return 0;     
//                 }
//    }
//    printf("We are not listening on port %d \r\n",psm);
//    return 1;
//}




void BTDevice::Control(const BD_ADDR* addr, const u8* data, int len)
{

    SocketInternal* sock;
    L2CAPSocket* s;
    int psm,scid,dcid,flags;
    
    printf("From address ");
    printf(addr);
    printf(" : ");

    int cc = data[8];
    printf(L2CAP_ComandCodeStr(cc));
    int result = LE16(data+16);
    printf(" Result %d\r\n",result);
    
    
    switch (cc)
    {
        case L2CAP_COMMAND_REJ:
           break;
        case L2CAP_CONN_REQ:
            {
            psm = LE16(data+12);
            scid =  LE16(data+14);
            printf("Connection request scid = %d psm = %d \r\n",scid,psm);   
           
            // check if we listen on the port  
            //if ( InUse(psm) ) {
            if ( Socket_InUse(SOCKET_L2CAP,psm) ) {
                RefuseResponse(data[9]);
                
                    
            } else {
                L2CAPAddr sockAddr;
                sockAddr.bdaddr = *addr;
                sock = Socket_Create(SOCKET_L2CAP, &sockAddr.hdr, psm);
                s = (L2CAPSocket*)sock; 
                s->scid = 0x40 + sock->ID-1;   // are these reserved?
                s->dcid = scid;
                AcceptResponse(data[9],s->scid,s->dcid);

                ConfigureRequest(s->dcid); // handshake
                
                sock->SetState(SocketState_Accept);
            }
           
            /**
            for (int i = 0; i < MAX_PORTS; i++){
                 if ( _listen[i] == psm ) {
                        printf("We are listening on port %d \r\n",psm); //in use
                        RefuseResponse(data[9]);
                        break;
                 }
            } 
            **/
            
            //L2CAPAddr sockAddr;
            //sockAddr.bdaddr = addr;
            //sockAddr.psm = psm;

            }
            break;
                        
        // Response to our initial connect from Remote
        case L2CAP_CONN_RSP:
            {
                if (result == 0)
                {
                    printf("Connect succeeded\r\n");
                    dcid = LE16(data+12);
                    scid = LE16(data+14);
                    L2CAPSocket* s = SCIDToSocket(scid);
                    if (s)
                    {
                        s->dcid = dcid;
                        ConfigureRequest(dcid);
                    }
                } else
                    printf("Connect failed?\r\n");
            }
            break;

        case L2CAP_CONF_RSP:
            {
                scid = LE16(data+12);
                SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
                if (s)
                    s->SetState(SocketState_Open);
            }
            break;

        case L2CAP_CONF_REQ:
            {
                u16 dcid = LE16(data+12);
                u16 flags = LE16(data+14);            
                printf("Config request dcid = %02X flags = %02X\r\n", dcid, flags);           
                scid = LE16(data+12);
                L2CAPSocket* s = SCIDToSocket(scid);
                if (s)
                    ConfigureResponse(data[9],s->dcid);
            }
            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;

    }
}

void BTDevice::ACLRecv(const BD_ADDR* addr, const u8* data, int len)
{
    //printfBytes("L2CP recv: ",data,16);
    int handle = LE16(data);
    if (handle != (0x2000 | _handle))
        return;

    int cid = LE16(data+6);
    if (cid == 1)
    {
        Control(addr,data,len);
        return;
    }

    SocketInternal* s = (SocketInternal*)SCIDToSocket(cid);
    if (s)
        s->Recv(data+8,LE16(data+2)-4);
    else
        printf("Bad event cid %d\r\n",cid);
}