Diff: hci.cpp

Revision:

0:003889bc474f

Child:

1:4a639fa342bc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hci.cpp	Sat Dec 07 14:19:00 2013 +0000
@@ -0,0 +1,605 @@
+
+/*
+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[] = "ROBO TX-599";
+
+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] = 4;
+    memcpy(b+7, pin, 4);
+    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
+}
+
+//===================================================================
+//===================================================================