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
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?
User | Revision | Line number | New 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 | } |