| User | Revision | Line number | New contents of line |
|---|
| abe00makoto |
0:e939856c1939
|
1
|
/*
|
| abe00makoto |
0:e939856c1939
|
2
|
Copyright (c) 2010 Peter Barrett
|
| abe00makoto |
0:e939856c1939
|
3
|
|
| abe00makoto |
0:e939856c1939
|
4
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
| abe00makoto |
0:e939856c1939
|
5
|
of this software and associated documentation files (the "Software"), to deal
|
| abe00makoto |
0:e939856c1939
|
6
|
in the Software without restriction, including without limitation the rights
|
| abe00makoto |
0:e939856c1939
|
7
|
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
| abe00makoto |
0:e939856c1939
|
8
|
copies of the Software, and to permit persons to whom the Software is
|
| abe00makoto |
0:e939856c1939
|
9
|
furnished to do so, subject to the following conditions:
|
| abe00makoto |
0:e939856c1939
|
10
|
|
| abe00makoto |
0:e939856c1939
|
11
|
The above copyright notice and this permission notice shall be included in
|
| abe00makoto |
0:e939856c1939
|
12
|
all copies or substantial portions of the Software.
|
| abe00makoto |
0:e939856c1939
|
13
|
|
| abe00makoto |
0:e939856c1939
|
14
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
| abe00makoto |
0:e939856c1939
|
15
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
| abe00makoto |
0:e939856c1939
|
16
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
| abe00makoto |
0:e939856c1939
|
17
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
| abe00makoto |
0:e939856c1939
|
18
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
| abe00makoto |
0:e939856c1939
|
19
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
| abe00makoto |
0:e939856c1939
|
20
|
THE SOFTWARE.
|
| abe00makoto |
0:e939856c1939
|
21
|
*/
|
| abe00makoto |
0:e939856c1939
|
22
|
|
| abe00makoto |
0:e939856c1939
|
23
|
|
| abe00makoto |
0:e939856c1939
|
24
|
#include <stdio.h>
|
| abe00makoto |
0:e939856c1939
|
25
|
#include <stdlib.h>
|
| abe00makoto |
0:e939856c1939
|
26
|
#include <stdio.h>
|
| abe00makoto |
0:e939856c1939
|
27
|
#include <string.h>
|
| abe00makoto |
0:e939856c1939
|
28
|
|
| abe00makoto |
0:e939856c1939
|
29
|
#include "Utils.h"
|
| abe00makoto |
0:e939856c1939
|
30
|
#include "hci.h"
|
| abe00makoto |
0:e939856c1939
|
31
|
|
| abe00makoto |
0:e939856c1939
|
32
|
#define L2CAP_COMMAND_REJ 0x01
|
| abe00makoto |
0:e939856c1939
|
33
|
#define L2CAP_CONN_REQ 0x02
|
| abe00makoto |
0:e939856c1939
|
34
|
#define L2CAP_CONN_RSP 0x03
|
| abe00makoto |
0:e939856c1939
|
35
|
#define L2CAP_CONF_REQ 0x04
|
| abe00makoto |
0:e939856c1939
|
36
|
#define L2CAP_CONF_RSP 0x05
|
| abe00makoto |
0:e939856c1939
|
37
|
#define L2CAP_DISCONN_REQ 0x06
|
| abe00makoto |
0:e939856c1939
|
38
|
#define L2CAP_DISCONN_RSP 0x07
|
| abe00makoto |
0:e939856c1939
|
39
|
#define L2CAP_ECHO_REQ 0x08
|
| abe00makoto |
0:e939856c1939
|
40
|
#define L2CAP_ECHO_RSP 0x09
|
| abe00makoto |
0:e939856c1939
|
41
|
#define L2CAP_INFO_REQ 0x0a
|
| abe00makoto |
0:e939856c1939
|
42
|
#define L2CAP_INFO_RSP 0x0b
|
| abe00makoto |
0:e939856c1939
|
43
|
|
| abe00makoto |
0:e939856c1939
|
44
|
|
| abe00makoto |
0:e939856c1939
|
45
|
/* L2CAP command codes */
|
| abe00makoto |
0:e939856c1939
|
46
|
const char* L2CAP_ComandCodeStr(int c)
|
| abe00makoto |
0:e939856c1939
|
47
|
{
|
| abe00makoto |
0:e939856c1939
|
48
|
switch (c)
|
| abe00makoto |
0:e939856c1939
|
49
|
{
|
| abe00makoto |
0:e939856c1939
|
50
|
case L2CAP_COMMAND_REJ: return "L2CAP_COMMAND_REJ";
|
| abe00makoto |
0:e939856c1939
|
51
|
case L2CAP_CONN_REQ: return "L2CAP_CONN_REQ";
|
| abe00makoto |
0:e939856c1939
|
52
|
case L2CAP_CONN_RSP: return "L2CAP_CONN_RSP";
|
| abe00makoto |
0:e939856c1939
|
53
|
case L2CAP_CONF_REQ: return "L2CAP_CONF_REQ";
|
| abe00makoto |
0:e939856c1939
|
54
|
case L2CAP_CONF_RSP: return "L2CAP_CONF_RSP";
|
| abe00makoto |
0:e939856c1939
|
55
|
case L2CAP_DISCONN_REQ: return "L2CAP_DISCONN_REQ";
|
| abe00makoto |
0:e939856c1939
|
56
|
case L2CAP_DISCONN_RSP: return "L2CAP_DISCONN_RSP";
|
| abe00makoto |
0:e939856c1939
|
57
|
case L2CAP_ECHO_REQ: return "L2CAP_ECHO_REQ";
|
| abe00makoto |
0:e939856c1939
|
58
|
case L2CAP_ECHO_RSP: return "L2CAP_ECHO_RSP";
|
| abe00makoto |
0:e939856c1939
|
59
|
case L2CAP_INFO_REQ: return "L2CAP_INFO_REQ";
|
| abe00makoto |
0:e939856c1939
|
60
|
case L2CAP_INFO_RSP: return "L2CAP_INFO_RSP";
|
| abe00makoto |
0:e939856c1939
|
61
|
}
|
| abe00makoto |
0:e939856c1939
|
62
|
return "unknown";
|
| abe00makoto |
0:e939856c1939
|
63
|
}
|
| abe00makoto |
0:e939856c1939
|
64
|
|
| abe00makoto |
0:e939856c1939
|
65
|
typedef struct
|
| abe00makoto |
0:e939856c1939
|
66
|
{
|
| abe00makoto |
0:e939856c1939
|
67
|
u16 handle;
|
| abe00makoto |
0:e939856c1939
|
68
|
u16 length; // total
|
| abe00makoto |
0:e939856c1939
|
69
|
u16 l2capLength; // length -4
|
| abe00makoto |
0:e939856c1939
|
70
|
u16 cid; // Signaling packet CID = 1
|
| abe00makoto |
0:e939856c1939
|
71
|
u8 data[64]; // Largest thing to send!!! todo
|
| abe00makoto |
0:e939856c1939
|
72
|
} L2CAPData;
|
| abe00makoto |
0:e939856c1939
|
73
|
|
| abe00makoto |
0:e939856c1939
|
74
|
typedef struct
|
| abe00makoto |
0:e939856c1939
|
75
|
{
|
| abe00makoto |
0:e939856c1939
|
76
|
u16 handle;
|
| abe00makoto |
0:e939856c1939
|
77
|
u16 length; // total
|
| abe00makoto |
0:e939856c1939
|
78
|
u16 l2capLength; // length -4
|
| abe00makoto |
0:e939856c1939
|
79
|
u16 cid; // Signaling packet CID = 1
|
| abe00makoto |
0:e939856c1939
|
80
|
|
| abe00makoto |
0:e939856c1939
|
81
|
// Payload
|
| abe00makoto |
0:e939856c1939
|
82
|
u8 cmd; //
|
| abe00makoto |
0:e939856c1939
|
83
|
u8 id;
|
| abe00makoto |
0:e939856c1939
|
84
|
u16 cmdLength; // total-8
|
| abe00makoto |
0:e939856c1939
|
85
|
u16 params[4]; // Params
|
| abe00makoto |
0:e939856c1939
|
86
|
} L2CAPCmd;
|
| abe00makoto |
0:e939856c1939
|
87
|
|
| abe00makoto |
0:e939856c1939
|
88
|
void printf(const BD_ADDR* addr);
|
| abe00makoto |
0:e939856c1939
|
89
|
|
| abe00makoto |
0:e939856c1939
|
90
|
//
|
| abe00makoto |
0:e939856c1939
|
91
|
void BTDevice::Init()
|
| abe00makoto |
0:e939856c1939
|
92
|
{
|
| abe00makoto |
0:e939856c1939
|
93
|
memset(&_info,0,sizeof(inquiry_info));
|
| abe00makoto |
0:e939856c1939
|
94
|
_handle = 0;
|
| abe00makoto |
0:e939856c1939
|
95
|
_name[0] = 0;
|
| abe00makoto |
0:e939856c1939
|
96
|
_state = 0;
|
| abe00makoto |
0:e939856c1939
|
97
|
}
|
| abe00makoto |
0:e939856c1939
|
98
|
|
| abe00makoto |
0:e939856c1939
|
99
|
// virtual SocketHandler
|
| abe00makoto |
0:e939856c1939
|
100
|
int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr)
|
| abe00makoto |
0:e939856c1939
|
101
|
{
|
| abe00makoto |
0:e939856c1939
|
102
|
printf("Call to BTDevice Open \r\n");
|
| abe00makoto |
0:e939856c1939
|
103
|
L2CAPSocket* s = (L2CAPSocket*)sock;
|
| abe00makoto |
0:e939856c1939
|
104
|
L2CAPAddr* a = (L2CAPAddr*)addr;
|
| abe00makoto |
0:e939856c1939
|
105
|
s->scid = 0x40 + sock->ID-1; // are these reserved?
|
| abe00makoto |
0:e939856c1939
|
106
|
s->dcid = 0;
|
| abe00makoto |
0:e939856c1939
|
107
|
Connect(s->scid,a->psm);
|
| abe00makoto |
0:e939856c1939
|
108
|
return sock->ID;
|
| abe00makoto |
0:e939856c1939
|
109
|
}
|
| abe00makoto |
0:e939856c1939
|
110
|
|
| abe00makoto |
0:e939856c1939
|
111
|
// virtual SocketHandler
|
| abe00makoto |
0:e939856c1939
|
112
|
int BTDevice::Create(SocketInternal* sock, SocketAddrHdr* addr)
|
| abe00makoto |
0:e939856c1939
|
113
|
{
|
| abe00makoto |
0:e939856c1939
|
114
|
printf("Call to BTDevice Create \r\n");
|
| abe00makoto |
0:e939856c1939
|
115
|
//L2CAPSocket* s = (L2CAPSocket*)sock;
|
| abe00makoto |
0:e939856c1939
|
116
|
//L2CAPAddr* a = (L2CAPAddr*)addr;
|
| abe00makoto |
0:e939856c1939
|
117
|
|
| abe00makoto |
0:e939856c1939
|
118
|
//Connect(s->scid,a->psm);
|
| abe00makoto |
0:e939856c1939
|
119
|
return sock->ID;
|
| abe00makoto |
0:e939856c1939
|
120
|
}
|
| abe00makoto |
0:e939856c1939
|
121
|
|
| abe00makoto |
0:e939856c1939
|
122
|
int BTDevice::Accept(SocketInternal* sock, SocketAddrHdr* addr)
|
| abe00makoto |
0:e939856c1939
|
123
|
{
|
| abe00makoto |
0:e939856c1939
|
124
|
printf("Call to BTDevice Accept \r\n");
|
| abe00makoto |
0:e939856c1939
|
125
|
L2CAPSocket* s = (L2CAPSocket*)sock;
|
| abe00makoto |
0:e939856c1939
|
126
|
//L2CAPAddr* a = (L2CAPAddr*)addr;
|
| abe00makoto |
0:e939856c1939
|
127
|
|
| abe00makoto |
0:e939856c1939
|
128
|
|
| abe00makoto |
0:e939856c1939
|
129
|
printf("ID = %d scid = %d dcid = %d \r\n",sock->ID, s->scid, s->dcid);
|
| abe00makoto |
0:e939856c1939
|
130
|
|
| abe00makoto |
0:e939856c1939
|
131
|
return sock->ID;
|
| abe00makoto |
0:e939856c1939
|
132
|
}
|
| abe00makoto |
0:e939856c1939
|
133
|
|
| abe00makoto |
0:e939856c1939
|
134
|
|
| abe00makoto |
0:e939856c1939
|
135
|
|
| abe00makoto |
0:e939856c1939
|
136
|
|
| abe00makoto |
0:e939856c1939
|
137
|
// virtual SocketHandler
|
| abe00makoto |
0:e939856c1939
|
138
|
int BTDevice::Send(SocketInternal* sock, const u8* data, int len)
|
| abe00makoto |
0:e939856c1939
|
139
|
{
|
| abe00makoto |
0:e939856c1939
|
140
|
printf("Call to BTDevice Send \r\n");
|
| abe00makoto |
0:e939856c1939
|
141
|
L2CAPData d;
|
| abe00makoto |
0:e939856c1939
|
142
|
L2CAPSocket* s = (L2CAPSocket*)sock;
|
| abe00makoto |
0:e939856c1939
|
143
|
|
| abe00makoto |
0:e939856c1939
|
144
|
d.handle = _handle | 0x2000;
|
| abe00makoto |
0:e939856c1939
|
145
|
d.length = 4 + len;
|
| abe00makoto |
0:e939856c1939
|
146
|
d.l2capLength = len;
|
| abe00makoto |
0:e939856c1939
|
147
|
d.cid = s->dcid;
|
| abe00makoto |
0:e939856c1939
|
148
|
|
| abe00makoto |
0:e939856c1939
|
149
|
if (len > 64)
|
| abe00makoto |
0:e939856c1939
|
150
|
return -1;
|
| abe00makoto |
0:e939856c1939
|
151
|
memcpy(d.data,data,len);
|
| abe00makoto |
0:e939856c1939
|
152
|
return Send((u8*)&d,len+8);
|
| abe00makoto |
0:e939856c1939
|
153
|
}
|
| abe00makoto |
0:e939856c1939
|
154
|
|
| abe00makoto |
0:e939856c1939
|
155
|
// virtual SocketHandler
|
| abe00makoto |
0:e939856c1939
|
156
|
int BTDevice::Close(SocketInternal* sock)
|
| abe00makoto |
0:e939856c1939
|
157
|
{
|
| abe00makoto |
0:e939856c1939
|
158
|
printf("L2CAP close %d\r\n",sock->ID);
|
| abe00makoto |
0:e939856c1939
|
159
|
L2CAPSocket* s = (L2CAPSocket*)sock;
|
| abe00makoto |
0:e939856c1939
|
160
|
return Disconnect(s->scid,s->dcid);
|
| abe00makoto |
0:e939856c1939
|
161
|
}
|
| abe00makoto |
0:e939856c1939
|
162
|
|
| abe00makoto |
0:e939856c1939
|
163
|
L2CAPSocket* BTDevice::SCIDToSocket(int scid)
|
| abe00makoto |
0:e939856c1939
|
164
|
{
|
| abe00makoto |
0:e939856c1939
|
165
|
return (L2CAPSocket*)GetSocketInternal(scid-0x40+1);
|
| abe00makoto |
0:e939856c1939
|
166
|
}
|
| abe00makoto |
0:e939856c1939
|
167
|
|
| abe00makoto |
0:e939856c1939
|
168
|
int BTDevice::Send(const u8* data, int len)
|
| abe00makoto |
0:e939856c1939
|
169
|
{
|
| abe00makoto |
0:e939856c1939
|
170
|
printfBytes("L2CP send: ",data,len);
|
| abe00makoto |
0:e939856c1939
|
171
|
_transport->ACLSend(data,len);
|
| abe00makoto |
0:e939856c1939
|
172
|
return 0;
|
| abe00makoto |
0:e939856c1939
|
173
|
}
|
| abe00makoto |
0:e939856c1939
|
174
|
|
| abe00makoto |
0:e939856c1939
|
175
|
int BTDevice::Send(u8 c, u8 id, u16* params, int count)
|
| abe00makoto |
0:e939856c1939
|
176
|
{
|
| abe00makoto |
0:e939856c1939
|
177
|
L2CAPCmd cmd;
|
| abe00makoto |
0:e939856c1939
|
178
|
cmd.handle = _handle | 0x2000;
|
| abe00makoto |
0:e939856c1939
|
179
|
cmd.length = 8 + count*2;
|
| abe00makoto |
0:e939856c1939
|
180
|
|
| abe00makoto |
0:e939856c1939
|
181
|
cmd.l2capLength = cmd.length-4;
|
| abe00makoto |
0:e939856c1939
|
182
|
cmd.cid = 1; // Signaling packet
|
| abe00makoto |
0:e939856c1939
|
183
|
|
| abe00makoto |
0:e939856c1939
|
184
|
cmd.cmd = c;
|
| abe00makoto |
0:e939856c1939
|
185
|
cmd.id = id;
|
| abe00makoto |
0:e939856c1939
|
186
|
cmd.cmdLength = count*2;
|
| abe00makoto |
0:e939856c1939
|
187
|
for (int i = 0; i < count; i++)
|
| abe00makoto |
0:e939856c1939
|
188
|
cmd.params[i] = params[i];
|
| abe00makoto |
0:e939856c1939
|
189
|
return Send((u8*)&cmd,cmd.length+4);
|
| abe00makoto |
0:e939856c1939
|
190
|
}
|
| abe00makoto |
0:e939856c1939
|
191
|
|
| abe00makoto |
0:e939856c1939
|
192
|
int BTDevice::Connect(int scid, int psm)
|
| abe00makoto |
0:e939856c1939
|
193
|
{
|
| abe00makoto |
0:e939856c1939
|
194
|
u16 p[2];
|
| abe00makoto |
0:e939856c1939
|
195
|
p[0] = psm;
|
| abe00makoto |
0:e939856c1939
|
196
|
p[1] = scid;
|
| abe00makoto |
0:e939856c1939
|
197
|
return Send(L2CAP_CONN_REQ,_txid++,p,2);
|
| abe00makoto |
0:e939856c1939
|
198
|
}
|
| abe00makoto |
0:e939856c1939
|
199
|
|
| abe00makoto |
0:e939856c1939
|
200
|
int BTDevice::Disconnect(int scid, int dcid)
|
| abe00makoto |
0:e939856c1939
|
201
|
{
|
| abe00makoto |
0:e939856c1939
|
202
|
u16 p[2];
|
| abe00makoto |
0:e939856c1939
|
203
|
p[0] = dcid;
|
| abe00makoto |
0:e939856c1939
|
204
|
p[1] = scid;
|
| abe00makoto |
0:e939856c1939
|
205
|
return Send(L2CAP_DISCONN_REQ,_txid++,p,2);
|
| abe00makoto |
0:e939856c1939
|
206
|
}
|
| abe00makoto |
0:e939856c1939
|
207
|
|
| abe00makoto |
0:e939856c1939
|
208
|
int BTDevice::ConfigureRequest(int dcid)
|
| abe00makoto |
0:e939856c1939
|
209
|
{
|
| abe00makoto |
0:e939856c1939
|
210
|
u16 p[4];
|
| abe00makoto |
0:e939856c1939
|
211
|
p[0] = dcid;
|
| abe00makoto |
0:e939856c1939
|
212
|
p[1] = 0;
|
| abe00makoto |
0:e939856c1939
|
213
|
p[2] = 0x0201; // Options
|
| abe00makoto |
0:e939856c1939
|
214
|
p[3] = 0x02A0; // 672
|
| abe00makoto |
0:e939856c1939
|
215
|
return Send(L2CAP_CONF_REQ,_txid++,p,4);
|
| abe00makoto |
0:e939856c1939
|
216
|
}
|
| abe00makoto |
0:e939856c1939
|
217
|
|
| abe00makoto |
0:e939856c1939
|
218
|
int BTDevice::ConfigureResponse(u8 rxid, int dcid)
|
| abe00makoto |
0:e939856c1939
|
219
|
{
|
| abe00makoto |
0:e939856c1939
|
220
|
u16 p[3];
|
| abe00makoto |
0:e939856c1939
|
221
|
p[0] = dcid;
|
| abe00makoto |
0:e939856c1939
|
222
|
p[1] = 0;
|
| abe00makoto |
0:e939856c1939
|
223
|
p[2] = 0;
|
| abe00makoto |
0:e939856c1939
|
224
|
return Send(L2CAP_CONF_RSP,rxid,p,3);
|
| abe00makoto |
0:e939856c1939
|
225
|
}
|
| abe00makoto |
0:e939856c1939
|
226
|
|
| abe00makoto |
0:e939856c1939
|
227
|
int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid)
|
| abe00makoto |
0:e939856c1939
|
228
|
{
|
| abe00makoto |
0:e939856c1939
|
229
|
u16 p[2];
|
| abe00makoto |
0:e939856c1939
|
230
|
p[0] = dcid;
|
| abe00makoto |
0:e939856c1939
|
231
|
p[1] = scid;
|
| abe00makoto |
0:e939856c1939
|
232
|
return Send(L2CAP_DISCONN_RSP,rxid,p,2);
|
| abe00makoto |
0:e939856c1939
|
233
|
}
|
| abe00makoto |
0:e939856c1939
|
234
|
|
| abe00makoto |
0:e939856c1939
|
235
|
int BTDevice::AcceptResponse(u8 rxid, int scid, int dcid)
|
| abe00makoto |
0:e939856c1939
|
236
|
{
|
| abe00makoto |
0:e939856c1939
|
237
|
printf("Connection accepted \r\n");
|
| abe00makoto |
0:e939856c1939
|
238
|
u16 p[4];
|
| abe00makoto |
0:e939856c1939
|
239
|
p[0] = scid;
|
| abe00makoto |
0:e939856c1939
|
240
|
p[1] = dcid;
|
| abe00makoto |
0:e939856c1939
|
241
|
p[2] = L2CAP_CONN_SUCCESS;
|
| abe00makoto |
0:e939856c1939
|
242
|
p[3] = 0;
|
| abe00makoto |
0:e939856c1939
|
243
|
|
| abe00makoto |
0:e939856c1939
|
244
|
return Send(L2CAP_CONN_RSP,rxid,p,4);
|
| abe00makoto |
0:e939856c1939
|
245
|
|
| abe00makoto |
0:e939856c1939
|
246
|
}
|
| abe00makoto |
0:e939856c1939
|
247
|
|
| abe00makoto |
0:e939856c1939
|
248
|
int BTDevice::RefuseResponse(u8 rxid)
|
| abe00makoto |
0:e939856c1939
|
249
|
{
|
| abe00makoto |
0:e939856c1939
|
250
|
printf("Connection refused \r\n");
|
| abe00makoto |
0:e939856c1939
|
251
|
u16 p[2];
|
| abe00makoto |
0:e939856c1939
|
252
|
p[0] = L2CAP_CONN_REF_PSM;
|
| abe00makoto |
0:e939856c1939
|
253
|
p[1] = 0;
|
| abe00makoto |
0:e939856c1939
|
254
|
|
| abe00makoto |
0:e939856c1939
|
255
|
return Send(L2CAP_CONN_RSP,rxid,p,2);
|
| abe00makoto |
0:e939856c1939
|
256
|
}
|
| abe00makoto |
0:e939856c1939
|
257
|
|
| abe00makoto |
0:e939856c1939
|
258
|
|
| abe00makoto |
0:e939856c1939
|
259
|
//int BTDevice::InUse(int psm)
|
| abe00makoto |
0:e939856c1939
|
260
|
//{
|
| abe00makoto |
0:e939856c1939
|
261
|
// for (int i = 0; i < MAX_PORTS; i++){
|
| abe00makoto |
0:e939856c1939
|
262
|
// printf("Listen Q %d = %d \r\n",i, _listen[i]);
|
| abe00makoto |
0:e939856c1939
|
263
|
// if ( _listen[i] == psm ) {
|
| abe00makoto |
0:e939856c1939
|
264
|
// printf("We are listening on port %d \r\n",psm); //in use
|
| abe00makoto |
0:e939856c1939
|
265
|
// return 0;
|
| abe00makoto |
0:e939856c1939
|
266
|
// }
|
| abe00makoto |
0:e939856c1939
|
267
|
// }
|
| abe00makoto |
0:e939856c1939
|
268
|
// printf("We are not listening on port %d \r\n",psm);
|
| abe00makoto |
0:e939856c1939
|
269
|
// return 1;
|
| abe00makoto |
0:e939856c1939
|
270
|
//}
|
| abe00makoto |
0:e939856c1939
|
271
|
|
| abe00makoto |
0:e939856c1939
|
272
|
|
| abe00makoto |
0:e939856c1939
|
273
|
|
| abe00makoto |
0:e939856c1939
|
274
|
|
| abe00makoto |
0:e939856c1939
|
275
|
void BTDevice::Control(const BD_ADDR* addr, const u8* data, int len)
|
| abe00makoto |
0:e939856c1939
|
276
|
{
|
| abe00makoto |
0:e939856c1939
|
277
|
|
| abe00makoto |
0:e939856c1939
|
278
|
SocketInternal* sock;
|
| abe00makoto |
0:e939856c1939
|
279
|
L2CAPSocket* s;
|
| abe00makoto |
0:e939856c1939
|
280
|
int psm,scid,dcid/*,flags*/;
|
| abe00makoto |
0:e939856c1939
|
281
|
|
| abe00makoto |
0:e939856c1939
|
282
|
printf("From address ");
|
| abe00makoto |
0:e939856c1939
|
283
|
printf(addr);
|
| abe00makoto |
0:e939856c1939
|
284
|
printf(" : ");
|
| abe00makoto |
0:e939856c1939
|
285
|
|
| abe00makoto |
0:e939856c1939
|
286
|
int cc = data[8];
|
| abe00makoto |
0:e939856c1939
|
287
|
printf(L2CAP_ComandCodeStr(cc));
|
| abe00makoto |
0:e939856c1939
|
288
|
int result = LE16(data+16);
|
| abe00makoto |
0:e939856c1939
|
289
|
printf(" Result %d\r\n",result);
|
| abe00makoto |
0:e939856c1939
|
290
|
|
| abe00makoto |
0:e939856c1939
|
291
|
|
| abe00makoto |
0:e939856c1939
|
292
|
switch (cc)
|
| abe00makoto |
0:e939856c1939
|
293
|
{
|
| abe00makoto |
0:e939856c1939
|
294
|
case L2CAP_COMMAND_REJ:
|
| abe00makoto |
0:e939856c1939
|
295
|
break;
|
| abe00makoto |
0:e939856c1939
|
296
|
case L2CAP_CONN_REQ:
|
| abe00makoto |
0:e939856c1939
|
297
|
{
|
| abe00makoto |
0:e939856c1939
|
298
|
psm = LE16(data+12);
|
| abe00makoto |
0:e939856c1939
|
299
|
scid = LE16(data+14);
|
| abe00makoto |
0:e939856c1939
|
300
|
printf("Connection request scid = %d psm = %d \r\n",scid,psm);
|
| abe00makoto |
0:e939856c1939
|
301
|
|
| abe00makoto |
0:e939856c1939
|
302
|
// check if we listen on the port
|
| abe00makoto |
0:e939856c1939
|
303
|
//if ( InUse(psm) ) {
|
| abe00makoto |
0:e939856c1939
|
304
|
if ( Socket_InUse(SOCKET_L2CAP,psm) ) {
|
| abe00makoto |
0:e939856c1939
|
305
|
RefuseResponse(data[9]);
|
| abe00makoto |
0:e939856c1939
|
306
|
|
| abe00makoto |
0:e939856c1939
|
307
|
|
| abe00makoto |
0:e939856c1939
|
308
|
} else {
|
| abe00makoto |
0:e939856c1939
|
309
|
L2CAPAddr sockAddr;
|
| abe00makoto |
0:e939856c1939
|
310
|
sockAddr.bdaddr = *addr;
|
| abe00makoto |
0:e939856c1939
|
311
|
sock = Socket_Create(SOCKET_L2CAP, &sockAddr.hdr, psm);
|
| abe00makoto |
0:e939856c1939
|
312
|
s = (L2CAPSocket*)sock;
|
| abe00makoto |
0:e939856c1939
|
313
|
s->scid = 0x40 + sock->ID-1; // are these reserved?
|
| abe00makoto |
0:e939856c1939
|
314
|
s->dcid = scid;
|
| abe00makoto |
0:e939856c1939
|
315
|
AcceptResponse(data[9],s->scid,s->dcid);
|
| abe00makoto |
0:e939856c1939
|
316
|
|
| abe00makoto |
0:e939856c1939
|
317
|
ConfigureRequest(s->dcid); // handshake
|
| abe00makoto |
0:e939856c1939
|
318
|
|
| abe00makoto |
0:e939856c1939
|
319
|
sock->SetState(SocketState_Accept);
|
| abe00makoto |
0:e939856c1939
|
320
|
}
|
| abe00makoto |
0:e939856c1939
|
321
|
|
| abe00makoto |
0:e939856c1939
|
322
|
/**
|
| abe00makoto |
0:e939856c1939
|
323
|
for (int i = 0; i < MAX_PORTS; i++){
|
| abe00makoto |
0:e939856c1939
|
324
|
if ( _listen[i] == psm ) {
|
| abe00makoto |
0:e939856c1939
|
325
|
printf("We are listening on port %d \r\n",psm); //in use
|
| abe00makoto |
0:e939856c1939
|
326
|
RefuseResponse(data[9]);
|
| abe00makoto |
0:e939856c1939
|
327
|
break;
|
| abe00makoto |
0:e939856c1939
|
328
|
}
|
| abe00makoto |
0:e939856c1939
|
329
|
}
|
| abe00makoto |
0:e939856c1939
|
330
|
**/
|
| abe00makoto |
0:e939856c1939
|
331
|
|
| abe00makoto |
0:e939856c1939
|
332
|
//L2CAPAddr sockAddr;
|
| abe00makoto |
0:e939856c1939
|
333
|
//sockAddr.bdaddr = addr;
|
| abe00makoto |
0:e939856c1939
|
334
|
//sockAddr.psm = psm;
|
| abe00makoto |
0:e939856c1939
|
335
|
|
| abe00makoto |
0:e939856c1939
|
336
|
}
|
| abe00makoto |
0:e939856c1939
|
337
|
break;
|
| abe00makoto |
0:e939856c1939
|
338
|
|
| abe00makoto |
0:e939856c1939
|
339
|
// Response to our initial connect from Remote
|
| abe00makoto |
0:e939856c1939
|
340
|
case L2CAP_CONN_RSP:
|
| abe00makoto |
0:e939856c1939
|
341
|
{
|
| abe00makoto |
0:e939856c1939
|
342
|
if (result == 0)
|
| abe00makoto |
0:e939856c1939
|
343
|
{
|
| abe00makoto |
0:e939856c1939
|
344
|
printf("Connect succeeded\r\n");
|
| abe00makoto |
0:e939856c1939
|
345
|
dcid = LE16(data+12);
|
| abe00makoto |
0:e939856c1939
|
346
|
scid = LE16(data+14);
|
| abe00makoto |
0:e939856c1939
|
347
|
L2CAPSocket* s = SCIDToSocket(scid);
|
| abe00makoto |
0:e939856c1939
|
348
|
if (s)
|
| abe00makoto |
0:e939856c1939
|
349
|
{
|
| abe00makoto |
0:e939856c1939
|
350
|
s->dcid = dcid;
|
| abe00makoto |
0:e939856c1939
|
351
|
ConfigureRequest(dcid);
|
| abe00makoto |
0:e939856c1939
|
352
|
}
|
| abe00makoto |
0:e939856c1939
|
353
|
} else
|
| abe00makoto |
0:e939856c1939
|
354
|
printf("Connect failed?\r\n");
|
| abe00makoto |
0:e939856c1939
|
355
|
}
|
| abe00makoto |
0:e939856c1939
|
356
|
break;
|
| abe00makoto |
0:e939856c1939
|
357
|
|
| abe00makoto |
0:e939856c1939
|
358
|
case L2CAP_CONF_RSP:
|
| abe00makoto |
0:e939856c1939
|
359
|
{
|
| abe00makoto |
0:e939856c1939
|
360
|
scid = LE16(data+12);
|
| abe00makoto |
0:e939856c1939
|
361
|
SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
|
| abe00makoto |
0:e939856c1939
|
362
|
if (s)
|
| abe00makoto |
0:e939856c1939
|
363
|
s->SetState(SocketState_Open);
|
| abe00makoto |
0:e939856c1939
|
364
|
}
|
| abe00makoto |
0:e939856c1939
|
365
|
break;
|
| abe00makoto |
0:e939856c1939
|
366
|
|
| abe00makoto |
0:e939856c1939
|
367
|
case L2CAP_CONF_REQ:
|
| abe00makoto |
0:e939856c1939
|
368
|
{
|
| abe00makoto |
0:e939856c1939
|
369
|
u16 dcid = LE16(data+12);
|
| abe00makoto |
0:e939856c1939
|
370
|
u16 flags = LE16(data+14);
|
| abe00makoto |
0:e939856c1939
|
371
|
printf("Config request dcid = %02X flags = %02X\r\n", dcid, flags);
|
| abe00makoto |
0:e939856c1939
|
372
|
scid = LE16(data+12);
|
| abe00makoto |
0:e939856c1939
|
373
|
L2CAPSocket* s = SCIDToSocket(scid);
|
| abe00makoto |
0:e939856c1939
|
374
|
if (s)
|
| abe00makoto |
0:e939856c1939
|
375
|
ConfigureResponse(data[9],s->dcid);
|
| abe00makoto |
0:e939856c1939
|
376
|
}
|
| abe00makoto |
0:e939856c1939
|
377
|
break;
|
| abe00makoto |
0:e939856c1939
|
378
|
case L2CAP_DISCONN_REQ: {
|
| abe00makoto |
0:e939856c1939
|
379
|
int dcid = LE16(data+12);
|
| abe00makoto |
0:e939856c1939
|
380
|
int scid = LE16(data+14);
|
| abe00makoto |
0:e939856c1939
|
381
|
L2CAPSocket* s = SCIDToSocket(scid);
|
| abe00makoto |
0:e939856c1939
|
382
|
//s->si.SetState(SocketState_Closed);
|
| abe00makoto |
0:e939856c1939
|
383
|
DisconnectResponse(data[9], scid, dcid);
|
| abe00makoto |
0:e939856c1939
|
384
|
}
|
| abe00makoto |
0:e939856c1939
|
385
|
break;
|
| abe00makoto |
0:e939856c1939
|
386
|
|
| abe00makoto |
0:e939856c1939
|
387
|
}
|
| abe00makoto |
0:e939856c1939
|
388
|
}
|
| abe00makoto |
0:e939856c1939
|
389
|
|
| abe00makoto |
0:e939856c1939
|
390
|
void BTDevice::ACLRecv(const BD_ADDR* addr, const u8* data, int len)
|
| abe00makoto |
0:e939856c1939
|
391
|
{
|
| abe00makoto |
0:e939856c1939
|
392
|
//printfBytes("L2CP recv: ",data,16);
|
| abe00makoto |
0:e939856c1939
|
393
|
int handle = LE16(data);
|
| abe00makoto |
0:e939856c1939
|
394
|
if (handle != (0x2000 | _handle))
|
| abe00makoto |
0:e939856c1939
|
395
|
return;
|
| abe00makoto |
0:e939856c1939
|
396
|
|
| abe00makoto |
0:e939856c1939
|
397
|
int cid = LE16(data+6);
|
| abe00makoto |
0:e939856c1939
|
398
|
if (cid == 1)
|
| abe00makoto |
0:e939856c1939
|
399
|
{
|
| abe00makoto |
0:e939856c1939
|
400
|
Control(addr,data,len);
|
| abe00makoto |
0:e939856c1939
|
401
|
return;
|
| abe00makoto |
0:e939856c1939
|
402
|
}
|
| abe00makoto |
0:e939856c1939
|
403
|
|
| abe00makoto |
0:e939856c1939
|
404
|
SocketInternal* s = (SocketInternal*)SCIDToSocket(cid);
|
| abe00makoto |
0:e939856c1939
|
405
|
if (s)
|
| abe00makoto |
0:e939856c1939
|
406
|
s->Recv(data+8,LE16(data+2)-4);
|
| abe00makoto |
0:e939856c1939
|
407
|
else
|
| abe00makoto |
0:e939856c1939
|
408
|
printf("Bad event cid %d\r\n",cid);
|
| abe00makoto |
0:e939856c1939
|
409
|
}
|
makoto abe