Birdy YAP / Birdy_BlueWIi

Dependents:   Test_Wiimote

L2CAP.cpp@2:5c2bfbd63297, 2011-12-17 (annotated)

Committer:
bediyap
Date:
Sat Dec 17 06:53:39 2011 +0000
Revision:
2:5c2bfbd63297
Parent:
0:f6f434d9a03a 
Add Wii rumble, led on/off support

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bediyap 0:f6f434d9a03a 1 /*
bediyap 0:f6f434d9a03a 2 Copyright (c) 2010 Peter Barrett
bediyap 0:f6f434d9a03a 3
bediyap 0:f6f434d9a03a 4 Permission is hereby granted, free of charge, to any person obtaining a copy
bediyap 0:f6f434d9a03a 5 of this software and associated documentation files (the "Software"), to deal
bediyap 0:f6f434d9a03a 6 in the Software without restriction, including without limitation the rights
bediyap 0:f6f434d9a03a 7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
bediyap 0:f6f434d9a03a 8 copies of the Software, and to permit persons to whom the Software is
bediyap 0:f6f434d9a03a 9 furnished to do so, subject to the following conditions:
bediyap 0:f6f434d9a03a 10
bediyap 0:f6f434d9a03a 11 The above copyright notice and this permission notice shall be included in
bediyap 0:f6f434d9a03a 12 all copies or substantial portions of the Software.
bediyap 0:f6f434d9a03a 13
bediyap 0:f6f434d9a03a 14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
bediyap 0:f6f434d9a03a 15 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
bediyap 0:f6f434d9a03a 16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
bediyap 0:f6f434d9a03a 17 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
bediyap 0:f6f434d9a03a 18 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
bediyap 0:f6f434d9a03a 19 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
bediyap 0:f6f434d9a03a 20 THE SOFTWARE.
bediyap 0:f6f434d9a03a 21 */
bediyap 0:f6f434d9a03a 22
bediyap 0:f6f434d9a03a 23
bediyap 0:f6f434d9a03a 24 #include <stdio.h>
bediyap 0:f6f434d9a03a 25 #include <stdlib.h>
bediyap 0:f6f434d9a03a 26 #include <stdio.h>
bediyap 0:f6f434d9a03a 27 #include <string.h>
bediyap 0:f6f434d9a03a 28
bediyap 0:f6f434d9a03a 29 #include "Utils.h"
bediyap 0:f6f434d9a03a 30 #include "hci.h"
bediyap 0:f6f434d9a03a 31
bediyap 0:f6f434d9a03a 32 #define L2CAP_COMMAND_REJ 0x01
bediyap 0:f6f434d9a03a 33 #define L2CAP_CONN_REQ 0x02
bediyap 0:f6f434d9a03a 34 #define L2CAP_CONN_RSP 0x03
bediyap 0:f6f434d9a03a 35 #define L2CAP_CONF_REQ 0x04
bediyap 0:f6f434d9a03a 36 #define L2CAP_CONF_RSP 0x05
bediyap 0:f6f434d9a03a 37 #define L2CAP_DISCONN_REQ 0x06
bediyap 0:f6f434d9a03a 38 #define L2CAP_DISCONN_RSP 0x07
bediyap 0:f6f434d9a03a 39 #define L2CAP_ECHO_REQ 0x08
bediyap 0:f6f434d9a03a 40 #define L2CAP_ECHO_RSP 0x09
bediyap 0:f6f434d9a03a 41 #define L2CAP_INFO_REQ 0x0a
bediyap 0:f6f434d9a03a 42 #define L2CAP_INFO_RSP 0x0b
bediyap 0:f6f434d9a03a 43
bediyap 0:f6f434d9a03a 44
bediyap 0:f6f434d9a03a 45 /* L2CAP command codes */
bediyap 0:f6f434d9a03a 46 const char* L2CAP_ComandCodeStr(int c)
bediyap 0:f6f434d9a03a 47 {
bediyap 0:f6f434d9a03a 48 switch (c)
bediyap 0:f6f434d9a03a 49 {
bediyap 0:f6f434d9a03a 50 case L2CAP_COMMAND_REJ: return "L2CAP_COMMAND_REJ";
bediyap 0:f6f434d9a03a 51 case L2CAP_CONN_REQ: return "L2CAP_CONN_REQ";
bediyap 0:f6f434d9a03a 52 case L2CAP_CONN_RSP: return "L2CAP_CONN_RSP";
bediyap 0:f6f434d9a03a 53 case L2CAP_CONF_REQ: return "L2CAP_CONF_REQ";
bediyap 0:f6f434d9a03a 54 case L2CAP_CONF_RSP: return "L2CAP_CONF_RSP";
bediyap 0:f6f434d9a03a 55 case L2CAP_DISCONN_REQ: return "L2CAP_DISCONN_REQ";
bediyap 0:f6f434d9a03a 56 case L2CAP_DISCONN_RSP: return "L2CAP_DISCONN_RSP";
bediyap 0:f6f434d9a03a 57 case L2CAP_ECHO_REQ: return "L2CAP_ECHO_REQ";
bediyap 0:f6f434d9a03a 58 case L2CAP_ECHO_RSP: return "L2CAP_ECHO_RSP";
bediyap 0:f6f434d9a03a 59 case L2CAP_INFO_REQ: return "L2CAP_INFO_REQ";
bediyap 0:f6f434d9a03a 60 case L2CAP_INFO_RSP: return "L2CAP_INFO_RSP";
bediyap 0:f6f434d9a03a 61 }
bediyap 0:f6f434d9a03a 62 return "unknown";
bediyap 0:f6f434d9a03a 63 }
bediyap 0:f6f434d9a03a 64
bediyap 0:f6f434d9a03a 65 typedef struct
bediyap 0:f6f434d9a03a 66 {
bediyap 0:f6f434d9a03a 67 u16 handle;
bediyap 0:f6f434d9a03a 68 u16 length; // total
bediyap 0:f6f434d9a03a 69 u16 l2capLength; // length -4
bediyap 0:f6f434d9a03a 70 u16 cid; // Signaling packet CID = 1
bediyap 0:f6f434d9a03a 71 u8 data[64]; // Largest thing to send!!! todo
bediyap 0:f6f434d9a03a 72 } L2CAPData;
bediyap 0:f6f434d9a03a 73
bediyap 0:f6f434d9a03a 74 typedef struct
bediyap 0:f6f434d9a03a 75 {
bediyap 0:f6f434d9a03a 76 u16 handle;
bediyap 0:f6f434d9a03a 77 u16 length; // total
bediyap 0:f6f434d9a03a 78 u16 l2capLength; // length -4
bediyap 0:f6f434d9a03a 79 u16 cid; // Signaling packet CID = 1
bediyap 0:f6f434d9a03a 80
bediyap 0:f6f434d9a03a 81 // Payload
bediyap 0:f6f434d9a03a 82 u8 cmd; //
bediyap 0:f6f434d9a03a 83 u8 id;
bediyap 0:f6f434d9a03a 84 u16 cmdLength; // total-8
bediyap 0:f6f434d9a03a 85 u16 params[4]; // Params
bediyap 0:f6f434d9a03a 86 } L2CAPCmd;
bediyap 0:f6f434d9a03a 87
bediyap 0:f6f434d9a03a 88 //
bediyap 0:f6f434d9a03a 89 void BTDevice::Init()
bediyap 0:f6f434d9a03a 90 {
bediyap 0:f6f434d9a03a 91 memset(&_info,0,sizeof(inquiry_info));
bediyap 0:f6f434d9a03a 92 _handle = 0;
bediyap 0:f6f434d9a03a 93 _name[0] = 0;
bediyap 0:f6f434d9a03a 94 _state = 0;
bediyap 0:f6f434d9a03a 95 }
bediyap 0:f6f434d9a03a 96
bediyap 0:f6f434d9a03a 97 // virtual SocketHandler
bediyap 0:f6f434d9a03a 98 int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr)
bediyap 0:f6f434d9a03a 99 {
bediyap 0:f6f434d9a03a 100 L2CAPSocket* s = (L2CAPSocket*)sock;
bediyap 0:f6f434d9a03a 101 L2CAPAddr* a = (L2CAPAddr*)addr;
bediyap 0:f6f434d9a03a 102 s->scid = 0x40 + sock->ID-1; // are these reserved?
bediyap 0:f6f434d9a03a 103 s->dcid = 0;
bediyap 0:f6f434d9a03a 104 Connect(s->scid,a->psm);
bediyap 0:f6f434d9a03a 105 return sock->ID;
bediyap 0:f6f434d9a03a 106 }
bediyap 0:f6f434d9a03a 107
bediyap 0:f6f434d9a03a 108 // virtual SocketHandler
bediyap 0:f6f434d9a03a 109 int BTDevice::Send(SocketInternal* sock, const u8* data, int len)
bediyap 0:f6f434d9a03a 110 {
bediyap 0:f6f434d9a03a 111 L2CAPData d;
bediyap 0:f6f434d9a03a 112 L2CAPSocket* s = (L2CAPSocket*)sock;
bediyap 0:f6f434d9a03a 113
bediyap 0:f6f434d9a03a 114 d.handle = _handle | 0x2000;
bediyap 0:f6f434d9a03a 115 d.length = 4 + len;
bediyap 0:f6f434d9a03a 116 d.l2capLength = len;
bediyap 0:f6f434d9a03a 117 d.cid = s->dcid;
bediyap 0:f6f434d9a03a 118
bediyap 0:f6f434d9a03a 119 if (len > 64)
bediyap 0:f6f434d9a03a 120 return -1;
bediyap 0:f6f434d9a03a 121 memcpy(d.data,data,len);
bediyap 0:f6f434d9a03a 122 return Send((u8*)&d,len+8);
bediyap 0:f6f434d9a03a 123 }
bediyap 0:f6f434d9a03a 124
bediyap 0:f6f434d9a03a 125 // virtual SocketHandler
bediyap 0:f6f434d9a03a 126 int BTDevice::Close(SocketInternal* sock)
bediyap 0:f6f434d9a03a 127 {
bediyap 2:5c2bfbd63297 128 #ifdef DEBUG_WII
bediyap 0:f6f434d9a03a 129 printf("L2CAP close %d\n",sock->ID);
bediyap 2:5c2bfbd63297 130 #endif
bediyap 0:f6f434d9a03a 131 L2CAPSocket* s = (L2CAPSocket*)sock;
bediyap 0:f6f434d9a03a 132 return Disconnect(s->scid,s->dcid);
bediyap 0:f6f434d9a03a 133 }
bediyap 0:f6f434d9a03a 134
bediyap 0:f6f434d9a03a 135 L2CAPSocket* BTDevice::SCIDToSocket(int scid)
bediyap 0:f6f434d9a03a 136 {
bediyap 0:f6f434d9a03a 137 return (L2CAPSocket*)GetSocketInternal(scid-0x40+1);
bediyap 0:f6f434d9a03a 138 }
bediyap 0:f6f434d9a03a 139
bediyap 0:f6f434d9a03a 140 int BTDevice::Send(const u8* data, int len)
bediyap 0:f6f434d9a03a 141 {
bediyap 0:f6f434d9a03a 142 _transport->ACLSend(data,len);
bediyap 0:f6f434d9a03a 143 return 0;
bediyap 0:f6f434d9a03a 144 }
bediyap 0:f6f434d9a03a 145
bediyap 0:f6f434d9a03a 146 int BTDevice::Send(u8 c, u8 id, u16* params, int count)
bediyap 0:f6f434d9a03a 147 {
bediyap 0:f6f434d9a03a 148 L2CAPCmd cmd;
bediyap 0:f6f434d9a03a 149 cmd.handle = _handle | 0x2000;
bediyap 0:f6f434d9a03a 150 cmd.length = 8 + count*2;
bediyap 0:f6f434d9a03a 151
bediyap 0:f6f434d9a03a 152 cmd.l2capLength = cmd.length-4;
bediyap 0:f6f434d9a03a 153 cmd.cid = 1; // Signaling packet
bediyap 0:f6f434d9a03a 154
bediyap 0:f6f434d9a03a 155 cmd.cmd = c;
bediyap 0:f6f434d9a03a 156 cmd.id = id;
bediyap 0:f6f434d9a03a 157 cmd.cmdLength = count*2;
bediyap 0:f6f434d9a03a 158 for (int i = 0; i < count; i++)
bediyap 0:f6f434d9a03a 159 cmd.params[i] = params[i];
bediyap 0:f6f434d9a03a 160 return Send((u8*)&cmd,cmd.length+4);
bediyap 0:f6f434d9a03a 161 }
bediyap 0:f6f434d9a03a 162
bediyap 0:f6f434d9a03a 163 int BTDevice::Connect(int scid, int psm)
bediyap 0:f6f434d9a03a 164 {
bediyap 0:f6f434d9a03a 165 u16 p[2];
bediyap 0:f6f434d9a03a 166 p[0] = psm;
bediyap 0:f6f434d9a03a 167 p[1] = scid;
bediyap 0:f6f434d9a03a 168 return Send(L2CAP_CONN_REQ,_txid++,p,2);
bediyap 0:f6f434d9a03a 169 }
bediyap 0:f6f434d9a03a 170
bediyap 0:f6f434d9a03a 171 int BTDevice::Disconnect(int scid, int dcid)
bediyap 0:f6f434d9a03a 172 {
bediyap 0:f6f434d9a03a 173 u16 p[2];
bediyap 0:f6f434d9a03a 174 p[0] = dcid;
bediyap 0:f6f434d9a03a 175 p[1] = scid;
bediyap 0:f6f434d9a03a 176 return Send(L2CAP_DISCONN_REQ,_txid++,p,2);
bediyap 0:f6f434d9a03a 177 }
bediyap 0:f6f434d9a03a 178
bediyap 0:f6f434d9a03a 179 int BTDevice::ConfigureRequest(int dcid)
bediyap 0:f6f434d9a03a 180 {
bediyap 0:f6f434d9a03a 181 u16 p[4];
bediyap 0:f6f434d9a03a 182 p[0] = dcid;
bediyap 0:f6f434d9a03a 183 p[1] = 0;
bediyap 0:f6f434d9a03a 184 p[2] = 0x0201; // Options
bediyap 0:f6f434d9a03a 185 p[3] = 0x02A0; // 672
bediyap 0:f6f434d9a03a 186 return Send(L2CAP_CONF_REQ,_txid++,p,4);
bediyap 0:f6f434d9a03a 187 }
bediyap 0:f6f434d9a03a 188
bediyap 0:f6f434d9a03a 189 int BTDevice::ConfigureResponse(u8 rxid, int dcid)
bediyap 0:f6f434d9a03a 190 {
bediyap 0:f6f434d9a03a 191 u16 p[3];
bediyap 0:f6f434d9a03a 192 p[0] = dcid;
bediyap 0:f6f434d9a03a 193 p[1] = 0;
bediyap 0:f6f434d9a03a 194 p[2] = 0;
bediyap 0:f6f434d9a03a 195 return Send(L2CAP_CONF_RSP,rxid,p,3);
bediyap 0:f6f434d9a03a 196 }
bediyap 0:f6f434d9a03a 197
bediyap 0:f6f434d9a03a 198 int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid)
bediyap 0:f6f434d9a03a 199 {
bediyap 0:f6f434d9a03a 200 u16 p[2];
bediyap 0:f6f434d9a03a 201 p[0] = dcid;
bediyap 0:f6f434d9a03a 202 p[1] = scid;
bediyap 0:f6f434d9a03a 203 return Send(L2CAP_DISCONN_RSP,rxid,p,2);
bediyap 0:f6f434d9a03a 204 }
bediyap 0:f6f434d9a03a 205
bediyap 0:f6f434d9a03a 206 void BTDevice::Control(const u8* data, int len)
bediyap 0:f6f434d9a03a 207 {
bediyap 2:5c2bfbd63297 208 int cc = data[8];
bediyap 0:f6f434d9a03a 209 int result = LE16(data+16);
bediyap 2:5c2bfbd63297 210 #ifdef DEBUG_WII
bediyap 2:5c2bfbd63297 211 printf(L2CAP_ComandCodeStr(cc)) ;
bediyap 0:f6f434d9a03a 212 printf(" Result %d\n",result);
bediyap 2:5c2bfbd63297 213 #endif
bediyap 0:f6f434d9a03a 214 switch (cc)
bediyap 0:f6f434d9a03a 215 {
bediyap 0:f6f434d9a03a 216 case L2CAP_COMMAND_REJ:
bediyap 0:f6f434d9a03a 217 break;
bediyap 0:f6f434d9a03a 218 case L2CAP_CONN_REQ:
bediyap 0:f6f434d9a03a 219 break;
bediyap 0:f6f434d9a03a 220
bediyap 0:f6f434d9a03a 221 // Response to our initial connect from Remote
bediyap 0:f6f434d9a03a 222 case L2CAP_CONN_RSP:
bediyap 0:f6f434d9a03a 223 {
bediyap 0:f6f434d9a03a 224 if (result == 0)
bediyap 0:f6f434d9a03a 225 {
bediyap 0:f6f434d9a03a 226 int dcid = LE16(data+12);
bediyap 0:f6f434d9a03a 227 int scid = LE16(data+14);
bediyap 0:f6f434d9a03a 228 L2CAPSocket* s = SCIDToSocket(scid);
bediyap 0:f6f434d9a03a 229 if (s)
bediyap 0:f6f434d9a03a 230 {
bediyap 0:f6f434d9a03a 231 s->dcid = dcid;
bediyap 0:f6f434d9a03a 232 ConfigureRequest(dcid);
bediyap 0:f6f434d9a03a 233 }
bediyap 2:5c2bfbd63297 234 } else {
bediyap 2:5c2bfbd63297 235 #ifdef DEBUG_WII
bediyap 0:f6f434d9a03a 236 printf("Connect failed?\n");
bediyap 2:5c2bfbd63297 237 #endif
bediyap 2:5c2bfbd63297 238 }
bediyap 0:f6f434d9a03a 239 }
bediyap 0:f6f434d9a03a 240 break;
bediyap 0:f6f434d9a03a 241
bediyap 0:f6f434d9a03a 242 case L2CAP_CONF_RSP:
bediyap 0:f6f434d9a03a 243 {
bediyap 0:f6f434d9a03a 244 int scid = LE16(data+12);
bediyap 0:f6f434d9a03a 245 SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
bediyap 0:f6f434d9a03a 246 if (s)
bediyap 0:f6f434d9a03a 247 s->SetState(SocketState_Open);
bediyap 0:f6f434d9a03a 248 }
bediyap 0:f6f434d9a03a 249 break;
bediyap 0:f6f434d9a03a 250
bediyap 0:f6f434d9a03a 251 case L2CAP_CONF_REQ:
bediyap 0:f6f434d9a03a 252 {
bediyap 0:f6f434d9a03a 253 int scid = LE16(data+12);
bediyap 0:f6f434d9a03a 254 L2CAPSocket* s = SCIDToSocket(scid);
bediyap 0:f6f434d9a03a 255 if (s)
bediyap 0:f6f434d9a03a 256 ConfigureResponse(data[9],s->dcid);
bediyap 0:f6f434d9a03a 257 }
bediyap 0:f6f434d9a03a 258 break;
bediyap 0:f6f434d9a03a 259 }
bediyap 0:f6f434d9a03a 260 }
bediyap 0:f6f434d9a03a 261
bediyap 0:f6f434d9a03a 262 void BTDevice::ACLRecv(const u8* data, int len)
bediyap 0:f6f434d9a03a 263 {
bediyap 0:f6f434d9a03a 264 // printfBytes("L2CP",data,16);
bediyap 0:f6f434d9a03a 265 int handle = LE16(data);
bediyap 0:f6f434d9a03a 266 if (handle != (0x2000 | _handle))
bediyap 0:f6f434d9a03a 267 return;
bediyap 0:f6f434d9a03a 268
bediyap 0:f6f434d9a03a 269 int cid = LE16(data+6);
bediyap 0:f6f434d9a03a 270 if (cid == 1)
bediyap 0:f6f434d9a03a 271 {
bediyap 0:f6f434d9a03a 272 Control(data,len);
bediyap 0:f6f434d9a03a 273 return;
bediyap 0:f6f434d9a03a 274 }
bediyap 0:f6f434d9a03a 275
bediyap 0:f6f434d9a03a 276 SocketInternal* s = (SocketInternal*)SCIDToSocket(cid);
bediyap 0:f6f434d9a03a 277 if (s)
bediyap 0:f6f434d9a03a 278 s->Recv(data+8,LE16(data+2)-4);
bediyap 2:5c2bfbd63297 279 else {
bediyap 2:5c2bfbd63297 280 #ifdef DEBUG_WII
bediyap 0:f6f434d9a03a 281 printf("Bad event cid %d\n",cid);
bediyap 2:5c2bfbd63297 282 #endif
bediyap 2:5c2bfbd63297 283 }
bediyap 0:f6f434d9a03a 284 }