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