Code to control a Traxster robot using a Wimote and Android app
Fork of BlueUSB by
Diff: L2CAP.cpp
- Revision:
- 1:accdaa84fe8d
- Parent:
- 0:606b230e5b4a
diff -r 606b230e5b4a -r accdaa84fe8d L2CAP.cpp --- a/L2CAP.cpp Sat Apr 10 00:30:24 2010 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,274 +0,0 @@ -/* -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); -}