![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Code to control a Traxster robot using a Wimote and Android app
Fork of BlueUSB by
L2CAP.cpp
- Committer:
- peterbarrett1967
- Date:
- 2010-04-10
- Revision:
- 0:606b230e5b4a
File content as of revision 0:606b230e5b4a:
/* 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); }