![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Test version of BlueUSB stack. Includes SDP and RFCOMM. As Client it allows to connect to my fischertechnik TX Controller. As Server it echo\\\\\\\'s characters to Putty. PIN=1234
Dependencies: mbed myUSBHost AvailableMemory
Dependents: mbed_TANK_Kinect myBlueUSB_ros ftusbClass
Diff: hci.cpp
- Revision:
- 13:327622e38551
- Parent:
- 12:57f7679dd651
--- a/hci.cpp Sun Jun 19 19:32:51 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,605 +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 "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 -} - -//=================================================================== -//===================================================================