春ロボ1班(元F3RC4班+) / Mbed 2 deprecated harurobo_PS3_3_5

3/5

Dependencies:   mbed

L2CAP.cpp@1:530908de68c6, 2019-03-05 (annotated)

Committer:
yuki0701
Date:
Tue Mar 05 04:30:35 2019 +0000
Revision:
1:530908de68c6
Parent:
0:a56be39653d0 
a

Who changed what in which revision?

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