Ad van der Weiden / Mbed 2 deprecated myBlueUSB

Test version of BlueUSB stack. Includes SDP and RFCOMM. As Client it allows to connect to my fischertechnik TX Controller. As Server it echo\\\\\\\'s characters to Putty. PIN=1234

Dependencies:   mbed myUSBHost AvailableMemory

Dependents:   mbed_TANK_Kinect myBlueUSB_ros ftusbClass

L2CAP.cpp@12:57f7679dd651, 2011-06-19 (annotated)

Committer:
networker
Date:
Sun Jun 19 19:32:51 2011 +0000
Revision:
12:57f7679dd651
Parent:
10:1928a3d02e32 
fixed default mtu, small impovement in config processing

Who changed what in which revision?

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