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      _transport->ACLSend(data,len);
00141      return 0;
00142 }
00143 
00144 int BTDevice::Send(u8 c, u8 id, u16* params, int count)
00145 {
00146     L2CAPCmd cmd;
00147     cmd.handle = _handle | 0x2000;
00148     cmd.length = 8 + count*2;
00149 
00150     cmd.l2capLength = cmd.length-4;
00151     cmd.cid = 1;    // Signaling packet
00152 
00153     cmd.cmd = c;
00154     cmd.id = id;
00155     cmd.cmdLength = count*2;
00156     for (int i = 0; i < count; i++)
00157         cmd.params[i] = params[i];
00158     return Send((u8*)&cmd,cmd.length+4);
00159 }
00160 
00161 int BTDevice::Connect(int scid, int psm)
00162 {
00163     u16 p[2];
00164     p[0] = psm;
00165     p[1] = scid;
00166     return Send(L2CAP_CONN_REQ,_txid++,p,2);
00167 }
00168 
00169 int BTDevice::Disconnect(int scid, int dcid)
00170 {
00171     u16 p[2];
00172     p[0] = dcid;
00173     p[1] = scid;
00174     return Send(L2CAP_DISCONN_REQ,_txid++,p,2);
00175 }
00176 
00177 int BTDevice::ConfigureRequest(int dcid)
00178 {
00179     u16 p[4];
00180     p[0] = dcid;
00181     p[1] = 0;
00182     p[2] = 0x0201;  // Options
00183     p[3] = 0x02A0;  // 672
00184     return Send(L2CAP_CONF_REQ,_txid++,p,4);
00185 }
00186 
00187 int BTDevice::ConfigureResponse(u8 rxid, int dcid)
00188 {
00189     u16 p[3];
00190     p[0] = dcid;
00191     p[1] = 0;
00192     p[2] = 0;
00193     return Send(L2CAP_CONF_RSP,rxid,p,3);
00194 }
00195 
00196 int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid)
00197 {
00198     u16 p[2];
00199     p[0] = dcid;
00200     p[1] = scid;
00201     return Send(L2CAP_DISCONN_RSP,rxid,p,2);
00202 }
00203 
00204 void BTDevice::Control(const u8* data, int len)
00205 {
00206     int cc = data[8];
00207     printf(L2CAP_ComandCodeStr(cc));
00208     int result = LE16(data+16);
00209     printf(" Result %d\n",result);
00210     switch (cc)
00211     {
00212         case L2CAP_COMMAND_REJ:
00213            break;
00214         case L2CAP_CONN_REQ:
00215             break;
00216 
00217         // Response to our initial connect from Remote
00218         case L2CAP_CONN_RSP:
00219             {
00220                 if (result == 0)
00221                 {
00222                     int dcid = LE16(data+12);
00223                     int scid = LE16(data+14);
00224                     L2CAPSocket* s = SCIDToSocket(scid);
00225                     if (s)
00226                     {
00227                         s->dcid = dcid;
00228                         ConfigureRequest(dcid);
00229                     }
00230                 } else
00231                     printf("Connect failed?\n");
00232             }
00233             break;
00234 
00235         case L2CAP_CONF_RSP:
00236             {
00237                 int scid = LE16(data+12);
00238                 SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
00239                 if (s)
00240                     s->SetState(SocketState_Open);
00241             }
00242             break;
00243 
00244         case L2CAP_CONF_REQ:
00245             {
00246                 int scid = LE16(data+12);
00247                 L2CAPSocket* s = SCIDToSocket(scid);
00248                 if (s)
00249                     ConfigureResponse(data[9],s->dcid);
00250             }
00251             break;
00252     }
00253 }
00254 
00255 void BTDevice::ACLRecv(const u8* data, int len)
00256 {
00257    // printfBytes("L2CP",data,16);
00258     int handle = LE16(data);
00259     if (handle != (0x2000 | _handle))
00260         return;
00261 
00262     int cid = LE16(data+6);
00263     if (cid == 1)
00264     {
00265         Control(data,len);
00266         return;
00267     }
00268 
00269     SocketInternal* s = (SocketInternal*)SCIDToSocket(cid);
00270     if (s)
00271         s->Recv(data+8,LE16(data+2)-4);
00272     else
00273         printf("Bad event cid %d\n",cid);
00274 }