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:
- 7:99068afea04d
- Child:
- 8:d29e42b5ae53
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hci.cpp Sun Jun 19 12:43:19 2011 +0000 @@ -0,0 +1,581 @@ + +/* +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::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 +} + +//=================================================================== +//===================================================================