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@0:81ed8b6e4a8b, 2011-04-04 (annotated)

Committer:
networker
Date:
Mon Apr 04 16:41:03 2011 +0000
Revision:
0:81ed8b6e4a8b
initial revision

Who changed what in which revision?

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