This is a for debugging \\\\\\\"BLUE USB\\\\\\\". You can connect with HCI mode. How to connect White Wizard Board TANK *White Wizard Board - Motor Driver Board * p 21 - IN_R1 * p 22 - IN_R2 * p 23 - IN_L2 * p 24 - IN_L1
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 // 00089 void BTDevice::Init() 00090 { 00091 memset(&_info,0,sizeof(inquiry_info)); 00092 _handle = 0; 00093 _name[0] = 0; 00094 _state = 0; 00095 } 00096 00097 // virtual SocketHandler 00098 int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr) 00099 { 00100 L2CAPSocket* s = (L2CAPSocket*)sock; 00101 L2CAPAddr* a = (L2CAPAddr*)addr; 00102 s->scid = 0x40 + sock->ID-1; // are these reserved? 00103 s->dcid = 0; 00104 Connect(s->scid,a->psm); 00105 return sock->ID; 00106 } 00107 00108 // virtual SocketHandler 00109 int BTDevice::Send(SocketInternal* sock, const u8* data, int len) 00110 { 00111 L2CAPData d; 00112 L2CAPSocket* s = (L2CAPSocket*)sock; 00113 00114 d.handle = _handle | 0x2000; 00115 d.length = 4 + len; 00116 d.l2capLength = len; 00117 d.cid = s->dcid; 00118 00119 if (len > 64) 00120 return -1; 00121 memcpy(d.data,data,len); 00122 return Send((u8*)&d,len+8); 00123 } 00124 00125 // virtual SocketHandler 00126 int BTDevice::Close(SocketInternal* sock) 00127 { 00128 printf("L2CAP close %d\n",sock->ID); 00129 L2CAPSocket* s = (L2CAPSocket*)sock; 00130 return Disconnect(s->scid,s->dcid); 00131 } 00132 00133 L2CAPSocket* BTDevice::SCIDToSocket(int scid) 00134 { 00135 return (L2CAPSocket*)GetSocketInternal(scid-0x40+1); 00136 } 00137 00138 int BTDevice::Send(const u8* data, int len) 00139 { 00140 //-----y.k----- 00141 int i; 00142 printf("-----y.k-----\n"); 00143 printf("-----ACLSend() :\n data[] = "); 00144 00145 for(i=0;i<sizeof(data);++i){ 00146 printf(" 0x%x : " ,data[i]); 00147 } 00148 00149 printf("len = %d \n", sizeof(data)); 00150 printf("-----y.k-----\n"); 00151 00152 //-----y.k----- 00153 00154 _transport->ACLSend(data,len); 00155 return 0; 00156 } 00157 00158 int BTDevice::Send(u8 c, u8 id, u16* params, int count) 00159 { 00160 L2CAPCmd cmd; 00161 cmd.handle = _handle | 0x2000; 00162 cmd.length = 8 + count*2; 00163 00164 cmd.l2capLength = cmd.length-4; 00165 cmd.cid = 1; // Signaling packet 00166 00167 cmd.cmd = c; 00168 cmd.id = id; 00169 cmd.cmdLength = count*2; 00170 for (int i = 0; i < count; i++) 00171 cmd.params[i] = params[i]; 00172 return Send((u8*)&cmd,cmd.length+4); 00173 } 00174 00175 int BTDevice::Connect(int scid, int psm) 00176 { 00177 u16 p[2]; 00178 p[0] = psm; 00179 p[1] = scid; 00180 return Send(L2CAP_CONN_REQ,_txid++,p,2); 00181 } 00182 00183 int BTDevice::Disconnect(int scid, int dcid) 00184 { 00185 u16 p[2]; 00186 p[0] = dcid; 00187 p[1] = scid; 00188 return Send(L2CAP_DISCONN_REQ,_txid++,p,2); 00189 } 00190 00191 int BTDevice::ConfigureRequest(int dcid) 00192 { 00193 u16 p[4]; 00194 p[0] = dcid; 00195 p[1] = 0; 00196 p[2] = 0x0201; // Options 00197 p[3] = 0x02A0; // 672 00198 return Send(L2CAP_CONF_REQ,_txid++,p,4); 00199 } 00200 00201 int BTDevice::ConfigureResponse(u8 rxid, int dcid) 00202 { 00203 u16 p[3]; 00204 p[0] = dcid; 00205 p[1] = 0; 00206 p[2] = 0; 00207 return Send(L2CAP_CONF_RSP,rxid,p,3); 00208 } 00209 00210 int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid) 00211 { 00212 u16 p[2]; 00213 p[0] = dcid; 00214 p[1] = scid; 00215 return Send(L2CAP_DISCONN_RSP,rxid,p,2); 00216 } 00217 00218 void BTDevice::Control(const u8* data, int len) 00219 { 00220 int cc = data[8]; 00221 printf(L2CAP_ComandCodeStr(cc)); 00222 int result = LE16(data+16); 00223 printf(" Result %d\n",result); 00224 switch (cc) 00225 { 00226 case L2CAP_COMMAND_REJ: 00227 break; 00228 case L2CAP_CONN_REQ: 00229 break; 00230 00231 // Response to our initial connect from Remote 00232 case L2CAP_CONN_RSP: 00233 { 00234 if (result == 0) 00235 { 00236 int dcid = LE16(data+12); 00237 int scid = LE16(data+14); 00238 L2CAPSocket* s = SCIDToSocket(scid); 00239 if (s) 00240 { 00241 s->dcid = dcid; 00242 ConfigureRequest(dcid); 00243 } 00244 } else 00245 printf("Connect failed?\n"); 00246 } 00247 break; 00248 00249 case L2CAP_CONF_RSP: 00250 { 00251 int scid = LE16(data+12); 00252 SocketInternal* s = (SocketInternal*)SCIDToSocket(scid); 00253 if (s) 00254 s->SetState(SocketState_Open); 00255 } 00256 break; 00257 00258 case L2CAP_CONF_REQ: 00259 { 00260 int scid = LE16(data+12); 00261 L2CAPSocket* s = SCIDToSocket(scid); 00262 if (s) 00263 ConfigureResponse(data[9],s->dcid); 00264 } 00265 break; 00266 } 00267 } 00268 00269 void BTDevice::ACLRecv(const u8* data, int len) 00270 { 00271 // printfBytes("L2CP",data,16); 00272 int handle = LE16(data); 00273 if (handle != (0x2000 | _handle)) 00274 return; 00275 00276 int cid = LE16(data+6); 00277 if (cid == 1) 00278 { 00279 Control(data,len); 00280 return; 00281 } 00282 00283 SocketInternal* s = (SocketInternal*)SCIDToSocket(cid); 00284 if (s) 00285 s->Recv(data+8,LE16(data+2)-4); 00286 else 00287 printf("Bad event cid %d\n",cid); 00288 }
Generated on Tue Jul 19 2022 02:02:54 by 1.7.2