![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PS3下の階
Dependencies: FatFileSystem PS3_BlueUSB QEI RoboClaw_mine_ mbed
Fork of PS3_BlueUSB by
Diff: L2CAP.cpp
- Revision:
- 0:99a111b75cb4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/L2CAP.cpp Tue Apr 26 16:09:17 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); +}