yoka konishi / Mbed 2 deprecated a1_SPI_master_arm_shudouki

手動機アーム、mbed基盤のspiをおくるだけのプログラムです（9/4）

Dependencies:   SPI_master_arm_shudouki mbed

Fork of SPI_master_arm_shudouki２ by F^3 RC 2班

L2CAP.cpp@2:faa028d2f2f8, 2017-09-04 (annotated)

Committer:
yoka06
Date:
Mon Sep 04 13:02:43 2017 +0000
Revision:
2:faa028d2f2f8
Parent:
0:76d1c7f13415 
?

Who changed what in which revision?

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