Sami Alshorman
Pulse Oximeter (NONIN) communicates with mbed via Bluetooth dongle and sends Heart Rate and Oxygen Saturation via GPRS module
Dependencies: C12832 GPS GSM mbed
Fork of
myBlueUSB_localfix
by 
Nobuaki Aoki
Diff: L2CAP.cpp
- Revision:
- 0:003889bc474f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/L2CAP.cpp Sat Dec 07 14:19:00 2013 +0000
@@ -0,0 +1,614 @@
+/*
+Copyright (c) 2010 Peter Barrett
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*/
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "Utils.h"
+#include "hci.h"
+#include "HCITransportUSB.h"
+#include "sdp.h"
+#include "RFCOMM.h"
+
+#define L2CAP_COMMAND_REJ 0x01
+#define L2CAP_CONN_REQ 0x02
+#define L2CAP_CONN_RSP 0x03
+#define L2CAP_CONF_REQ 0x04
+#define L2CAP_CONF_RSP 0x05
+#define L2CAP_DISCONN_REQ 0x06
+#define L2CAP_DISCONN_RSP 0x07
+#define L2CAP_ECHO_REQ 0x08
+#define L2CAP_ECHO_RSP 0x09
+#define L2CAP_INFO_REQ 0x0a
+#define L2CAP_INFO_RSP 0x0b
+
+#define TXID (++_txid?_txid:1)
+//template <class T> T min(T a, T b) { return a<b ? a : b;}
+
+/* L2CAP command codes */
+const char* L2CAP_ComandCodeStr(int c) {
+ switch (c) {
+ case L2CAP_COMMAND_REJ:
+ return "L2CAP_COMMAND_REJ";
+ case L2CAP_CONN_REQ:
+ return "L2CAP_CONN_REQ";
+ case L2CAP_CONN_RSP:
+ return "L2CAP_CONN_RSP";
+ case L2CAP_CONF_REQ:
+ return "L2CAP_CONF_REQ";
+ case L2CAP_CONF_RSP:
+ return "L2CAP_CONF_RSP";
+ case L2CAP_DISCONN_REQ:
+ return "L2CAP_DISCONN_REQ";
+ case L2CAP_DISCONN_RSP:
+ return "L2CAP_DISCONN_RSP";
+ case L2CAP_ECHO_REQ:
+ return "L2CAP_ECHO_REQ";
+ case L2CAP_ECHO_RSP:
+ return "L2CAP_ECHO_RSP";
+ case L2CAP_INFO_REQ:
+ return "L2CAP_INFO_REQ";
+ case L2CAP_INFO_RSP:
+ return "L2CAP_INFO_RSP";
+ }
+ return "unknown";
+}
+
+#define OFFSET 8 //means the buffer also has space for the l2cap/hci headers and need not be allocated and copied
+//#define OFFSET 0 //means the buffer only has space for the payload which need to be copied
+#if OFFSET == 0
+#define L2CAPBUFSIZE 128
+#else
+#define L2CAPBUFSIZE 0
+#endif
+
+typedef struct {
+ u16 handle;
+ u16 length; // total
+ u16 l2capLength; // length -4
+ u16 cid; // Signaling packet CID = 1
+ u8 data[L2CAPBUFSIZE]; // Largest thing to send!!! todo
+} L2CAPData;
+
+//
+void BTDevice::Init() {
+ memset(&_info,0,sizeof(inquiry_info));
+ _handle = 0;
+ _name[0] = 0;
+ _state = 0;
+ _txid = 1;
+ //cntr_cred = 1;
+}
+
+// virtual SocketHandler
+int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr) {
+ L2CAPSocket* s = (L2CAPSocket*)sock;
+ L2CAPAddr* a = (L2CAPAddr*)addr;
+ s->scid = 0x40 + sock->ID-1; // are these reserved?
+ s->dcid = 0;
+ Connect(s->scid,a->psm);
+ sock->State = SocketState_L2CAP_WaitConnectRsp;
+ contState = 0;
+ return sock->ID;
+}
+
+// virtual SocketHandler
+int BTDevice::Accept(SocketInternal* sock, int scid, int rxid) {
+ L2CAPSocket* s = (L2CAPSocket*)sock;
+ s->scid = 0x40 + sock->ID-1; // are these reserved?
+ s->dcid = scid;
+ u16 p[4];
+ p[0] = s->scid;
+ p[1] = scid;
+ p[2] = 0; //success
+ p[3] = 0; //no further information
+ Send(L2CAP_CONN_RSP,rxid,p,4);
+ printf("send conn_rsp with dcid=%#x and scid=%#x\n", p[0],p[1]);
+ sock->State = SocketState_L2CAP_Config_wait;
+ contState = 0;
+ return sock->ID;
+}
+
+// virtual SocketHandler, called from HCI which is ABOVE L2CAP
+int BTDevice::Send(SocketInternal* sock, const u8* data, int len) {
+ L2CAPSocket* s = (L2CAPSocket*)sock;
+#if OFFSET == 8 //sizeof L2CAPData header
+ L2CAPData &d = *const_cast<L2CAPData*>((L2CAPData*)data);
+#else
+ L2CAPData d;
+#endif
+ if (len > peer_mtu) {//mtu concerns the l2cap mtu, because we use basic mode we cannot segment
+ printf("MTU (%d) for outgoing packet (%d) exceeded\n", peer_mtu, len);
+ return 0;
+ }
+ d.handle = _handle | 0x2000;
+ d.length = 4 + len - OFFSET;
+ d.l2capLength = len - OFFSET;
+ d.cid = s->dcid;
+ //printf("cid=%d: ", d.cid);
+ //printfBytes("sending: ", data, len);
+#if OFFSET == 0
+ if (len > L2CAPBUFSIZE)
+ return -1;
+ memcpy(d.data,data,len);
+ return Send((u8*)&d,len+8);
+#else
+ return Send(data, len);
+#endif
+}
+
+// virtual SocketHandler
+int BTDevice::Close(SocketInternal* sock) {
+ printf("L2CAP close %d\n",sock->ID);
+ sock->State = SocketState_L2CAP_WaitDisconnect;
+ L2CAPSocket* s = (L2CAPSocket*)sock;
+ return Disconnect(s->scid,s->dcid);
+}
+
+L2CAPSocket* BTDevice::SCIDToSocket(int scid) {
+ return (L2CAPSocket*)GetSocketInternal(scid-0x40+1);
+}
+
+int BTDevice::Send(const u8* data, int len) {//printfBytes("Transport : ", data, len);
+#ifdef HOST_CONTR_FLOW
+ pkts_sent++;
+#endif
+ _transport->ACLSend(data,len);
+ return 0;
+}
+
+void BTDevice::repeat_cmd() {
+ printf("Cmd on handle %#x timed out, resending txid=%d\n", _handle, last_req.id);
+// Send ((u8*)&last_req, last_req.length+4);//danger! interrupt context, Send is not reentrant
+ //optionally set new larger timeout
+}
+
+int BTDevice::Send(u8 c, u8 id, u16* params, int count) {
+ L2CAPCmd cmd;
+ cmd.handle = _handle | 0x2000;
+ cmd.length = 8 + count*2;
+
+ cmd.l2capLength = cmd.length-4;
+ cmd.cid = 1; // Signaling packet
+
+ cmd.cmd = c;
+ cmd.id = id;
+ cmd.cmdLength = count*2;
+ for (int i = 0; i < count; i++)
+ cmd.params[i] = params[i];
+ if ((c & 1) == 0) { //this is a request
+ last_req = cmd;
+ rtx.attach(this, &BTDevice::repeat_cmd, 30.0);
+ //printf("Starting timeout for %#x, txid=%d\n", _handle, id);
+ }
+ return Send((u8*)&cmd,cmd.length+4);
+}
+
+int BTDevice::Connect(int scid, int psm) {
+ u16 p[2];
+ p[0] = psm;
+ p[1] = scid;
+ return Send(L2CAP_CONN_REQ,TXID,p,2);
+}
+
+int BTDevice::Disconnect(int scid, int dcid) {
+ u16 p[2];
+ p[0] = dcid;
+ p[1] = scid;
+ return Send(L2CAP_DISCONN_REQ,TXID,p,2);
+}
+
+int BTDevice::ConfigureRequest(int dcid) {
+ u16 p[4];
+ p[0] = dcid;
+ p[1] = 0;
+ p[2] = 0x0201; // Options
+ p[3] = min(0x02A0, MAX_ACL_SIZE); // my receiving MTU 672
+ return Send(L2CAP_CONF_REQ,TXID,p,4);
+}
+
+int BTDevice::CommandReject(u16 reason, u16 data0, u16 data1) {
+ u16 p[3];
+ p[0] = reason;
+ p[1] = data0;
+ p[2] = data1;
+ int parlen = 2;
+ switch (reason) {
+ case 0: //command not understood
+ break;
+ case 1: //MTU exceeded
+ parlen = 4; //return actual mtu in data
+ break;
+ case 2: //invalid CID
+ parlen = 6; //return local, remote cid
+ break;
+ }
+ return Send(L2CAP_COMMAND_REJ,TXID,p,parlen);
+}
+
+int BTDevice::ConfigureResponse(u8 rxid, int dcid) {
+ u16 p[3];
+ p[0] = dcid; //source cid
+ p[1] = 0; //flags (no continuation)
+ p[2] = 0; //result (success)
+ return Send(L2CAP_CONF_RSP,rxid,p,3);
+}
+
+int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid) {
+ u16 p[2];
+ p[0] = dcid;
+ p[1] = scid;
+ return Send(L2CAP_DISCONN_RSP,rxid,p,2);
+}
+
+void server(int socket, SocketState state, const u8* data, int len, void* userData) {
+ // printfBytes("Server: ", data, len);
+ if (state==SocketState_Open && len>0)
+ SDP.SDPServer(socket, state, data, len, userData);
+}
+
+void serserver(int socket, SocketState state, const u8* data, int len, void* userData) {
+ printfBytes("serserver: ", data, len);
+ SocketHandler *h = (SocketHandler*)userData;
+ printf("userData refers to %s, state = %d\n", h->Name(), state);
+ if (state==SocketState_Open) {
+ if (len == 0) { //assume that the socket has just been opened and bind it to a new rfcomm server entity
+ printf("Calling RFCOMMManager::BindSocket\n");
+ rfcomm_manager.BindSocket(socket);
+ } else {
+ printf("Calling RFCOMMManager::SerServer\n");
+ rfcomm_manager.SerServer(socket, state, data, len, userData);
+ }
+ } else if (state==SocketState_L2CAP_WaitDisconnect) {
+ printf("Calling RFCOMMManager::SerServer\n");
+ rfcomm_manager.SerServer(socket, state, data, len, userData);
+ }
+}
+
+//code8, tid8, lengthData16
+// 0, 1, 2, 3
+void BTDevice::Control(const u8* data, int len) { //control channel receive
+ printf("\x1B[%dm", 31);
+ int cc = data[0];//command code
+ if (cc & 1) { //it is a response or a reject
+ rtx.detach(); //kill the timeout
+ //printf("timeout cancelled for handle %#x, txid=%d\n", _handle, data[1]);
+ }
+ printf(L2CAP_ComandCodeStr(cc));
+ switch (cc) {
+ case L2CAP_COMMAND_REJ://bad command, eg. MTU, check (reason)
+ printf(" rejection reason=%d\n", LE16(data+4));
+ break;
+ case L2CAP_CONN_REQ://incoming connection request, not expected but should reply with proper rejection (or accept)
+ //when a connection is accepted a new socket must be opened
+ printf(" Remote side requested a connection\n");
+ {
+ int scid = LE16(data+6);
+ int psm = LE16(data+4);
+ int rxid = data[1];
+ u16 p[4];
+ p[0] = 0; //no dcid
+ p[1] = scid;
+ p[3] = 0; //no further information
+ printf(" scid=%d, psm=%d\n", scid, psm);
+ peer_mtu = 672; //default mtu
+ int s = 0;
+ switch (psm) {
+ case L2CAP_PSM_SDP:
+ s = Socket_Accept(SOCKET_SDP, scid, rxid, server, this);//allocate an sdp socket but use it as L2CAP
+ break;
+ case L2CAP_PSM_RFCOMM: //SOCKET_RFCOM;
+#if 0
+ s = Socket_Accept(SOCKET_RFCOM, scid, rxid, serserver, this);//allocate an rfcomm socket
+ //using L2CAP i.o. RFCOM makes little difference in processing but it also changes the handler to HCI i.o. RFCOMMManager
+#else
+//an RFCOMM requests comes in from a known (this) device
+//the channel is not yet known
+ s = rfcomm_manager.FindSocket(this);//this should return 0 otherwise the remote device was asking a second rfcomm on the same device
+ if (s==0) {
+ printf("No connection to this device yet, allocate L2CAP Socket and accept\n");
+ //accept the connection, even though there may be no listener???
+ //have no choice because w/o acceptance no rfcomm req.
+ s = Socket_Accept(SOCKET_L2CAP, scid, rxid, serserver, this);//allocate an l2cap socket
+ //get a new l2cap socket, call HCI::Accept (fill in btdevice internals), then call BTDevice::Accept (send accept message)
+ //serserver is called on state changes (SocketInternal::SetState) and on received packets from the peer device to the new l2cap handle
+ //after sending the accept message, the devices will execute the normal l2cap connection state-machine
+ //ending in a call to SetState(Open) which will invoke 'serserver' for the first time
+//or something like:
+// s = Socket_Create(SOCKET_L2CAP, serserver, this);//allocate an l2cap socket
+// Accept(GetSocketInternal(s), scid, rxid);//send accept response, this would bypass HCI::Accept()
+ } else {
+ printf("Already had an L2CAP connection on socket %d\n", s);
+ }
+#endif
+ break;
+ default:
+ printf("PSM %d not supported\n", psm);
+ }
+ switch (s) {
+ case 0:
+ printf("Not a valid socket\n");
+ break;
+ case ERR_SOCKET_TYPE_NOT_FOUND:
+ p[2] = 2; //psm not supported
+ Send(L2CAP_CONN_RSP,rxid,p,4);
+ break;
+ case ERR_SOCKET_NONE_LEFT:
+ p[2] = 4; //no resources available
+ Send(L2CAP_CONN_RSP,rxid,p,4);
+ break;
+ }
+ }
+ break;
+ // Response to our initial connect from Remote
+ case L2CAP_CONN_RSP: {
+ int dcid = LE16(data+4);
+ int scid = LE16(data+6);
+ L2CAPSocket* s = SCIDToSocket(scid);
+ int result = LE16(data+8);
+ printf(" Result=%d, Status = %d\n", result, LE16(data+10));
+ if (s->si.State != SocketState_L2CAP_WaitConnectRsp) {
+ printf("Unexpected event ignored\n");
+ break;
+ }
+ if (result == 0) {
+ if (s) {
+ s->si.State = SocketState_L2CAP_Config_wait;
+ s->dcid = dcid;
+ ConfigureRequest(dcid);
+ s->si.State = SocketState_L2CAP_Config_wait_reqrsp;
+ printf("Sent ConfigureRequest, state=WAIT_CONFIG_REQ_RSP\n");
+ }
+ } else if (result == 1) {//pending, stay in the present state
+ } else {
+ s->si.SetState(SocketState_Closed);
+ printf("Connect failed\n");
+ }
+ }
+ break;
+
+ case L2CAP_CONF_RSP: {
+ int result = LE16(data+8);
+ printf("Result=%d, datalen=%d, %smore conf to follow\n", result, LE16(data+2), LE16(data+6)?"":"No ");
+ //should parse the config
+ printfBytes("CONF RSP:", data, LE16(data+2)+4);
+ int scid = LE16(data+4);
+ SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
+ if (s == 0) break;
+ if (s->State != SocketState_L2CAP_Config_wait_reqrsp && s->State != SocketState_L2CAP_Config_wait_rsp) {
+ printf("Unexpected event ignored\n");
+ break;
+ }
+ if (result == 0) { //configuration acceptable
+ if (s->State == SocketState_L2CAP_Config_wait_reqrsp) {
+ s->State = SocketState_L2CAP_Config_wait_req;
+ printf("State=WAIT_CONFIG_REQ\n");
+ } else {
+ ConfigureResponse(data[1],((L2CAPSocket*)s)->dcid);//data[1]==txid
+ printf("Sent ConfigureResponse, state=Open\n");
+ s->SetState(SocketState_Open);
+ }
+ } else {
+ printf("Renegotiate configuration\n");
+ }
+ }
+ break;
+
+ case L2CAP_CONF_REQ: {
+ int len = LE16(data+2);
+ int scid = LE16(data+4);//flags (data[6] LSB is continuation flag, data[10],[11] are the MTU
+ int flags = LE16(data+6);
+ const u8* conf = data+8;
+ if (flags)
+ printf("Warning! Continuation flag in L2CAP configuration not supported\n");
+ L2CAPSocket* s = SCIDToSocket(scid);
+ printfBytes("CONF REQ: ", data, LE16(data+2)+4);//data+8 contains option type 1-4 1=MTU, 2=flush timeout, 3=QoS, 4=FCM
+ if (s == 0) break;
+ if (s->si.State == SocketState_Closed ||
+ s->si.State == SocketState_L2CAP_WaitConnectRsp ||
+ s->si.State == SocketState_L2CAP_WaitDisconnect) {
+ //Send Reject command
+ printf("Connection should be rejected\n");
+ break;
+ }
+#if 0
+ if (len > 4)
+ switch (data[8]) {
+ case 1:
+ peer_mtu = LE16(data+10);
+ printf("Peer L2CAP MTU = %d bytes\n", peer_mtu);
+ break;
+ case 2: //flush timeout
+ case 3: //QOS
+ case 4: //retrans and FC option
+ default:
+ printf("Unsupported configuration option %d, value = %#X\n", data[8], LE16(data+10));
+ break;
+ }
+ else
+ printf("Empty config req. peer_mtu = %d\n", peer_mtu);
+#else
+ while (conf < data+len+4) {
+ bool hint = conf[0] & 0x80;
+ switch (conf[0] & 0x7F) {
+ case 1:
+ peer_mtu = LE16(conf+2);
+ printf("Peer L2CAP MTU = %d bytes\n", peer_mtu);
+ break;
+ case 2: //flush timeout
+ case 3: //QOS
+ case 4: //retrans and FC option
+ default:
+ printf("Unsupported configuration option %d, value = %#X\n", conf[0], LE16(conf+2));
+ break;
+ }
+ conf += conf[1]+2;
+ }
+#endif
+ if (1 /* options acceptable */) {
+ if (flags == 0) {
+ printf("Sending ConfigureResponse, old state=%d ", s->si.State);
+ ConfigureResponse(data[1],s->dcid);//data[1]==txid, success
+ switch (s->si.State) {
+ case SocketState_L2CAP_Config_wait:
+ s->si.State = SocketState_L2CAP_Config_wait_send;
+ ConfigureRequest(s->dcid);
+ s->si.State = SocketState_L2CAP_Config_wait_rsp;
+ break;
+ case SocketState_L2CAP_Config_wait_req:
+ ((SocketInternal*)s)->SetState(SocketState_Open);
+ break;
+ case SocketState_L2CAP_Config_wait_rsp:
+ break;
+ case SocketState_L2CAP_Config_wait_reqrsp:
+ s->si.State = SocketState_L2CAP_Config_wait_rsp;
+ break;
+ }
+ printf("new state=%d\n", s->si.State);
+ } else
+ printf("L2CAP config continuation, delaying response...\n");
+ } else { //options not acceptable
+ printf("Configure failure should be indicated\n");
+ ConfigureResponse(data[1],s->dcid);//indicates success but should indicate fail
+ }
+ }
+ break;
+ case L2CAP_DISCONN_REQ: {
+ int dcid = LE16(data+4);
+ int scid = LE16(data+6);
+ L2CAPSocket* s = SCIDToSocket(dcid);
+ if (s) {
+ s->si.SetState(SocketState_Closed);
+ DisconnectResponse(data[1], scid, dcid);
+ } else {
+ printf("request to disconnect cid %d fails, no such cid\n", dcid);
+ CommandReject(0, dcid, scid);
+ }
+ }
+ break;
+ case L2CAP_DISCONN_RSP: {
+ int scid = LE16(data+6);
+ L2CAPSocket* s = SCIDToSocket(scid);
+ if (s->si.State == SocketState_L2CAP_WaitDisconnect)
+ s->si.SetState(SocketState_Closed);
+ }
+ break;
+ default:
+ printf("Unsupported L2CAP message %d\n", cc);
+ }
+ printf("\x1b[0m");
+}
+
+void BTDevice::ACLFwd(const u8* data, int len) {
+ if (l2cap_sock == 1)
+ Control(data, len);
+ else {
+ SocketInternal* s = (SocketInternal*)SCIDToSocket(l2cap_sock);//in fact cid in the l2cap header
+ if (s)
+ s->Recv(data,len);//forward to the sockethandler for the type
+ else
+ printf("Bad event cid %d\n",l2cap_sock);
+ }
+}
+//sometimes acl packets are segmented, in that case the l2cap payload length does not correspond to the acl pkt length
+//and the l2cap packet length. L2CAP works in basic mode and cannot be segmented hence the l2cap pkt size corresponds to
+//the acl pkt size
+int BTDevice::ACLRecv(const u8* data, int acllen) {
+ //printfBytes("L2CP",data,acllen);
+ //cntr_cred--;
+ u16 handle = LE16(data);
+ if ((handle&0x0fff) != _handle) {
+ printf("unexpected handle %#x, this _handle=%#x\n", handle, _handle);
+ return 1;
+ }
+ //below is the ACL packet recombination engine
+ char pb = (handle>>12) & 3;
+ if (pb == 2)
+ segments = 1;
+ else
+ segments++;
+ int p = 4; //start of l2cap packet
+ int len = LE16(data+2); //length of l2cap pkt
+ while (p < acllen)
+ switch (contState) {
+ case 0://allow even for fragmented length field
+ plen = data[p++];//payload length lsb
+ contState = 1;
+ break;
+ case 1:
+ plen += data[p++]<<8; //payload length msb
+ if (pb == 2 && plen == acllen-8) {//normal case, l2cap pkt is contained completely in this hci pkt
+ l2cap_sock = data[p] + (data[p+1]<<8);
+ contState = 0;
+ ACLFwd(data+8, plen); //forward the packet in its original buffer
+ return segments; //all data was dealt with
+ } else { //packet is segmented
+ printf("ACL packet is segmented\n");
+ contState = 2;
+ contBuf = new unsigned char[plen];//allocate recombination buffer
+ contPos = 0;
+ }
+ break;
+ case 2:
+ l2cap_sock = data[p++];
+ contState = 3;
+ break;
+ case 3:
+ l2cap_sock += data[p++]<<8;
+ contState = 4;
+ break;
+ case 4: //data, recombine segmented ACL (not l2cap!) frames
+ if (contPos < plen) {//buffer not yet full
+ int datalen = acllen - p; //data in this incoming pkt
+ int remcap = plen - contPos; //remaining capacity in the recombination buffer
+ if (datalen <= remcap) {
+ memcpy(contBuf+contPos, data+p, datalen);
+ contPos += datalen;
+ p = acllen;//end of data, stop the while loop
+ if (contPos == plen) {//buffer is full now
+ printfBytes("Recombined packet is:", contBuf, plen);
+ ACLFwd(contBuf, plen); //forward the recombination buffer
+ delete[] contBuf;//and free the buffer
+ contState = 0;
+ return segments;
+ }//else stay in this state to wait for the rest
+ } else {//data contains (part of) next packet, never seen this happen
+ memcpy(contBuf+contPos, data+p, plen-contPos);//this packet is complete
+ p += plen-contPos;
+ printfBytes("Recombined packet is:", contBuf, plen);
+ printfBytes("Next packet starts with:", data+p, acllen-p);
+ ACLFwd(contBuf, plen); //forward the recombination buffer
+ delete[] contBuf;//and free the buffer
+ contState = 0; //continue with the next packet
+ }
+ } else {
+ printf("Cannot append to buffer (size=%d, pos=%d, datalen = %d)\n", plen, contPos, len-p);
+ contState = 0;
+ return segments;//flushed
+ }
+ break;
+ }//switch (and while)
+ return 0;//the buffers are not processed yet
+}