Diff: L2CAP.cpp

Revision:

0:f6f434d9a03a

Child:

2:5c2bfbd63297

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L2CAP.cpp	Sat Dec 17 02:17:39 2011 +0000
@@ -0,0 +1,274 @@
+/*
+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"
+
+#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
+
+
+ /* 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";
+ }
+
+typedef struct
+{
+    u16    handle;
+    u16    length;            // total
+    u16    l2capLength;    // length -4
+    u16    cid;            // Signaling packet CID = 1
+    u8  data[64];       // Largest thing to send!!! todo
+} L2CAPData;
+
+typedef struct
+{
+    u16    handle;
+    u16    length;            // total
+    u16    l2capLength;    // length -4
+    u16    cid;            // Signaling packet CID = 1
+
+    // Payload
+    u8    cmd;            //
+    u8    id;
+    u16    cmdLength;        // total-8
+    u16 params[4];      // Params
+} L2CAPCmd;
+
+//
+void BTDevice::Init()
+{
+    memset(&_info,0,sizeof(inquiry_info));
+    _handle = 0;
+    _name[0] = 0;
+    _state = 0;
+}
+
+// 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);
+    return sock->ID;
+}
+
+// virtual SocketHandler
+int BTDevice::Send(SocketInternal* sock, const u8* data, int len)
+{
+    L2CAPData d;
+    L2CAPSocket* s = (L2CAPSocket*)sock;
+
+    d.handle = _handle | 0x2000;
+    d.length = 4 + len;
+    d.l2capLength = len;
+    d.cid = s->dcid;
+
+    if (len > 64)
+        return -1;
+    memcpy(d.data,data,len);
+    return Send((u8*)&d,len+8);
+}
+
+// virtual SocketHandler
+int BTDevice::Close(SocketInternal* sock)
+{
+    printf("L2CAP close %d\n",sock->ID);
+    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)
+{
+     _transport->ACLSend(data,len);
+     return 0;
+}
+
+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];
+    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] = 0x02A0;  // 672
+    return Send(L2CAP_CONF_REQ,_txid++,p,4);
+}
+
+int BTDevice::ConfigureResponse(u8 rxid, int dcid)
+{
+    u16 p[3];
+    p[0] = dcid;
+    p[1] = 0;
+    p[2] = 0;
+    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 BTDevice::Control(const u8* data, int len)
+{
+    int cc = data[8];
+    printf(L2CAP_ComandCodeStr(cc));
+    int result = LE16(data+16);
+    printf(" Result %d\n",result);
+    switch (cc)
+    {
+        case L2CAP_COMMAND_REJ:
+           break;
+        case L2CAP_CONN_REQ:
+            break;
+
+        // Response to our initial connect from Remote
+        case L2CAP_CONN_RSP:
+            {
+                if (result == 0)
+                {
+                    int dcid = LE16(data+12);
+                    int scid = LE16(data+14);
+                    L2CAPSocket* s = SCIDToSocket(scid);
+                    if (s)
+                    {
+                        s->dcid = dcid;
+                        ConfigureRequest(dcid);
+                    }
+                } else
+                    printf("Connect failed?\n");
+            }
+            break;
+
+        case L2CAP_CONF_RSP:
+            {
+                int scid = LE16(data+12);
+                SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
+                if (s)
+                    s->SetState(SocketState_Open);
+            }
+            break;
+
+        case L2CAP_CONF_REQ:
+            {
+                int scid = LE16(data+12);
+                L2CAPSocket* s = SCIDToSocket(scid);
+                if (s)
+                    ConfigureResponse(data[9],s->dcid);
+            }
+            break;
+    }
+}
+
+void BTDevice::ACLRecv(const u8* data, int len)
+{
+   // printfBytes("L2CP",data,16);
+    int handle = LE16(data);
+    if (handle != (0x2000 | _handle))
+        return;
+
+    int cid = LE16(data+6);
+    if (cid == 1)
+    {
+        Control(data,len);
+        return;
+    }
+
+    SocketInternal* s = (SocketInternal*)SCIDToSocket(cid);
+    if (s)
+        s->Recv(data+8,LE16(data+2)-4);
+    else
+        printf("Bad event cid %d\n",cid);
+}