Nobuaki Aoki / Mbed 2 deprecated myBlueUSB_localfix

local fix version of myBlueUSB (http://mbed.org/users/networker/code/myBlueUSB/). - merge deleted files which are required to compile. - enable echo back of received data via RFCOMM.

Dependencies:   AvailableMemory FatFileSystem mbed myUSBHost

L2CAP.cpp@2:9f25a7fa1a54, 2013-12-08 (annotated)

Committer:
nobukuma
Date:
Sun Dec 08 21:52:09 2013 +0000
Revision:
2:9f25a7fa1a54
Parent:
0:003889bc474f 
???BT??????????????????; ?????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nobukuma 0:003889bc474f 1 /*
nobukuma 0:003889bc474f 2 Copyright (c) 2010 Peter Barrett
nobukuma 0:003889bc474f 3
nobukuma 0:003889bc474f 4 Permission is hereby granted, free of charge, to any person obtaining a copy
nobukuma 0:003889bc474f 5 of this software and associated documentation files (the "Software"), to deal
nobukuma 0:003889bc474f 6 in the Software without restriction, including without limitation the rights
nobukuma 0:003889bc474f 7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
nobukuma 0:003889bc474f 8 copies of the Software, and to permit persons to whom the Software is
nobukuma 0:003889bc474f 9 furnished to do so, subject to the following conditions:
nobukuma 0:003889bc474f 10
nobukuma 0:003889bc474f 11 The above copyright notice and this permission notice shall be included in
nobukuma 0:003889bc474f 12 all copies or substantial portions of the Software.
nobukuma 0:003889bc474f 13
nobukuma 0:003889bc474f 14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
nobukuma 0:003889bc474f 15 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
nobukuma 0:003889bc474f 16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
nobukuma 0:003889bc474f 17 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
nobukuma 0:003889bc474f 18 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
nobukuma 0:003889bc474f 19 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
nobukuma 0:003889bc474f 20 THE SOFTWARE.
nobukuma 0:003889bc474f 21 */
nobukuma 0:003889bc474f 22
nobukuma 0:003889bc474f 23
nobukuma 0:003889bc474f 24 #include <stdio.h>
nobukuma 0:003889bc474f 25 #include <stdlib.h>
nobukuma 0:003889bc474f 26 #include <stdio.h>
nobukuma 0:003889bc474f 27 #include <string.h>
nobukuma 0:003889bc474f 28
nobukuma 0:003889bc474f 29 #include "Utils.h"
nobukuma 0:003889bc474f 30 #include "hci.h"
nobukuma 0:003889bc474f 31 #include "HCITransportUSB.h"
nobukuma 0:003889bc474f 32 #include "sdp.h"
nobukuma 0:003889bc474f 33 #include "RFCOMM.h"
nobukuma 0:003889bc474f 34
nobukuma 0:003889bc474f 35 #define L2CAP_COMMAND_REJ 0x01
nobukuma 0:003889bc474f 36 #define L2CAP_CONN_REQ 0x02
nobukuma 0:003889bc474f 37 #define L2CAP_CONN_RSP 0x03
nobukuma 0:003889bc474f 38 #define L2CAP_CONF_REQ 0x04
nobukuma 0:003889bc474f 39 #define L2CAP_CONF_RSP 0x05
nobukuma 0:003889bc474f 40 #define L2CAP_DISCONN_REQ 0x06
nobukuma 0:003889bc474f 41 #define L2CAP_DISCONN_RSP 0x07
nobukuma 0:003889bc474f 42 #define L2CAP_ECHO_REQ 0x08
nobukuma 0:003889bc474f 43 #define L2CAP_ECHO_RSP 0x09
nobukuma 0:003889bc474f 44 #define L2CAP_INFO_REQ 0x0a
nobukuma 0:003889bc474f 45 #define L2CAP_INFO_RSP 0x0b
nobukuma 0:003889bc474f 46
nobukuma 0:003889bc474f 47 #define TXID (++_txid?_txid:1)
nobukuma 0:003889bc474f 48 //template <class T> T min(T a, T b) { return a<b ? a : b;}
nobukuma 0:003889bc474f 49
nobukuma 0:003889bc474f 50 /* L2CAP command codes */
nobukuma 0:003889bc474f 51 const char* L2CAP_ComandCodeStr(int c) {
nobukuma 0:003889bc474f 52 switch (c) {
nobukuma 0:003889bc474f 53 case L2CAP_COMMAND_REJ:
nobukuma 0:003889bc474f 54 return "L2CAP_COMMAND_REJ";
nobukuma 0:003889bc474f 55 case L2CAP_CONN_REQ:
nobukuma 0:003889bc474f 56 return "L2CAP_CONN_REQ";
nobukuma 0:003889bc474f 57 case L2CAP_CONN_RSP:
nobukuma 0:003889bc474f 58 return "L2CAP_CONN_RSP";
nobukuma 0:003889bc474f 59 case L2CAP_CONF_REQ:
nobukuma 0:003889bc474f 60 return "L2CAP_CONF_REQ";
nobukuma 0:003889bc474f 61 case L2CAP_CONF_RSP:
nobukuma 0:003889bc474f 62 return "L2CAP_CONF_RSP";
nobukuma 0:003889bc474f 63 case L2CAP_DISCONN_REQ:
nobukuma 0:003889bc474f 64 return "L2CAP_DISCONN_REQ";
nobukuma 0:003889bc474f 65 case L2CAP_DISCONN_RSP:
nobukuma 0:003889bc474f 66 return "L2CAP_DISCONN_RSP";
nobukuma 0:003889bc474f 67 case L2CAP_ECHO_REQ:
nobukuma 0:003889bc474f 68 return "L2CAP_ECHO_REQ";
nobukuma 0:003889bc474f 69 case L2CAP_ECHO_RSP:
nobukuma 0:003889bc474f 70 return "L2CAP_ECHO_RSP";
nobukuma 0:003889bc474f 71 case L2CAP_INFO_REQ:
nobukuma 0:003889bc474f 72 return "L2CAP_INFO_REQ";
nobukuma 0:003889bc474f 73 case L2CAP_INFO_RSP:
nobukuma 0:003889bc474f 74 return "L2CAP_INFO_RSP";
nobukuma 0:003889bc474f 75 }
nobukuma 0:003889bc474f 76 return "unknown";
nobukuma 0:003889bc474f 77 }
nobukuma 0:003889bc474f 78
nobukuma 0:003889bc474f 79 #define OFFSET 8 //means the buffer also has space for the l2cap/hci headers and need not be allocated and copied
nobukuma 0:003889bc474f 80 //#define OFFSET 0 //means the buffer only has space for the payload which need to be copied
nobukuma 0:003889bc474f 81 #if OFFSET == 0
nobukuma 0:003889bc474f 82 #define L2CAPBUFSIZE 128
nobukuma 0:003889bc474f 83 #else
nobukuma 0:003889bc474f 84 #define L2CAPBUFSIZE 0
nobukuma 0:003889bc474f 85 #endif
nobukuma 0:003889bc474f 86
nobukuma 0:003889bc474f 87 typedef struct {
nobukuma 0:003889bc474f 88 u16 handle;
nobukuma 0:003889bc474f 89 u16 length; // total
nobukuma 0:003889bc474f 90 u16 l2capLength; // length -4
nobukuma 0:003889bc474f 91 u16 cid; // Signaling packet CID = 1
nobukuma 0:003889bc474f 92 u8 data[L2CAPBUFSIZE]; // Largest thing to send!!! todo
nobukuma 0:003889bc474f 93 } L2CAPData;
nobukuma 0:003889bc474f 94
nobukuma 0:003889bc474f 95 //
nobukuma 0:003889bc474f 96 void BTDevice::Init() {
nobukuma 0:003889bc474f 97 memset(&_info,0,sizeof(inquiry_info));
nobukuma 0:003889bc474f 98 _handle = 0;
nobukuma 0:003889bc474f 99 _name[0] = 0;
nobukuma 0:003889bc474f 100 _state = 0;
nobukuma 0:003889bc474f 101 _txid = 1;
nobukuma 0:003889bc474f 102 //cntr_cred = 1;
nobukuma 0:003889bc474f 103 }
nobukuma 0:003889bc474f 104
nobukuma 0:003889bc474f 105 // virtual SocketHandler
nobukuma 0:003889bc474f 106 int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr) {
nobukuma 0:003889bc474f 107 L2CAPSocket* s = (L2CAPSocket*)sock;
nobukuma 0:003889bc474f 108 L2CAPAddr* a = (L2CAPAddr*)addr;
nobukuma 0:003889bc474f 109 s->scid = 0x40 + sock->ID-1; // are these reserved?
nobukuma 0:003889bc474f 110 s->dcid = 0;
nobukuma 0:003889bc474f 111 Connect(s->scid,a->psm);
nobukuma 0:003889bc474f 112 sock->State = SocketState_L2CAP_WaitConnectRsp;
nobukuma 0:003889bc474f 113 contState = 0;
nobukuma 0:003889bc474f 114 return sock->ID;
nobukuma 0:003889bc474f 115 }
nobukuma 0:003889bc474f 116
nobukuma 0:003889bc474f 117 // virtual SocketHandler
nobukuma 0:003889bc474f 118 int BTDevice::Accept(SocketInternal* sock, int scid, int rxid) {
nobukuma 0:003889bc474f 119 L2CAPSocket* s = (L2CAPSocket*)sock;
nobukuma 0:003889bc474f 120 s->scid = 0x40 + sock->ID-1; // are these reserved?
nobukuma 0:003889bc474f 121 s->dcid = scid;
nobukuma 0:003889bc474f 122 u16 p[4];
nobukuma 0:003889bc474f 123 p[0] = s->scid;
nobukuma 0:003889bc474f 124 p[1] = scid;
nobukuma 0:003889bc474f 125 p[2] = 0; //success
nobukuma 0:003889bc474f 126 p[3] = 0; //no further information
nobukuma 0:003889bc474f 127 Send(L2CAP_CONN_RSP,rxid,p,4);
nobukuma 0:003889bc474f 128 printf("send conn_rsp with dcid=%#x and scid=%#x\n", p[0],p[1]);
nobukuma 0:003889bc474f 129 sock->State = SocketState_L2CAP_Config_wait;
nobukuma 0:003889bc474f 130 contState = 0;
nobukuma 0:003889bc474f 131 return sock->ID;
nobukuma 0:003889bc474f 132 }
nobukuma 0:003889bc474f 133
nobukuma 0:003889bc474f 134 // virtual SocketHandler, called from HCI which is ABOVE L2CAP
nobukuma 0:003889bc474f 135 int BTDevice::Send(SocketInternal* sock, const u8* data, int len) {
nobukuma 0:003889bc474f 136 L2CAPSocket* s = (L2CAPSocket*)sock;
nobukuma 0:003889bc474f 137 #if OFFSET == 8 //sizeof L2CAPData header
nobukuma 0:003889bc474f 138 L2CAPData &d = *const_cast<L2CAPData*>((L2CAPData*)data);
nobukuma 0:003889bc474f 139 #else
nobukuma 0:003889bc474f 140 L2CAPData d;
nobukuma 0:003889bc474f 141 #endif
nobukuma 0:003889bc474f 142 if (len > peer_mtu) {//mtu concerns the l2cap mtu, because we use basic mode we cannot segment
nobukuma 0:003889bc474f 143 printf("MTU (%d) for outgoing packet (%d) exceeded\n", peer_mtu, len);
nobukuma 0:003889bc474f 144 return 0;
nobukuma 0:003889bc474f 145 }
nobukuma 0:003889bc474f 146 d.handle = _handle | 0x2000;
nobukuma 0:003889bc474f 147 d.length = 4 + len - OFFSET;
nobukuma 0:003889bc474f 148 d.l2capLength = len - OFFSET;
nobukuma 0:003889bc474f 149 d.cid = s->dcid;
nobukuma 0:003889bc474f 150 //printf("cid=%d: ", d.cid);
nobukuma 0:003889bc474f 151 //printfBytes("sending: ", data, len);
nobukuma 0:003889bc474f 152 #if OFFSET == 0
nobukuma 0:003889bc474f 153 if (len > L2CAPBUFSIZE)
nobukuma 0:003889bc474f 154 return -1;
nobukuma 0:003889bc474f 155 memcpy(d.data,data,len);
nobukuma 0:003889bc474f 156 return Send((u8*)&d,len+8);
nobukuma 0:003889bc474f 157 #else
nobukuma 0:003889bc474f 158 return Send(data, len);
nobukuma 0:003889bc474f 159 #endif
nobukuma 0:003889bc474f 160 }
nobukuma 0:003889bc474f 161
nobukuma 0:003889bc474f 162 // virtual SocketHandler
nobukuma 0:003889bc474f 163 int BTDevice::Close(SocketInternal* sock) {
nobukuma 0:003889bc474f 164 printf("L2CAP close %d\n",sock->ID);
nobukuma 0:003889bc474f 165 sock->State = SocketState_L2CAP_WaitDisconnect;
nobukuma 0:003889bc474f 166 L2CAPSocket* s = (L2CAPSocket*)sock;
nobukuma 0:003889bc474f 167 return Disconnect(s->scid,s->dcid);
nobukuma 0:003889bc474f 168 }
nobukuma 0:003889bc474f 169
nobukuma 0:003889bc474f 170 L2CAPSocket* BTDevice::SCIDToSocket(int scid) {
nobukuma 0:003889bc474f 171 return (L2CAPSocket*)GetSocketInternal(scid-0x40+1);
nobukuma 0:003889bc474f 172 }
nobukuma 0:003889bc474f 173
nobukuma 0:003889bc474f 174 int BTDevice::Send(const u8* data, int len) {//printfBytes("Transport : ", data, len);
nobukuma 0:003889bc474f 175 #ifdef HOST_CONTR_FLOW
nobukuma 0:003889bc474f 176 pkts_sent++;
nobukuma 0:003889bc474f 177 #endif
nobukuma 0:003889bc474f 178 _transport->ACLSend(data,len);
nobukuma 0:003889bc474f 179 return 0;
nobukuma 0:003889bc474f 180 }
nobukuma 0:003889bc474f 181
nobukuma 0:003889bc474f 182 void BTDevice::repeat_cmd() {
nobukuma 0:003889bc474f 183 printf("Cmd on handle %#x timed out, resending txid=%d\n", _handle, last_req.id);
nobukuma 0:003889bc474f 184 // Send ((u8*)&last_req, last_req.length+4);//danger! interrupt context, Send is not reentrant
nobukuma 0:003889bc474f 185 //optionally set new larger timeout
nobukuma 0:003889bc474f 186 }
nobukuma 0:003889bc474f 187
nobukuma 0:003889bc474f 188 int BTDevice::Send(u8 c, u8 id, u16* params, int count) {
nobukuma 0:003889bc474f 189 L2CAPCmd cmd;
nobukuma 0:003889bc474f 190 cmd.handle = _handle | 0x2000;
nobukuma 0:003889bc474f 191 cmd.length = 8 + count*2;
nobukuma 0:003889bc474f 192
nobukuma 0:003889bc474f 193 cmd.l2capLength = cmd.length-4;
nobukuma 0:003889bc474f 194 cmd.cid = 1; // Signaling packet
nobukuma 0:003889bc474f 195
nobukuma 0:003889bc474f 196 cmd.cmd = c;
nobukuma 0:003889bc474f 197 cmd.id = id;
nobukuma 0:003889bc474f 198 cmd.cmdLength = count*2;
nobukuma 0:003889bc474f 199 for (int i = 0; i < count; i++)
nobukuma 0:003889bc474f 200 cmd.params[i] = params[i];
nobukuma 0:003889bc474f 201 if ((c & 1) == 0) { //this is a request
nobukuma 0:003889bc474f 202 last_req = cmd;
nobukuma 0:003889bc474f 203 rtx.attach(this, &BTDevice::repeat_cmd, 30.0);
nobukuma 0:003889bc474f 204 //printf("Starting timeout for %#x, txid=%d\n", _handle, id);
nobukuma 0:003889bc474f 205 }
nobukuma 0:003889bc474f 206 return Send((u8*)&cmd,cmd.length+4);
nobukuma 0:003889bc474f 207 }
nobukuma 0:003889bc474f 208
nobukuma 0:003889bc474f 209 int BTDevice::Connect(int scid, int psm) {
nobukuma 0:003889bc474f 210 u16 p[2];
nobukuma 0:003889bc474f 211 p[0] = psm;
nobukuma 0:003889bc474f 212 p[1] = scid;
nobukuma 0:003889bc474f 213 return Send(L2CAP_CONN_REQ,TXID,p,2);
nobukuma 0:003889bc474f 214 }
nobukuma 0:003889bc474f 215
nobukuma 0:003889bc474f 216 int BTDevice::Disconnect(int scid, int dcid) {
nobukuma 0:003889bc474f 217 u16 p[2];
nobukuma 0:003889bc474f 218 p[0] = dcid;
nobukuma 0:003889bc474f 219 p[1] = scid;
nobukuma 0:003889bc474f 220 return Send(L2CAP_DISCONN_REQ,TXID,p,2);
nobukuma 0:003889bc474f 221 }
nobukuma 0:003889bc474f 222
nobukuma 0:003889bc474f 223 int BTDevice::ConfigureRequest(int dcid) {
nobukuma 0:003889bc474f 224 u16 p[4];
nobukuma 0:003889bc474f 225 p[0] = dcid;
nobukuma 0:003889bc474f 226 p[1] = 0;
nobukuma 0:003889bc474f 227 p[2] = 0x0201; // Options
nobukuma 0:003889bc474f 228 p[3] = min(0x02A0, MAX_ACL_SIZE); // my receiving MTU 672
nobukuma 0:003889bc474f 229 return Send(L2CAP_CONF_REQ,TXID,p,4);
nobukuma 0:003889bc474f 230 }
nobukuma 0:003889bc474f 231
nobukuma 0:003889bc474f 232 int BTDevice::CommandReject(u16 reason, u16 data0, u16 data1) {
nobukuma 0:003889bc474f 233 u16 p[3];
nobukuma 0:003889bc474f 234 p[0] = reason;
nobukuma 0:003889bc474f 235 p[1] = data0;
nobukuma 0:003889bc474f 236 p[2] = data1;
nobukuma 0:003889bc474f 237 int parlen = 2;
nobukuma 0:003889bc474f 238 switch (reason) {
nobukuma 0:003889bc474f 239 case 0: //command not understood
nobukuma 0:003889bc474f 240 break;
nobukuma 0:003889bc474f 241 case 1: //MTU exceeded
nobukuma 0:003889bc474f 242 parlen = 4; //return actual mtu in data
nobukuma 0:003889bc474f 243 break;
nobukuma 0:003889bc474f 244 case 2: //invalid CID
nobukuma 0:003889bc474f 245 parlen = 6; //return local, remote cid
nobukuma 0:003889bc474f 246 break;
nobukuma 0:003889bc474f 247 }
nobukuma 0:003889bc474f 248 return Send(L2CAP_COMMAND_REJ,TXID,p,parlen);
nobukuma 0:003889bc474f 249 }
nobukuma 0:003889bc474f 250
nobukuma 0:003889bc474f 251 int BTDevice::ConfigureResponse(u8 rxid, int dcid) {
nobukuma 0:003889bc474f 252 u16 p[3];
nobukuma 0:003889bc474f 253 p[0] = dcid; //source cid
nobukuma 0:003889bc474f 254 p[1] = 0; //flags (no continuation)
nobukuma 0:003889bc474f 255 p[2] = 0; //result (success)
nobukuma 0:003889bc474f 256 return Send(L2CAP_CONF_RSP,rxid,p,3);
nobukuma 0:003889bc474f 257 }
nobukuma 0:003889bc474f 258
nobukuma 0:003889bc474f 259 int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid) {
nobukuma 0:003889bc474f 260 u16 p[2];
nobukuma 0:003889bc474f 261 p[0] = dcid;
nobukuma 0:003889bc474f 262 p[1] = scid;
nobukuma 0:003889bc474f 263 return Send(L2CAP_DISCONN_RSP,rxid,p,2);
nobukuma 0:003889bc474f 264 }
nobukuma 0:003889bc474f 265
nobukuma 0:003889bc474f 266 void server(int socket, SocketState state, const u8* data, int len, void* userData) {
nobukuma 0:003889bc474f 267 // printfBytes("Server: ", data, len);
nobukuma 0:003889bc474f 268 if (state==SocketState_Open && len>0)
nobukuma 0:003889bc474f 269 SDP.SDPServer(socket, state, data, len, userData);
nobukuma 0:003889bc474f 270 }
nobukuma 0:003889bc474f 271
nobukuma 0:003889bc474f 272 void serserver(int socket, SocketState state, const u8* data, int len, void* userData) {
nobukuma 0:003889bc474f 273 printfBytes("serserver: ", data, len);
nobukuma 0:003889bc474f 274 SocketHandler *h = (SocketHandler*)userData;
nobukuma 0:003889bc474f 275 printf("userData refers to %s, state = %d\n", h->Name(), state);
nobukuma 0:003889bc474f 276 if (state==SocketState_Open) {
nobukuma 0:003889bc474f 277 if (len == 0) { //assume that the socket has just been opened and bind it to a new rfcomm server entity
nobukuma 0:003889bc474f 278 printf("Calling RFCOMMManager::BindSocket\n");
nobukuma 0:003889bc474f 279 rfcomm_manager.BindSocket(socket);
nobukuma 0:003889bc474f 280 } else {
nobukuma 0:003889bc474f 281 printf("Calling RFCOMMManager::SerServer\n");
nobukuma 0:003889bc474f 282 rfcomm_manager.SerServer(socket, state, data, len, userData);
nobukuma 0:003889bc474f 283 }
nobukuma 0:003889bc474f 284 } else if (state==SocketState_L2CAP_WaitDisconnect) {
nobukuma 0:003889bc474f 285 printf("Calling RFCOMMManager::SerServer\n");
nobukuma 0:003889bc474f 286 rfcomm_manager.SerServer(socket, state, data, len, userData);
nobukuma 0:003889bc474f 287 }
nobukuma 0:003889bc474f 288 }
nobukuma 0:003889bc474f 289
nobukuma 0:003889bc474f 290 //code8, tid8, lengthData16
nobukuma 0:003889bc474f 291 // 0, 1, 2, 3
nobukuma 0:003889bc474f 292 void BTDevice::Control(const u8* data, int len) { //control channel receive
nobukuma 0:003889bc474f 293 printf("\x1B[%dm", 31);
nobukuma 0:003889bc474f 294 int cc = data[0];//command code
nobukuma 0:003889bc474f 295 if (cc & 1) { //it is a response or a reject
nobukuma 0:003889bc474f 296 rtx.detach(); //kill the timeout
nobukuma 0:003889bc474f 297 //printf("timeout cancelled for handle %#x, txid=%d\n", _handle, data[1]);
nobukuma 0:003889bc474f 298 }
nobukuma 0:003889bc474f 299 printf(L2CAP_ComandCodeStr(cc));
nobukuma 0:003889bc474f 300 switch (cc) {
nobukuma 0:003889bc474f 301 case L2CAP_COMMAND_REJ://bad command, eg. MTU, check (reason)
nobukuma 0:003889bc474f 302 printf(" rejection reason=%d\n", LE16(data+4));
nobukuma 0:003889bc474f 303 break;
nobukuma 0:003889bc474f 304 case L2CAP_CONN_REQ://incoming connection request, not expected but should reply with proper rejection (or accept)
nobukuma 0:003889bc474f 305 //when a connection is accepted a new socket must be opened
nobukuma 0:003889bc474f 306 printf(" Remote side requested a connection\n");
nobukuma 0:003889bc474f 307 {
nobukuma 0:003889bc474f 308 int scid = LE16(data+6);
nobukuma 0:003889bc474f 309 int psm = LE16(data+4);
nobukuma 0:003889bc474f 310 int rxid = data[1];
nobukuma 0:003889bc474f 311 u16 p[4];
nobukuma 0:003889bc474f 312 p[0] = 0; //no dcid
nobukuma 0:003889bc474f 313 p[1] = scid;
nobukuma 0:003889bc474f 314 p[3] = 0; //no further information
nobukuma 0:003889bc474f 315 printf(" scid=%d, psm=%d\n", scid, psm);
nobukuma 0:003889bc474f 316 peer_mtu = 672; //default mtu
nobukuma 0:003889bc474f 317 int s = 0;
nobukuma 0:003889bc474f 318 switch (psm) {
nobukuma 0:003889bc474f 319 case L2CAP_PSM_SDP:
nobukuma 0:003889bc474f 320 s = Socket_Accept(SOCKET_SDP, scid, rxid, server, this);//allocate an sdp socket but use it as L2CAP
nobukuma 0:003889bc474f 321 break;
nobukuma 0:003889bc474f 322 case L2CAP_PSM_RFCOMM: //SOCKET_RFCOM;
nobukuma 0:003889bc474f 323 #if 0
nobukuma 0:003889bc474f 324 s = Socket_Accept(SOCKET_RFCOM, scid, rxid, serserver, this);//allocate an rfcomm socket
nobukuma 0:003889bc474f 325 //using L2CAP i.o. RFCOM makes little difference in processing but it also changes the handler to HCI i.o. RFCOMMManager
nobukuma 0:003889bc474f 326 #else
nobukuma 0:003889bc474f 327 //an RFCOMM requests comes in from a known (this) device
nobukuma 0:003889bc474f 328 //the channel is not yet known
nobukuma 0:003889bc474f 329 s = rfcomm_manager.FindSocket(this);//this should return 0 otherwise the remote device was asking a second rfcomm on the same device
nobukuma 0:003889bc474f 330 if (s==0) {
nobukuma 0:003889bc474f 331 printf("No connection to this device yet, allocate L2CAP Socket and accept\n");
nobukuma 0:003889bc474f 332 //accept the connection, even though there may be no listener???
nobukuma 0:003889bc474f 333 //have no choice because w/o acceptance no rfcomm req.
nobukuma 0:003889bc474f 334 s = Socket_Accept(SOCKET_L2CAP, scid, rxid, serserver, this);//allocate an l2cap socket
nobukuma 0:003889bc474f 335 //get a new l2cap socket, call HCI::Accept (fill in btdevice internals), then call BTDevice::Accept (send accept message)
nobukuma 0:003889bc474f 336 //serserver is called on state changes (SocketInternal::SetState) and on received packets from the peer device to the new l2cap handle
nobukuma 0:003889bc474f 337 //after sending the accept message, the devices will execute the normal l2cap connection state-machine
nobukuma 0:003889bc474f 338 //ending in a call to SetState(Open) which will invoke 'serserver' for the first time
nobukuma 0:003889bc474f 339 //or something like:
nobukuma 0:003889bc474f 340 // s = Socket_Create(SOCKET_L2CAP, serserver, this);//allocate an l2cap socket
nobukuma 0:003889bc474f 341 // Accept(GetSocketInternal(s), scid, rxid);//send accept response, this would bypass HCI::Accept()
nobukuma 0:003889bc474f 342 } else {
nobukuma 0:003889bc474f 343 printf("Already had an L2CAP connection on socket %d\n", s);
nobukuma 0:003889bc474f 344 }
nobukuma 0:003889bc474f 345 #endif
nobukuma 0:003889bc474f 346 break;
nobukuma 0:003889bc474f 347 default:
nobukuma 0:003889bc474f 348 printf("PSM %d not supported\n", psm);
nobukuma 0:003889bc474f 349 }
nobukuma 0:003889bc474f 350 switch (s) {
nobukuma 0:003889bc474f 351 case 0:
nobukuma 0:003889bc474f 352 printf("Not a valid socket\n");
nobukuma 0:003889bc474f 353 break;
nobukuma 0:003889bc474f 354 case ERR_SOCKET_TYPE_NOT_FOUND:
nobukuma 0:003889bc474f 355 p[2] = 2; //psm not supported
nobukuma 0:003889bc474f 356 Send(L2CAP_CONN_RSP,rxid,p,4);
nobukuma 0:003889bc474f 357 break;
nobukuma 0:003889bc474f 358 case ERR_SOCKET_NONE_LEFT:
nobukuma 0:003889bc474f 359 p[2] = 4; //no resources available
nobukuma 0:003889bc474f 360 Send(L2CAP_CONN_RSP,rxid,p,4);
nobukuma 0:003889bc474f 361 break;
nobukuma 0:003889bc474f 362 }
nobukuma 0:003889bc474f 363 }
nobukuma 0:003889bc474f 364 break;
nobukuma 0:003889bc474f 365 // Response to our initial connect from Remote
nobukuma 0:003889bc474f 366 case L2CAP_CONN_RSP: {
nobukuma 0:003889bc474f 367 int dcid = LE16(data+4);
nobukuma 0:003889bc474f 368 int scid = LE16(data+6);
nobukuma 0:003889bc474f 369 L2CAPSocket* s = SCIDToSocket(scid);
nobukuma 0:003889bc474f 370 int result = LE16(data+8);
nobukuma 0:003889bc474f 371 printf(" Result=%d, Status = %d\n", result, LE16(data+10));
nobukuma 0:003889bc474f 372 if (s->si.State != SocketState_L2CAP_WaitConnectRsp) {
nobukuma 0:003889bc474f 373 printf("Unexpected event ignored\n");
nobukuma 0:003889bc474f 374 break;
nobukuma 0:003889bc474f 375 }
nobukuma 0:003889bc474f 376 if (result == 0) {
nobukuma 0:003889bc474f 377 if (s) {
nobukuma 0:003889bc474f 378 s->si.State = SocketState_L2CAP_Config_wait;
nobukuma 0:003889bc474f 379 s->dcid = dcid;
nobukuma 0:003889bc474f 380 ConfigureRequest(dcid);
nobukuma 0:003889bc474f 381 s->si.State = SocketState_L2CAP_Config_wait_reqrsp;
nobukuma 0:003889bc474f 382 printf("Sent ConfigureRequest, state=WAIT_CONFIG_REQ_RSP\n");
nobukuma 0:003889bc474f 383 }
nobukuma 0:003889bc474f 384 } else if (result == 1) {//pending, stay in the present state
nobukuma 0:003889bc474f 385 } else {
nobukuma 0:003889bc474f 386 s->si.SetState(SocketState_Closed);
nobukuma 0:003889bc474f 387 printf("Connect failed\n");
nobukuma 0:003889bc474f 388 }
nobukuma 0:003889bc474f 389 }
nobukuma 0:003889bc474f 390 break;
nobukuma 0:003889bc474f 391
nobukuma 0:003889bc474f 392 case L2CAP_CONF_RSP: {
nobukuma 0:003889bc474f 393 int result = LE16(data+8);
nobukuma 0:003889bc474f 394 printf("Result=%d, datalen=%d, %smore conf to follow\n", result, LE16(data+2), LE16(data+6)?"":"No ");
nobukuma 0:003889bc474f 395 //should parse the config
nobukuma 0:003889bc474f 396 printfBytes("CONF RSP:", data, LE16(data+2)+4);
nobukuma 0:003889bc474f 397 int scid = LE16(data+4);
nobukuma 0:003889bc474f 398 SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
nobukuma 0:003889bc474f 399 if (s == 0) break;
nobukuma 0:003889bc474f 400 if (s->State != SocketState_L2CAP_Config_wait_reqrsp && s->State != SocketState_L2CAP_Config_wait_rsp) {
nobukuma 0:003889bc474f 401 printf("Unexpected event ignored\n");
nobukuma 0:003889bc474f 402 break;
nobukuma 0:003889bc474f 403 }
nobukuma 0:003889bc474f 404 if (result == 0) { //configuration acceptable
nobukuma 0:003889bc474f 405 if (s->State == SocketState_L2CAP_Config_wait_reqrsp) {
nobukuma 0:003889bc474f 406 s->State = SocketState_L2CAP_Config_wait_req;
nobukuma 0:003889bc474f 407 printf("State=WAIT_CONFIG_REQ\n");
nobukuma 0:003889bc474f 408 } else {
nobukuma 0:003889bc474f 409 ConfigureResponse(data[1],((L2CAPSocket*)s)->dcid);//data[1]==txid
nobukuma 0:003889bc474f 410 printf("Sent ConfigureResponse, state=Open\n");
nobukuma 0:003889bc474f 411 s->SetState(SocketState_Open);
nobukuma 0:003889bc474f 412 }
nobukuma 0:003889bc474f 413 } else {
nobukuma 0:003889bc474f 414 printf("Renegotiate configuration\n");
nobukuma 0:003889bc474f 415 }
nobukuma 0:003889bc474f 416 }
nobukuma 0:003889bc474f 417 break;
nobukuma 0:003889bc474f 418
nobukuma 0:003889bc474f 419 case L2CAP_CONF_REQ: {
nobukuma 0:003889bc474f 420 int len = LE16(data+2);
nobukuma 0:003889bc474f 421 int scid = LE16(data+4);//flags (data[6] LSB is continuation flag, data[10],[11] are the MTU
nobukuma 0:003889bc474f 422 int flags = LE16(data+6);
nobukuma 0:003889bc474f 423 const u8* conf = data+8;
nobukuma 0:003889bc474f 424 if (flags)
nobukuma 0:003889bc474f 425 printf("Warning! Continuation flag in L2CAP configuration not supported\n");
nobukuma 0:003889bc474f 426 L2CAPSocket* s = SCIDToSocket(scid);
nobukuma 0:003889bc474f 427 printfBytes("CONF REQ: ", data, LE16(data+2)+4);//data+8 contains option type 1-4 1=MTU, 2=flush timeout, 3=QoS, 4=FCM
nobukuma 0:003889bc474f 428 if (s == 0) break;
nobukuma 0:003889bc474f 429 if (s->si.State == SocketState_Closed ||
nobukuma 0:003889bc474f 430 s->si.State == SocketState_L2CAP_WaitConnectRsp ||
nobukuma 0:003889bc474f 431 s->si.State == SocketState_L2CAP_WaitDisconnect) {
nobukuma 0:003889bc474f 432 //Send Reject command
nobukuma 0:003889bc474f 433 printf("Connection should be rejected\n");
nobukuma 0:003889bc474f 434 break;
nobukuma 0:003889bc474f 435 }
nobukuma 0:003889bc474f 436 #if 0
nobukuma 0:003889bc474f 437 if (len > 4)
nobukuma 0:003889bc474f 438 switch (data[8]) {
nobukuma 0:003889bc474f 439 case 1:
nobukuma 0:003889bc474f 440 peer_mtu = LE16(data+10);
nobukuma 0:003889bc474f 441 printf("Peer L2CAP MTU = %d bytes\n", peer_mtu);
nobukuma 0:003889bc474f 442 break;
nobukuma 0:003889bc474f 443 case 2: //flush timeout
nobukuma 0:003889bc474f 444 case 3: //QOS
nobukuma 0:003889bc474f 445 case 4: //retrans and FC option
nobukuma 0:003889bc474f 446 default:
nobukuma 0:003889bc474f 447 printf("Unsupported configuration option %d, value = %#X\n", data[8], LE16(data+10));
nobukuma 0:003889bc474f 448 break;
nobukuma 0:003889bc474f 449 }
nobukuma 0:003889bc474f 450 else
nobukuma 0:003889bc474f 451 printf("Empty config req. peer_mtu = %d\n", peer_mtu);
nobukuma 0:003889bc474f 452 #else
nobukuma 0:003889bc474f 453 while (conf < data+len+4) {
nobukuma 0:003889bc474f 454 bool hint = conf[0] & 0x80;
nobukuma 0:003889bc474f 455 switch (conf[0] & 0x7F) {
nobukuma 0:003889bc474f 456 case 1:
nobukuma 0:003889bc474f 457 peer_mtu = LE16(conf+2);
nobukuma 0:003889bc474f 458 printf("Peer L2CAP MTU = %d bytes\n", peer_mtu);
nobukuma 0:003889bc474f 459 break;
nobukuma 0:003889bc474f 460 case 2: //flush timeout
nobukuma 0:003889bc474f 461 case 3: //QOS
nobukuma 0:003889bc474f 462 case 4: //retrans and FC option
nobukuma 0:003889bc474f 463 default:
nobukuma 0:003889bc474f 464 printf("Unsupported configuration option %d, value = %#X\n", conf[0], LE16(conf+2));
nobukuma 0:003889bc474f 465 break;
nobukuma 0:003889bc474f 466 }
nobukuma 0:003889bc474f 467 conf += conf[1]+2;
nobukuma 0:003889bc474f 468 }
nobukuma 0:003889bc474f 469 #endif
nobukuma 0:003889bc474f 470 if (1 /* options acceptable */) {
nobukuma 0:003889bc474f 471 if (flags == 0) {
nobukuma 0:003889bc474f 472 printf("Sending ConfigureResponse, old state=%d ", s->si.State);
nobukuma 0:003889bc474f 473 ConfigureResponse(data[1],s->dcid);//data[1]==txid, success
nobukuma 0:003889bc474f 474 switch (s->si.State) {
nobukuma 0:003889bc474f 475 case SocketState_L2CAP_Config_wait:
nobukuma 0:003889bc474f 476 s->si.State = SocketState_L2CAP_Config_wait_send;
nobukuma 0:003889bc474f 477 ConfigureRequest(s->dcid);
nobukuma 0:003889bc474f 478 s->si.State = SocketState_L2CAP_Config_wait_rsp;
nobukuma 0:003889bc474f 479 break;
nobukuma 0:003889bc474f 480 case SocketState_L2CAP_Config_wait_req:
nobukuma 0:003889bc474f 481 ((SocketInternal*)s)->SetState(SocketState_Open);
nobukuma 0:003889bc474f 482 break;
nobukuma 0:003889bc474f 483 case SocketState_L2CAP_Config_wait_rsp:
nobukuma 0:003889bc474f 484 break;
nobukuma 0:003889bc474f 485 case SocketState_L2CAP_Config_wait_reqrsp:
nobukuma 0:003889bc474f 486 s->si.State = SocketState_L2CAP_Config_wait_rsp;
nobukuma 0:003889bc474f 487 break;
nobukuma 0:003889bc474f 488 }
nobukuma 0:003889bc474f 489 printf("new state=%d\n", s->si.State);
nobukuma 0:003889bc474f 490 } else
nobukuma 0:003889bc474f 491 printf("L2CAP config continuation, delaying response...\n");
nobukuma 0:003889bc474f 492 } else { //options not acceptable
nobukuma 0:003889bc474f 493 printf("Configure failure should be indicated\n");
nobukuma 0:003889bc474f 494 ConfigureResponse(data[1],s->dcid);//indicates success but should indicate fail
nobukuma 0:003889bc474f 495 }
nobukuma 0:003889bc474f 496 }
nobukuma 0:003889bc474f 497 break;
nobukuma 0:003889bc474f 498 case L2CAP_DISCONN_REQ: {
nobukuma 0:003889bc474f 499 int dcid = LE16(data+4);
nobukuma 0:003889bc474f 500 int scid = LE16(data+6);
nobukuma 0:003889bc474f 501 L2CAPSocket* s = SCIDToSocket(dcid);
nobukuma 0:003889bc474f 502 if (s) {
nobukuma 0:003889bc474f 503 s->si.SetState(SocketState_Closed);
nobukuma 0:003889bc474f 504 DisconnectResponse(data[1], scid, dcid);
nobukuma 0:003889bc474f 505 } else {
nobukuma 0:003889bc474f 506 printf("request to disconnect cid %d fails, no such cid\n", dcid);
nobukuma 0:003889bc474f 507 CommandReject(0, dcid, scid);
nobukuma 0:003889bc474f 508 }
nobukuma 0:003889bc474f 509 }
nobukuma 0:003889bc474f 510 break;
nobukuma 0:003889bc474f 511 case L2CAP_DISCONN_RSP: {
nobukuma 0:003889bc474f 512 int scid = LE16(data+6);
nobukuma 0:003889bc474f 513 L2CAPSocket* s = SCIDToSocket(scid);
nobukuma 0:003889bc474f 514 if (s->si.State == SocketState_L2CAP_WaitDisconnect)
nobukuma 0:003889bc474f 515 s->si.SetState(SocketState_Closed);
nobukuma 0:003889bc474f 516 }
nobukuma 0:003889bc474f 517 break;
nobukuma 0:003889bc474f 518 default:
nobukuma 0:003889bc474f 519 printf("Unsupported L2CAP message %d\n", cc);
nobukuma 0:003889bc474f 520 }
nobukuma 0:003889bc474f 521 printf("\x1b[0m");
nobukuma 0:003889bc474f 522 }
nobukuma 0:003889bc474f 523
nobukuma 0:003889bc474f 524 void BTDevice::ACLFwd(const u8* data, int len) {
nobukuma 0:003889bc474f 525 if (l2cap_sock == 1)
nobukuma 0:003889bc474f 526 Control(data, len);
nobukuma 0:003889bc474f 527 else {
nobukuma 0:003889bc474f 528 SocketInternal* s = (SocketInternal*)SCIDToSocket(l2cap_sock);//in fact cid in the l2cap header
nobukuma 0:003889bc474f 529 if (s)
nobukuma 0:003889bc474f 530 s->Recv(data,len);//forward to the sockethandler for the type
nobukuma 0:003889bc474f 531 else
nobukuma 0:003889bc474f 532 printf("Bad event cid %d\n",l2cap_sock);
nobukuma 0:003889bc474f 533 }
nobukuma 0:003889bc474f 534 }
nobukuma 0:003889bc474f 535 //sometimes acl packets are segmented, in that case the l2cap payload length does not correspond to the acl pkt length
nobukuma 0:003889bc474f 536 //and the l2cap packet length. L2CAP works in basic mode and cannot be segmented hence the l2cap pkt size corresponds to
nobukuma 0:003889bc474f 537 //the acl pkt size
nobukuma 0:003889bc474f 538 int BTDevice::ACLRecv(const u8* data, int acllen) {
nobukuma 0:003889bc474f 539 //printfBytes("L2CP",data,acllen);
nobukuma 0:003889bc474f 540 //cntr_cred--;
nobukuma 0:003889bc474f 541 u16 handle = LE16(data);
nobukuma 0:003889bc474f 542 if ((handle&0x0fff) != _handle) {
nobukuma 0:003889bc474f 543 printf("unexpected handle %#x, this _handle=%#x\n", handle, _handle);
nobukuma 0:003889bc474f 544 return 1;
nobukuma 0:003889bc474f 545 }
nobukuma 0:003889bc474f 546 //below is the ACL packet recombination engine
nobukuma 0:003889bc474f 547 char pb = (handle>>12) & 3;
nobukuma 0:003889bc474f 548 if (pb == 2)
nobukuma 0:003889bc474f 549 segments = 1;
nobukuma 0:003889bc474f 550 else
nobukuma 0:003889bc474f 551 segments++;
nobukuma 0:003889bc474f 552 int p = 4; //start of l2cap packet
nobukuma 0:003889bc474f 553 int len = LE16(data+2); //length of l2cap pkt
nobukuma 0:003889bc474f 554 while (p < acllen)
nobukuma 0:003889bc474f 555 switch (contState) {
nobukuma 0:003889bc474f 556 case 0://allow even for fragmented length field
nobukuma 0:003889bc474f 557 plen = data[p++];//payload length lsb
nobukuma 0:003889bc474f 558 contState = 1;
nobukuma 0:003889bc474f 559 break;
nobukuma 0:003889bc474f 560 case 1:
nobukuma 0:003889bc474f 561 plen += data[p++]<<8; //payload length msb
nobukuma 0:003889bc474f 562 if (pb == 2 && plen == acllen-8) {//normal case, l2cap pkt is contained completely in this hci pkt
nobukuma 0:003889bc474f 563 l2cap_sock = data[p] + (data[p+1]<<8);
nobukuma 0:003889bc474f 564 contState = 0;
nobukuma 0:003889bc474f 565 ACLFwd(data+8, plen); //forward the packet in its original buffer
nobukuma 0:003889bc474f 566 return segments; //all data was dealt with
nobukuma 0:003889bc474f 567 } else { //packet is segmented
nobukuma 0:003889bc474f 568 printf("ACL packet is segmented\n");
nobukuma 0:003889bc474f 569 contState = 2;
nobukuma 0:003889bc474f 570 contBuf = new unsigned char[plen];//allocate recombination buffer
nobukuma 0:003889bc474f 571 contPos = 0;
nobukuma 0:003889bc474f 572 }
nobukuma 0:003889bc474f 573 break;
nobukuma 0:003889bc474f 574 case 2:
nobukuma 0:003889bc474f 575 l2cap_sock = data[p++];
nobukuma 0:003889bc474f 576 contState = 3;
nobukuma 0:003889bc474f 577 break;
nobukuma 0:003889bc474f 578 case 3:
nobukuma 0:003889bc474f 579 l2cap_sock += data[p++]<<8;
nobukuma 0:003889bc474f 580 contState = 4;
nobukuma 0:003889bc474f 581 break;
nobukuma 0:003889bc474f 582 case 4: //data, recombine segmented ACL (not l2cap!) frames
nobukuma 0:003889bc474f 583 if (contPos < plen) {//buffer not yet full
nobukuma 0:003889bc474f 584 int datalen = acllen - p; //data in this incoming pkt
nobukuma 0:003889bc474f 585 int remcap = plen - contPos; //remaining capacity in the recombination buffer
nobukuma 0:003889bc474f 586 if (datalen <= remcap) {
nobukuma 0:003889bc474f 587 memcpy(contBuf+contPos, data+p, datalen);
nobukuma 0:003889bc474f 588 contPos += datalen;
nobukuma 0:003889bc474f 589 p = acllen;//end of data, stop the while loop
nobukuma 0:003889bc474f 590 if (contPos == plen) {//buffer is full now
nobukuma 0:003889bc474f 591 printfBytes("Recombined packet is:", contBuf, plen);
nobukuma 0:003889bc474f 592 ACLFwd(contBuf, plen); //forward the recombination buffer
nobukuma 0:003889bc474f 593 delete[] contBuf;//and free the buffer
nobukuma 0:003889bc474f 594 contState = 0;
nobukuma 0:003889bc474f 595 return segments;
nobukuma 0:003889bc474f 596 }//else stay in this state to wait for the rest
nobukuma 0:003889bc474f 597 } else {//data contains (part of) next packet, never seen this happen
nobukuma 0:003889bc474f 598 memcpy(contBuf+contPos, data+p, plen-contPos);//this packet is complete
nobukuma 0:003889bc474f 599 p += plen-contPos;
nobukuma 0:003889bc474f 600 printfBytes("Recombined packet is:", contBuf, plen);
nobukuma 0:003889bc474f 601 printfBytes("Next packet starts with:", data+p, acllen-p);
nobukuma 0:003889bc474f 602 ACLFwd(contBuf, plen); //forward the recombination buffer
nobukuma 0:003889bc474f 603 delete[] contBuf;//and free the buffer
nobukuma 0:003889bc474f 604 contState = 0; //continue with the next packet
nobukuma 0:003889bc474f 605 }
nobukuma 0:003889bc474f 606 } else {
nobukuma 0:003889bc474f 607 printf("Cannot append to buffer (size=%d, pos=%d, datalen = %d)\n", plen, contPos, len-p);
nobukuma 0:003889bc474f 608 contState = 0;
nobukuma 0:003889bc474f 609 return segments;//flushed
nobukuma 0:003889bc474f 610 }
nobukuma 0:003889bc474f 611 break;
nobukuma 0:003889bc474f 612 }//switch (and while)
nobukuma 0:003889bc474f 613 return 0;//the buffers are not processed yet
nobukuma 0:003889bc474f 614 }