Peter Cooper / Mbed 2 deprecated LazySusan

A USB/360deg Rotational device, communicating via blue-tooth, test code

Dependencies:   FatFileSystem mbed

L2CAP.cpp@0:87da38093be9, 2012-12-21 (annotated)

Committer:
lolpcc
Date:
Fri Dec 21 11:37:38 2012 +0000
Revision:
0:87da38093be9
Used to test the Lazy Susan at work, hence the A2D code in Utils.cpp

Who changed what in which revision?

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