L2CAP.cpp

00001 /*
00002 Copyright (c) 2010 Peter Barrett
00003 
00004 Permission is hereby granted, free of charge, to any person obtaining a copy
00005 of this software and associated documentation files (the "Software"), to deal
00006 in the Software without restriction, including without limitation the rights
00007 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008 copies of the Software, and to permit persons to whom the Software is
00009 furnished to do so, subject to the following conditions:
00010 
00011 The above copyright notice and this permission notice shall be included in
00012 all copies or substantial portions of the Software.
00013 
00014 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020 THE SOFTWARE.
00021 */
00022 
00023 
00024 #include <stdio.h>
00025 #include <stdlib.h>
00026 #include <stdio.h>
00027 #include <string.h>
00028 
00029 #include "Utils.h"
00030 #include "hci.h"
00031 #include "HCITransportUSB.h"
00032 #include "sdp.h"
00033 #include "RFCOMM.h"
00034 
00035 #define L2CAP_COMMAND_REJ       0x01
00036 #define L2CAP_CONN_REQ          0x02
00037 #define L2CAP_CONN_RSP          0x03
00038 #define L2CAP_CONF_REQ          0x04
00039 #define L2CAP_CONF_RSP          0x05
00040 #define L2CAP_DISCONN_REQ       0x06
00041 #define L2CAP_DISCONN_RSP       0x07
00042 #define L2CAP_ECHO_REQ          0x08
00043 #define L2CAP_ECHO_RSP          0x09
00044 #define L2CAP_INFO_REQ          0x0a
00045 #define L2CAP_INFO_RSP          0x0b
00046 
00047 #define TXID    (++_txid?_txid:1)
00048 //template <class T> T min(T a, T b) { return a<b ? a : b;}
00049 
00050 /* L2CAP command codes */
00051 const char* L2CAP_ComandCodeStr(int c) {
00052     switch (c) {
00053         case L2CAP_COMMAND_REJ:
00054             return "L2CAP_COMMAND_REJ";
00055         case L2CAP_CONN_REQ:
00056             return "L2CAP_CONN_REQ";
00057         case L2CAP_CONN_RSP:
00058             return "L2CAP_CONN_RSP";
00059         case L2CAP_CONF_REQ:
00060             return "L2CAP_CONF_REQ";
00061         case L2CAP_CONF_RSP:
00062             return "L2CAP_CONF_RSP";
00063         case L2CAP_DISCONN_REQ:
00064             return "L2CAP_DISCONN_REQ";
00065         case L2CAP_DISCONN_RSP:
00066             return "L2CAP_DISCONN_RSP";
00067         case L2CAP_ECHO_REQ:
00068             return "L2CAP_ECHO_REQ";
00069         case L2CAP_ECHO_RSP:
00070             return "L2CAP_ECHO_RSP";
00071         case L2CAP_INFO_REQ:
00072             return "L2CAP_INFO_REQ";
00073         case L2CAP_INFO_RSP:
00074             return "L2CAP_INFO_RSP";
00075     }
00076     return "unknown";
00077 }
00078 
00079 #define OFFSET  8 //means the buffer also has space for the l2cap/hci headers and need not be allocated and copied
00080 //#define OFFSET  0 //means the buffer only has space for the payload which need to be copied
00081 #if OFFSET == 0
00082 #define L2CAPBUFSIZE    128
00083 #else
00084 #define L2CAPBUFSIZE    0
00085 #endif
00086 
00087 typedef struct {
00088     u16    handle;
00089     u16    length;            // total
00090     u16    l2capLength;    // length -4
00091     u16    cid;            // Signaling packet CID = 1
00092     u8  data[L2CAPBUFSIZE];       // Largest thing to send!!! todo
00093 } L2CAPData;
00094 
00095 //
00096 void BTDevice::Init() {
00097     memset(&_info,0,sizeof(inquiry_info));
00098     _handle = 0;
00099     _name[0] = 0;
00100     _state = 0;
00101     _txid = 1;
00102     //cntr_cred = 1;
00103 }
00104 
00105 // virtual SocketHandler
00106 int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr) {
00107     L2CAPSocket* s = (L2CAPSocket*)sock;
00108     L2CAPAddr* a = (L2CAPAddr*)addr;
00109     s->scid = 0x40 + sock->ID-1;   // are these reserved?
00110     s->dcid = 0;
00111     Connect(s->scid,a->psm);
00112     sock->State = SocketState_L2CAP_WaitConnectRsp;
00113     contState = 0;
00114     return sock->ID;
00115 }
00116 
00117 // virtual SocketHandler
00118 int BTDevice::Accept(SocketInternal* sock, int scid, int rxid) {
00119     L2CAPSocket* s = (L2CAPSocket*)sock;
00120     s->scid = 0x40 + sock->ID-1;   // are these reserved?
00121     s->dcid = scid;
00122     u16 p[4];
00123     p[0] = s->scid;
00124     p[1] = scid;
00125     p[2] = 0; //success
00126     p[3] = 0; //no further information
00127     Send(L2CAP_CONN_RSP,rxid,p,4);
00128     printf("send conn_rsp with dcid=%#x and scid=%#x\n", p[0],p[1]);
00129     sock->State = SocketState_L2CAP_Config_wait;
00130     contState = 0;
00131     return sock->ID;
00132 }
00133 
00134 // virtual SocketHandler, called from HCI which is ABOVE L2CAP
00135 int BTDevice::Send(SocketInternal* sock, const u8* data, int len) {
00136     L2CAPSocket* s = (L2CAPSocket*)sock;
00137 #if OFFSET == 8  //sizeof L2CAPData header
00138     L2CAPData &d = *const_cast<L2CAPData*>((L2CAPData*)data);
00139 #else
00140     L2CAPData d;
00141 #endif
00142     if (len > peer_mtu) {//mtu concerns the l2cap mtu, because we use basic mode we cannot segment
00143         printf("MTU (%d) for outgoing packet (%d) exceeded\n", peer_mtu, len);
00144         return 0;
00145     }
00146     d.handle = _handle | 0x2000;
00147     d.length = 4 + len - OFFSET;
00148     d.l2capLength = len - OFFSET;
00149     d.cid = s->dcid;
00150     //printf("cid=%d: ", d.cid);
00151     //printfBytes("sending: ", data, len);
00152 #if OFFSET == 0
00153     if (len > L2CAPBUFSIZE)
00154         return -1;
00155     memcpy(d.data,data,len);
00156     return Send((u8*)&d,len+8);
00157 #else
00158     return Send(data, len);
00159 #endif
00160 }
00161 
00162 // virtual SocketHandler
00163 int BTDevice::Close(SocketInternal* sock) {
00164     printf("L2CAP close %d\n",sock->ID);
00165     sock->State = SocketState_L2CAP_WaitDisconnect;
00166     L2CAPSocket* s = (L2CAPSocket*)sock;
00167     return Disconnect(s->scid,s->dcid);
00168 }
00169 
00170 L2CAPSocket* BTDevice::SCIDToSocket(int scid) {
00171     return (L2CAPSocket*)GetSocketInternal(scid-0x40+1);
00172 }
00173 
00174 int BTDevice::Send(const u8* data, int len) {//printfBytes("Transport : ", data, len);
00175 #ifdef HOST_CONTR_FLOW
00176     pkts_sent++;
00177 #endif
00178     _transport->ACLSend(data,len);
00179     return 0;
00180 }
00181 
00182 void BTDevice::repeat_cmd() {
00183     printf("Cmd on handle %#x timed out, resending txid=%d\n", _handle, last_req.id);
00184 //  Send ((u8*)&last_req, last_req.length+4);//danger! interrupt context, Send is not reentrant
00185     //optionally set new larger timeout
00186 }
00187 
00188 int BTDevice::Send(u8 c, u8 id, u16* params, int count) {
00189     L2CAPCmd cmd;
00190     cmd.handle = _handle | 0x2000;
00191     cmd.length = 8 + count*2;
00192 
00193     cmd.l2capLength = cmd.length-4;
00194     cmd.cid = 1;    // Signaling packet
00195 
00196     cmd.cmd = c;
00197     cmd.id = id;
00198     cmd.cmdLength = count*2;
00199     for (int i = 0; i < count; i++)
00200         cmd.params[i] = params[i];
00201     if ((c & 1) == 0) { //this is a request
00202         last_req = cmd;
00203         rtx.attach(this, &BTDevice::repeat_cmd, 30.0);
00204         //printf("Starting timeout for %#x, txid=%d\n", _handle, id);
00205     }
00206     return Send((u8*)&cmd,cmd.length+4);
00207 }
00208 
00209 int BTDevice::Connect(int scid, int psm) {
00210     u16 p[2];
00211     p[0] = psm;
00212     p[1] = scid;
00213     return Send(L2CAP_CONN_REQ,TXID,p,2);
00214 }
00215 
00216 int BTDevice::Disconnect(int scid, int dcid) {
00217     u16 p[2];
00218     p[0] = dcid;
00219     p[1] = scid;
00220     return Send(L2CAP_DISCONN_REQ,TXID,p,2);
00221 }
00222 
00223 int BTDevice::ConfigureRequest(int dcid) {
00224     u16 p[4];
00225     p[0] = dcid;
00226     p[1] = 0;
00227     p[2] = 0x0201;  // Options
00228     p[3] = min(0x02A0, MAX_ACL_SIZE);  // my receiving MTU 672
00229     return Send(L2CAP_CONF_REQ,TXID,p,4);
00230 }
00231 
00232 int BTDevice::CommandReject(u16 reason, u16 data0, u16 data1) {
00233     u16 p[3];
00234     p[0] = reason;
00235     p[1] = data0;
00236     p[2] = data1;
00237     int parlen = 2;
00238     switch (reason) {
00239         case 0: //command not understood
00240             break;
00241         case 1: //MTU exceeded
00242             parlen = 4; //return actual mtu in data
00243             break;
00244         case 2: //invalid CID
00245             parlen = 6; //return local, remote cid
00246             break;
00247     }
00248     return Send(L2CAP_COMMAND_REJ,TXID,p,parlen);
00249 }
00250 
00251 int BTDevice::ConfigureResponse(u8 rxid, int dcid) {
00252     u16 p[3];
00253     p[0] = dcid; //source cid
00254     p[1] = 0;    //flags  (no continuation)
00255     p[2] = 0;    //result (success)
00256     return Send(L2CAP_CONF_RSP,rxid,p,3);
00257 }
00258 
00259 int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid) {
00260     u16 p[2];
00261     p[0] = dcid;
00262     p[1] = scid;
00263     return Send(L2CAP_DISCONN_RSP,rxid,p,2);
00264 }
00265 
00266 void server(int socket, SocketState state, const u8* data, int len, void* userData) {
00267     // printfBytes("Server: ", data, len);
00268     if (state==SocketState_Open && len>0)
00269         SDP.SDPServer(socket, state, data, len, userData);
00270 }
00271 
00272 void serserver(int socket, SocketState state, const u8* data, int len, void* userData) {
00273     printfBytes("serserver: ", data, len);
00274     SocketHandler *h = (SocketHandler*)userData;
00275     printf("userData refers to %s, state = %d\n", h->Name(), state);
00276     if (state==SocketState_Open) {
00277         if (len == 0) { //assume that the socket has just been opened and bind it to a new rfcomm server entity
00278             printf("Calling RFCOMMManager::BindSocket\n");
00279             rfcomm_manager.BindSocket(socket);
00280         } else {
00281             printf("Calling RFCOMMManager::SerServer\n");
00282             rfcomm_manager.SerServer(socket, state, data, len, userData);
00283         }
00284     } else if (state==SocketState_L2CAP_WaitDisconnect) {
00285         printf("Calling RFCOMMManager::SerServer\n");
00286         rfcomm_manager.SerServer(socket, state, data, len, userData);
00287     }
00288 }
00289 
00290 //code8, tid8, lengthData16
00291 //   0,    1,     2, 3
00292 void BTDevice::Control(const u8* data, int len) { //control channel receive
00293     printf("\x1B[%dm", 31);
00294     int cc = data[0];//command code
00295     if (cc & 1) { //it is a response or a reject
00296         rtx.detach(); //kill the timeout
00297         //printf("timeout cancelled for handle %#x, txid=%d\n", _handle, data[1]);
00298     }
00299     printf(L2CAP_ComandCodeStr(cc));
00300     switch (cc) {
00301         case L2CAP_COMMAND_REJ://bad command, eg. MTU, check (reason)
00302             printf(" rejection reason=%d\n", LE16(data+4));
00303             break;
00304         case L2CAP_CONN_REQ://incoming connection request, not expected but should reply with proper rejection (or accept)
00305             //when a connection is accepted a new socket must be opened
00306             printf(" Remote side requested a connection\n");
00307             {
00308                 int scid = LE16(data+6);
00309                 int psm = LE16(data+4);
00310                 int rxid = data[1];
00311                 u16 p[4];
00312                 p[0] = 0; //no dcid
00313                 p[1] = scid;
00314                 p[3] = 0; //no further information
00315                 printf(" scid=%d, psm=%d\n", scid, psm);
00316                 peer_mtu = 672; //default mtu
00317                 int s = 0;
00318                 switch (psm) {
00319                     case L2CAP_PSM_SDP:
00320                         s = Socket_Accept(SOCKET_SDP, scid, rxid, server, this);//allocate an sdp socket but use it as L2CAP
00321                         break;
00322                     case L2CAP_PSM_RFCOMM: //SOCKET_RFCOM;
00323 #if 0
00324                         s = Socket_Accept(SOCKET_RFCOM, scid, rxid, serserver, this);//allocate an rfcomm socket
00325                         //using L2CAP i.o. RFCOM makes little difference in processing but it also changes the handler to HCI i.o. RFCOMMManager
00326 #else
00327 //an RFCOMM requests comes in from a known (this) device
00328 //the channel is not yet known
00329                         s = rfcomm_manager.FindSocket(this);//this should return 0 otherwise the remote device was asking a second rfcomm on the same device
00330                         if (s==0) {
00331                             printf("No connection to this device yet, allocate L2CAP Socket and accept\n");
00332                             //accept the connection, even though there may be no listener???
00333                             //have no choice because w/o acceptance no rfcomm req.
00334                             s = Socket_Accept(SOCKET_L2CAP, scid, rxid, serserver, this);//allocate an l2cap socket
00335                             //get a new l2cap socket, call HCI::Accept (fill in btdevice internals), then call BTDevice::Accept (send accept message)
00336                             //serserver is called on state changes (SocketInternal::SetState) and on received packets from the peer device to the new l2cap handle
00337                             //after sending the accept message, the devices will execute the normal l2cap connection state-machine
00338                             //ending in a call to SetState(Open) which will invoke 'serserver' for the first time
00339 //or something like:
00340 //                            s = Socket_Create(SOCKET_L2CAP, serserver, this);//allocate an l2cap socket
00341 //                            Accept(GetSocketInternal(s), scid, rxid);//send accept response, this would bypass HCI::Accept()
00342                         } else {
00343                             printf("Already had an L2CAP connection on socket %d\n", s);
00344                         }
00345 #endif
00346                         break;
00347                     default:
00348                         printf("PSM %d not supported\n", psm);
00349                 }
00350                 switch (s) {
00351                     case 0:
00352                         printf("Not a valid socket\n");
00353                         break;
00354                     case ERR_SOCKET_TYPE_NOT_FOUND:
00355                         p[2] = 2; //psm not supported
00356                         Send(L2CAP_CONN_RSP,rxid,p,4);
00357                         break;
00358                     case ERR_SOCKET_NONE_LEFT:
00359                         p[2] = 4; //no resources available
00360                         Send(L2CAP_CONN_RSP,rxid,p,4);
00361                         break;
00362                 }
00363             }
00364             break;
00365             // Response to our initial connect from Remote
00366         case L2CAP_CONN_RSP: {
00367             int dcid = LE16(data+4);
00368             int scid = LE16(data+6);
00369             L2CAPSocket* s = SCIDToSocket(scid);
00370             int result = LE16(data+8);
00371             printf(" Result=%d, Status = %d\n", result, LE16(data+10));
00372             if (s->si.State != SocketState_L2CAP_WaitConnectRsp) {
00373                 printf("Unexpected event ignored\n");
00374                 break;
00375             }
00376             if (result == 0) {
00377                 if (s) {
00378                     s->si.State = SocketState_L2CAP_Config_wait;
00379                     s->dcid = dcid;
00380                     ConfigureRequest(dcid);
00381                     s->si.State = SocketState_L2CAP_Config_wait_reqrsp;
00382                     printf("Sent ConfigureRequest, state=WAIT_CONFIG_REQ_RSP\n");
00383                 }
00384             } else if (result == 1) {//pending, stay in the present state
00385             } else {
00386                 s->si.SetState(SocketState_Closed);
00387                 printf("Connect failed\n");
00388             }
00389         }
00390         break;
00391 
00392         case L2CAP_CONF_RSP: {
00393             int result = LE16(data+8);
00394             printf("Result=%d, datalen=%d, %smore conf to follow\n", result, LE16(data+2), LE16(data+6)?"":"No ");
00395             //should parse the config
00396             printfBytes("CONF RSP:", data, LE16(data+2)+4);
00397             int scid = LE16(data+4);
00398             SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
00399             if (s == 0) break;
00400             if (s->State != SocketState_L2CAP_Config_wait_reqrsp && s->State != SocketState_L2CAP_Config_wait_rsp) {
00401                 printf("Unexpected event ignored\n");
00402                 break;
00403             }
00404             if (result == 0) { //configuration acceptable
00405                 if (s->State == SocketState_L2CAP_Config_wait_reqrsp) {
00406                     s->State = SocketState_L2CAP_Config_wait_req;
00407                     printf("State=WAIT_CONFIG_REQ\n");
00408                 } else {
00409                     ConfigureResponse(data[1],((L2CAPSocket*)s)->dcid);//data[1]==txid
00410                     printf("Sent ConfigureResponse, state=Open\n");
00411                     s->SetState(SocketState_Open);
00412                 }
00413             } else {
00414                 printf("Renegotiate configuration\n");
00415             }
00416         }
00417         break;
00418 
00419         case L2CAP_CONF_REQ: {
00420             int len = LE16(data+2);
00421             int scid = LE16(data+4);//flags (data[6] LSB is continuation flag, data[10],[11] are the MTU
00422             int flags = LE16(data+6);
00423             const u8* conf = data+8;
00424             if (flags)
00425                 printf("Warning! Continuation flag in L2CAP configuration not supported\n");
00426             L2CAPSocket* s = SCIDToSocket(scid);
00427             printfBytes("CONF REQ: ", data, LE16(data+2)+4);//data+8 contains option type 1-4 1=MTU, 2=flush timeout, 3=QoS, 4=FCM
00428             if (s == 0) break;
00429             if (s->si.State == SocketState_Closed ||
00430                     s->si.State == SocketState_L2CAP_WaitConnectRsp ||
00431                     s->si.State == SocketState_L2CAP_WaitDisconnect) {
00432                 //Send Reject command
00433                 printf("Connection should be rejected\n");
00434                 break;
00435             }
00436 #if 0
00437             if (len > 4)
00438                 switch (data[8]) {
00439                     case 1:
00440                         peer_mtu = LE16(data+10);
00441                         printf("Peer L2CAP MTU = %d bytes\n", peer_mtu);
00442                         break;
00443                     case 2: //flush timeout
00444                     case 3: //QOS
00445                     case 4: //retrans and FC option
00446                     default:
00447                         printf("Unsupported configuration option %d, value = %#X\n", data[8], LE16(data+10));
00448                         break;
00449                 }
00450             else
00451                 printf("Empty config req. peer_mtu = %d\n", peer_mtu);
00452 #else
00453             while (conf < data+len+4) {
00454                 bool hint = conf[0] & 0x80;
00455                 switch (conf[0] & 0x7F) {
00456                     case 1:
00457                         peer_mtu = LE16(conf+2);
00458                         printf("Peer L2CAP MTU = %d bytes\n", peer_mtu);
00459                         break;
00460                     case 2: //flush timeout
00461                     case 3: //QOS
00462                     case 4: //retrans and FC option
00463                     default:
00464                         printf("Unsupported configuration option %d, value = %#X\n", conf[0], LE16(conf+2));
00465                         break;
00466                 }
00467                 conf += conf[1]+2;
00468             }
00469 #endif
00470             if (1 /* options acceptable */) {
00471                 if (flags == 0) {
00472                     printf("Sending ConfigureResponse, old state=%d ", s->si.State);
00473                     ConfigureResponse(data[1],s->dcid);//data[1]==txid, success
00474                     switch (s->si.State) {
00475                         case SocketState_L2CAP_Config_wait:
00476                             s->si.State = SocketState_L2CAP_Config_wait_send;
00477                             ConfigureRequest(s->dcid);
00478                             s->si.State = SocketState_L2CAP_Config_wait_rsp;
00479                             break;
00480                         case SocketState_L2CAP_Config_wait_req:
00481                             ((SocketInternal*)s)->SetState(SocketState_Open);
00482                             break;
00483                         case SocketState_L2CAP_Config_wait_rsp:
00484                             break;
00485                         case SocketState_L2CAP_Config_wait_reqrsp:
00486                             s->si.State = SocketState_L2CAP_Config_wait_rsp;
00487                             break;
00488                     }
00489                     printf("new state=%d\n", s->si.State);
00490                 } else
00491                     printf("L2CAP config continuation, delaying response...\n");
00492             } else { //options not acceptable
00493                 printf("Configure failure should be indicated\n");
00494                 ConfigureResponse(data[1],s->dcid);//indicates success but should indicate fail
00495             }
00496         }
00497         break;
00498         case L2CAP_DISCONN_REQ:  {
00499             int dcid = LE16(data+4);
00500             int scid = LE16(data+6);
00501             L2CAPSocket* s = SCIDToSocket(dcid);
00502             if (s) {
00503                 s->si.SetState(SocketState_Closed);
00504                 DisconnectResponse(data[1], scid, dcid);
00505             } else {
00506                 printf("request to disconnect cid %d fails, no such cid\n", dcid);
00507                 CommandReject(0, dcid, scid);
00508             }
00509         }
00510         break;
00511         case L2CAP_DISCONN_RSP: {
00512             int scid = LE16(data+6);
00513             L2CAPSocket* s = SCIDToSocket(scid);
00514             if (s->si.State == SocketState_L2CAP_WaitDisconnect)
00515                 s->si.SetState(SocketState_Closed);
00516         }
00517         break;
00518         default:
00519             printf("Unsupported L2CAP message %d\n", cc);
00520     }
00521     printf("\x1b[0m");
00522 }
00523 
00524 void BTDevice::ACLFwd(const u8* data, int len) {
00525     if (l2cap_sock == 1)
00526         Control(data, len);
00527     else {
00528         SocketInternal* s = (SocketInternal*)SCIDToSocket(l2cap_sock);//in fact cid in the l2cap header
00529         if (s)
00530             s->Recv(data,len);//forward to the sockethandler for the type
00531         else
00532             printf("Bad event cid %d\n",l2cap_sock);
00533     }
00534 }
00535 //sometimes acl packets are segmented, in that case the l2cap payload length does not correspond to the acl pkt length
00536 //and the l2cap packet length. L2CAP works in basic mode and cannot be segmented hence the l2cap pkt size corresponds to
00537 //the acl pkt size
00538 int BTDevice::ACLRecv(const u8* data, int acllen) {
00539     //printfBytes("L2CP",data,acllen);
00540     //cntr_cred--;
00541     u16 handle = LE16(data);
00542     if ((handle&0x0fff) !=  _handle) {
00543         printf("unexpected handle %#x, this _handle=%#x\n", handle, _handle);
00544         return 1;
00545     }
00546     //below is the ACL packet recombination engine
00547     char pb = (handle>>12) & 3;
00548     if (pb == 2)
00549         segments = 1;
00550     else
00551         segments++;
00552     int p = 4; //start of l2cap packet
00553     int len = LE16(data+2); //length of l2cap pkt
00554     while (p < acllen)
00555         switch (contState) {
00556             case 0://allow even for fragmented length field
00557                 plen = data[p++];//payload length lsb
00558                 contState = 1;
00559                 break;
00560             case 1:
00561                 plen += data[p++]<<8; //payload length msb
00562                 if (pb == 2 && plen == acllen-8) {//normal case, l2cap pkt is contained completely in this hci pkt
00563                     l2cap_sock = data[p] + (data[p+1]<<8);
00564                     contState = 0;
00565                     ACLFwd(data+8, plen); //forward the packet in its original buffer
00566                     return segments; //all data was dealt with
00567                 } else { //packet is segmented
00568                     printf("ACL packet is segmented\n");
00569                     contState = 2;
00570                     contBuf = new unsigned char[plen];//allocate recombination buffer
00571                     contPos = 0;
00572                 }
00573                 break;
00574             case 2:
00575                 l2cap_sock = data[p++];
00576                 contState = 3;
00577                 break;
00578             case 3:
00579                 l2cap_sock += data[p++]<<8;
00580                 contState = 4;
00581                 break;
00582             case 4: //data, recombine segmented ACL (not l2cap!) frames
00583                 if (contPos < plen) {//buffer not yet full
00584                     int datalen = acllen - p; //data in this incoming pkt
00585                     int remcap = plen - contPos; //remaining capacity in the recombination buffer
00586                     if (datalen <= remcap) {
00587                         memcpy(contBuf+contPos, data+p, datalen);
00588                         contPos += datalen;
00589                         p = acllen;//end of data, stop the while loop
00590                         if (contPos == plen) {//buffer is full now
00591                             printfBytes("Recombined packet is:", contBuf, plen);
00592                             ACLFwd(contBuf, plen); //forward the recombination buffer
00593                             delete[] contBuf;//and free the buffer
00594                             contState = 0;
00595                             return segments;
00596                         }//else stay in this state to wait for the rest
00597                     } else {//data contains (part of) next packet, never seen this happen
00598                         memcpy(contBuf+contPos, data+p, plen-contPos);//this packet is complete
00599                         p += plen-contPos;
00600                         printfBytes("Recombined packet is:", contBuf, plen);
00601                         printfBytes("Next packet starts with:", data+p, acllen-p);
00602                         ACLFwd(contBuf, plen); //forward the recombination buffer
00603                         delete[] contBuf;//and free the buffer
00604                         contState = 0; //continue with the next packet
00605                     }
00606                 } else {
00607                     printf("Cannot append to buffer (size=%d, pos=%d, datalen = %d)\n", plen, contPos, len-p);
00608                     contState = 0;
00609                     return segments;//flushed
00610                 }
00611                 break;
00612         }//switch (and while)
00613     return 0;//the buffers are not processed yet
00614 }