kohei matsumoto / Mbed 2 deprecated spring_ps3_3

test

Dependencies:   mbed KondoServoLibrary Encoder

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

Committer:
koheim
Date:
Thu Mar 12 07:32:03 2020 +0000
Revision:
21:0f12ee2322e4
Parent:
0:736c76a75def 
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 #include <stdio.h>
hirokimineshita 0:736c76a75def 25 #include <stdlib.h>
hirokimineshita 0:736c76a75def 26 #include <stdio.h>
hirokimineshita 0:736c76a75def 27 #include <string.h>
hirokimineshita 0:736c76a75def 28
hirokimineshita 0:736c76a75def 29 #include "Utils.h"
hirokimineshita 0:736c76a75def 30 #include "hci.h"
hirokimineshita 0:736c76a75def 31
hirokimineshita 0:736c76a75def 32 #define L2CAP_COMMAND_REJ 0x01
hirokimineshita 0:736c76a75def 33 #define L2CAP_CONN_REQ 0x02
hirokimineshita 0:736c76a75def 34 #define L2CAP_CONN_RSP 0x03
hirokimineshita 0:736c76a75def 35 #define L2CAP_CONF_REQ 0x04
hirokimineshita 0:736c76a75def 36 #define L2CAP_CONF_RSP 0x05
hirokimineshita 0:736c76a75def 37 #define L2CAP_DISCONN_REQ 0x06
hirokimineshita 0:736c76a75def 38 #define L2CAP_DISCONN_RSP 0x07
hirokimineshita 0:736c76a75def 39 #define L2CAP_ECHO_REQ 0x08
hirokimineshita 0:736c76a75def 40 #define L2CAP_ECHO_RSP 0x09
hirokimineshita 0:736c76a75def 41 #define L2CAP_INFO_REQ 0x0a
hirokimineshita 0:736c76a75def 42 #define L2CAP_INFO_RSP 0x0b
hirokimineshita 0:736c76a75def 43
hirokimineshita 0:736c76a75def 44
hirokimineshita 0:736c76a75def 45 /* L2CAP command codes */
hirokimineshita 0:736c76a75def 46 const char* L2CAP_ComandCodeStr(int c)
hirokimineshita 0:736c76a75def 47 {
hirokimineshita 0:736c76a75def 48 switch (c)
hirokimineshita 0:736c76a75def 49 {
hirokimineshita 0:736c76a75def 50 case L2CAP_COMMAND_REJ: return "L2CAP_COMMAND_REJ";
hirokimineshita 0:736c76a75def 51 case L2CAP_CONN_REQ: return "L2CAP_CONN_REQ";
hirokimineshita 0:736c76a75def 52 case L2CAP_CONN_RSP: return "L2CAP_CONN_RSP";
hirokimineshita 0:736c76a75def 53 case L2CAP_CONF_REQ: return "L2CAP_CONF_REQ";
hirokimineshita 0:736c76a75def 54 case L2CAP_CONF_RSP: return "L2CAP_CONF_RSP";
hirokimineshita 0:736c76a75def 55 case L2CAP_DISCONN_REQ: return "L2CAP_DISCONN_REQ";
hirokimineshita 0:736c76a75def 56 case L2CAP_DISCONN_RSP: return "L2CAP_DISCONN_RSP";
hirokimineshita 0:736c76a75def 57 case L2CAP_ECHO_REQ: return "L2CAP_ECHO_REQ";
hirokimineshita 0:736c76a75def 58 case L2CAP_ECHO_RSP: return "L2CAP_ECHO_RSP";
hirokimineshita 0:736c76a75def 59 case L2CAP_INFO_REQ: return "L2CAP_INFO_REQ";
hirokimineshita 0:736c76a75def 60 case L2CAP_INFO_RSP: return "L2CAP_INFO_RSP";
hirokimineshita 0:736c76a75def 61 }
hirokimineshita 0:736c76a75def 62 return "unknown";
hirokimineshita 0:736c76a75def 63 }
hirokimineshita 0:736c76a75def 64
hirokimineshita 0:736c76a75def 65 typedef struct
hirokimineshita 0:736c76a75def 66 {
hirokimineshita 0:736c76a75def 67 u16 handle;
hirokimineshita 0:736c76a75def 68 u16 length; // total
hirokimineshita 0:736c76a75def 69 u16 l2capLength; // length -4
hirokimineshita 0:736c76a75def 70 u16 cid; // Signaling packet CID = 1
hirokimineshita 0:736c76a75def 71 u8 data[64]; // Largest thing to send!!! todo
hirokimineshita 0:736c76a75def 72 } L2CAPData;
hirokimineshita 0:736c76a75def 73
hirokimineshita 0:736c76a75def 74 typedef struct
hirokimineshita 0:736c76a75def 75 {
hirokimineshita 0:736c76a75def 76 u16 handle;
hirokimineshita 0:736c76a75def 77 u16 length; // total
hirokimineshita 0:736c76a75def 78 u16 l2capLength; // length -4
hirokimineshita 0:736c76a75def 79 u16 cid; // Signaling packet CID = 1
hirokimineshita 0:736c76a75def 80
hirokimineshita 0:736c76a75def 81 // Payload
hirokimineshita 0:736c76a75def 82 u8 cmd; //
hirokimineshita 0:736c76a75def 83 u8 id;
hirokimineshita 0:736c76a75def 84 u16 cmdLength; // total-8
hirokimineshita 0:736c76a75def 85 u16 params[4]; // Params
hirokimineshita 0:736c76a75def 86 } L2CAPCmd;
hirokimineshita 0:736c76a75def 87
hirokimineshita 0:736c76a75def 88 void printf(const BD_ADDR* addr);
hirokimineshita 0:736c76a75def 89
hirokimineshita 0:736c76a75def 90 //
hirokimineshita 0:736c76a75def 91 void BTDevice::Init()
hirokimineshita 0:736c76a75def 92 {
hirokimineshita 0:736c76a75def 93 memset(&_info,0,sizeof(inquiry_info));
hirokimineshita 0:736c76a75def 94 _handle = 0;
hirokimineshita 0:736c76a75def 95 _name[0] = 0;
hirokimineshita 0:736c76a75def 96 _state = 0;
hirokimineshita 0:736c76a75def 97 }
hirokimineshita 0:736c76a75def 98
hirokimineshita 0:736c76a75def 99 // virtual SocketHandler
hirokimineshita 0:736c76a75def 100 int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr)
hirokimineshita 0:736c76a75def 101 {
hirokimineshita 0:736c76a75def 102 printf("Call to BTDevice Open \r\n");
hirokimineshita 0:736c76a75def 103 L2CAPSocket* s = (L2CAPSocket*)sock;
hirokimineshita 0:736c76a75def 104 L2CAPAddr* a = (L2CAPAddr*)addr;
hirokimineshita 0:736c76a75def 105 s->scid = 0x40 + sock->ID-1; // are these reserved?
hirokimineshita 0:736c76a75def 106 s->dcid = 0;
hirokimineshita 0:736c76a75def 107 Connect(s->scid,a->psm);
hirokimineshita 0:736c76a75def 108 return sock->ID;
hirokimineshita 0:736c76a75def 109 }
hirokimineshita 0:736c76a75def 110
hirokimineshita 0:736c76a75def 111 // virtual SocketHandler
hirokimineshita 0:736c76a75def 112 int BTDevice::Create(SocketInternal* sock, SocketAddrHdr* addr)
hirokimineshita 0:736c76a75def 113 {
hirokimineshita 0:736c76a75def 114 printf("Call to BTDevice Create \r\n");
hirokimineshita 0:736c76a75def 115 L2CAPSocket* s = (L2CAPSocket*)sock;
hirokimineshita 0:736c76a75def 116 L2CAPAddr* a = (L2CAPAddr*)addr;
hirokimineshita 0:736c76a75def 117
hirokimineshita 0:736c76a75def 118 //Connect(s->scid,a->psm);
hirokimineshita 0:736c76a75def 119 return sock->ID;
hirokimineshita 0:736c76a75def 120 }
hirokimineshita 0:736c76a75def 121
hirokimineshita 0:736c76a75def 122 int BTDevice::Accept(SocketInternal* sock, SocketAddrHdr* addr)
hirokimineshita 0:736c76a75def 123 {
hirokimineshita 0:736c76a75def 124 printf("Call to BTDevice Accept \r\n");
hirokimineshita 0:736c76a75def 125 L2CAPSocket* s = (L2CAPSocket*)sock;
hirokimineshita 0:736c76a75def 126 L2CAPAddr* a = (L2CAPAddr*)addr;
hirokimineshita 0:736c76a75def 127
hirokimineshita 0:736c76a75def 128
hirokimineshita 0:736c76a75def 129 printf("ID = %d scid = %d dcid = %d \r\n",sock->ID, s->scid, s->dcid);
hirokimineshita 0:736c76a75def 130
hirokimineshita 0:736c76a75def 131 return sock->ID;
hirokimineshita 0:736c76a75def 132 }
hirokimineshita 0:736c76a75def 133
hirokimineshita 0:736c76a75def 134
hirokimineshita 0:736c76a75def 135
hirokimineshita 0:736c76a75def 136
hirokimineshita 0:736c76a75def 137 // virtual SocketHandler
hirokimineshita 0:736c76a75def 138 int BTDevice::Send(SocketInternal* sock, const u8* data, int len)
hirokimineshita 0:736c76a75def 139 {
hirokimineshita 0:736c76a75def 140 printf("Call to BTDevice Send \r\n");
hirokimineshita 0:736c76a75def 141 L2CAPData d;
hirokimineshita 0:736c76a75def 142 L2CAPSocket* s = (L2CAPSocket*)sock;
hirokimineshita 0:736c76a75def 143
hirokimineshita 0:736c76a75def 144 d.handle = _handle | 0x2000;
hirokimineshita 0:736c76a75def 145 d.length = 4 + len;
hirokimineshita 0:736c76a75def 146 d.l2capLength = len;
hirokimineshita 0:736c76a75def 147 d.cid = s->dcid;
hirokimineshita 0:736c76a75def 148
hirokimineshita 0:736c76a75def 149 if (len > 64)
hirokimineshita 0:736c76a75def 150 return -1;
hirokimineshita 0:736c76a75def 151 memcpy(d.data,data,len);
hirokimineshita 0:736c76a75def 152 return Send((u8*)&d,len+8);
hirokimineshita 0:736c76a75def 153 }
hirokimineshita 0:736c76a75def 154
hirokimineshita 0:736c76a75def 155 // virtual SocketHandler
hirokimineshita 0:736c76a75def 156 int BTDevice::Close(SocketInternal* sock)
hirokimineshita 0:736c76a75def 157 {
hirokimineshita 0:736c76a75def 158 printf("L2CAP close %d\r\n",sock->ID);
hirokimineshita 0:736c76a75def 159 L2CAPSocket* s = (L2CAPSocket*)sock;
hirokimineshita 0:736c76a75def 160 return Disconnect(s->scid,s->dcid);
hirokimineshita 0:736c76a75def 161 }
hirokimineshita 0:736c76a75def 162
hirokimineshita 0:736c76a75def 163 L2CAPSocket* BTDevice::SCIDToSocket(int scid)
hirokimineshita 0:736c76a75def 164 {
hirokimineshita 0:736c76a75def 165 return (L2CAPSocket*)GetSocketInternal(scid-0x40+1);
hirokimineshita 0:736c76a75def 166 }
hirokimineshita 0:736c76a75def 167
hirokimineshita 0:736c76a75def 168 int BTDevice::Send(const u8* data, int len)
hirokimineshita 0:736c76a75def 169 {
hirokimineshita 0:736c76a75def 170 printfBytes("L2CP send: ",data,len);
hirokimineshita 0:736c76a75def 171 _transport->ACLSend(data,len);
hirokimineshita 0:736c76a75def 172 return 0;
hirokimineshita 0:736c76a75def 173 }
hirokimineshita 0:736c76a75def 174
hirokimineshita 0:736c76a75def 175 int BTDevice::Send(u8 c, u8 id, u16* params, int count)
hirokimineshita 0:736c76a75def 176 {
hirokimineshita 0:736c76a75def 177 L2CAPCmd cmd;
hirokimineshita 0:736c76a75def 178 cmd.handle = _handle | 0x2000;
hirokimineshita 0:736c76a75def 179 cmd.length = 8 + count*2;
hirokimineshita 0:736c76a75def 180
hirokimineshita 0:736c76a75def 181 cmd.l2capLength = cmd.length-4;
hirokimineshita 0:736c76a75def 182 cmd.cid = 1; // Signaling packet
hirokimineshita 0:736c76a75def 183
hirokimineshita 0:736c76a75def 184 cmd.cmd = c;
hirokimineshita 0:736c76a75def 185 cmd.id = id;
hirokimineshita 0:736c76a75def 186 cmd.cmdLength = count*2;
hirokimineshita 0:736c76a75def 187 for (int i = 0; i < count; i++)
hirokimineshita 0:736c76a75def 188 cmd.params[i] = params[i];
hirokimineshita 0:736c76a75def 189 return Send((u8*)&cmd,cmd.length+4);
hirokimineshita 0:736c76a75def 190 }
hirokimineshita 0:736c76a75def 191
hirokimineshita 0:736c76a75def 192 int BTDevice::Connect(int scid, int psm)
hirokimineshita 0:736c76a75def 193 {
hirokimineshita 0:736c76a75def 194 u16 p[2];
hirokimineshita 0:736c76a75def 195 p[0] = psm;
hirokimineshita 0:736c76a75def 196 p[1] = scid;
hirokimineshita 0:736c76a75def 197 return Send(L2CAP_CONN_REQ,_txid++,p,2);
hirokimineshita 0:736c76a75def 198 }
hirokimineshita 0:736c76a75def 199
hirokimineshita 0:736c76a75def 200 int BTDevice::Disconnect(int scid, int dcid)
hirokimineshita 0:736c76a75def 201 {
hirokimineshita 0:736c76a75def 202 u16 p[2];
hirokimineshita 0:736c76a75def 203 p[0] = dcid;
hirokimineshita 0:736c76a75def 204 p[1] = scid;
hirokimineshita 0:736c76a75def 205 return Send(L2CAP_DISCONN_REQ,_txid++,p,2);
hirokimineshita 0:736c76a75def 206 }
hirokimineshita 0:736c76a75def 207
hirokimineshita 0:736c76a75def 208 int BTDevice::ConfigureRequest(int dcid)
hirokimineshita 0:736c76a75def 209 {
hirokimineshita 0:736c76a75def 210 u16 p[4];
hirokimineshita 0:736c76a75def 211 p[0] = dcid;
hirokimineshita 0:736c76a75def 212 p[1] = 0;
hirokimineshita 0:736c76a75def 213 p[2] = 0x0201; // Options
hirokimineshita 0:736c76a75def 214 p[3] = 0x02A0; // 672
hirokimineshita 0:736c76a75def 215 return Send(L2CAP_CONF_REQ,_txid++,p,4);
hirokimineshita 0:736c76a75def 216 }
hirokimineshita 0:736c76a75def 217
hirokimineshita 0:736c76a75def 218 int BTDevice::ConfigureResponse(u8 rxid, int dcid)
hirokimineshita 0:736c76a75def 219 {
hirokimineshita 0:736c76a75def 220 u16 p[3];
hirokimineshita 0:736c76a75def 221 p[0] = dcid;
hirokimineshita 0:736c76a75def 222 p[1] = 0;
hirokimineshita 0:736c76a75def 223 p[2] = 0;
hirokimineshita 0:736c76a75def 224 return Send(L2CAP_CONF_RSP,rxid,p,3);
hirokimineshita 0:736c76a75def 225 }
hirokimineshita 0:736c76a75def 226
hirokimineshita 0:736c76a75def 227 int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid)
hirokimineshita 0:736c76a75def 228 {
hirokimineshita 0:736c76a75def 229 u16 p[2];
hirokimineshita 0:736c76a75def 230 p[0] = dcid;
hirokimineshita 0:736c76a75def 231 p[1] = scid;
hirokimineshita 0:736c76a75def 232 return Send(L2CAP_DISCONN_RSP,rxid,p,2);
hirokimineshita 0:736c76a75def 233 }
hirokimineshita 0:736c76a75def 234
hirokimineshita 0:736c76a75def 235 int BTDevice::AcceptResponse(u8 rxid, int scid, int dcid)
hirokimineshita 0:736c76a75def 236 {
hirokimineshita 0:736c76a75def 237 printf("Connection accepted \r\n");
hirokimineshita 0:736c76a75def 238 u16 p[4];
hirokimineshita 0:736c76a75def 239 p[0] = scid;
hirokimineshita 0:736c76a75def 240 p[1] = dcid;
hirokimineshita 0:736c76a75def 241 p[2] = L2CAP_CONN_SUCCESS;
hirokimineshita 0:736c76a75def 242 p[3] = 0;
hirokimineshita 0:736c76a75def 243
hirokimineshita 0:736c76a75def 244 return Send(L2CAP_CONN_RSP,rxid,p,4);
hirokimineshita 0:736c76a75def 245
hirokimineshita 0:736c76a75def 246 }
hirokimineshita 0:736c76a75def 247
hirokimineshita 0:736c76a75def 248 int BTDevice::RefuseResponse(u8 rxid)
hirokimineshita 0:736c76a75def 249 {
hirokimineshita 0:736c76a75def 250 printf("Connection refused \r\n");
hirokimineshita 0:736c76a75def 251 u16 p[2];
hirokimineshita 0:736c76a75def 252 p[0] = L2CAP_CONN_REF_PSM;
hirokimineshita 0:736c76a75def 253 p[1] = 0;
hirokimineshita 0:736c76a75def 254
hirokimineshita 0:736c76a75def 255 return Send(L2CAP_CONN_RSP,rxid,p,2);
hirokimineshita 0:736c76a75def 256 }
hirokimineshita 0:736c76a75def 257
hirokimineshita 0:736c76a75def 258
hirokimineshita 0:736c76a75def 259 //int BTDevice::InUse(int psm)
hirokimineshita 0:736c76a75def 260 //{
hirokimineshita 0:736c76a75def 261 // for (int i = 0; i < MAX_PORTS; i++){
hirokimineshita 0:736c76a75def 262 // printf("Listen Q %d = %d \r\n",i, _listen[i]);
hirokimineshita 0:736c76a75def 263 // if ( _listen[i] == psm ) {
hirokimineshita 0:736c76a75def 264 // printf("We are listening on port %d \r\n",psm); //in use
hirokimineshita 0:736c76a75def 265 // return 0;
hirokimineshita 0:736c76a75def 266 // }
hirokimineshita 0:736c76a75def 267 // }
hirokimineshita 0:736c76a75def 268 // printf("We are not listening on port %d \r\n",psm);
hirokimineshita 0:736c76a75def 269 // return 1;
hirokimineshita 0:736c76a75def 270 //}
hirokimineshita 0:736c76a75def 271
hirokimineshita 0:736c76a75def 272
hirokimineshita 0:736c76a75def 273
hirokimineshita 0:736c76a75def 274
hirokimineshita 0:736c76a75def 275 void BTDevice::Control(const BD_ADDR* addr, const u8* data, int len)
hirokimineshita 0:736c76a75def 276 {
hirokimineshita 0:736c76a75def 277
hirokimineshita 0:736c76a75def 278 SocketInternal* sock;
hirokimineshita 0:736c76a75def 279 L2CAPSocket* s;
hirokimineshita 0:736c76a75def 280 int psm,scid,dcid,flags;
hirokimineshita 0:736c76a75def 281
hirokimineshita 0:736c76a75def 282 printf("From address ");
hirokimineshita 0:736c76a75def 283 printf(addr);
hirokimineshita 0:736c76a75def 284 printf(" : ");
hirokimineshita 0:736c76a75def 285
hirokimineshita 0:736c76a75def 286 int cc = data[8];
hirokimineshita 0:736c76a75def 287 printf(L2CAP_ComandCodeStr(cc));
hirokimineshita 0:736c76a75def 288 int result = LE16(data+16);
hirokimineshita 0:736c76a75def 289 printf(" Result %d\r\n",result);
hirokimineshita 0:736c76a75def 290
hirokimineshita 0:736c76a75def 291
hirokimineshita 0:736c76a75def 292 switch (cc)
hirokimineshita 0:736c76a75def 293 {
hirokimineshita 0:736c76a75def 294 case L2CAP_COMMAND_REJ:
hirokimineshita 0:736c76a75def 295 break;
hirokimineshita 0:736c76a75def 296 case L2CAP_CONN_REQ:
hirokimineshita 0:736c76a75def 297 {
hirokimineshita 0:736c76a75def 298 psm = LE16(data+12);
hirokimineshita 0:736c76a75def 299 scid = LE16(data+14);
hirokimineshita 0:736c76a75def 300 printf("Connection request scid = %d psm = %d \r\n",scid,psm);
hirokimineshita 0:736c76a75def 301
hirokimineshita 0:736c76a75def 302 // check if we listen on the port
hirokimineshita 0:736c76a75def 303 //if ( InUse(psm) ) {
hirokimineshita 0:736c76a75def 304 if ( Socket_InUse(SOCKET_L2CAP,psm) ) {
hirokimineshita 0:736c76a75def 305 RefuseResponse(data[9]);
hirokimineshita 0:736c76a75def 306
hirokimineshita 0:736c76a75def 307
hirokimineshita 0:736c76a75def 308 } else {
hirokimineshita 0:736c76a75def 309 L2CAPAddr sockAddr;
hirokimineshita 0:736c76a75def 310 sockAddr.bdaddr = *addr;
hirokimineshita 0:736c76a75def 311 sock = Socket_Create(SOCKET_L2CAP, &sockAddr.hdr, psm);
hirokimineshita 0:736c76a75def 312 s = (L2CAPSocket*)sock;
hirokimineshita 0:736c76a75def 313 s->scid = 0x40 + sock->ID-1; // are these reserved?
hirokimineshita 0:736c76a75def 314 s->dcid = scid;
hirokimineshita 0:736c76a75def 315 AcceptResponse(data[9],s->scid,s->dcid);
hirokimineshita 0:736c76a75def 316
hirokimineshita 0:736c76a75def 317 ConfigureRequest(s->dcid); // handshake
hirokimineshita 0:736c76a75def 318
hirokimineshita 0:736c76a75def 319 sock->SetState(SocketState_Accept);
hirokimineshita 0:736c76a75def 320 }
hirokimineshita 0:736c76a75def 321
hirokimineshita 0:736c76a75def 322 /**
hirokimineshita 0:736c76a75def 323 for (int i = 0; i < MAX_PORTS; i++){
hirokimineshita 0:736c76a75def 324 if ( _listen[i] == psm ) {
hirokimineshita 0:736c76a75def 325 printf("We are listening on port %d \r\n",psm); //in use
hirokimineshita 0:736c76a75def 326 RefuseResponse(data[9]);
hirokimineshita 0:736c76a75def 327 break;
hirokimineshita 0:736c76a75def 328 }
hirokimineshita 0:736c76a75def 329 }
hirokimineshita 0:736c76a75def 330 **/
hirokimineshita 0:736c76a75def 331
hirokimineshita 0:736c76a75def 332 //L2CAPAddr sockAddr;
hirokimineshita 0:736c76a75def 333 //sockAddr.bdaddr = addr;
hirokimineshita 0:736c76a75def 334 //sockAddr.psm = psm;
hirokimineshita 0:736c76a75def 335
hirokimineshita 0:736c76a75def 336 }
hirokimineshita 0:736c76a75def 337 break;
hirokimineshita 0:736c76a75def 338
hirokimineshita 0:736c76a75def 339 // Response to our initial connect from Remote
hirokimineshita 0:736c76a75def 340 case L2CAP_CONN_RSP:
hirokimineshita 0:736c76a75def 341 {
hirokimineshita 0:736c76a75def 342 if (result == 0)
hirokimineshita 0:736c76a75def 343 {
hirokimineshita 0:736c76a75def 344 printf("Connect succeeded\r\n");
hirokimineshita 0:736c76a75def 345 dcid = LE16(data+12);
hirokimineshita 0:736c76a75def 346 scid = LE16(data+14);
hirokimineshita 0:736c76a75def 347 L2CAPSocket* s = SCIDToSocket(scid);
hirokimineshita 0:736c76a75def 348 if (s)
hirokimineshita 0:736c76a75def 349 {
hirokimineshita 0:736c76a75def 350 s->dcid = dcid;
hirokimineshita 0:736c76a75def 351 ConfigureRequest(dcid);
hirokimineshita 0:736c76a75def 352 }
hirokimineshita 0:736c76a75def 353 } else
hirokimineshita 0:736c76a75def 354 printf("Connect failed?\r\n");
hirokimineshita 0:736c76a75def 355 }
hirokimineshita 0:736c76a75def 356 break;
hirokimineshita 0:736c76a75def 357
hirokimineshita 0:736c76a75def 358 case L2CAP_CONF_RSP:
hirokimineshita 0:736c76a75def 359 {
hirokimineshita 0:736c76a75def 360 scid = LE16(data+12);
hirokimineshita 0:736c76a75def 361 SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
hirokimineshita 0:736c76a75def 362 if (s)
hirokimineshita 0:736c76a75def 363 s->SetState(SocketState_Open);
hirokimineshita 0:736c76a75def 364 }
hirokimineshita 0:736c76a75def 365 break;
hirokimineshita 0:736c76a75def 366
hirokimineshita 0:736c76a75def 367 case L2CAP_CONF_REQ:
hirokimineshita 0:736c76a75def 368 {
hirokimineshita 0:736c76a75def 369 u16 dcid = LE16(data+12);
hirokimineshita 0:736c76a75def 370 u16 flags = LE16(data+14);
hirokimineshita 0:736c76a75def 371 printf("Config request dcid = %02X flags = %02X\r\n", dcid, flags);
hirokimineshita 0:736c76a75def 372 scid = LE16(data+12);
hirokimineshita 0:736c76a75def 373 L2CAPSocket* s = SCIDToSocket(scid);
hirokimineshita 0:736c76a75def 374 if (s)
hirokimineshita 0:736c76a75def 375 ConfigureResponse(data[9],s->dcid);
hirokimineshita 0:736c76a75def 376 }
hirokimineshita 0:736c76a75def 377 break;
hirokimineshita 0:736c76a75def 378 case L2CAP_DISCONN_REQ: {
hirokimineshita 0:736c76a75def 379 int dcid = LE16(data+12);
hirokimineshita 0:736c76a75def 380 int scid = LE16(data+14);
hirokimineshita 0:736c76a75def 381 L2CAPSocket* s = SCIDToSocket(scid);
hirokimineshita 0:736c76a75def 382 //s->si.SetState(SocketState_Closed);
hirokimineshita 0:736c76a75def 383 DisconnectResponse(data[9], scid, dcid);
hirokimineshita 0:736c76a75def 384 }
hirokimineshita 0:736c76a75def 385 break;
hirokimineshita 0:736c76a75def 386
hirokimineshita 0:736c76a75def 387 }
hirokimineshita 0:736c76a75def 388 }
hirokimineshita 0:736c76a75def 389
hirokimineshita 0:736c76a75def 390 void BTDevice::ACLRecv(const BD_ADDR* addr, const u8* data, int len)
hirokimineshita 0:736c76a75def 391 {
hirokimineshita 0:736c76a75def 392 //printfBytes("L2CP recv: ",data,16);
hirokimineshita 0:736c76a75def 393 int handle = LE16(data);
hirokimineshita 0:736c76a75def 394 if (handle != (0x2000 | _handle))
hirokimineshita 0:736c76a75def 395 return;
hirokimineshita 0:736c76a75def 396
hirokimineshita 0:736c76a75def 397 int cid = LE16(data+6);
hirokimineshita 0:736c76a75def 398 if (cid == 1)
hirokimineshita 0:736c76a75def 399 {
hirokimineshita 0:736c76a75def 400 Control(addr,data,len);
hirokimineshita 0:736c76a75def 401 return;
hirokimineshita 0:736c76a75def 402 }
hirokimineshita 0:736c76a75def 403
hirokimineshita 0:736c76a75def 404 SocketInternal* s = (SocketInternal*)SCIDToSocket(cid);
hirokimineshita 0:736c76a75def 405 if (s)
hirokimineshita 0:736c76a75def 406 s->Recv(data+8,LE16(data+2)-4);
hirokimineshita 0:736c76a75def 407 else
hirokimineshita 0:736c76a75def 408 printf("Bad event cid %d\r\n",cid);
hirokimineshita 0:736c76a75def 409 }