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 
00032 #define L2CAP_COMMAND_REJ       0x01
00033 #define L2CAP_CONN_REQ          0x02
00034 #define L2CAP_CONN_RSP          0x03
00035 #define L2CAP_CONF_REQ          0x04
00036 #define L2CAP_CONF_RSP          0x05
00037 #define L2CAP_DISCONN_REQ       0x06
00038 #define L2CAP_DISCONN_RSP       0x07
00039 #define L2CAP_ECHO_REQ          0x08
00040 #define L2CAP_ECHO_RSP          0x09
00041 #define L2CAP_INFO_REQ          0x0a
00042 #define L2CAP_INFO_RSP          0x0b
00043 
00044 
00045  /* L2CAP command codes */
00046  const char* L2CAP_ComandCodeStr(int c)
00047  {
00048      switch (c)
00049      {
00050         case L2CAP_COMMAND_REJ:    return "L2CAP_COMMAND_REJ";
00051         case L2CAP_CONN_REQ:    return "L2CAP_CONN_REQ";
00052         case L2CAP_CONN_RSP:    return "L2CAP_CONN_RSP";
00053         case L2CAP_CONF_REQ:    return "L2CAP_CONF_REQ";
00054         case L2CAP_CONF_RSP:    return "L2CAP_CONF_RSP";
00055         case L2CAP_DISCONN_REQ:    return "L2CAP_DISCONN_REQ";
00056         case L2CAP_DISCONN_RSP:    return "L2CAP_DISCONN_RSP";
00057         case L2CAP_ECHO_REQ:    return "L2CAP_ECHO_REQ";
00058         case L2CAP_ECHO_RSP:    return "L2CAP_ECHO_RSP";
00059         case L2CAP_INFO_REQ:    return "L2CAP_INFO_REQ";
00060         case L2CAP_INFO_RSP:    return "L2CAP_INFO_RSP";
00061      }
00062      return "unknown";
00063  }
00064 
00065 typedef struct
00066 {
00067     u16    handle;
00068     u16    length;            // total
00069     u16    l2capLength;    // length -4
00070     u16    cid;            // Signaling packet CID = 1
00071     u8  data[64];       // Largest thing to send!!! todo
00072 } L2CAPData;
00073 
00074 typedef struct
00075 {
00076     u16    handle;
00077     u16    length;            // total
00078     u16    l2capLength;    // length -4
00079     u16    cid;            // Signaling packet CID = 1
00080 
00081     // Payload
00082     u8    cmd;            //
00083     u8    id;
00084     u16    cmdLength;        // total-8
00085     u16 params[4];      // Params
00086 } L2CAPCmd;
00087 
00088 void printf(const BD_ADDR* addr);
00089 
00090 //
00091 void BTDevice::Init()
00092 {
00093     memset(&_info,0,sizeof(inquiry_info));
00094     _handle = 0;
00095     _name[0] = 0;
00096     _state = 0;
00097 }
00098 
00099 // virtual SocketHandler
00100 int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr)
00101 {
00102     printf("Call to BTDevice Open \r\n");
00103     L2CAPSocket* s = (L2CAPSocket*)sock;
00104     L2CAPAddr* a = (L2CAPAddr*)addr;
00105     s->scid = 0x40 + sock->ID-1;   // are these reserved?
00106     s->dcid = 0;
00107     Connect(s->scid,a->psm);
00108     return sock->ID;
00109 }
00110 
00111 // virtual SocketHandler
00112 int BTDevice::Create(SocketInternal* sock, SocketAddrHdr* addr)
00113 {
00114     printf("Call to BTDevice Create \r\n");
00115     L2CAPSocket* s = (L2CAPSocket*)sock;
00116     L2CAPAddr* a = (L2CAPAddr*)addr;
00117 
00118     //Connect(s->scid,a->psm);
00119     return sock->ID;
00120 }
00121 
00122 int BTDevice::Accept(SocketInternal* sock, SocketAddrHdr* addr)
00123 {
00124     printf("Call to BTDevice Accept \r\n");
00125     L2CAPSocket* s = (L2CAPSocket*)sock;
00126     L2CAPAddr* a = (L2CAPAddr*)addr;
00127     
00128     
00129     printf("ID = %d scid = %d dcid = %d \r\n",sock->ID, s->scid, s->dcid); 
00130      
00131     return sock->ID;
00132 }
00133 
00134 
00135 
00136 
00137 // virtual SocketHandler
00138 int BTDevice::Send(SocketInternal* sock, const u8* data, int len)
00139 {
00140     printf("Call to BTDevice Send \r\n");
00141     L2CAPData d;
00142     L2CAPSocket* s = (L2CAPSocket*)sock;
00143 
00144     d.handle = _handle | 0x2000;
00145     d.length = 4 + len;
00146     d.l2capLength = len;
00147     d.cid = s->dcid;
00148 
00149     if (len > 64)
00150         return -1;
00151     memcpy(d.data,data,len);
00152     return Send((u8*)&d,len+8);
00153 }
00154 
00155 // virtual SocketHandler
00156 int BTDevice::Close(SocketInternal* sock)
00157 {
00158     printf("L2CAP close %d\r\n",sock->ID);
00159     L2CAPSocket* s = (L2CAPSocket*)sock;
00160     return Disconnect(s->scid,s->dcid);
00161 }
00162 
00163 L2CAPSocket* BTDevice::SCIDToSocket(int scid)
00164 {
00165     return (L2CAPSocket*)GetSocketInternal(scid-0x40+1);
00166 }
00167 
00168 int BTDevice::Send(const u8* data, int len)
00169 {
00170      printfBytes("L2CP send: ",data,len);
00171      _transport->ACLSend(data,len);
00172      return 0;
00173 }
00174 
00175 int BTDevice::Send(u8 c, u8 id, u16* params, int count)
00176 {
00177     L2CAPCmd cmd;
00178     cmd.handle = _handle | 0x2000;
00179     cmd.length = 8 + count*2;
00180 
00181     cmd.l2capLength = cmd.length-4;
00182     cmd.cid = 1;    // Signaling packet
00183 
00184     cmd.cmd = c;
00185     cmd.id = id;
00186     cmd.cmdLength = count*2;
00187     for (int i = 0; i < count; i++)
00188         cmd.params[i] = params[i];
00189     return Send((u8*)&cmd,cmd.length+4);
00190 }
00191 
00192 int BTDevice::Connect(int scid, int psm)
00193 {
00194     u16 p[2];
00195     p[0] = psm;
00196     p[1] = scid;
00197     return Send(L2CAP_CONN_REQ,_txid++,p,2);
00198 }
00199 
00200 int BTDevice::Disconnect(int scid, int dcid)
00201 {
00202     u16 p[2];
00203     p[0] = dcid;
00204     p[1] = scid;
00205     return Send(L2CAP_DISCONN_REQ,_txid++,p,2);
00206 }
00207 
00208 int BTDevice::ConfigureRequest(int dcid)
00209 {
00210     u16 p[4];
00211     p[0] = dcid;
00212     p[1] = 0;
00213     p[2] = 0x0201;  // Options
00214     p[3] = 0x02A0;  // 672
00215     return Send(L2CAP_CONF_REQ,_txid++,p,4);
00216 }
00217 
00218 int BTDevice::ConfigureResponse(u8 rxid, int dcid)
00219 {
00220     u16 p[3];
00221     p[0] = dcid;
00222     p[1] = 0;
00223     p[2] = 0;
00224     return Send(L2CAP_CONF_RSP,rxid,p,3);
00225 }
00226 
00227 int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid)
00228 {
00229     u16 p[2];
00230     p[0] = dcid;
00231     p[1] = scid;
00232     return Send(L2CAP_DISCONN_RSP,rxid,p,2);
00233 }
00234 
00235 int BTDevice::AcceptResponse(u8 rxid, int scid, int dcid)
00236 {
00237     printf("Connection accepted \r\n");
00238     u16 p[4];
00239     p[0] = scid;
00240     p[1] = dcid;
00241     p[2] = L2CAP_CONN_SUCCESS;
00242     p[3] = 0;
00243 
00244     return Send(L2CAP_CONN_RSP,rxid,p,4);
00245 
00246 }
00247 
00248 int BTDevice::RefuseResponse(u8 rxid)
00249 {
00250     printf("Connection refused \r\n");
00251     u16 p[2];
00252     p[0] = L2CAP_CONN_REF_PSM;
00253     p[1] = 0;
00254     
00255     return Send(L2CAP_CONN_RSP,rxid,p,2);
00256 }
00257 
00258 
00259 //int BTDevice::InUse(int psm)
00260 //{
00261 //    for (int i = 0; i < MAX_PORTS; i++){
00262 //                 printf("Listen Q %d = %d \r\n",i, _listen[i]);
00263 //                 if ( _listen[i] == psm ) {
00264 //                        printf("We are listening on port %d \r\n",psm); //in use
00265 //                        return 0;     
00266 //                 }
00267 //    }
00268 //    printf("We are not listening on port %d \r\n",psm);
00269 //    return 1;
00270 //}
00271 
00272 
00273 
00274 
00275 void BTDevice::Control(const BD_ADDR* addr, const u8* data, int len)
00276 {
00277 
00278     SocketInternal* sock;
00279     L2CAPSocket* s;
00280     int psm,scid,dcid,flags;
00281     
00282     printf("From address ");
00283     printf(addr);
00284     printf(" : ");
00285 
00286     int cc = data[8];
00287     printf(L2CAP_ComandCodeStr(cc));
00288     int result = LE16(data+16);
00289     printf(" Result %d\r\n",result);
00290     
00291     
00292     switch (cc)
00293     {
00294         case L2CAP_COMMAND_REJ:
00295            break;
00296         case L2CAP_CONN_REQ:
00297             {
00298             psm = LE16(data+12);
00299             scid =  LE16(data+14);
00300             printf("Connection request scid = %d psm = %d \r\n",scid,psm);   
00301            
00302             // check if we listen on the port  
00303             //if ( InUse(psm) ) {
00304             if ( Socket_InUse(SOCKET_L2CAP,psm) ) {
00305                 RefuseResponse(data[9]);
00306                 
00307                     
00308             } else {
00309                 L2CAPAddr sockAddr;
00310                 sockAddr.bdaddr = *addr;
00311                 sock = Socket_Create(SOCKET_L2CAP, &sockAddr.hdr, psm);
00312                 s = (L2CAPSocket*)sock; 
00313                 s->scid = 0x40 + sock->ID-1;   // are these reserved?
00314                 s->dcid = scid;
00315                 AcceptResponse(data[9],s->scid,s->dcid);
00316 
00317                 ConfigureRequest(s->dcid); // handshake
00318                 
00319                 sock->SetState(SocketState_Accept);
00320             }
00321            
00322             /**
00323             for (int i = 0; i < MAX_PORTS; i++){
00324                  if ( _listen[i] == psm ) {
00325                         printf("We are listening on port %d \r\n",psm); //in use
00326                         RefuseResponse(data[9]);
00327                         break;
00328                  }
00329             } 
00330             **/
00331             
00332             //L2CAPAddr sockAddr;
00333             //sockAddr.bdaddr = addr;
00334             //sockAddr.psm = psm;
00335 
00336             }
00337             break;
00338                         
00339         // Response to our initial connect from Remote
00340         case L2CAP_CONN_RSP:
00341             {
00342                 if (result == 0)
00343                 {
00344                     printf("Connect succeeded\r\n");
00345                     dcid = LE16(data+12);
00346                     scid = LE16(data+14);
00347                     L2CAPSocket* s = SCIDToSocket(scid);
00348                     if (s)
00349                     {
00350                         s->dcid = dcid;
00351                         ConfigureRequest(dcid);
00352                     }
00353                 } else
00354                     printf("Connect failed?\r\n");
00355             }
00356             break;
00357 
00358         case L2CAP_CONF_RSP:
00359             {
00360                 scid = LE16(data+12);
00361                 SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
00362                 if (s)
00363                     s->SetState(SocketState_Open);
00364             }
00365             break;
00366 
00367         case L2CAP_CONF_REQ:
00368             {
00369                 u16 dcid = LE16(data+12);
00370                 u16 flags = LE16(data+14);            
00371                 printf("Config request dcid = %02X flags = %02X\r\n", dcid, flags);           
00372                 scid = LE16(data+12);
00373                 L2CAPSocket* s = SCIDToSocket(scid);
00374                 if (s)
00375                     ConfigureResponse(data[9],s->dcid);
00376             }
00377             break;
00378         case L2CAP_DISCONN_REQ:  {
00379               int dcid = LE16(data+12);
00380               int scid = LE16(data+14);
00381               L2CAPSocket* s = SCIDToSocket(scid);
00382               //s->si.SetState(SocketState_Closed);
00383               DisconnectResponse(data[9], scid, dcid);
00384           }
00385           break;
00386 
00387     }
00388 }
00389 
00390 void BTDevice::ACLRecv(const BD_ADDR* addr, const u8* data, int len)
00391 {
00392     //printfBytes("L2CP recv: ",data,16);
00393     int handle = LE16(data);
00394     if (handle != (0x2000 | _handle))
00395         return;
00396 
00397     int cid = LE16(data+6);
00398     if (cid == 1)
00399     {
00400         Control(addr,data,len);
00401         return;
00402     }
00403 
00404     SocketInternal* s = (SocketInternal*)SCIDToSocket(cid);
00405     if (s)
00406         s->Recv(data+8,LE16(data+2)-4);
00407     else
00408         printf("Bad event cid %d\r\n",cid);
00409 }