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 //
00089 void BTDevice::Init()
00090 {
00091     memset(&_info,0,sizeof(inquiry_info));
00092     _handle = 0;
00093     _name[0] = 0;
00094     _state = 0;
00095 }
00096 
00097 // virtual SocketHandler
00098 int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr)
00099 {
00100     L2CAPSocket* s = (L2CAPSocket*)sock;
00101     L2CAPAddr* a = (L2CAPAddr*)addr;
00102     s->scid = 0x40 + sock->ID-1;   // are these reserved?
00103     s->dcid = 0;
00104     Connect(s->scid,a->psm);
00105     return sock->ID;
00106 }
00107 
00108 // virtual SocketHandler
00109 int BTDevice::Send(SocketInternal* sock, const u8* data, int len)
00110 {
00111     L2CAPData d;
00112     L2CAPSocket* s = (L2CAPSocket*)sock;
00113 
00114     d.handle = _handle | 0x2000;
00115     d.length = 4 + len;
00116     d.l2capLength = len;
00117     d.cid = s->dcid;
00118 
00119     if (len > 64)
00120         return -1;
00121     memcpy(d.data,data,len);
00122     return Send((u8*)&d,len+8);
00123 }
00124 
00125 // virtual SocketHandler
00126 int BTDevice::Close(SocketInternal* sock)
00127 {
00128     printf("L2CAP close %d\n",sock->ID);
00129     L2CAPSocket* s = (L2CAPSocket*)sock;
00130     return Disconnect(s->scid,s->dcid);
00131 }
00132 
00133 L2CAPSocket* BTDevice::SCIDToSocket(int scid)
00134 {
00135     return (L2CAPSocket*)GetSocketInternal(scid-0x40+1);
00136 }
00137 
00138 int BTDevice::Send(const u8* data, int len)
00139 {
00140 //-----y.k-----
00141     int i;
00142     printf("-----y.k-----\n");
00143     printf("-----ACLSend() :\n data[] = ");
00144         
00145     for(i=0;i<sizeof(data);++i){
00146        printf(" 0x%x : " ,data[i]);
00147     }
00148         
00149     printf("len = %d \n", sizeof(data));
00150     printf("-----y.k-----\n");
00151         
00152 //-----y.k-----
00153 
00154      _transport->ACLSend(data,len);
00155      return 0;
00156 }
00157 
00158 int BTDevice::Send(u8 c, u8 id, u16* params, int count)
00159 {
00160     L2CAPCmd cmd;
00161     cmd.handle = _handle | 0x2000;
00162     cmd.length = 8 + count*2;
00163 
00164     cmd.l2capLength = cmd.length-4;
00165     cmd.cid = 1;    // Signaling packet
00166 
00167     cmd.cmd = c;
00168     cmd.id = id;
00169     cmd.cmdLength = count*2;
00170     for (int i = 0; i < count; i++)
00171         cmd.params[i] = params[i];
00172     return Send((u8*)&cmd,cmd.length+4);
00173 }
00174 
00175 int BTDevice::Connect(int scid, int psm)
00176 {
00177     u16 p[2];
00178     p[0] = psm;
00179     p[1] = scid;
00180     return Send(L2CAP_CONN_REQ,_txid++,p,2);
00181 }
00182 
00183 int BTDevice::Disconnect(int scid, int dcid)
00184 {
00185     u16 p[2];
00186     p[0] = dcid;
00187     p[1] = scid;
00188     return Send(L2CAP_DISCONN_REQ,_txid++,p,2);
00189 }
00190 
00191 int BTDevice::ConfigureRequest(int dcid)
00192 {
00193     u16 p[4];
00194     p[0] = dcid;
00195     p[1] = 0;
00196     p[2] = 0x0201;  // Options
00197     p[3] = 0x02A0;  // 672
00198     return Send(L2CAP_CONF_REQ,_txid++,p,4);
00199 }
00200 
00201 int BTDevice::ConfigureResponse(u8 rxid, int dcid)
00202 {
00203     u16 p[3];
00204     p[0] = dcid;
00205     p[1] = 0;
00206     p[2] = 0;
00207     return Send(L2CAP_CONF_RSP,rxid,p,3);
00208 }
00209 
00210 int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid)
00211 {
00212     u16 p[2];
00213     p[0] = dcid;
00214     p[1] = scid;
00215     return Send(L2CAP_DISCONN_RSP,rxid,p,2);
00216 }
00217 
00218 void BTDevice::Control(const u8* data, int len)
00219 {
00220     int cc = data[8];
00221     printf(L2CAP_ComandCodeStr(cc));
00222     int result = LE16(data+16);
00223     printf(" Result %d\n",result);
00224     switch (cc)
00225     {
00226         case L2CAP_COMMAND_REJ:
00227            break;
00228         case L2CAP_CONN_REQ:
00229             break;
00230 
00231         // Response to our initial connect from Remote
00232         case L2CAP_CONN_RSP:
00233             {
00234                 if (result == 0)
00235                 {
00236                     int dcid = LE16(data+12);
00237                     int scid = LE16(data+14);
00238                     L2CAPSocket* s = SCIDToSocket(scid);
00239                     if (s)
00240                     {
00241                         s->dcid = dcid;
00242                         ConfigureRequest(dcid);
00243                     }
00244                 } else
00245                     printf("Connect failed?\n");
00246             }
00247             break;
00248 
00249         case L2CAP_CONF_RSP:
00250             {
00251                 int scid = LE16(data+12);
00252                 SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
00253                 if (s)
00254                     s->SetState(SocketState_Open);
00255             }
00256             break;
00257 
00258         case L2CAP_CONF_REQ:
00259             {
00260                 int scid = LE16(data+12);
00261                 L2CAPSocket* s = SCIDToSocket(scid);
00262                 if (s)
00263                     ConfigureResponse(data[9],s->dcid);
00264             }
00265             break;
00266     }
00267 }
00268 
00269 void BTDevice::ACLRecv(const u8* data, int len)
00270 {
00271    // printfBytes("L2CP",data,16);
00272     int handle = LE16(data);
00273     if (handle != (0x2000 | _handle))
00274         return;
00275 
00276     int cid = LE16(data+6);
00277     if (cid == 1)
00278     {
00279         Control(data,len);
00280         return;
00281     }
00282 
00283     SocketInternal* s = (SocketInternal*)SCIDToSocket(cid);
00284     if (s)
00285         s->Recv(data+8,LE16(data+2)-4);
00286     else
00287         printf("Bad event cid %d\n",cid);
00288 }