Sami Alshorman / Mbed 2 deprecated BluetoothNONIN

Pulse Oximeter (NONIN) communicates with mbed via Bluetooth dongle and sends Heart Rate and Oxygen Saturation via GPRS module

Dependencies:   C12832 GPS GSM mbed

Fork of myBlueUSB_localfix by Nobuaki Aoki

L2CAP.cpp@3:55a622e3dbb5, 2015-04-14 (annotated)

Committer:
samialshorman
Date:
Tue Apr 14 21:48:07 2015 +0000
Revision:
3:55a622e3dbb5
Parent:
0:003889bc474f 
Nonin (Pulse Oximeter) connected to mbed lpc 1768 by Bluetooth dongle and sends SMS including Heart Rate and Oxygen saturation by GPRS module

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 }