BlueUSBをつかってみた
Dependencies: FatFileSystem mbed
Fork of PS3_BlueUSB by
L2CAP.cpp
00001 /* 00002 Copyright (c) 2010 Peter Barrett 00003 00004 Permission is hereby granted, free of charge, to any person obtaining a copy 00005 of this software and associated documentation files (the "Software"), to deal 00006 in the Software without restriction, including without limitation the rights 00007 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00008 copies of the Software, and to permit persons to whom the Software is 00009 furnished to do so, subject to the following conditions: 00010 00011 The above copyright notice and this permission notice shall be included in 00012 all copies or substantial portions of the Software. 00013 00014 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00015 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00016 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00017 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00018 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00019 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00020 THE SOFTWARE. 00021 */ 00022 00023 00024 #include <stdio.h> 00025 #include <stdlib.h> 00026 #include <stdio.h> 00027 #include <string.h> 00028 00029 #include "Utils.h" 00030 #include "hci.h" 00031 00032 #define L2CAP_COMMAND_REJ 0x01 00033 #define L2CAP_CONN_REQ 0x02 00034 #define L2CAP_CONN_RSP 0x03 00035 #define L2CAP_CONF_REQ 0x04 00036 #define L2CAP_CONF_RSP 0x05 00037 #define L2CAP_DISCONN_REQ 0x06 00038 #define L2CAP_DISCONN_RSP 0x07 00039 #define L2CAP_ECHO_REQ 0x08 00040 #define L2CAP_ECHO_RSP 0x09 00041 #define L2CAP_INFO_REQ 0x0a 00042 #define L2CAP_INFO_RSP 0x0b 00043 00044 00045 /* L2CAP command codes */ 00046 const char* L2CAP_ComandCodeStr(int c) 00047 { 00048 switch (c) 00049 { 00050 case L2CAP_COMMAND_REJ: return "L2CAP_COMMAND_REJ"; 00051 case L2CAP_CONN_REQ: return "L2CAP_CONN_REQ"; 00052 case L2CAP_CONN_RSP: return "L2CAP_CONN_RSP"; 00053 case L2CAP_CONF_REQ: return "L2CAP_CONF_REQ"; 00054 case L2CAP_CONF_RSP: return "L2CAP_CONF_RSP"; 00055 case L2CAP_DISCONN_REQ: return "L2CAP_DISCONN_REQ"; 00056 case L2CAP_DISCONN_RSP: return "L2CAP_DISCONN_RSP"; 00057 case L2CAP_ECHO_REQ: return "L2CAP_ECHO_REQ"; 00058 case L2CAP_ECHO_RSP: return "L2CAP_ECHO_RSP"; 00059 case L2CAP_INFO_REQ: return "L2CAP_INFO_REQ"; 00060 case L2CAP_INFO_RSP: return "L2CAP_INFO_RSP"; 00061 } 00062 return "unknown"; 00063 } 00064 00065 typedef struct 00066 { 00067 u16 handle; 00068 u16 length; // total 00069 u16 l2capLength; // length -4 00070 u16 cid; // Signaling packet CID = 1 00071 u8 data[64]; // Largest thing to send!!! todo 00072 } L2CAPData; 00073 00074 typedef struct 00075 { 00076 u16 handle; 00077 u16 length; // total 00078 u16 l2capLength; // length -4 00079 u16 cid; // Signaling packet CID = 1 00080 00081 // Payload 00082 u8 cmd; // 00083 u8 id; 00084 u16 cmdLength; // total-8 00085 u16 params[4]; // Params 00086 } L2CAPCmd; 00087 00088 void printf(const BD_ADDR* addr); 00089 00090 // 00091 void BTDevice::Init() 00092 { 00093 memset(&_info,0,sizeof(inquiry_info)); 00094 _handle = 0; 00095 _name[0] = 0; 00096 _state = 0; 00097 } 00098 00099 // virtual SocketHandler 00100 int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr) 00101 { 00102 printf("Call to BTDevice Open \r\n"); 00103 L2CAPSocket* s = (L2CAPSocket*)sock; 00104 L2CAPAddr* a = (L2CAPAddr*)addr; 00105 s->scid = 0x40 + sock->ID-1; // are these reserved? 00106 s->dcid = 0; 00107 Connect(s->scid,a->psm); 00108 return sock->ID; 00109 } 00110 00111 // virtual SocketHandler 00112 int BTDevice::Create(SocketInternal* sock, SocketAddrHdr* addr) 00113 { 00114 printf("Call to BTDevice Create \r\n"); 00115 L2CAPSocket* s = (L2CAPSocket*)sock; 00116 L2CAPAddr* a = (L2CAPAddr*)addr; 00117 00118 //Connect(s->scid,a->psm); 00119 return sock->ID; 00120 } 00121 00122 int BTDevice::Accept(SocketInternal* sock, SocketAddrHdr* addr) 00123 { 00124 printf("Call to BTDevice Accept \r\n"); 00125 L2CAPSocket* s = (L2CAPSocket*)sock; 00126 L2CAPAddr* a = (L2CAPAddr*)addr; 00127 00128 00129 printf("ID = %d scid = %d dcid = %d \r\n",sock->ID, s->scid, s->dcid); 00130 00131 return sock->ID; 00132 } 00133 00134 00135 00136 00137 // virtual SocketHandler 00138 int BTDevice::Send(SocketInternal* sock, const u8* data, int len) 00139 { 00140 printf("Call to BTDevice Send \r\n"); 00141 L2CAPData d; 00142 L2CAPSocket* s = (L2CAPSocket*)sock; 00143 00144 d.handle = _handle | 0x2000; 00145 d.length = 4 + len; 00146 d.l2capLength = len; 00147 d.cid = s->dcid; 00148 00149 if (len > 64) 00150 return -1; 00151 memcpy(d.data,data,len); 00152 return Send((u8*)&d,len+8); 00153 } 00154 00155 // virtual SocketHandler 00156 int BTDevice::Close(SocketInternal* sock) 00157 { 00158 printf("L2CAP close %d\r\n",sock->ID); 00159 L2CAPSocket* s = (L2CAPSocket*)sock; 00160 return Disconnect(s->scid,s->dcid); 00161 } 00162 00163 L2CAPSocket* BTDevice::SCIDToSocket(int scid) 00164 { 00165 return (L2CAPSocket*)GetSocketInternal(scid-0x40+1); 00166 } 00167 00168 int BTDevice::Send(const u8* data, int len) 00169 { 00170 printfBytes("L2CP send: ",data,len); 00171 _transport->ACLSend(data,len); 00172 return 0; 00173 } 00174 00175 int BTDevice::Send(u8 c, u8 id, u16* params, int count) 00176 { 00177 L2CAPCmd cmd; 00178 cmd.handle = _handle | 0x2000; 00179 cmd.length = 8 + count*2; 00180 00181 cmd.l2capLength = cmd.length-4; 00182 cmd.cid = 1; // Signaling packet 00183 00184 cmd.cmd = c; 00185 cmd.id = id; 00186 cmd.cmdLength = count*2; 00187 for (int i = 0; i < count; i++) 00188 cmd.params[i] = params[i]; 00189 return Send((u8*)&cmd,cmd.length+4); 00190 } 00191 00192 int BTDevice::Connect(int scid, int psm) 00193 { 00194 u16 p[2]; 00195 p[0] = psm; 00196 p[1] = scid; 00197 return Send(L2CAP_CONN_REQ,_txid++,p,2); 00198 } 00199 00200 int BTDevice::Disconnect(int scid, int dcid) 00201 { 00202 u16 p[2]; 00203 p[0] = dcid; 00204 p[1] = scid; 00205 return Send(L2CAP_DISCONN_REQ,_txid++,p,2); 00206 } 00207 00208 int BTDevice::ConfigureRequest(int dcid) 00209 { 00210 u16 p[4]; 00211 p[0] = dcid; 00212 p[1] = 0; 00213 p[2] = 0x0201; // Options 00214 p[3] = 0x02A0; // 672 00215 return Send(L2CAP_CONF_REQ,_txid++,p,4); 00216 } 00217 00218 int BTDevice::ConfigureResponse(u8 rxid, int dcid) 00219 { 00220 u16 p[3]; 00221 p[0] = dcid; 00222 p[1] = 0; 00223 p[2] = 0; 00224 return Send(L2CAP_CONF_RSP,rxid,p,3); 00225 } 00226 00227 int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid) 00228 { 00229 u16 p[2]; 00230 p[0] = dcid; 00231 p[1] = scid; 00232 return Send(L2CAP_DISCONN_RSP,rxid,p,2); 00233 } 00234 00235 int BTDevice::AcceptResponse(u8 rxid, int scid, int dcid) 00236 { 00237 printf("Connection accepted \r\n"); 00238 u16 p[4]; 00239 p[0] = scid; 00240 p[1] = dcid; 00241 p[2] = L2CAP_CONN_SUCCESS; 00242 p[3] = 0; 00243 00244 return Send(L2CAP_CONN_RSP,rxid,p,4); 00245 00246 } 00247 00248 int BTDevice::RefuseResponse(u8 rxid) 00249 { 00250 printf("Connection refused \r\n"); 00251 u16 p[2]; 00252 p[0] = L2CAP_CONN_REF_PSM; 00253 p[1] = 0; 00254 00255 return Send(L2CAP_CONN_RSP,rxid,p,2); 00256 } 00257 00258 00259 //int BTDevice::InUse(int psm) 00260 //{ 00261 // for (int i = 0; i < MAX_PORTS; i++){ 00262 // printf("Listen Q %d = %d \r\n",i, _listen[i]); 00263 // if ( _listen[i] == psm ) { 00264 // printf("We are listening on port %d \r\n",psm); //in use 00265 // return 0; 00266 // } 00267 // } 00268 // printf("We are not listening on port %d \r\n",psm); 00269 // return 1; 00270 //} 00271 00272 00273 00274 00275 void BTDevice::Control(const BD_ADDR* addr, const u8* data, int len) 00276 { 00277 00278 SocketInternal* sock; 00279 L2CAPSocket* s; 00280 int psm,scid,dcid,flags; 00281 00282 printf("From address "); 00283 printf(addr); 00284 printf(" : "); 00285 00286 int cc = data[8]; 00287 printf(L2CAP_ComandCodeStr(cc)); 00288 int result = LE16(data+16); 00289 printf(" Result %d\r\n",result); 00290 00291 00292 switch (cc) 00293 { 00294 case L2CAP_COMMAND_REJ: 00295 break; 00296 case L2CAP_CONN_REQ: 00297 { 00298 psm = LE16(data+12); 00299 scid = LE16(data+14); 00300 printf("Connection request scid = %d psm = %d \r\n",scid,psm); 00301 00302 // check if we listen on the port 00303 //if ( InUse(psm) ) { 00304 if ( Socket_InUse(SOCKET_L2CAP,psm) ) { 00305 RefuseResponse(data[9]); 00306 00307 00308 } else { 00309 L2CAPAddr sockAddr; 00310 sockAddr.bdaddr = *addr; 00311 sock = Socket_Create(SOCKET_L2CAP, &sockAddr.hdr, psm); 00312 s = (L2CAPSocket*)sock; 00313 s->scid = 0x40 + sock->ID-1; // are these reserved? 00314 s->dcid = scid; 00315 AcceptResponse(data[9],s->scid,s->dcid); 00316 00317 ConfigureRequest(s->dcid); // handshake 00318 00319 sock->SetState(SocketState_Accept); 00320 } 00321 00322 /** 00323 for (int i = 0; i < MAX_PORTS; i++){ 00324 if ( _listen[i] == psm ) { 00325 printf("We are listening on port %d \r\n",psm); //in use 00326 RefuseResponse(data[9]); 00327 break; 00328 } 00329 } 00330 **/ 00331 00332 //L2CAPAddr sockAddr; 00333 //sockAddr.bdaddr = addr; 00334 //sockAddr.psm = psm; 00335 00336 } 00337 break; 00338 00339 // Response to our initial connect from Remote 00340 case L2CAP_CONN_RSP: 00341 { 00342 if (result == 0) 00343 { 00344 printf("Connect succeeded\r\n"); 00345 dcid = LE16(data+12); 00346 scid = LE16(data+14); 00347 L2CAPSocket* s = SCIDToSocket(scid); 00348 if (s) 00349 { 00350 s->dcid = dcid; 00351 ConfigureRequest(dcid); 00352 } 00353 } else 00354 printf("Connect failed?\r\n"); 00355 } 00356 break; 00357 00358 case L2CAP_CONF_RSP: 00359 { 00360 scid = LE16(data+12); 00361 SocketInternal* s = (SocketInternal*)SCIDToSocket(scid); 00362 if (s) 00363 s->SetState(SocketState_Open); 00364 } 00365 break; 00366 00367 case L2CAP_CONF_REQ: 00368 { 00369 u16 dcid = LE16(data+12); 00370 u16 flags = LE16(data+14); 00371 printf("Config request dcid = %02X flags = %02X\r\n", dcid, flags); 00372 scid = LE16(data+12); 00373 L2CAPSocket* s = SCIDToSocket(scid); 00374 if (s) 00375 ConfigureResponse(data[9],s->dcid); 00376 } 00377 break; 00378 case L2CAP_DISCONN_REQ: { 00379 int dcid = LE16(data+12); 00380 int scid = LE16(data+14); 00381 L2CAPSocket* s = SCIDToSocket(scid); 00382 //s->si.SetState(SocketState_Closed); 00383 DisconnectResponse(data[9], scid, dcid); 00384 } 00385 break; 00386 00387 } 00388 } 00389 00390 void BTDevice::ACLRecv(const BD_ADDR* addr, const u8* data, int len) 00391 { 00392 //printfBytes("L2CP recv: ",data,16); 00393 int handle = LE16(data); 00394 if (handle != (0x2000 | _handle)) 00395 return; 00396 00397 int cid = LE16(data+6); 00398 if (cid == 1) 00399 { 00400 Control(addr,data,len); 00401 return; 00402 } 00403 00404 SocketInternal* s = (SocketInternal*)SCIDToSocket(cid); 00405 if (s) 00406 s->Recv(data+8,LE16(data+2)-4); 00407 else 00408 printf("Bad event cid %d\r\n",cid); 00409 }
Generated on Wed Jul 13 2022 12:19:22 by 1.7.2