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@0:003889bc474f, 2013-12-07 (annotated)
- Committer:
- nobukuma
- Date:
- Sat Dec 07 14:19:00 2013 +0000
- Revision:
- 0:003889bc474f
http://mbed.org/users/networker/code/myBlueUSB/ rev13??rev12??????????
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 | } |