Diff: L2CAP.cpp
- Revision:
- 0:e939856c1939
diff -r 000000000000 -r e939856c1939 L2CAP.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/L2CAP.cpp Wed May 25 14:49:20 2011 +0000
@@ -0,0 +1,409 @@
+/*
+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 printf(const BD_ADDR* addr);
+
+//
+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)
+{
+ printf("Call to BTDevice Open \r\n");
+ 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::Create(SocketInternal* sock, SocketAddrHdr* addr)
+{
+ printf("Call to BTDevice Create \r\n");
+ //L2CAPSocket* s = (L2CAPSocket*)sock;
+ //L2CAPAddr* a = (L2CAPAddr*)addr;
+
+ //Connect(s->scid,a->psm);
+ return sock->ID;
+}
+
+int BTDevice::Accept(SocketInternal* sock, SocketAddrHdr* addr)
+{
+ printf("Call to BTDevice Accept \r\n");
+ L2CAPSocket* s = (L2CAPSocket*)sock;
+ //L2CAPAddr* a = (L2CAPAddr*)addr;
+
+
+ printf("ID = %d scid = %d dcid = %d \r\n",sock->ID, s->scid, s->dcid);
+
+ return sock->ID;
+}
+
+
+
+
+// virtual SocketHandler
+int BTDevice::Send(SocketInternal* sock, const u8* data, int len)
+{
+ printf("Call to BTDevice Send \r\n");
+ 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\r\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)
+{
+ printfBytes("L2CP send: ",data,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);
+}
+
+int BTDevice::AcceptResponse(u8 rxid, int scid, int dcid)
+{
+ printf("Connection accepted \r\n");
+ u16 p[4];
+ p[0] = scid;
+ p[1] = dcid;
+ p[2] = L2CAP_CONN_SUCCESS;
+ p[3] = 0;
+
+ return Send(L2CAP_CONN_RSP,rxid,p,4);
+
+}
+
+int BTDevice::RefuseResponse(u8 rxid)
+{
+ printf("Connection refused \r\n");
+ u16 p[2];
+ p[0] = L2CAP_CONN_REF_PSM;
+ p[1] = 0;
+
+ return Send(L2CAP_CONN_RSP,rxid,p,2);
+}
+
+
+//int BTDevice::InUse(int psm)
+//{
+// for (int i = 0; i < MAX_PORTS; i++){
+// printf("Listen Q %d = %d \r\n",i, _listen[i]);
+// if ( _listen[i] == psm ) {
+// printf("We are listening on port %d \r\n",psm); //in use
+// return 0;
+// }
+// }
+// printf("We are not listening on port %d \r\n",psm);
+// return 1;
+//}
+
+
+
+
+void BTDevice::Control(const BD_ADDR* addr, const u8* data, int len)
+{
+
+ SocketInternal* sock;
+ L2CAPSocket* s;
+ int psm,scid,dcid/*,flags*/;
+
+ printf("From address ");
+ printf(addr);
+ printf(" : ");
+
+ int cc = data[8];
+ printf(L2CAP_ComandCodeStr(cc));
+ int result = LE16(data+16);
+ printf(" Result %d\r\n",result);
+
+
+ switch (cc)
+ {
+ case L2CAP_COMMAND_REJ:
+ break;
+ case L2CAP_CONN_REQ:
+ {
+ psm = LE16(data+12);
+ scid = LE16(data+14);
+ printf("Connection request scid = %d psm = %d \r\n",scid,psm);
+
+ // check if we listen on the port
+ //if ( InUse(psm) ) {
+ if ( Socket_InUse(SOCKET_L2CAP,psm) ) {
+ RefuseResponse(data[9]);
+
+
+ } else {
+ L2CAPAddr sockAddr;
+ sockAddr.bdaddr = *addr;
+ sock = Socket_Create(SOCKET_L2CAP, &sockAddr.hdr, psm);
+ s = (L2CAPSocket*)sock;
+ s->scid = 0x40 + sock->ID-1; // are these reserved?
+ s->dcid = scid;
+ AcceptResponse(data[9],s->scid,s->dcid);
+
+ ConfigureRequest(s->dcid); // handshake
+
+ sock->SetState(SocketState_Accept);
+ }
+
+ /**
+ for (int i = 0; i < MAX_PORTS; i++){
+ if ( _listen[i] == psm ) {
+ printf("We are listening on port %d \r\n",psm); //in use
+ RefuseResponse(data[9]);
+ break;
+ }
+ }
+ **/
+
+ //L2CAPAddr sockAddr;
+ //sockAddr.bdaddr = addr;
+ //sockAddr.psm = psm;
+
+ }
+ break;
+
+ // Response to our initial connect from Remote
+ case L2CAP_CONN_RSP:
+ {
+ if (result == 0)
+ {
+ printf("Connect succeeded\r\n");
+ dcid = LE16(data+12);
+ scid = LE16(data+14);
+ L2CAPSocket* s = SCIDToSocket(scid);
+ if (s)
+ {
+ s->dcid = dcid;
+ ConfigureRequest(dcid);
+ }
+ } else
+ printf("Connect failed?\r\n");
+ }
+ break;
+
+ case L2CAP_CONF_RSP:
+ {
+ scid = LE16(data+12);
+ SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
+ if (s)
+ s->SetState(SocketState_Open);
+ }
+ break;
+
+ case L2CAP_CONF_REQ:
+ {
+ u16 dcid = LE16(data+12);
+ u16 flags = LE16(data+14);
+ printf("Config request dcid = %02X flags = %02X\r\n", dcid, flags);
+ scid = LE16(data+12);
+ L2CAPSocket* s = SCIDToSocket(scid);
+ if (s)
+ ConfigureResponse(data[9],s->dcid);
+ }
+ break;
+ case L2CAP_DISCONN_REQ: {
+ int dcid = LE16(data+12);
+ int scid = LE16(data+14);
+ L2CAPSocket* s = SCIDToSocket(scid);
+ //s->si.SetState(SocketState_Closed);
+ DisconnectResponse(data[9], scid, dcid);
+ }
+ break;
+
+ }
+}
+
+void BTDevice::ACLRecv(const BD_ADDR* addr, const u8* data, int len)
+{
+ //printfBytes("L2CP recv: ",data,16);
+ int handle = LE16(data);
+ if (handle != (0x2000 | _handle))
+ return;
+
+ int cid = LE16(data+6);
+ if (cid == 1)
+ {
+ Control(addr,data,len);
+ return;
+ }
+
+ SocketInternal* s = (SocketInternal*)SCIDToSocket(cid);
+ if (s)
+ s->Recv(data+8,LE16(data+2)-4);
+ else
+ printf("Bad event cid %d\r\n",cid);
+}