春ロボ　ロケット団 / Mbed 2 deprecated spring_ps3_2

test

Dependencies:   mbed KondoServoLibrary Encoder

hci.cpp@21:0f12ee2322e4, 2020-03-12 (annotated)

Committer:
koheim
Date:
Thu Mar 12 07:32:03 2020 +0000
Revision:
21:0f12ee2322e4
Parent:
8:6d0de6154e8f 
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hirokimineshita 0:736c76a75def 1 /*
hirokimineshita 0:736c76a75def 2 Copyright (c) 2010 Peter Barrett
hirokimineshita 0:736c76a75def 3
hirokimineshita 0:736c76a75def 4 Permission is hereby granted, free of charge, to any person obtaining a copy
hirokimineshita 0:736c76a75def 5 of this software and associated documentation files (the "Software"), to deal
hirokimineshita 0:736c76a75def 6 in the Software without restriction, including without limitation the rights
hirokimineshita 0:736c76a75def 7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
hirokimineshita 0:736c76a75def 8 copies of the Software, and to permit persons to whom the Software is
hirokimineshita 0:736c76a75def 9 furnished to do so, subject to the following conditions:
hirokimineshita 0:736c76a75def 10
hirokimineshita 0:736c76a75def 11 The above copyright notice and this permission notice shall be included in
hirokimineshita 0:736c76a75def 12 all copies or substantial portions of the Software.
hirokimineshita 0:736c76a75def 13
hirokimineshita 0:736c76a75def 14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
hirokimineshita 0:736c76a75def 15 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
hirokimineshita 0:736c76a75def 16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
hirokimineshita 0:736c76a75def 17 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
hirokimineshita 0:736c76a75def 18 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
hirokimineshita 0:736c76a75def 19 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
hirokimineshita 0:736c76a75def 20 THE SOFTWARE.
hirokimineshita 0:736c76a75def 21 */
hirokimineshita 0:736c76a75def 22
hirokimineshita 0:736c76a75def 23 /*
hirokimineshita 0:736c76a75def 24 Tue Apr 26 2011 Bart Janssens: added HCI connection accept
hirokimineshita 0:736c76a75def 25 */
hirokimineshita 0:736c76a75def 26
hirokimineshita 0:736c76a75def 27 #include <stdio.h>
hirokimineshita 0:736c76a75def 28 #include <stdlib.h>
hirokimineshita 0:736c76a75def 29 #include <stdio.h>
hirokimineshita 0:736c76a75def 30 #include <string.h>
hirokimineshita 8:6d0de6154e8f 31 #include <mbed.h>
hirokimineshita 0:736c76a75def 32
hirokimineshita 0:736c76a75def 33 #include "Utils.h"
hirokimineshita 0:736c76a75def 34 #include "hci.h"
hirokimineshita 0:736c76a75def 35 #include "hci_private.h"
hirokimineshita 0:736c76a75def 36
hirokimineshita 0:736c76a75def 37 enum hci_callback_evt
hirokimineshita 0:736c76a75def 38 {
hirokimineshita 0:736c76a75def 39 NONE,
hirokimineshita 0:736c76a75def 40 CONNECT,
hirokimineshita 0:736c76a75def 41 DISCONECT,
hirokimineshita 0:736c76a75def 42 INQUIRYRESULT
hirokimineshita 0:736c76a75def 43 };
hirokimineshita 0:736c76a75def 44
hirokimineshita 0:736c76a75def 45 #define MAX_BLUETOOTH_ADAPTERS 1
hirokimineshita 0:736c76a75def 46
hirokimineshita 0:736c76a75def 47 enum StateMask {
hirokimineshita 0:736c76a75def 48 MASK_RESET = 1,
hirokimineshita 0:736c76a75def 49 MASK_READ_BUFFER_SIZE = 2,
hirokimineshita 0:736c76a75def 50 MASK_READ_BD_ADDR = 4,
hirokimineshita 0:736c76a75def 51 MASK_INITED = 8,
hirokimineshita 0:736c76a75def 52 MASK_INQUIRY = 16,
hirokimineshita 0:736c76a75def 53 MASK_REMOTE_NAME = 32,
hirokimineshita 0:736c76a75def 54 MASK_CREATE_CONNECTION = 64
hirokimineshita 0:736c76a75def 55 };
hirokimineshita 0:736c76a75def 56
hirokimineshita 0:736c76a75def 57 int HCI::Open(HCITransport* transport, HCICallback callback)
hirokimineshita 0:736c76a75def 58 {
hirokimineshita 0:736c76a75def 59 _transport = transport;
hirokimineshita 0:736c76a75def 60 _transport->Set(this);
hirokimineshita 0:736c76a75def 61 _callback = callback;
hirokimineshita 0:736c76a75def 62 _state = 0;
hirokimineshita 0:736c76a75def 63 for (int i = 0; i < MAX_BTDEVICES; i++)
hirokimineshita 0:736c76a75def 64 {
hirokimineshita 0:736c76a75def 65 _devices[i].Init();
hirokimineshita 0:736c76a75def 66 _devices[i]._transport = transport;
hirokimineshita 0:736c76a75def 67 }
hirokimineshita 0:736c76a75def 68 return SendCmd(HCI_OP_RESET);
hirokimineshita 0:736c76a75def 69 }
hirokimineshita 0:736c76a75def 70
hirokimineshita 0:736c76a75def 71 void printf(const BD_ADDR* addr);
hirokimineshita 8:6d0de6154e8f 72 void fprintf(FILE *fp,const BD_ADDR* addr);
hirokimineshita 0:736c76a75def 73
hirokimineshita 0:736c76a75def 74 BTDevice* HCI::Find(const BD_ADDR* addr)
hirokimineshita 0:736c76a75def 75 {
hirokimineshita 0:736c76a75def 76 for (int i = 0; i < MAX_BTDEVICES; i++)
hirokimineshita 0:736c76a75def 77 if (_devices[i]._state != 0 && memcmp(addr,&_devices[i]._info.bdaddr,6) == 0)
hirokimineshita 0:736c76a75def 78 return &_devices[i];
hirokimineshita 0:736c76a75def 79 return 0;
hirokimineshita 0:736c76a75def 80 }
hirokimineshita 0:736c76a75def 81
hirokimineshita 0:736c76a75def 82 BTDevice* HCI::Find(int handle)
hirokimineshita 0:736c76a75def 83 {
hirokimineshita 0:736c76a75def 84 for (int i = 0; i < MAX_BTDEVICES; i++)
hirokimineshita 0:736c76a75def 85 if (_devices[i]._state != 0 && handle == _devices[i]._handle)
hirokimineshita 0:736c76a75def 86 return &_devices[i];
hirokimineshita 0:736c76a75def 87 return 0;
hirokimineshita 0:736c76a75def 88 }
hirokimineshita 0:736c76a75def 89 //
hirokimineshita 0:736c76a75def 90 bool HCI::Busy()
hirokimineshita 0:736c76a75def 91 {
hirokimineshita 0:736c76a75def 92 return (_state & (MASK_INQUIRY | MASK_REMOTE_NAME | MASK_CREATE_CONNECTION)) != 0;
hirokimineshita 0:736c76a75def 93 }
hirokimineshita 0:736c76a75def 94
hirokimineshita 0:736c76a75def 95 int HCI::Inquiry(int duration)
hirokimineshita 0:736c76a75def 96 {
hirokimineshita 0:736c76a75def 97 _state |= MASK_INQUIRY;
hirokimineshita 0:736c76a75def 98 u8 buf[5];
hirokimineshita 0:736c76a75def 99 buf[0] = 0x33;
hirokimineshita 0:736c76a75def 100 buf[1] = 0x8B;
hirokimineshita 0:736c76a75def 101 buf[2] = 0x9E;
hirokimineshita 0:736c76a75def 102 buf[3] = duration;
hirokimineshita 0:736c76a75def 103 buf[4] = 5; // 5 results
hirokimineshita 0:736c76a75def 104 SendCmd(HCI_OP_INQUIRY,buf,sizeof(buf));
hirokimineshita 0:736c76a75def 105 return 0;
hirokimineshita 0:736c76a75def 106 }
hirokimineshita 0:736c76a75def 107
hirokimineshita 0:736c76a75def 108 int HCI::WriteScanEnable()
hirokimineshita 0:736c76a75def 109 {
hirokimineshita 0:736c76a75def 110
hirokimineshita 0:736c76a75def 111 //u8 buf[2];
hirokimineshita 0:736c76a75def 112 //buf[0] = 0x03;
hirokimineshita 0:736c76a75def 113 //buf[1] = 0x01;
hirokimineshita 0:736c76a75def 114 u8 buf[1];
hirokimineshita 0:736c76a75def 115 buf[0] = 0x03;
hirokimineshita 0:736c76a75def 116
hirokimineshita 0:736c76a75def 117 SendCmd(HCI_OP_WRITE_SCAN_ENABLE,buf,sizeof(buf));
hirokimineshita 0:736c76a75def 118 return 0;
hirokimineshita 0:736c76a75def 119 }
hirokimineshita 0:736c76a75def 120
hirokimineshita 0:736c76a75def 121 int HCI::AcceptConnection(const BD_ADDR* addr)
hirokimineshita 0:736c76a75def 122 {
hirokimineshita 0:736c76a75def 123 //u8 buf[6+4];
hirokimineshita 0:736c76a75def 124 //memset(buf,0,sizeof(buf));
hirokimineshita 0:736c76a75def 125 //memcpy(buf,addr,6);
hirokimineshita 0:736c76a75def 126 //buf[7] = 0;
hirokimineshita 0:736c76a75def 127 u8 buf[6+1];
hirokimineshita 0:736c76a75def 128 memset(buf,0,sizeof(buf));
hirokimineshita 0:736c76a75def 129 memcpy(buf,addr,6);
hirokimineshita 0:736c76a75def 130 buf[6] = 0;
hirokimineshita 0:736c76a75def 131
hirokimineshita 0:736c76a75def 132 SendCmd(HCI_OP_ACCEPT_CONN_REQ,buf,sizeof(buf));
hirokimineshita 0:736c76a75def 133 return 0;
hirokimineshita 0:736c76a75def 134 }
hirokimineshita 0:736c76a75def 135
hirokimineshita 0:736c76a75def 136 int HCI::SendCmd(int cmd, const u8* params, int len)
hirokimineshita 0:736c76a75def 137 {
hirokimineshita 0:736c76a75def 138 u8 b[32];
hirokimineshita 0:736c76a75def 139 b[0] = cmd;
hirokimineshita 0:736c76a75def 140 b[1] = (cmd >> 8);
hirokimineshita 0:736c76a75def 141 b[2] = len;
hirokimineshita 0:736c76a75def 142 if (params)
hirokimineshita 0:736c76a75def 143 memcpy(b+3,params,len);
hirokimineshita 0:736c76a75def 144 _transport->HCISend(b,len+3);
hirokimineshita 0:736c76a75def 145 return 0;
hirokimineshita 0:736c76a75def 146 }
hirokimineshita 0:736c76a75def 147
hirokimineshita 0:736c76a75def 148 void HCI::OnCommandComplete(int cmd, const u8* data, int len)
hirokimineshita 0:736c76a75def 149 {
hirokimineshita 0:736c76a75def 150 // printf("%04X %s",cmd,CmdStr(cmd));
hirokimineshita 0:736c76a75def 151 if (len < 0)
hirokimineshita 0:736c76a75def 152 return;
hirokimineshita 0:736c76a75def 153 //printfBytes(" complete",data,min(16,len));
hirokimineshita 0:736c76a75def 154
hirokimineshita 0:736c76a75def 155 switch (cmd)
hirokimineshita 0:736c76a75def 156 {
hirokimineshita 0:736c76a75def 157 // Init phase 0
hirokimineshita 0:736c76a75def 158 case HCI_OP_RESET: // Reset done, init chain to HCI_OP_READ_LOCAL_NAME
hirokimineshita 0:736c76a75def 159 SendCmd(HCI_OP_READ_BUFFER_SIZE);
hirokimineshita 0:736c76a75def 160 _state |= MASK_RESET;
hirokimineshita 0:736c76a75def 161 break;
hirokimineshita 0:736c76a75def 162
hirokimineshita 0:736c76a75def 163 // Init phase 1
hirokimineshita 0:736c76a75def 164 case HCI_OP_READ_BUFFER_SIZE:
hirokimineshita 0:736c76a75def 165 _acl_mtu = LE16(data);
hirokimineshita 0:736c76a75def 166 _sco_mtu = data[2];
hirokimineshita 0:736c76a75def 167 _acl_max_pkt = LE16(data+3);
hirokimineshita 0:736c76a75def 168 _sco_max_pkt = LE16(data+5);
hirokimineshita 0:736c76a75def 169 SendCmd(HCI_OP_READ_BD_ADDR);
hirokimineshita 0:736c76a75def 170 _state |= MASK_READ_BUFFER_SIZE;
hirokimineshita 0:736c76a75def 171 break;
hirokimineshita 0:736c76a75def 172
hirokimineshita 0:736c76a75def 173 // Init phase 2
hirokimineshita 0:736c76a75def 174 case HCI_OP_READ_BD_ADDR:
hirokimineshita 8:6d0de6154e8f 175 {
hirokimineshita 8:6d0de6154e8f 176 LocalFileSystem local("local");
hirokimineshita 8:6d0de6154e8f 177 FILE *fp;
hirokimineshita 8:6d0de6154e8f 178 fp = fopen("/local/data.p3b", "w");
hirokimineshita 0:736c76a75def 179 _localAddr = *((BD_ADDR*)data); // Local Address
hirokimineshita 0:736c76a75def 180 printf("Local Address: ");
hirokimineshita 0:736c76a75def 181 printf(&_localAddr);
hirokimineshita 8:6d0de6154e8f 182 fprintf(fp,&_localAddr);
hirokimineshita 8:6d0de6154e8f 183 fclose(fp);
hirokimineshita 0:736c76a75def 184 printf("\r\n");
hirokimineshita 0:736c76a75def 185 _state |= MASK_READ_BD_ADDR;
hirokimineshita 0:736c76a75def 186 _state |= MASK_INITED;
hirokimineshita 8:6d0de6154e8f 187 Callback(CALLBACK_READY,data,6);};
hirokimineshita 0:736c76a75def 188 break;
hirokimineshita 0:736c76a75def 189
hirokimineshita 0:736c76a75def 190 // 0CXX
hirokimineshita 0:736c76a75def 191 case HCI_OP_READ_LOCAL_NAME:
hirokimineshita 0:736c76a75def 192 break;
hirokimineshita 0:736c76a75def 193
hirokimineshita 0:736c76a75def 194 case HCI_OP_READ_LOCAL_VERSION:
hirokimineshita 0:736c76a75def 195 // params
hirokimineshita 0:736c76a75def 196 //SendCmd(HCI_OP_READ_LOCAL_NAME);
hirokimineshita 0:736c76a75def 197 break;
hirokimineshita 0:736c76a75def 198
hirokimineshita 0:736c76a75def 199 case HCI_OP_READ_LOCAL_COMMANDS:
hirokimineshita 0:736c76a75def 200 break;
hirokimineshita 0:736c76a75def 201
hirokimineshita 0:736c76a75def 202 case HCI_OP_READ_LOCAL_FEATURES:
hirokimineshita 0:736c76a75def 203 //SendCmd(HCI_OP_READ_LOCAL_VERSION);
hirokimineshita 0:736c76a75def 204 break;
hirokimineshita 0:736c76a75def 205
hirokimineshita 0:736c76a75def 206 case HCI_OP_READ_LOCAL_EXT_FEATURES:
hirokimineshita 0:736c76a75def 207 break;
hirokimineshita 0:736c76a75def 208
hirokimineshita 0:736c76a75def 209 case HCI_OP_PIN_CODE_REPLY:
hirokimineshita 0:736c76a75def 210 printf("Got pin reply\r\n");
hirokimineshita 0:736c76a75def 211 break;
hirokimineshita 0:736c76a75def 212
hirokimineshita 0:736c76a75def 213 default:
hirokimineshita 0:736c76a75def 214 //printf("Unrecognized Command %04X\r\n",cmd);
hirokimineshita 0:736c76a75def 215 break;
hirokimineshita 0:736c76a75def 216 }
hirokimineshita 0:736c76a75def 217 }
hirokimineshita 0:736c76a75def 218
hirokimineshita 0:736c76a75def 219 void HCI::Callback(HCI_CALLBACK_EVENT c, const u8* data, int len)
hirokimineshita 0:736c76a75def 220 {
hirokimineshita 0:736c76a75def 221 _callback(this,c,data,len);
hirokimineshita 0:736c76a75def 222 }
hirokimineshita 0:736c76a75def 223
hirokimineshita 0:736c76a75def 224 int HCI::RemoteNameRequest(const BD_ADDR* addr)
hirokimineshita 0:736c76a75def 225 {
hirokimineshita 0:736c76a75def 226 _state |= MASK_REMOTE_NAME;
hirokimineshita 0:736c76a75def 227 u8 buf[6+4];
hirokimineshita 0:736c76a75def 228 memset(buf,0,sizeof(buf));
hirokimineshita 0:736c76a75def 229 memcpy(buf,addr,6);
hirokimineshita 0:736c76a75def 230 buf[7] = 1;
hirokimineshita 0:736c76a75def 231 return SendCmd(HCI_OP_REMOTE_NAME_REQ,buf,sizeof(buf));
hirokimineshita 0:736c76a75def 232 }
hirokimineshita 0:736c76a75def 233
hirokimineshita 0:736c76a75def 234 int HCI::CreateConnection(const BD_ADDR* remoteAddr)
hirokimineshita 0:736c76a75def 235 {
hirokimineshita 0:736c76a75def 236 _state |= MASK_CREATE_CONNECTION;
hirokimineshita 0:736c76a75def 237 u8 buf[6+7];
hirokimineshita 0:736c76a75def 238 memset(buf,0,sizeof(buf));
hirokimineshita 0:736c76a75def 239 memcpy(buf,remoteAddr,6);
hirokimineshita 0:736c76a75def 240 buf[6] = 0x18; // DM1,DH1
hirokimineshita 0:736c76a75def 241 buf[7] = 0xCC; // DM3, DH3, DM5, DH5
hirokimineshita 0:736c76a75def 242 buf[8] = 1; // Page Repetition R1
hirokimineshita 0:736c76a75def 243 return SendCmd(HCI_OP_CREATE_CONN,buf,sizeof(buf));
hirokimineshita 0:736c76a75def 244 }
hirokimineshita 0:736c76a75def 245
hirokimineshita 0:736c76a75def 246 int HCI::Disconnect(const BD_ADDR* bdaddr)
hirokimineshita 0:736c76a75def 247 {
hirokimineshita 0:736c76a75def 248 BTDevice* d = Find(bdaddr);
hirokimineshita 0:736c76a75def 249 if (!d)
hirokimineshita 0:736c76a75def 250 return ERR_HCI_DEVICE_NOT_FOUND;
hirokimineshita 0:736c76a75def 251 int handle = d->_handle;
hirokimineshita 0:736c76a75def 252 printf("Disconnect from %d\r\n",handle);
hirokimineshita 0:736c76a75def 253 _state |= MASK_CREATE_CONNECTION;
hirokimineshita 0:736c76a75def 254 u8 buf[3];
hirokimineshita 0:736c76a75def 255 buf[0] = handle;
hirokimineshita 0:736c76a75def 256 buf[1] = (handle >> 8);
hirokimineshita 0:736c76a75def 257 buf[2] = 0x13;
hirokimineshita 0:736c76a75def 258 return SendCmd(HCI_OP_DISCONNECT,buf,sizeof(buf));
hirokimineshita 0:736c76a75def 259 }
hirokimineshita 0:736c76a75def 260
hirokimineshita 0:736c76a75def 261 void HCI::DisconnectComplete(int handle)
hirokimineshita 0:736c76a75def 262 {
hirokimineshita 0:736c76a75def 263 BTDevice* d = Find(handle);
hirokimineshita 0:736c76a75def 264 if (!d)
hirokimineshita 0:736c76a75def 265 return;
hirokimineshita 0:736c76a75def 266 d->_handle = 0;
hirokimineshita 0:736c76a75def 267 }
hirokimineshita 0:736c76a75def 268
hirokimineshita 0:736c76a75def 269 int HCI::DisconnectAll()
hirokimineshita 0:736c76a75def 270 {
hirokimineshita 0:736c76a75def 271 BTDevice* devs[8];
hirokimineshita 0:736c76a75def 272 int count = GetDevices(devs,8);
hirokimineshita 0:736c76a75def 273 for (int i = 0; i < count; i++)
hirokimineshita 0:736c76a75def 274 Disconnect(&devs[i]->_info.bdaddr);
hirokimineshita 0:736c76a75def 275 return 0;
hirokimineshita 0:736c76a75def 276 }
hirokimineshita 0:736c76a75def 277
hirokimineshita 0:736c76a75def 278 int HCI::PinCodeReply(const u8* data)
hirokimineshita 0:736c76a75def 279 {
hirokimineshita 0:736c76a75def 280 u8 b[6+1+16];
hirokimineshita 0:736c76a75def 281 memset(b,0,sizeof(b));
hirokimineshita 0:736c76a75def 282 memcpy(b,data,6);
hirokimineshita 0:736c76a75def 283 b[6] = 4;
hirokimineshita 0:736c76a75def 284 b[7] = '0';
hirokimineshita 0:736c76a75def 285 b[8] = '0';
hirokimineshita 0:736c76a75def 286 b[9] = '0';
hirokimineshita 0:736c76a75def 287 b[10] = '0';
hirokimineshita 0:736c76a75def 288 return SendCmd(HCI_OP_PIN_CODE_REPLY,b,sizeof(b));
hirokimineshita 0:736c76a75def 289 }
hirokimineshita 0:736c76a75def 290
hirokimineshita 0:736c76a75def 291 void HCI::InquiryResult(const inquiry_info* info)
hirokimineshita 0:736c76a75def 292 {
hirokimineshita 0:736c76a75def 293 BTDevice* bt = Find(&info->bdaddr);
hirokimineshita 0:736c76a75def 294 if (!bt) // new device
hirokimineshita 0:736c76a75def 295 {
hirokimineshita 0:736c76a75def 296 for (int i = 0; i < MAX_BTDEVICES; i++)
hirokimineshita 0:736c76a75def 297 {
hirokimineshita 0:736c76a75def 298 if (_devices[i]._state == 0)
hirokimineshita 0:736c76a75def 299 {
hirokimineshita 0:736c76a75def 300 bt = _devices + i;
hirokimineshita 0:736c76a75def 301 bt->_state = 1;
hirokimineshita 0:736c76a75def 302 break;
hirokimineshita 0:736c76a75def 303 }
hirokimineshita 0:736c76a75def 304 }
hirokimineshita 0:736c76a75def 305 if (!bt)
hirokimineshita 0:736c76a75def 306 {
hirokimineshita 0:736c76a75def 307 printf("HCI::InquiryResult too many devices\r\n");
hirokimineshita 0:736c76a75def 308 return; // Too many devices!
hirokimineshita 0:736c76a75def 309 }
hirokimineshita 0:736c76a75def 310 }
hirokimineshita 0:736c76a75def 311
hirokimineshita 0:736c76a75def 312 bt->_info = *info;
hirokimineshita 0:736c76a75def 313 }
hirokimineshita 0:736c76a75def 314
hirokimineshita 0:736c76a75def 315 int HCI::GetDevices(BTDevice** devices, int maxDevices)
hirokimineshita 0:736c76a75def 316 {
hirokimineshita 0:736c76a75def 317 int j = 0;
hirokimineshita 0:736c76a75def 318 for (int i = 0; i < MAX_BTDEVICES; i++)
hirokimineshita 0:736c76a75def 319 {
hirokimineshita 0:736c76a75def 320 if (_devices[i]._state != 0)
hirokimineshita 0:736c76a75def 321 {
hirokimineshita 0:736c76a75def 322 devices[j++] = _devices + i;
hirokimineshita 0:736c76a75def 323 if (j == maxDevices)
hirokimineshita 0:736c76a75def 324 break;
hirokimineshita 0:736c76a75def 325 }
hirokimineshita 0:736c76a75def 326 }
hirokimineshita 0:736c76a75def 327 return j;
hirokimineshita 0:736c76a75def 328 }
hirokimineshita 0:736c76a75def 329
hirokimineshita 0:736c76a75def 330 void HCI::RemoteName(const BD_ADDR* addr, const char* name)
hirokimineshita 0:736c76a75def 331 {
hirokimineshita 0:736c76a75def 332 BTDevice* d = Find(addr);
hirokimineshita 0:736c76a75def 333 if (d)
hirokimineshita 0:736c76a75def 334 {
hirokimineshita 0:736c76a75def 335 strncpy(d->_name,name,sizeof(d->_name)-1);
hirokimineshita 0:736c76a75def 336 d->_name[sizeof(d->_name)-1] = 0;
hirokimineshita 0:736c76a75def 337 }
hirokimineshita 0:736c76a75def 338 }
hirokimineshita 0:736c76a75def 339
hirokimineshita 0:736c76a75def 340 void HCI::ConnectComplete(const connection_info* info)
hirokimineshita 0:736c76a75def 341 {
hirokimineshita 0:736c76a75def 342 BTDevice* d = Find(&info->bdaddr);
hirokimineshita 0:736c76a75def 343 if (!d)
hirokimineshita 0:736c76a75def 344 return;
hirokimineshita 0:736c76a75def 345 if (info->status == 0)
hirokimineshita 0:736c76a75def 346 {
hirokimineshita 0:736c76a75def 347 d->_handle = info->handle;
hirokimineshita 0:736c76a75def 348 printf("Connected on %04X\r\n",info->handle);
hirokimineshita 0:736c76a75def 349 } else
hirokimineshita 0:736c76a75def 350 printf("Connection failed with %d\r\n",info->status);
hirokimineshita 0:736c76a75def 351 }
hirokimineshita 0:736c76a75def 352
hirokimineshita 0:736c76a75def 353 void HCI::ConnectRequest(const request_info* info)
hirokimineshita 0:736c76a75def 354 {
hirokimineshita 0:736c76a75def 355 BTDevice* bt = Find(&info->bdaddr);
hirokimineshita 0:736c76a75def 356 if (!bt) // new device
hirokimineshita 0:736c76a75def 357 {
hirokimineshita 0:736c76a75def 358 for (int i = 0; i < MAX_BTDEVICES; i++)
hirokimineshita 0:736c76a75def 359 {
hirokimineshita 0:736c76a75def 360 if (_devices[i]._state == 0)
hirokimineshita 0:736c76a75def 361 {
hirokimineshita 0:736c76a75def 362 bt = _devices + i;
hirokimineshita 0:736c76a75def 363 bt->_state = 1;
hirokimineshita 0:736c76a75def 364 break;
hirokimineshita 0:736c76a75def 365 }
hirokimineshita 0:736c76a75def 366 }
hirokimineshita 0:736c76a75def 367 if (!bt)
hirokimineshita 0:736c76a75def 368 {
hirokimineshita 0:736c76a75def 369 printf("HCI::ConnectRequest too many devices\r\n");
hirokimineshita 0:736c76a75def 370 return; // Too many devices!
hirokimineshita 0:736c76a75def 371 }
hirokimineshita 0:736c76a75def 372 }
hirokimineshita 0:736c76a75def 373 bt->_info.bdaddr = info->bdaddr;
hirokimineshita 0:736c76a75def 374 memcpy(bt->_info.dev_class,info->dev_class,3);
hirokimineshita 0:736c76a75def 375 AcceptConnection(&info->bdaddr);
hirokimineshita 0:736c76a75def 376 }
hirokimineshita 0:736c76a75def 377
hirokimineshita 0:736c76a75def 378 void HCI::HCIRecv(const u8* data, int len)
hirokimineshita 0:736c76a75def 379 {
hirokimineshita 0:736c76a75def 380 //printfBytes(EvtStr(data[0]),data,len);
hirokimineshita 0:736c76a75def 381 //printfBytes(EvtStr(data[0]),data,min(len,16));
hirokimineshita 0:736c76a75def 382 //printf("HCI Event %02X\r\n",data[0]);
hirokimineshita 0:736c76a75def 383 switch (data[0])
hirokimineshita 0:736c76a75def 384 {
hirokimineshita 0:736c76a75def 385 case HCI_EV_INQUIRY_COMPLETE:
hirokimineshita 0:736c76a75def 386 printfBytes("Inquiry Complete",data,data[1]);
hirokimineshita 0:736c76a75def 387 _state &= ~MASK_INQUIRY;
hirokimineshita 0:736c76a75def 388 Callback(CALLBACK_INQUIRY_DONE,0,0);
hirokimineshita 0:736c76a75def 389 WriteScanEnable();
hirokimineshita 0:736c76a75def 390 break;
hirokimineshita 0:736c76a75def 391
hirokimineshita 0:736c76a75def 392 case HCI_EV_INQUIRY_RESULT:
hirokimineshita 0:736c76a75def 393 {
hirokimineshita 0:736c76a75def 394 const u8* end = data[1] + data + 2;
hirokimineshita 0:736c76a75def 395 data += 3;
hirokimineshita 0:736c76a75def 396 while (data < end)
hirokimineshita 0:736c76a75def 397 {
hirokimineshita 0:736c76a75def 398 inquiry_info align;
hirokimineshita 0:736c76a75def 399 memcpy(&align,data,sizeof(inquiry_info));
hirokimineshita 0:736c76a75def 400 InquiryResult(&align);
hirokimineshita 0:736c76a75def 401 Callback(CALLBACK_INQUIRY_RESULT,(u8*)&align,sizeof(inquiry_info));
hirokimineshita 0:736c76a75def 402 data += 14;
hirokimineshita 0:736c76a75def 403 }
hirokimineshita 0:736c76a75def 404 }
hirokimineshita 0:736c76a75def 405 break;
hirokimineshita 0:736c76a75def 406
hirokimineshita 0:736c76a75def 407 case HCI_EV_CONN_COMPLETE:
hirokimineshita 0:736c76a75def 408 _state &= ~MASK_CREATE_CONNECTION;
hirokimineshita 0:736c76a75def 409 {
hirokimineshita 0:736c76a75def 410 connection_info align;
hirokimineshita 0:736c76a75def 411 memcpy(&align,data+2,sizeof(connection_info));
hirokimineshita 0:736c76a75def 412 ConnectComplete(&align);
hirokimineshita 0:736c76a75def 413 Callback(CALLBACK_CONNECTION_COMPLETE,(u8*)&align,sizeof(connection_info));
hirokimineshita 0:736c76a75def 414 }
hirokimineshita 0:736c76a75def 415 break;
hirokimineshita 0:736c76a75def 416
hirokimineshita 0:736c76a75def 417 case HCI_EV_CONN_REQUEST:
hirokimineshita 0:736c76a75def 418 {
hirokimineshita 0:736c76a75def 419 request_info align;
hirokimineshita 0:736c76a75def 420 memcpy(&align,data+2,sizeof(request_info));
hirokimineshita 0:736c76a75def 421 ConnectRequest(&align);
hirokimineshita 0:736c76a75def 422 }
hirokimineshita 0:736c76a75def 423 break;
hirokimineshita 0:736c76a75def 424
hirokimineshita 0:736c76a75def 425 case HCI_EV_DISCONN_COMPLETE:
hirokimineshita 0:736c76a75def 426 DisconnectComplete(LE16(data+3));
hirokimineshita 0:736c76a75def 427 break;
hirokimineshita 0:736c76a75def 428
hirokimineshita 0:736c76a75def 429 case HCI_EV_REMOTE_NAME:
hirokimineshita 0:736c76a75def 430 {
hirokimineshita 0:736c76a75def 431 BD_ADDR* addr = (BD_ADDR*)(data+3);
hirokimineshita 0:736c76a75def 432 const char* name = (const char*)(data + 9);
hirokimineshita 0:736c76a75def 433 RemoteName(addr,name);
hirokimineshita 0:736c76a75def 434 }
hirokimineshita 0:736c76a75def 435 Callback(CALLBACK_REMOTE_NAME,data+3,LE16(data+1)); // addr is in here too
hirokimineshita 0:736c76a75def 436 _state &= ~MASK_REMOTE_NAME;
hirokimineshita 0:736c76a75def 437 break;
hirokimineshita 0:736c76a75def 438
hirokimineshita 0:736c76a75def 439 case HCI_EV_CMD_STATUS:
hirokimineshita 0:736c76a75def 440 {
hirokimineshita 0:736c76a75def 441 const char* errs = HCIErrStr(data[2]);
hirokimineshita 0:736c76a75def 442 printf("Status %s %s\r\n",CmdStr(LE16(data+4)),errs);
hirokimineshita 0:736c76a75def 443 }
hirokimineshita 0:736c76a75def 444 break;
hirokimineshita 0:736c76a75def 445
hirokimineshita 0:736c76a75def 446 case HCI_EV_CMD_COMPLETE:
hirokimineshita 0:736c76a75def 447 OnCommandComplete(data[3] | (data[4] << 8),data+6,data[1]-4);
hirokimineshita 0:736c76a75def 448 break;
hirokimineshita 0:736c76a75def 449
hirokimineshita 0:736c76a75def 450 case HCI_EV_PIN_CODE_REQ:
hirokimineshita 0:736c76a75def 451 PinCodeReply(data+2);
hirokimineshita 0:736c76a75def 452 break;
hirokimineshita 0:736c76a75def 453
hirokimineshita 0:736c76a75def 454 case HCI_EV_LINK_KEY_REQ:
hirokimineshita 0:736c76a75def 455 SendCmd(HCI_OP_LINK_KEY_NEG_REPLY,data+2,6);
hirokimineshita 0:736c76a75def 456 break;
hirokimineshita 0:736c76a75def 457
hirokimineshita 0:736c76a75def 458 default:
hirokimineshita 0:736c76a75def 459 ;
hirokimineshita 0:736c76a75def 460 // printfBytes("HCI Event:",data,data[1]+2);
hirokimineshita 0:736c76a75def 461 }
hirokimineshita 0:736c76a75def 462 }
hirokimineshita 0:736c76a75def 463
hirokimineshita 0:736c76a75def 464 int HCI::Open(SocketInternal* sock, SocketAddrHdr* addr)
hirokimineshita 0:736c76a75def 465 {
hirokimineshita 0:736c76a75def 466 L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
hirokimineshita 0:736c76a75def 467 L2CAPAddr* l2capaddr = (L2CAPAddr*)addr;
hirokimineshita 0:736c76a75def 468 BTDevice* bt = Find(&l2capaddr->bdaddr);
hirokimineshita 0:736c76a75def 469 if (!bt)
hirokimineshita 0:736c76a75def 470 {
hirokimineshita 0:736c76a75def 471 printf("Can't open l2cap %d on ",l2capaddr->psm);
hirokimineshita 0:736c76a75def 472 printf(&l2capaddr->bdaddr);
hirokimineshita 0:736c76a75def 473 printf("\r\n");
hirokimineshita 0:736c76a75def 474 return ERR_HCI_DEVICE_NOT_FOUND;
hirokimineshita 0:736c76a75def 475 }
hirokimineshita 0:736c76a75def 476 l2capsock->btdevice = bt;
hirokimineshita 0:736c76a75def 477 return bt->Open(sock,addr);
hirokimineshita 0:736c76a75def 478 }
hirokimineshita 0:736c76a75def 479
hirokimineshita 0:736c76a75def 480 int HCI::Create(SocketInternal* sock, SocketAddrHdr* addr)
hirokimineshita 0:736c76a75def 481 {
hirokimineshita 0:736c76a75def 482 L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
hirokimineshita 0:736c76a75def 483 L2CAPAddr* l2capaddr = (L2CAPAddr*)addr;
hirokimineshita 0:736c76a75def 484 BTDevice* bt = Find(&l2capaddr->bdaddr);
hirokimineshita 0:736c76a75def 485 if (!bt)
hirokimineshita 0:736c76a75def 486 {
hirokimineshita 0:736c76a75def 487 printf("HCI Create: Can't open l2cap on ");
hirokimineshita 0:736c76a75def 488 printf(&l2capaddr->bdaddr);
hirokimineshita 0:736c76a75def 489 printf("\r\n");
hirokimineshita 0:736c76a75def 490 return ERR_HCI_DEVICE_NOT_FOUND;
hirokimineshita 0:736c76a75def 491 }
hirokimineshita 0:736c76a75def 492 l2capsock->btdevice = bt;
hirokimineshita 0:736c76a75def 493 //return 0;
hirokimineshita 0:736c76a75def 494 return bt->Create(sock,addr);
hirokimineshita 0:736c76a75def 495 }
hirokimineshita 0:736c76a75def 496
hirokimineshita 0:736c76a75def 497
hirokimineshita 0:736c76a75def 498 int HCI::Accept(SocketInternal* sock, SocketAddrHdr* addr)
hirokimineshita 0:736c76a75def 499 {
hirokimineshita 0:736c76a75def 500
hirokimineshita 0:736c76a75def 501 printf("Call to HCI Accept \r\n");
hirokimineshita 0:736c76a75def 502
hirokimineshita 0:736c76a75def 503 L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
hirokimineshita 0:736c76a75def 504 L2CAPAddr* l2capaddr = (L2CAPAddr*)addr;
hirokimineshita 0:736c76a75def 505 BTDevice* bt = Find(&l2capaddr->bdaddr);
hirokimineshita 0:736c76a75def 506 if (!bt)
hirokimineshita 0:736c76a75def 507 {
hirokimineshita 0:736c76a75def 508 //printf("Can't open l2cap %d on ",l2capaddr->psm);
hirokimineshita 0:736c76a75def 509 printf("HCI Accept: Can't open l2cap on ");
hirokimineshita 0:736c76a75def 510 printf(&l2capaddr->bdaddr);
hirokimineshita 0:736c76a75def 511 printf("\r\n");
hirokimineshita 0:736c76a75def 512 return ERR_HCI_DEVICE_NOT_FOUND;
hirokimineshita 0:736c76a75def 513 }
hirokimineshita 0:736c76a75def 514 //l2capsock->btdevice = bt;
hirokimineshita 0:736c76a75def 515 return bt->Open(sock,addr);
hirokimineshita 0:736c76a75def 516
hirokimineshita 0:736c76a75def 517 }
hirokimineshita 0:736c76a75def 518
hirokimineshita 0:736c76a75def 519 /**
hirokimineshita 0:736c76a75def 520 SocketInternal* HCI::Create(SocketInternal* sock)
hirokimineshita 0:736c76a75def 521 {
hirokimineshita 0:736c76a75def 522 return sock;
hirokimineshita 0:736c76a75def 523 }
hirokimineshita 0:736c76a75def 524 **/
hirokimineshita 0:736c76a75def 525
hirokimineshita 0:736c76a75def 526
hirokimineshita 0:736c76a75def 527 int HCI::Send(SocketInternal* sock, const u8* data, int len)
hirokimineshita 0:736c76a75def 528 {
hirokimineshita 0:736c76a75def 529 L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
hirokimineshita 0:736c76a75def 530 return l2capsock->btdevice->Send(sock,data,len); // Pointless double dispatch
hirokimineshita 0:736c76a75def 531 }
hirokimineshita 0:736c76a75def 532
hirokimineshita 0:736c76a75def 533 int HCI::Close(SocketInternal* sock)
hirokimineshita 0:736c76a75def 534 {
hirokimineshita 0:736c76a75def 535 L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
hirokimineshita 0:736c76a75def 536 return l2capsock->btdevice->Close(sock); // Pointless double dispatch
hirokimineshita 0:736c76a75def 537 }
hirokimineshita 0:736c76a75def 538
hirokimineshita 0:736c76a75def 539 void HCI::ACLRecv(const u8* data, int len)
hirokimineshita 0:736c76a75def 540 {
hirokimineshita 0:736c76a75def 541 int handle = LE16(data);
hirokimineshita 0:736c76a75def 542 BD_ADDR* addr;
hirokimineshita 0:736c76a75def 543 BTDevice* d = Find(handle & 0x0FFF);
hirokimineshita 0:736c76a75def 544 addr = &d->_info.bdaddr;
hirokimineshita 0:736c76a75def 545
hirokimineshita 0:736c76a75def 546 if (d)
hirokimineshita 0:736c76a75def 547 d->ACLRecv(addr,data,len);
hirokimineshita 0:736c76a75def 548 }
hirokimineshita 0:736c76a75def 549
hirokimineshita 0:736c76a75def 550 //===================================================================
hirokimineshita 0:736c76a75def 551 //===================================================================