File content as of revision 3:55a622e3dbb5:

/*
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 "hci_private.h"
#include "USBHost.h" //for USBLoop
#include "HCITransportUSB.h" //for ACL/HCL buffer size
#include "neighbourhood.h"

extern const char FtDevClass[];
const char FtDevClass[3] = {0x00, 0x1F, 0x82 };

enum hci_callback_evt {
    NONE,
    CONNECT,
    DISCONECT,
    INQUIRYRESULT
};

#define MAX_BLUETOOTH_ADAPTERS 1

enum StateMask {
    MASK_RESET = 1,
    MASK_READ_BUFFER_SIZE = 2,
    MASK_READ_BD_ADDR = 4,
    MASK_INITED = 8,
    MASK_INQUIRY = 16,
    MASK_REMOTE_NAME = 32,
    MASK_CREATE_CONNECTION = 64
};

//static const u8 local_name[] = "MBED";
static const u8 local_name[] = "Nonin_Medical_Inc._681010";

int  HCI::Open(HCITransport* transport, HCICallback callback) {
    _transport = transport;
    _transport->Set(this);
    _callback = callback;
    _state = 0;
    for (int i = 0; i < MAX_BTDEVICES; i++) {
        _devices[i].Init();
        _devices[i]._transport = transport;
    }
#ifdef COMMAND_FLOW
    cmd_credits = 1;
#endif
    return SendCmd(HCI_OP_RESET);
}

void printf(const BD_ADDR* addr);

BTDevice* HCI::Find(const BD_ADDR* addr) {
    for (int i = 0; i < MAX_BTDEVICES; i++)
        if (_devices[i]._state != 0 && memcmp(addr,&_devices[i]._info.bdaddr,6) == 0)
            return &_devices[i];
    return 0;
}

BTDevice* HCI::Find(int handle) {
    for (int i = 0; i < MAX_BTDEVICES; i++)
        if (_devices[i]._state != 0 && handle == _devices[i]._handle)
            return &_devices[i];
    return 0;
}

//reports that some commands are still in progress
bool HCI::Busy() {
    return (_state & (MASK_INQUIRY | MASK_REMOTE_NAME | MASK_CREATE_CONNECTION)) != 0;
}

int HCI::Inquiry(int duration) {
    _state |= MASK_INQUIRY;
    u8 buf[5];
    buf[0] = 0x33;//LAP=0x9e8b33
    buf[1] = 0x8B;
    buf[2] = 0x9E;
    buf[3] = duration;
    buf[4] = 5;  // 5 results
    SendCmd(HCI_OP_INQUIRY,buf,sizeof(buf));
    return 0;
}

int HCI::SetEventFilter(u8 filterType, u8 filterConditionType, u8* condition) {
    int len = 2;
    u8 buf[8];
    buf[0] = filterType;
    buf[1] = filterConditionType;
    switch (filterConditionType) {
        case 0://all devices
            if (filterType==2) { //connection setup
                buf[2] = condition[0];
                len++;
            }
            break;
        case 1: //filter by class
        case 2: //filter by BDADDR
            memcpy(buf+2, condition, 6);
            len += 6;
            break;
        default:
            printf("Unknown filter condition type %d, filter type=%d\n", filterConditionType, filterType);
    }
    SendCmd(HCI_OP_SET_EVENT_FLT, buf, len);
    return 0;
}

int HCI::SendCmd(int cmd, const u8* params, int len) {
    u8 b[256];
    b[0] = cmd;
    b[1] = (cmd >> 8);
    b[2] = len;
    if (params)
        memcpy(b+3,params,len);
#ifdef COMMAND_FLOW
    //printf("%d cmd_credits\n", cmd_credits);
    while (cmd_credits == 0) {//blocks when command credits run out
        USBLoop();
        putc('_', stdout);
    }
#endif
    _transport->HCISend(b,len+3);
    return 0;
}

void HCI::OnCommandComplete(int cmd, const u8* data, int len) {//data is exclusive the status byte
    //printf("%04X %s",cmd,CmdStr(cmd));
    if (len < 0)
        return;
    //printfBytes(" complete",data,len/*min(16,len)*/);

    switch (cmd) {
        case 0: //NOP
            printf("Received NOP command (for cmd_credits)\n");
            break;
            //  Init phase 0
        case HCI_OP_RESET:  // Reset done, init chain to HCI_OP_READ_LOCAL_NAME
            SendCmd(HCI_OP_READ_BUFFER_SIZE);
            _state |= MASK_RESET;
            break;

            //  Init phase 1
        case HCI_OP_READ_BUFFER_SIZE:
            _acl_mtu = LE16(data);
            _sco_mtu = data[2];
            _acl_max_pkt = LE16(data+3);
            _sco_max_pkt = LE16(data+5);
            printf("acl_mtu=%d, acl_max_pkt=%d\n", _acl_mtu, _acl_max_pkt);
#ifdef HOST_CONTR_FLOW
            _transport->data_credits = _acl_max_pkt;
            _transport->_acl_mtu = _acl_mtu;
#endif
            SendCmd(HCI_OP_READ_BD_ADDR);
            _state |= MASK_READ_BUFFER_SIZE;
            break;

            //  Init phase 2
        case HCI_OP_READ_BD_ADDR:
            _localAddr = *((BD_ADDR*)data); // Local Address
            _state |= MASK_READ_BD_ADDR;
            _state |= MASK_INITED;
            {
#ifdef CONTR_HOST_FLOW
                unsigned char param[7];
                param[0] = (u8)(MAX_ACL_SIZE-8);
                param[1] = (u8)((MAX_ACL_SIZE-8)>>8);
                param[2] = 0;//MAX_HCL_SIZE-8;
                param[3] = 10;
                param[4] = 0; //1 ACL buffer
                param[5] = 0;
                param[6] = 0; //0 Synchronous buffers
                SendCmd(HCI_OP_HOST_BUFFER_SIZE, param, 7);
                const unsigned char flow = 1;//ACL on, Synchonous off
                SendCmd(HCI_OP_CONTR_TO_HOST_FLOW, &flow, 1);
#endif
                const unsigned char scan_enable = 3;
                SendCmd(HCI_OP_WRITE_SCAN_ENABLE, &scan_enable, 1);
                SendCmd(HCI_OP_WRITE_CLASS_OF_DEV, (const u8*)FtDevClass, 3);
                //SendCmd(HCI_OP_READ_LOCAL_VERSION, 0, 0);
                SendCmd(HCI_OP_WRITE_LOCAL_NAME, local_name, 248);
            }
            Callback(CALLBACK_READY,data,6);
            break;

            // 0CXX
        case HCI_OP_READ_LOCAL_NAME:
        case HCI_OP_LINK_KEY_NEG_REPLY:
        case HCI_OP_WRITE_SCAN_ENABLE:
        case HCI_OP_WRITE_LOCAL_NAME:
            break;
#ifdef CONTR_HOST_FLOW
        case HCI_OP_CONTR_TO_HOST_FLOW:
        case HCI_OP_HOST_BUFFER_SIZE:
            break;
        case HCI_OP_NUM_COMP_PKTS:
            printf("Host number of Completed Packets: Invalid HCI Command Parameter\n");
            break;
#endif
        case HCI_OP_READ_LOCAL_VERSION:
            // params
            //SendCmd(HCI_OP_READ_LOCAL_NAME);
            break;

        case HCI_OP_READ_LOCAL_COMMANDS:
            break;

        case HCI_OP_READ_LOCAL_FEATURES:
            //SendCmd(HCI_OP_READ_LOCAL_VERSION);
            break;

        case HCI_OP_READ_LOCAL_EXT_FEATURES:
            break;

        case HCI_OP_PIN_CODE_REPLY:
            printf("Got pin reply\n");
            break;
        case HCI_READ_STORED_LINK_KEY:
            neighbors->set_cap(LE16(data), LE16(data+2));
            break;

        default:
            printf("Unrecognized Command Completion %04X\n",cmd);
            break;
    }
}

void HCI::Callback(HCI_CALLBACK_EVENT c, const u8* data, int len) {
    if (_callback) _callback(this,c,data,len);
//    (this->*_callback)(c, data, len);
}

int HCI::RemoteNameRequest(const BD_ADDR* addr) {
    _state |= MASK_REMOTE_NAME;
    u8 buf[6+4];
    memset(buf,0,sizeof(buf));
    memcpy(buf,addr,6);
    //buf[7] = 1;
    return SendCmd(HCI_OP_REMOTE_NAME_REQ,buf,sizeof(buf));
}

int HCI::RemoteNameRequest(inquiry_info *ii) {
    _state |= MASK_REMOTE_NAME;
    u8 buf[6+4];
    //memset(buf,0,sizeof(buf));
    memcpy(buf,&ii->bdaddr,6);
    buf[6] = ii->pscan_rep_mode;
    buf[7] = 0;
    *(unsigned short*)(buf+8) = 0;
    return SendCmd(HCI_OP_REMOTE_NAME_REQ,buf,sizeof(buf));
}

int HCI::CreateConnection(const BD_ADDR* remoteAddr) {
    _state |= MASK_CREATE_CONNECTION;
    u8 buf[6+7];
    memset(buf,0,sizeof(buf));
    memcpy(buf,remoteAddr,6);
    buf[6] = 0x18;  // DM1,DH1
    buf[7] = 0xCC;  // DM3, DH3, DM5, DH5
    buf[8] = 1;     // Page Repetition R1
    return SendCmd(HCI_OP_CREATE_CONN,buf,sizeof(buf));
}

int HCI::Disconnect(const BD_ADDR* bdaddr) {
    BTDevice* d = Find(bdaddr);
    if (!d)
        return ERR_HCI_DEVICE_NOT_FOUND;
    int handle = d->_handle;
    printf("Disconnect from %d\n",handle);
    _state |= MASK_CREATE_CONNECTION;
    u8 buf[3];
    buf[0] = handle;
    buf[1] = (handle >> 8);
    buf[2] = 0x13;
    return SendCmd(HCI_OP_DISCONNECT,buf,sizeof(buf));
}

void HCI::DisconnectComplete(int handle) {
    BTDevice* d = Find(handle);
    if (!d)
        return;
    d->_handle = 0;
}

int HCI::DisconnectAll() {
    BTDevice* devs[8];
    int count = GetDevices(devs,8);
    for (int i = 0; i < count; i++)
        Disconnect(&devs[i]->_info.bdaddr);
    return 0;
}

int HCI::PinCodeReply(const u8* data, const u8* pin) {
    u8 b[6+1+16];
    memset(b,0,sizeof(b));
    memcpy(b,data,6);
    b[6] = 6;
    memcpy(b+7, pin, 6);
    return SendCmd(HCI_OP_PIN_CODE_REPLY,b,sizeof(b));
}

void HCI::InquiryResult(const inquiry_info* info) {
    BTDevice* bt = Find(&info->bdaddr);
    if (!bt) {  // new device
        for (int i = 0; i < MAX_BTDEVICES; i++) {
            if (_devices[i]._state == 0) {
                bt = _devices + i;
                bt->_state = 1;
                break;
            }
        }
        if (!bt) {
            printf("HCI::InquiryResult too many devices\n");
            return; // Too many devices!
        }
    }

    bt->_info = *info;
}

int HCI::GetDevices(BTDevice** devices, int maxDevices) {
    int j = 0;
    for (int i = 0; i < MAX_BTDEVICES; i++) {
        if (_devices[i]._state != 0) {
            devices[j++] = _devices + i;
            if (j == maxDevices)
                break;
        }
    }
    return j;
}

void HCI::RemoteName(const BD_ADDR* addr, const char* name) {
    BTDevice* d = Find(addr);
    if (d) {
        strncpy(d->_name,name,sizeof(d->_name)-1);
        d->_name[sizeof(d->_name)-1] = 0;
    }
}

void HCI::ConnectComplete(const connection_info* info) {
    BTDevice* d = Find(&info->bdaddr);
    if (!d) {
        printf("BT Device not known!?! ");
        printf(&info->bdaddr);
        printf("\n");
        return;
    }
    if (info->status == 0) {
        d->_handle = info->handle;
#ifdef HOST_CONTR_FLOW
        d->pkts_sent = 0;
#endif
        printf("Connected on %04X\n",info->handle);
    } else
        printf("Connection failed with %d\n",info->status);
}

void HCI::Accept_Connection(const BD_ADDR* addr, bool slave) {
    unsigned char b[7];
    memcpy(b, addr, 6);
    b[6] = slave;
    BTDevice* bt = Find(addr);
    if (!bt) {
        printf("Received connection request from undiscovered device\n");
        for (int i = 0; i < MAX_BTDEVICES; i++) {
            if (_devices[i]._state == 0) {
                bt = _devices + i;
                bt->_state = 1;
                memcpy(&(bt->_info.bdaddr), addr, 6);//rest of inquiry info unknown!!!
                break;
            }
        }
        if (!bt) {
            printf("HCI::InquiryResult too many devices\n");
            return; // Too many devices!
        }
    }
    SendCmd(HCI_OP_ACCEPT_CONN_REQ, b , 7);
}

void HCI::HCIRecv(const u8* data, int len) {//[0]=event, [1]=parlen, [2...]=pars
    printfBytes(EvtStr(data[0]),data,min(len,16));
    switch (data[0]) {
        case HCI_EV_INQUIRY_COMPLETE:
            printfBytes("Inquiry Complete",data,data[1]);
            _state &= ~MASK_INQUIRY;
            Callback(CALLBACK_INQUIRY_DONE,0,0);
            break;

        case HCI_EV_INQUIRY_RESULT: {
            const u8* end = data[1] + data + 2;
            data += 3;
            while (data < end) {
                inquiry_info align;
                memcpy(&align,data,sizeof(inquiry_info));
                InquiryResult(&align);
                Callback(CALLBACK_INQUIRY_RESULT,(u8*)&align,sizeof(inquiry_info));
                data += 14;
            }
        }
        break;

        case HCI_EV_CONN_COMPLETE:
            _state &= ~MASK_CREATE_CONNECTION;
            {
                connection_info align;
                memcpy(&align,data+2,sizeof(connection_info));
                ConnectComplete(&align);
                Callback(CALLBACK_CONNECTION_COMPLETE,(u8*)&align,sizeof(connection_info));
            }
            break;

        case HCI_EV_CONN_REQUEST:
            printf("Got Connection request \n");
            Callback(CALLBACK_CONNECTION_REQUEST, data+2, data[1]);
            Accept_Connection((BD_ADDR*)(data+2));
            break;

        case HCI_EV_DISCONN_COMPLETE:
            DisconnectComplete(LE16(data+3));
            break;

        case HCI_EV_REMOTE_NAME: {
            BD_ADDR* addr = (BD_ADDR*)(data+3);
            const char* name = (const char*)(data + 9);
            RemoteName(addr,name);
        }
        Callback(CALLBACK_REMOTE_NAME,data+3,LE16(data+1));    // addr is in here too
        _state &= ~MASK_REMOTE_NAME;
        break;

        case HCI_EV_CMD_STATUS: {
            const char* errs = HCIErrStr(data[2]);
            printf("Status %s %s %d cmd pkts\n",CmdStr(LE16(data+4)),errs, data[3]);
#ifdef COMMAND_FLOW
            cmd_credits = data[3];
#endif
        }
        Callback(CALLBACK_CMD_STATUS, data+2, 4);
        break;

        case HCI_EV_CMD_COMPLETE://[2]=cmd-pkts, [3-4]=cmd, [5...]=pars
            if (data[5]) { //[5]=usually status
                printf("HCIRecv error status: %s\n", HCIErrStr(data[5]));
            }
            OnCommandComplete(data[3] | (data[4] << 8), data+6, data[1]-4);
#ifdef COMMAND_FLOW
            cmd_credits = data[2];
#endif
            break;

        case HCI_EV_PIN_CODE_REQ:
            Callback(CALLBACK_PIN_REQ, data+2, 6);
            //PinCodeReply(data+2);
            break;

        case HCI_EV_LINK_KEY_REQ: {
            u8 param[22];
            if (neighbors->get((BD_ADDR*)(data+2), param+sizeof(BD_ADDR))){
                memcpy(param, data+2, sizeof(BD_ADDR));
                SendCmd(HCI_OP_LINK_KEY_REPLY,param,sizeof(param));
            } else
                SendCmd(HCI_OP_LINK_KEY_NEG_REPLY,data+2,6);
        }
        break;
#ifdef HOST_CONTR_FLOW
        case HCI_EV_NUM_COMP_PKTS:
            for (int k = 0; k < data[2]; k++) {//data[2] and 'c' are usually 1
                u16 h = LE16(data+3+2*k);
                u16 c = LE16(data+5+2*k);
                BTDevice *d = Find(h);
                if (!d)
                    continue;//skip no existing devices
                if (d->pkts_sent >= c) {
                    d->pkts_sent -= c;
                    _transport->data_credits += c;
                } else
                    d->pkts_sent = 0;
                //printf("%d Outstanding pkts for handle %03X (total credits=%d)\n", d->pkts_sent, h, _transport->data_credits);
            }
            break;
#endif
        case HCI_EV_LINK_KEY_NOTIFY:
            neighbors->add((BD_ADDR*)(data+2), data+8);
            break;
        case HCI_EV_RETURN_LINK_KEYS:
            for (int i = 0; i < data[2]; i++)
               neighbors->add((BD_ADDR*)(data+3+22*i), data+9+22*i, true);
            break;
        case HCI_EV_ENCRYPT_CHANGE:
            //for(int k=0; k<1000000;k++) USBLoop();
            break;
        case HCI_EV_VENDOR:
            Callback(CALLBACK_VENDOR, data+2, data[1]);
            break;
        default:
            printfBytes("HCIRecv:",data,data[1]+2);
            break;
    }
}

int HCI::Open(SocketInternal* sock, SocketAddrHdr* addr) {
    L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
    L2CAPAddr* l2capaddr = (L2CAPAddr*)addr;
    BTDevice* bt = Find(&l2capaddr->bdaddr);
    if (!bt) {
        printf("Can't open l2cap %d on ",l2capaddr->psm);
        printf(&l2capaddr->bdaddr);
        printf("\n");
        return ERR_HCI_DEVICE_NOT_FOUND;
    }
    l2capsock->btdevice = bt;
    return bt->Open(sock,addr);
}

int HCI::Accept(SocketInternal* sock, int scid, int rxid) {
    L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
    BTDevice* bt = (BTDevice*)sock->userData;
    if (!bt) {
        printf("Can't accept l2cap on socket %d\n", sock->ID);
        return ERR_HCI_DEVICE_NOT_FOUND;
    }
    l2capsock->btdevice = bt;
    return bt->Accept(sock, scid, rxid);
}

int HCI::Send(SocketInternal* sock, const u8* data, int len) {//check here for appropriate buffersize on the device
    /* these checks are HCI functions but this 'Send' does not catch all ACL traffic, so it is better done in L2CAP or transport
    //assume acl packet
    //FIXME: treatment of OFFSET is dubious, OFFSET is not defined?!
    #if OFFSET==8 //sizeof ACL/L2CAP is include in data/len
        if (len > _acl_mtu)
    #else //OFFSET==0, data is bare application frame
        if (len+8 > _acl_mtu)
    #endif
        { printf("Max outgoing packet(%d) size exceeded, segmenting necessary, pktlen = %d\n", _acl_mtu, len);
        }
        if (data_credits == 0) {
            printf("Out of ACL data credits\n");
            return 0;
        }
        data_credits--;
    */
    L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
    return l2capsock->btdevice->Send(sock,data,len);    // Pointless double dispatch
}

int HCI::Close(SocketInternal* sock) {
    L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
    return l2capsock->btdevice->Close(sock);    // Pointless double dispatch
}

void HCI::Compl_pkts(int handle, u8 p) {
    u8 b[8] = {(u8)HCI_OP_NUM_COMP_PKTS, HCI_OP_NUM_COMP_PKTS >> 8, 5, 1, 0, 0, 1, 0};
    b[4] = handle;
    b[5] = (handle&0x0f00)>>8;
    b[6] = p;//only one packet
    _transport->HCISend(b, 8);//directly call the transport layer to prevent the command flow control from interfering
}

void HCI::ACLRecv(const u8* data, int len) {
    int handle = LE16(data);
    BTDevice* d = Find(handle & 0x0FFF);
    int bufs = 1;
    if (!d) {
        printfBytes("unk. dest. HCI:ACLRecv ", data, len);
    } else
        bufs = d->ACLRecv(data,len);
    //controller to host flow control
#ifdef CONTR_HOST_FLOW
//the ACLRecv function returned so we assume that the buffer is free, and tell this to the controller
    if (bufs) {
        Compl_pkts(handle, bufs);//this packet is completed
        printf("%d ACL buffers completed\n", bufs);
    }
#endif
}

//===================================================================
//===================================================================