Chris Styles / Mbed 2 deprecated m3pi_WiiRacing

Simons Wii controlled m3pi program

Dependencies:   mbed m3pi ID12RFIDIRQ

blueusb/L2CAP.cpp@0:0ab65a1aef12, 2011-05-18 (annotated)

Committer:
chris
Date:
Wed May 18 14:50:46 2011 +0000
Revision:
0:0ab65a1aef12
Added some printing to the m3pi screen

Who changed what in which revision?

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