![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PS3下の階
Dependencies: FatFileSystem PS3_BlueUSB QEI RoboClaw_mine_ mbed
Fork of PS3_BlueUSB by
Diff: hci.cpp
- Revision:
- 0:99a111b75cb4
- Child:
- 1:b2063ffa4927
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hci.cpp Tue Apr 26 16:09:17 2011 +0000 @@ -0,0 +1,538 @@ +/* +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. +*/ + +/* +Tue Apr 26 2011 Bart Janssens: added HCI connection accept +*/ + +#include <stdio.h> +#include <stdlib.h> +#include <stdio.h> +#include <string.h> + +#include "Utils.h" +#include "hci.h" +#include "hci_private.h" + +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 +}; + +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; + } + 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; +} +// +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; + 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::WriteScanEnable() +{ + + u8 buf[2]; + buf[0] = 0x03; + buf[1] = 0x01; + + SendCmd(HCI_OP_WRITE_SCAN_ENABLE,buf,sizeof(buf)); + return 0; +} + +int HCI::AcceptConnection(const BD_ADDR* addr) +{ + u8 buf[6+4]; + memset(buf,0,sizeof(buf)); + memcpy(buf,addr,6); + buf[7] = 0; + + SendCmd(HCI_OP_ACCEPT_CONN_REQ,buf,sizeof(buf)); + return 0; +} + +int HCI::SendCmd(int cmd, const u8* params, int len) +{ + u8 b[32]; + b[0] = cmd; + b[1] = (cmd >> 8); + b[2] = len; + if (params) + memcpy(b+3,params,len); + _transport->HCISend(b,len+3); + return 0; +} + +void HCI::OnCommandComplete(int cmd, const u8* data, int len) +{ + // printf("%04X %s",cmd,CmdStr(cmd)); + if (len < 0) + return; + //printfBytes(" complete",data,min(16,len)); + + switch (cmd) + { + // 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); + 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 + printf("Local Address: "); + printf(&_localAddr); + printf("\r\n"); + + _state |= MASK_READ_BD_ADDR; + _state |= MASK_INITED; + Callback(CALLBACK_READY,data,6); + break; + + // 0CXX + case HCI_OP_READ_LOCAL_NAME: + break; + + 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\r\n"); + break; + + default: + //printf("Unrecognized Command %04X\r\n",cmd); + break; + } +} + +void HCI::Callback(HCI_CALLBACK_EVENT c, const u8* data, int len) +{ + _callback(this,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::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\r\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) +{ + u8 b[6+1+16]; + memset(b,0,sizeof(b)); + memcpy(b,data,6); + b[6] = 4; + b[7] = '0'; + b[8] = '0'; + b[9] = '0'; + b[10] = '0'; + 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\r\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) + return; + if (info->status == 0) + { + d->_handle = info->handle; + printf("Connected on %04X\r\n",info->handle); + } else + printf("Connection failed with %d\r\n",info->status); +} + +void HCI::ConnectRequest(const request_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::ConnectRequest too many devices\r\n"); + return; // Too many devices! + } + } + bt->_info.bdaddr = info->bdaddr; + memcpy(bt->_info.dev_class,info->dev_class,3); + AcceptConnection(&info->bdaddr); +} + +void HCI::HCIRecv(const u8* data, int len) +{ + //printfBytes(EvtStr(data[0]),data,len); + //printfBytes(EvtStr(data[0]),data,min(len,16)); + //printf("HCI Event %02X\r\n",data[0]); + switch (data[0]) + { + case HCI_EV_INQUIRY_COMPLETE: + printfBytes("Inquiry Complete",data,data[1]); + _state &= ~MASK_INQUIRY; + Callback(CALLBACK_INQUIRY_DONE,0,0); + WriteScanEnable(); + 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: + { + request_info align; + memcpy(&align,data+2,sizeof(request_info)); + ConnectRequest(&align); + } + 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\r\n",CmdStr(LE16(data+4)),errs); + } + break; + + case HCI_EV_CMD_COMPLETE: + OnCommandComplete(data[3] | (data[4] << 8),data+6,data[1]-4); + break; + + case HCI_EV_PIN_CODE_REQ: + PinCodeReply(data+2); + break; + + case HCI_EV_LINK_KEY_REQ: + SendCmd(HCI_OP_LINK_KEY_NEG_REPLY,data+2,6); + break; + + default: + ; + // printfBytes("HCI Event:",data,data[1]+2); + } +} + +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("\r\n"); + return ERR_HCI_DEVICE_NOT_FOUND; + } + l2capsock->btdevice = bt; + return bt->Open(sock,addr); +} + +int HCI::Create(SocketInternal* sock, SocketAddrHdr* addr) +{ + L2CAPSocket* l2capsock = (L2CAPSocket*)sock; + L2CAPAddr* l2capaddr = (L2CAPAddr*)addr; + BTDevice* bt = Find(&l2capaddr->bdaddr); + if (!bt) + { + printf("HCI Create: Can't open l2cap on "); + printf(&l2capaddr->bdaddr); + printf("\r\n"); + return ERR_HCI_DEVICE_NOT_FOUND; + } + l2capsock->btdevice = bt; + //return 0; + return bt->Create(sock,addr); +} + + +int HCI::Accept(SocketInternal* sock, SocketAddrHdr* addr) +{ + + printf("Call to HCI Accept \r\n"); + + 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("HCI Accept: Can't open l2cap on "); + printf(&l2capaddr->bdaddr); + printf("\r\n"); + return ERR_HCI_DEVICE_NOT_FOUND; + } + //l2capsock->btdevice = bt; + return bt->Open(sock,addr); + +} + +/** +SocketInternal* HCI::Create(SocketInternal* sock) +{ + return sock; +} +**/ + + +int HCI::Send(SocketInternal* sock, const u8* data, int len) +{ + 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::ACLRecv(const u8* data, int len) +{ + int handle = LE16(data); + BD_ADDR* addr; + BTDevice* d = Find(handle & 0x0FFF); + addr = &d->_info.bdaddr; + + if (d) + d->ACLRecv(addr,data,len); +} + +//=================================================================== +//===================================================================