root / hw / bt-hci.c @ 795928f6
History | View | Annotate | Download (67.5 kB)
1 |
/*
|
---|---|
2 |
* QEMU Bluetooth HCI logic.
|
3 |
*
|
4 |
* Copyright (C) 2007 OpenMoko, Inc.
|
5 |
* Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.org>
|
6 |
*
|
7 |
* This program is free software; you can redistribute it and/or
|
8 |
* modify it under the terms of the GNU General Public License as
|
9 |
* published by the Free Software Foundation; either version 2 of
|
10 |
* the License, or (at your option) any later version.
|
11 |
*
|
12 |
* This program is distributed in the hope that it will be useful,
|
13 |
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
14 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
15 |
* GNU General Public License for more details.
|
16 |
*
|
17 |
* You should have received a copy of the GNU General Public License
|
18 |
* along with this program; if not, see <http://www.gnu.org/licenses/>.
|
19 |
*/
|
20 |
|
21 |
#include "qemu-common.h" |
22 |
#include "qemu-timer.h" |
23 |
#include "usb.h" |
24 |
#include "net.h" |
25 |
#include "bt.h" |
26 |
|
27 |
struct bt_hci_s {
|
28 |
uint8_t *(*evt_packet)(void *opaque);
|
29 |
void (*evt_submit)(void *opaque, int len); |
30 |
void *opaque;
|
31 |
uint8_t evt_buf[256];
|
32 |
|
33 |
uint8_t acl_buf[4096];
|
34 |
int acl_len;
|
35 |
|
36 |
uint16_t asb_handle; |
37 |
uint16_t psb_handle; |
38 |
|
39 |
int last_cmd; /* Note: Always little-endian */ |
40 |
|
41 |
struct bt_device_s *conn_req_host;
|
42 |
|
43 |
struct {
|
44 |
int inquire;
|
45 |
int periodic;
|
46 |
int responses_left;
|
47 |
int responses;
|
48 |
QEMUTimer *inquiry_done; |
49 |
QEMUTimer *inquiry_next; |
50 |
int inquiry_length;
|
51 |
int inquiry_period;
|
52 |
int inquiry_mode;
|
53 |
|
54 |
#define HCI_HANDLE_OFFSET 0x20 |
55 |
#define HCI_HANDLES_MAX 0x10 |
56 |
struct bt_hci_master_link_s {
|
57 |
struct bt_link_s *link;
|
58 |
void (*lmp_acl_data)(struct bt_link_s *link, |
59 |
const uint8_t *data, int start, int len); |
60 |
QEMUTimer *acl_mode_timer; |
61 |
} handle[HCI_HANDLES_MAX]; |
62 |
uint32_t role_bmp; |
63 |
int last_handle;
|
64 |
int connecting;
|
65 |
bdaddr_t awaiting_bdaddr[HCI_HANDLES_MAX]; |
66 |
} lm; |
67 |
|
68 |
uint8_t event_mask[8];
|
69 |
uint16_t voice_setting; /* Notw: Always little-endian */
|
70 |
uint16_t conn_accept_tout; |
71 |
QEMUTimer *conn_accept_timer; |
72 |
|
73 |
struct HCIInfo info;
|
74 |
struct bt_device_s device;
|
75 |
}; |
76 |
|
77 |
#define DEFAULT_RSSI_DBM 20 |
78 |
|
79 |
#define hci_from_info(ptr) container_of((ptr), struct bt_hci_s, info) |
80 |
#define hci_from_device(ptr) container_of((ptr), struct bt_hci_s, device) |
81 |
|
82 |
struct bt_hci_link_s {
|
83 |
struct bt_link_s btlink;
|
84 |
uint16_t handle; /* Local */
|
85 |
}; |
86 |
|
87 |
/* LMP layer emulation */
|
88 |
#if 0
|
89 |
static void bt_submit_lmp(struct bt_device_s *bt, int length, uint8_t *data)
|
90 |
{
|
91 |
int resp, resplen, error, op, tr;
|
92 |
uint8_t respdata[17];
|
93 |
|
94 |
if (length < 1)
|
95 |
return;
|
96 |
|
97 |
tr = *data & 1;
|
98 |
op = *(data ++) >> 1;
|
99 |
resp = LMP_ACCEPTED;
|
100 |
resplen = 2;
|
101 |
respdata[1] = op;
|
102 |
error = 0;
|
103 |
length --;
|
104 |
|
105 |
if (op >= 0x7c) { /* Extended opcode */
|
106 |
op |= *(data ++) << 8;
|
107 |
resp = LMP_ACCEPTED_EXT;
|
108 |
resplen = 4;
|
109 |
respdata[0] = op >> 8;
|
110 |
respdata[1] = op & 0xff;
|
111 |
length --;
|
112 |
}
|
113 |
|
114 |
switch (op) {
|
115 |
case LMP_ACCEPTED:
|
116 |
/* data[0] Op code
|
117 |
*/
|
118 |
if (length < 1) {
|
119 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
120 |
goto not_accepted;
|
121 |
}
|
122 |
resp = 0;
|
123 |
break;
|
124 |
|
125 |
case LMP_ACCEPTED_EXT:
|
126 |
/* data[0] Escape op code
|
127 |
* data[1] Extended op code
|
128 |
*/
|
129 |
if (length < 2) {
|
130 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
131 |
goto not_accepted;
|
132 |
}
|
133 |
resp = 0;
|
134 |
break;
|
135 |
|
136 |
case LMP_NOT_ACCEPTED:
|
137 |
/* data[0] Op code
|
138 |
* data[1] Error code
|
139 |
*/
|
140 |
if (length < 2) {
|
141 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
142 |
goto not_accepted;
|
143 |
}
|
144 |
resp = 0;
|
145 |
break;
|
146 |
|
147 |
case LMP_NOT_ACCEPTED_EXT:
|
148 |
/* data[0] Op code
|
149 |
* data[1] Extended op code
|
150 |
* data[2] Error code
|
151 |
*/
|
152 |
if (length < 3) {
|
153 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
154 |
goto not_accepted;
|
155 |
}
|
156 |
resp = 0;
|
157 |
break;
|
158 |
|
159 |
case LMP_HOST_CONNECTION_REQ:
|
160 |
break;
|
161 |
|
162 |
case LMP_SETUP_COMPLETE:
|
163 |
resp = LMP_SETUP_COMPLETE;
|
164 |
resplen = 1;
|
165 |
bt->setup = 1;
|
166 |
break;
|
167 |
|
168 |
case LMP_DETACH:
|
169 |
/* data[0] Error code
|
170 |
*/
|
171 |
if (length < 1) {
|
172 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
173 |
goto not_accepted;
|
174 |
}
|
175 |
bt->setup = 0;
|
176 |
resp = 0;
|
177 |
break;
|
178 |
|
179 |
case LMP_SUPERVISION_TIMEOUT:
|
180 |
/* data[0,1] Supervision timeout
|
181 |
*/
|
182 |
if (length < 2) {
|
183 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
184 |
goto not_accepted;
|
185 |
}
|
186 |
resp = 0;
|
187 |
break;
|
188 |
|
189 |
case LMP_QUALITY_OF_SERVICE:
|
190 |
resp = 0;
|
191 |
/* Fall through */
|
192 |
case LMP_QOS_REQ:
|
193 |
/* data[0,1] Poll interval
|
194 |
* data[2] N(BC)
|
195 |
*/
|
196 |
if (length < 3) {
|
197 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
198 |
goto not_accepted;
|
199 |
}
|
200 |
break;
|
201 |
|
202 |
case LMP_MAX_SLOT:
|
203 |
resp = 0;
|
204 |
/* Fall through */
|
205 |
case LMP_MAX_SLOT_REQ:
|
206 |
/* data[0] Max slots
|
207 |
*/
|
208 |
if (length < 1) {
|
209 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
210 |
goto not_accepted;
|
211 |
}
|
212 |
break;
|
213 |
|
214 |
case LMP_AU_RAND:
|
215 |
case LMP_IN_RAND:
|
216 |
case LMP_COMB_KEY:
|
217 |
/* data[0-15] Random number
|
218 |
*/
|
219 |
if (length < 16) {
|
220 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
221 |
goto not_accepted;
|
222 |
}
|
223 |
if (op == LMP_AU_RAND) {
|
224 |
if (bt->key_present) {
|
225 |
resp = LMP_SRES;
|
226 |
resplen = 5;
|
227 |
/* XXX: [Part H] Section 6.1 on page 801 */
|
228 |
} else {
|
229 |
error = HCI_PIN_OR_KEY_MISSING;
|
230 |
goto not_accepted;
|
231 |
}
|
232 |
} else if (op == LMP_IN_RAND) {
|
233 |
error = HCI_PAIRING_NOT_ALLOWED;
|
234 |
goto not_accepted;
|
235 |
} else {
|
236 |
/* XXX: [Part H] Section 3.2 on page 779 */
|
237 |
resp = LMP_UNIT_KEY;
|
238 |
resplen = 17;
|
239 |
memcpy(respdata + 1, bt->key, 16);
|
240 |
|
241 |
error = HCI_UNIT_LINK_KEY_USED;
|
242 |
goto not_accepted;
|
243 |
}
|
244 |
break;
|
245 |
|
246 |
case LMP_UNIT_KEY:
|
247 |
/* data[0-15] Key
|
248 |
*/
|
249 |
if (length < 16) {
|
250 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
251 |
goto not_accepted;
|
252 |
}
|
253 |
memcpy(bt->key, data, 16);
|
254 |
bt->key_present = 1;
|
255 |
break;
|
256 |
|
257 |
case LMP_SRES:
|
258 |
/* data[0-3] Authentication response
|
259 |
*/
|
260 |
if (length < 4) {
|
261 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
262 |
goto not_accepted;
|
263 |
}
|
264 |
break;
|
265 |
|
266 |
case LMP_CLKOFFSET_REQ:
|
267 |
resp = LMP_CLKOFFSET_RES;
|
268 |
resplen = 3;
|
269 |
respdata[1] = 0x33;
|
270 |
respdata[2] = 0x33;
|
271 |
break;
|
272 |
|
273 |
case LMP_CLKOFFSET_RES:
|
274 |
/* data[0,1] Clock offset
|
275 |
* (Slave to master only)
|
276 |
*/
|
277 |
if (length < 2) {
|
278 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
279 |
goto not_accepted;
|
280 |
}
|
281 |
break;
|
282 |
|
283 |
case LMP_VERSION_REQ:
|
284 |
case LMP_VERSION_RES:
|
285 |
/* data[0] VersNr
|
286 |
* data[1,2] CompId
|
287 |
* data[3,4] SubVersNr
|
288 |
*/
|
289 |
if (length < 5) {
|
290 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
291 |
goto not_accepted;
|
292 |
}
|
293 |
if (op == LMP_VERSION_REQ) {
|
294 |
resp = LMP_VERSION_RES;
|
295 |
resplen = 6;
|
296 |
respdata[1] = 0x20;
|
297 |
respdata[2] = 0xff;
|
298 |
respdata[3] = 0xff;
|
299 |
respdata[4] = 0xff;
|
300 |
respdata[5] = 0xff;
|
301 |
} else
|
302 |
resp = 0;
|
303 |
break;
|
304 |
|
305 |
case LMP_FEATURES_REQ:
|
306 |
case LMP_FEATURES_RES:
|
307 |
/* data[0-7] Features
|
308 |
*/
|
309 |
if (length < 8) {
|
310 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
311 |
goto not_accepted;
|
312 |
}
|
313 |
if (op == LMP_FEATURES_REQ) {
|
314 |
resp = LMP_FEATURES_RES;
|
315 |
resplen = 9;
|
316 |
respdata[1] = (bt->lmp_caps >> 0) & 0xff;
|
317 |
respdata[2] = (bt->lmp_caps >> 8) & 0xff;
|
318 |
respdata[3] = (bt->lmp_caps >> 16) & 0xff;
|
319 |
respdata[4] = (bt->lmp_caps >> 24) & 0xff;
|
320 |
respdata[5] = (bt->lmp_caps >> 32) & 0xff;
|
321 |
respdata[6] = (bt->lmp_caps >> 40) & 0xff;
|
322 |
respdata[7] = (bt->lmp_caps >> 48) & 0xff;
|
323 |
respdata[8] = (bt->lmp_caps >> 56) & 0xff;
|
324 |
} else
|
325 |
resp = 0;
|
326 |
break;
|
327 |
|
328 |
case LMP_NAME_REQ:
|
329 |
/* data[0] Name offset
|
330 |
*/
|
331 |
if (length < 1) {
|
332 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
333 |
goto not_accepted;
|
334 |
}
|
335 |
resp = LMP_NAME_RES;
|
336 |
resplen = 17;
|
337 |
respdata[1] = data[0];
|
338 |
respdata[2] = strlen(bt->lmp_name);
|
339 |
memset(respdata + 3, 0x00, 14);
|
340 |
if (respdata[2] > respdata[1])
|
341 |
memcpy(respdata + 3, bt->lmp_name + respdata[1],
|
342 |
respdata[2] - respdata[1]);
|
343 |
break;
|
344 |
|
345 |
case LMP_NAME_RES:
|
346 |
/* data[0] Name offset
|
347 |
* data[1] Name length
|
348 |
* data[2-15] Name fragment
|
349 |
*/
|
350 |
if (length < 16) {
|
351 |
error = HCI_UNSUPPORTED_LMP_PARAMETER_VALUE;
|
352 |
goto not_accepted;
|
353 |
}
|
354 |
resp = 0;
|
355 |
break;
|
356 |
|
357 |
default:
|
358 |
error = HCI_UNKNOWN_LMP_PDU;
|
359 |
/* Fall through */
|
360 |
not_accepted:
|
361 |
if (op >> 8) {
|
362 |
resp = LMP_NOT_ACCEPTED_EXT;
|
363 |
resplen = 5;
|
364 |
respdata[0] = op >> 8;
|
365 |
respdata[1] = op & 0xff;
|
366 |
respdata[2] = error;
|
367 |
} else {
|
368 |
resp = LMP_NOT_ACCEPTED;
|
369 |
resplen = 3;
|
370 |
respdata[0] = op & 0xff;
|
371 |
respdata[1] = error;
|
372 |
}
|
373 |
}
|
374 |
|
375 |
if (resp == 0)
|
376 |
return;
|
377 |
|
378 |
if (resp >> 8) {
|
379 |
respdata[0] = resp >> 8;
|
380 |
respdata[1] = resp & 0xff;
|
381 |
} else
|
382 |
respdata[0] = resp & 0xff;
|
383 |
|
384 |
respdata[0] <<= 1;
|
385 |
respdata[0] |= tr;
|
386 |
}
|
387 |
|
388 |
static void bt_submit_raw_acl(struct bt_piconet_s *net, int length, uint8_t *data)
|
389 |
{
|
390 |
struct bt_device_s *slave;
|
391 |
if (length < 1)
|
392 |
return;
|
393 |
|
394 |
slave = 0;
|
395 |
#if 0
|
396 |
slave = net->slave;
|
397 |
#endif
|
398 |
|
399 |
switch (data[0] & 3) { |
400 |
case LLID_ACLC:
|
401 |
bt_submit_lmp(slave, length - 1, data + 1); |
402 |
break;
|
403 |
case LLID_ACLU_START:
|
404 |
#if 0
|
405 |
bt_sumbit_l2cap(slave, length - 1, data + 1, (data[0] >> 2) & 1);
|
406 |
breka;
|
407 |
#endif
|
408 |
default:
|
409 |
case LLID_ACLU_CONT:
|
410 |
break;
|
411 |
} |
412 |
} |
413 |
#endif
|
414 |
|
415 |
/* HCI layer emulation */
|
416 |
|
417 |
/* Note: we could ignore endiannes because unswapped handles will still
|
418 |
* be valid as connection identifiers for the guest - they don't have to
|
419 |
* be continuously allocated. We do it though, to preserve similar
|
420 |
* behaviour between hosts. Some things, like the BD_ADDR cannot be
|
421 |
* preserved though (for example if a real hci is used). */
|
422 |
#ifdef HOST_WORDS_BIGENDIAN
|
423 |
# define HNDL(raw) bswap16(raw)
|
424 |
#else
|
425 |
# define HNDL(raw) (raw)
|
426 |
#endif
|
427 |
|
428 |
static const uint8_t bt_event_reserved_mask[8] = { |
429 |
0xff, 0x9f, 0xfb, 0xff, 0x07, 0x18, 0x00, 0x00, |
430 |
}; |
431 |
|
432 |
static inline uint8_t *bt_hci_event_start(struct bt_hci_s *hci, |
433 |
int evt, int len) |
434 |
{ |
435 |
uint8_t *packet, mask; |
436 |
int mask_byte;
|
437 |
|
438 |
if (len > 255) { |
439 |
fprintf(stderr, "%s: HCI event params too long (%ib)\n",
|
440 |
__FUNCTION__, len); |
441 |
exit(-1);
|
442 |
} |
443 |
|
444 |
mask_byte = (evt - 1) >> 3; |
445 |
mask = 1 << ((evt - 1) & 3); |
446 |
if (mask & bt_event_reserved_mask[mask_byte] & ~hci->event_mask[mask_byte])
|
447 |
return NULL; |
448 |
|
449 |
packet = hci->evt_packet(hci->opaque); |
450 |
packet[0] = evt;
|
451 |
packet[1] = len;
|
452 |
|
453 |
return &packet[2]; |
454 |
} |
455 |
|
456 |
static inline void bt_hci_event(struct bt_hci_s *hci, int evt, |
457 |
void *params, int len) |
458 |
{ |
459 |
uint8_t *packet = bt_hci_event_start(hci, evt, len); |
460 |
|
461 |
if (!packet)
|
462 |
return;
|
463 |
|
464 |
if (len)
|
465 |
memcpy(packet, params, len); |
466 |
|
467 |
hci->evt_submit(hci->opaque, len + 2);
|
468 |
} |
469 |
|
470 |
static inline void bt_hci_event_status(struct bt_hci_s *hci, int status) |
471 |
{ |
472 |
evt_cmd_status params = { |
473 |
.status = status, |
474 |
.ncmd = 1,
|
475 |
.opcode = hci->last_cmd, |
476 |
}; |
477 |
|
478 |
bt_hci_event(hci, EVT_CMD_STATUS, ¶ms, EVT_CMD_STATUS_SIZE); |
479 |
} |
480 |
|
481 |
static inline void bt_hci_event_complete(struct bt_hci_s *hci, |
482 |
void *ret, int len) |
483 |
{ |
484 |
uint8_t *packet = bt_hci_event_start(hci, EVT_CMD_COMPLETE, |
485 |
len + EVT_CMD_COMPLETE_SIZE); |
486 |
evt_cmd_complete *params = (evt_cmd_complete *) packet; |
487 |
|
488 |
if (!packet)
|
489 |
return;
|
490 |
|
491 |
params->ncmd = 1;
|
492 |
params->opcode = hci->last_cmd; |
493 |
if (len)
|
494 |
memcpy(&packet[EVT_CMD_COMPLETE_SIZE], ret, len); |
495 |
|
496 |
hci->evt_submit(hci->opaque, len + EVT_CMD_COMPLETE_SIZE + 2);
|
497 |
} |
498 |
|
499 |
static void bt_hci_inquiry_done(void *opaque) |
500 |
{ |
501 |
struct bt_hci_s *hci = (struct bt_hci_s *) opaque; |
502 |
uint8_t status = HCI_SUCCESS; |
503 |
|
504 |
if (!hci->lm.periodic)
|
505 |
hci->lm.inquire = 0;
|
506 |
|
507 |
/* The specification is inconsistent about this one. Page 565 reads
|
508 |
* "The event parameters of Inquiry Complete event will have a summary
|
509 |
* of the result from the Inquiry process, which reports the number of
|
510 |
* nearby Bluetooth devices that responded [so hci->responses].", but
|
511 |
* Event Parameters (see page 729) has only Status. */
|
512 |
bt_hci_event(hci, EVT_INQUIRY_COMPLETE, &status, 1);
|
513 |
} |
514 |
|
515 |
static void bt_hci_inquiry_result_standard(struct bt_hci_s *hci, |
516 |
struct bt_device_s *slave)
|
517 |
{ |
518 |
inquiry_info params = { |
519 |
.num_responses = 1,
|
520 |
.bdaddr = BAINIT(&slave->bd_addr), |
521 |
.pscan_rep_mode = 0x00, /* R0 */ |
522 |
.pscan_period_mode = 0x00, /* P0 - deprecated */ |
523 |
.pscan_mode = 0x00, /* Standard scan - deprecated */ |
524 |
.dev_class[0] = slave->class[0], |
525 |
.dev_class[1] = slave->class[1], |
526 |
.dev_class[2] = slave->class[2], |
527 |
/* TODO: return the clkoff *differenece* */
|
528 |
.clock_offset = slave->clkoff, /* Note: no swapping */
|
529 |
}; |
530 |
|
531 |
bt_hci_event(hci, EVT_INQUIRY_RESULT, ¶ms, INQUIRY_INFO_SIZE); |
532 |
} |
533 |
|
534 |
static void bt_hci_inquiry_result_with_rssi(struct bt_hci_s *hci, |
535 |
struct bt_device_s *slave)
|
536 |
{ |
537 |
inquiry_info_with_rssi params = { |
538 |
.num_responses = 1,
|
539 |
.bdaddr = BAINIT(&slave->bd_addr), |
540 |
.pscan_rep_mode = 0x00, /* R0 */ |
541 |
.pscan_period_mode = 0x00, /* P0 - deprecated */ |
542 |
.dev_class[0] = slave->class[0], |
543 |
.dev_class[1] = slave->class[1], |
544 |
.dev_class[2] = slave->class[2], |
545 |
/* TODO: return the clkoff *differenece* */
|
546 |
.clock_offset = slave->clkoff, /* Note: no swapping */
|
547 |
.rssi = DEFAULT_RSSI_DBM, |
548 |
}; |
549 |
|
550 |
bt_hci_event(hci, EVT_INQUIRY_RESULT_WITH_RSSI, |
551 |
¶ms, INQUIRY_INFO_WITH_RSSI_SIZE); |
552 |
} |
553 |
|
554 |
static void bt_hci_inquiry_result(struct bt_hci_s *hci, |
555 |
struct bt_device_s *slave)
|
556 |
{ |
557 |
if (!slave->inquiry_scan || !hci->lm.responses_left)
|
558 |
return;
|
559 |
|
560 |
hci->lm.responses_left --; |
561 |
hci->lm.responses ++; |
562 |
|
563 |
switch (hci->lm.inquiry_mode) {
|
564 |
case 0x00: |
565 |
bt_hci_inquiry_result_standard(hci, slave); |
566 |
return;
|
567 |
case 0x01: |
568 |
bt_hci_inquiry_result_with_rssi(hci, slave); |
569 |
return;
|
570 |
default:
|
571 |
fprintf(stderr, "%s: bad inquiry mode %02x\n", __FUNCTION__,
|
572 |
hci->lm.inquiry_mode); |
573 |
exit(-1);
|
574 |
} |
575 |
} |
576 |
|
577 |
static void bt_hci_mod_timer_1280ms(QEMUTimer *timer, int period) |
578 |
{ |
579 |
qemu_mod_timer(timer, qemu_get_clock_ns(vm_clock) + |
580 |
muldiv64(period << 7, get_ticks_per_sec(), 100)); |
581 |
} |
582 |
|
583 |
static void bt_hci_inquiry_start(struct bt_hci_s *hci, int length) |
584 |
{ |
585 |
struct bt_device_s *slave;
|
586 |
|
587 |
hci->lm.inquiry_length = length; |
588 |
for (slave = hci->device.net->slave; slave; slave = slave->next)
|
589 |
/* Don't uncover ourselves. */
|
590 |
if (slave != &hci->device)
|
591 |
bt_hci_inquiry_result(hci, slave); |
592 |
|
593 |
/* TODO: register for a callback on a new device's addition to the
|
594 |
* scatternet so that if it's added before inquiry_length expires,
|
595 |
* an Inquiry Result is generated immediately. Alternatively re-loop
|
596 |
* through the devices on the inquiry_length expiration and report
|
597 |
* devices not seen before. */
|
598 |
if (hci->lm.responses_left)
|
599 |
bt_hci_mod_timer_1280ms(hci->lm.inquiry_done, hci->lm.inquiry_length); |
600 |
else
|
601 |
bt_hci_inquiry_done(hci); |
602 |
|
603 |
if (hci->lm.periodic)
|
604 |
bt_hci_mod_timer_1280ms(hci->lm.inquiry_next, hci->lm.inquiry_period); |
605 |
} |
606 |
|
607 |
static void bt_hci_inquiry_next(void *opaque) |
608 |
{ |
609 |
struct bt_hci_s *hci = (struct bt_hci_s *) opaque; |
610 |
|
611 |
hci->lm.responses_left += hci->lm.responses; |
612 |
hci->lm.responses = 0;
|
613 |
bt_hci_inquiry_start(hci, hci->lm.inquiry_length); |
614 |
} |
615 |
|
616 |
static inline int bt_hci_handle_bad(struct bt_hci_s *hci, uint16_t handle) |
617 |
{ |
618 |
return !(handle & HCI_HANDLE_OFFSET) ||
|
619 |
handle >= (HCI_HANDLE_OFFSET | HCI_HANDLES_MAX) || |
620 |
!hci->lm.handle[handle & ~HCI_HANDLE_OFFSET].link; |
621 |
} |
622 |
|
623 |
static inline int bt_hci_role_master(struct bt_hci_s *hci, uint16_t handle) |
624 |
{ |
625 |
return !!(hci->lm.role_bmp & (1 << (handle & ~HCI_HANDLE_OFFSET))); |
626 |
} |
627 |
|
628 |
static inline struct bt_device_s *bt_hci_remote_dev(struct bt_hci_s *hci, |
629 |
uint16_t handle) |
630 |
{ |
631 |
struct bt_link_s *link = hci->lm.handle[handle & ~HCI_HANDLE_OFFSET].link;
|
632 |
|
633 |
return bt_hci_role_master(hci, handle) ? link->slave : link->host;
|
634 |
} |
635 |
|
636 |
static void bt_hci_mode_tick(void *opaque); |
637 |
static void bt_hci_lmp_link_establish(struct bt_hci_s *hci, |
638 |
struct bt_link_s *link, int master) |
639 |
{ |
640 |
hci->lm.handle[hci->lm.last_handle].link = link; |
641 |
|
642 |
if (master) {
|
643 |
/* We are the master side of an ACL link */
|
644 |
hci->lm.role_bmp |= 1 << hci->lm.last_handle;
|
645 |
|
646 |
hci->lm.handle[hci->lm.last_handle].lmp_acl_data = |
647 |
link->slave->lmp_acl_data; |
648 |
} else {
|
649 |
/* We are the slave side of an ACL link */
|
650 |
hci->lm.role_bmp &= ~(1 << hci->lm.last_handle);
|
651 |
|
652 |
hci->lm.handle[hci->lm.last_handle].lmp_acl_data = |
653 |
link->host->lmp_acl_resp; |
654 |
} |
655 |
|
656 |
/* Mode */
|
657 |
if (master) {
|
658 |
link->acl_mode = acl_active; |
659 |
hci->lm.handle[hci->lm.last_handle].acl_mode_timer = |
660 |
qemu_new_timer_ns(vm_clock, bt_hci_mode_tick, link); |
661 |
} |
662 |
} |
663 |
|
664 |
static void bt_hci_lmp_link_teardown(struct bt_hci_s *hci, uint16_t handle) |
665 |
{ |
666 |
handle &= ~HCI_HANDLE_OFFSET; |
667 |
hci->lm.handle[handle].link = NULL;
|
668 |
|
669 |
if (bt_hci_role_master(hci, handle)) {
|
670 |
qemu_del_timer(hci->lm.handle[handle].acl_mode_timer); |
671 |
qemu_free_timer(hci->lm.handle[handle].acl_mode_timer); |
672 |
} |
673 |
} |
674 |
|
675 |
static int bt_hci_connect(struct bt_hci_s *hci, bdaddr_t *bdaddr) |
676 |
{ |
677 |
struct bt_device_s *slave;
|
678 |
struct bt_link_s link;
|
679 |
|
680 |
for (slave = hci->device.net->slave; slave; slave = slave->next)
|
681 |
if (slave->page_scan && !bacmp(&slave->bd_addr, bdaddr))
|
682 |
break;
|
683 |
if (!slave || slave == &hci->device)
|
684 |
return -ENODEV;
|
685 |
|
686 |
bacpy(&hci->lm.awaiting_bdaddr[hci->lm.connecting ++], &slave->bd_addr); |
687 |
|
688 |
link.slave = slave; |
689 |
link.host = &hci->device; |
690 |
link.slave->lmp_connection_request(&link); /* Always last */
|
691 |
|
692 |
return 0; |
693 |
} |
694 |
|
695 |
static void bt_hci_connection_reject(struct bt_hci_s *hci, |
696 |
struct bt_device_s *host, uint8_t because)
|
697 |
{ |
698 |
struct bt_link_s link = {
|
699 |
.slave = &hci->device, |
700 |
.host = host, |
701 |
/* Rest uninitialised */
|
702 |
}; |
703 |
|
704 |
host->reject_reason = because; |
705 |
host->lmp_connection_complete(&link); |
706 |
} |
707 |
|
708 |
static void bt_hci_connection_reject_event(struct bt_hci_s *hci, |
709 |
bdaddr_t *bdaddr) |
710 |
{ |
711 |
evt_conn_complete params; |
712 |
|
713 |
params.status = HCI_NO_CONNECTION; |
714 |
params.handle = 0;
|
715 |
bacpy(¶ms.bdaddr, bdaddr); |
716 |
params.link_type = ACL_LINK; |
717 |
params.encr_mode = 0x00; /* Encryption not required */ |
718 |
bt_hci_event(hci, EVT_CONN_COMPLETE, ¶ms, EVT_CONN_COMPLETE_SIZE); |
719 |
} |
720 |
|
721 |
static void bt_hci_connection_accept(struct bt_hci_s *hci, |
722 |
struct bt_device_s *host)
|
723 |
{ |
724 |
struct bt_hci_link_s *link = g_malloc0(sizeof(struct bt_hci_link_s)); |
725 |
evt_conn_complete params; |
726 |
uint16_t handle; |
727 |
uint8_t status = HCI_SUCCESS; |
728 |
int tries = HCI_HANDLES_MAX;
|
729 |
|
730 |
/* Make a connection handle */
|
731 |
do {
|
732 |
while (hci->lm.handle[++ hci->lm.last_handle].link && -- tries)
|
733 |
hci->lm.last_handle &= HCI_HANDLES_MAX - 1;
|
734 |
handle = hci->lm.last_handle | HCI_HANDLE_OFFSET; |
735 |
} while ((handle == hci->asb_handle || handle == hci->psb_handle) &&
|
736 |
tries); |
737 |
|
738 |
if (!tries) {
|
739 |
g_free(link); |
740 |
bt_hci_connection_reject(hci, host, HCI_REJECTED_LIMITED_RESOURCES); |
741 |
status = HCI_NO_CONNECTION; |
742 |
goto complete;
|
743 |
} |
744 |
|
745 |
link->btlink.slave = &hci->device; |
746 |
link->btlink.host = host; |
747 |
link->handle = handle; |
748 |
|
749 |
/* Link established */
|
750 |
bt_hci_lmp_link_establish(hci, &link->btlink, 0);
|
751 |
|
752 |
complete:
|
753 |
params.status = status; |
754 |
params.handle = HNDL(handle); |
755 |
bacpy(¶ms.bdaddr, &host->bd_addr); |
756 |
params.link_type = ACL_LINK; |
757 |
params.encr_mode = 0x00; /* Encryption not required */ |
758 |
bt_hci_event(hci, EVT_CONN_COMPLETE, ¶ms, EVT_CONN_COMPLETE_SIZE); |
759 |
|
760 |
/* Neets to be done at the very end because it can trigger a (nested)
|
761 |
* disconnected, in case the other and had cancelled the request
|
762 |
* locally. */
|
763 |
if (status == HCI_SUCCESS) {
|
764 |
host->reject_reason = 0;
|
765 |
host->lmp_connection_complete(&link->btlink); |
766 |
} |
767 |
} |
768 |
|
769 |
static void bt_hci_lmp_connection_request(struct bt_link_s *link) |
770 |
{ |
771 |
struct bt_hci_s *hci = hci_from_device(link->slave);
|
772 |
evt_conn_request params; |
773 |
|
774 |
if (hci->conn_req_host) {
|
775 |
bt_hci_connection_reject(hci, link->host, |
776 |
HCI_REJECTED_LIMITED_RESOURCES); |
777 |
return;
|
778 |
} |
779 |
hci->conn_req_host = link->host; |
780 |
/* TODO: if masked and auto-accept, then auto-accept,
|
781 |
* if masked and not auto-accept, then auto-reject */
|
782 |
/* TODO: kick the hci->conn_accept_timer, timeout after
|
783 |
* hci->conn_accept_tout * 0.625 msec */
|
784 |
|
785 |
bacpy(¶ms.bdaddr, &link->host->bd_addr); |
786 |
memcpy(¶ms.dev_class, &link->host->class, sizeof(params.dev_class));
|
787 |
params.link_type = ACL_LINK; |
788 |
bt_hci_event(hci, EVT_CONN_REQUEST, ¶ms, EVT_CONN_REQUEST_SIZE); |
789 |
return;
|
790 |
} |
791 |
|
792 |
static void bt_hci_conn_accept_timeout(void *opaque) |
793 |
{ |
794 |
struct bt_hci_s *hci = (struct bt_hci_s *) opaque; |
795 |
|
796 |
if (!hci->conn_req_host)
|
797 |
/* Already accepted or rejected. If the other end cancelled the
|
798 |
* connection request then we still have to reject or accept it
|
799 |
* and then we'll get a disconnect. */
|
800 |
return;
|
801 |
|
802 |
/* TODO */
|
803 |
} |
804 |
|
805 |
/* Remove from the list of devices which we wanted to connect to and
|
806 |
* are awaiting a response from. If the callback sees a response from
|
807 |
* a device which is not on the list it will assume it's a connection
|
808 |
* that's been cancelled by the host in the meantime and immediately
|
809 |
* try to detach the link and send a Connection Complete. */
|
810 |
static int bt_hci_lmp_connection_ready(struct bt_hci_s *hci, |
811 |
bdaddr_t *bdaddr) |
812 |
{ |
813 |
int i;
|
814 |
|
815 |
for (i = 0; i < hci->lm.connecting; i ++) |
816 |
if (!bacmp(&hci->lm.awaiting_bdaddr[i], bdaddr)) {
|
817 |
if (i < -- hci->lm.connecting)
|
818 |
bacpy(&hci->lm.awaiting_bdaddr[i], |
819 |
&hci->lm.awaiting_bdaddr[hci->lm.connecting]); |
820 |
return 0; |
821 |
} |
822 |
|
823 |
return 1; |
824 |
} |
825 |
|
826 |
static void bt_hci_lmp_connection_complete(struct bt_link_s *link) |
827 |
{ |
828 |
struct bt_hci_s *hci = hci_from_device(link->host);
|
829 |
evt_conn_complete params; |
830 |
uint16_t handle; |
831 |
uint8_t status = HCI_SUCCESS; |
832 |
int tries = HCI_HANDLES_MAX;
|
833 |
|
834 |
if (bt_hci_lmp_connection_ready(hci, &link->slave->bd_addr)) {
|
835 |
if (!hci->device.reject_reason)
|
836 |
link->slave->lmp_disconnect_slave(link); |
837 |
handle = 0;
|
838 |
status = HCI_NO_CONNECTION; |
839 |
goto complete;
|
840 |
} |
841 |
|
842 |
if (hci->device.reject_reason) {
|
843 |
handle = 0;
|
844 |
status = hci->device.reject_reason; |
845 |
goto complete;
|
846 |
} |
847 |
|
848 |
/* Make a connection handle */
|
849 |
do {
|
850 |
while (hci->lm.handle[++ hci->lm.last_handle].link && -- tries)
|
851 |
hci->lm.last_handle &= HCI_HANDLES_MAX - 1;
|
852 |
handle = hci->lm.last_handle | HCI_HANDLE_OFFSET; |
853 |
} while ((handle == hci->asb_handle || handle == hci->psb_handle) &&
|
854 |
tries); |
855 |
|
856 |
if (!tries) {
|
857 |
link->slave->lmp_disconnect_slave(link); |
858 |
status = HCI_NO_CONNECTION; |
859 |
goto complete;
|
860 |
} |
861 |
|
862 |
/* Link established */
|
863 |
link->handle = handle; |
864 |
bt_hci_lmp_link_establish(hci, link, 1);
|
865 |
|
866 |
complete:
|
867 |
params.status = status; |
868 |
params.handle = HNDL(handle); |
869 |
params.link_type = ACL_LINK; |
870 |
bacpy(¶ms.bdaddr, &link->slave->bd_addr); |
871 |
params.encr_mode = 0x00; /* Encryption not required */ |
872 |
bt_hci_event(hci, EVT_CONN_COMPLETE, ¶ms, EVT_CONN_COMPLETE_SIZE); |
873 |
} |
874 |
|
875 |
static void bt_hci_disconnect(struct bt_hci_s *hci, |
876 |
uint16_t handle, int reason)
|
877 |
{ |
878 |
struct bt_link_s *btlink =
|
879 |
hci->lm.handle[handle & ~HCI_HANDLE_OFFSET].link; |
880 |
struct bt_hci_link_s *link;
|
881 |
evt_disconn_complete params; |
882 |
|
883 |
if (bt_hci_role_master(hci, handle)) {
|
884 |
btlink->slave->reject_reason = reason; |
885 |
btlink->slave->lmp_disconnect_slave(btlink); |
886 |
/* The link pointer is invalid from now on */
|
887 |
|
888 |
goto complete;
|
889 |
} |
890 |
|
891 |
btlink->host->reject_reason = reason; |
892 |
btlink->host->lmp_disconnect_master(btlink); |
893 |
|
894 |
/* We are the slave, we get to clean this burden */
|
895 |
link = (struct bt_hci_link_s *) btlink;
|
896 |
g_free(link); |
897 |
|
898 |
complete:
|
899 |
bt_hci_lmp_link_teardown(hci, handle); |
900 |
|
901 |
params.status = HCI_SUCCESS; |
902 |
params.handle = HNDL(handle); |
903 |
params.reason = HCI_CONNECTION_TERMINATED; |
904 |
bt_hci_event(hci, EVT_DISCONN_COMPLETE, |
905 |
¶ms, EVT_DISCONN_COMPLETE_SIZE); |
906 |
} |
907 |
|
908 |
/* TODO: use only one function */
|
909 |
static void bt_hci_lmp_disconnect_host(struct bt_link_s *link) |
910 |
{ |
911 |
struct bt_hci_s *hci = hci_from_device(link->host);
|
912 |
uint16_t handle = link->handle; |
913 |
evt_disconn_complete params; |
914 |
|
915 |
bt_hci_lmp_link_teardown(hci, handle); |
916 |
|
917 |
params.status = HCI_SUCCESS; |
918 |
params.handle = HNDL(handle); |
919 |
params.reason = hci->device.reject_reason; |
920 |
bt_hci_event(hci, EVT_DISCONN_COMPLETE, |
921 |
¶ms, EVT_DISCONN_COMPLETE_SIZE); |
922 |
} |
923 |
|
924 |
static void bt_hci_lmp_disconnect_slave(struct bt_link_s *btlink) |
925 |
{ |
926 |
struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink; |
927 |
struct bt_hci_s *hci = hci_from_device(btlink->slave);
|
928 |
uint16_t handle = link->handle; |
929 |
evt_disconn_complete params; |
930 |
|
931 |
g_free(link); |
932 |
|
933 |
bt_hci_lmp_link_teardown(hci, handle); |
934 |
|
935 |
params.status = HCI_SUCCESS; |
936 |
params.handle = HNDL(handle); |
937 |
params.reason = hci->device.reject_reason; |
938 |
bt_hci_event(hci, EVT_DISCONN_COMPLETE, |
939 |
¶ms, EVT_DISCONN_COMPLETE_SIZE); |
940 |
} |
941 |
|
942 |
static int bt_hci_name_req(struct bt_hci_s *hci, bdaddr_t *bdaddr) |
943 |
{ |
944 |
struct bt_device_s *slave;
|
945 |
evt_remote_name_req_complete params; |
946 |
int len;
|
947 |
|
948 |
for (slave = hci->device.net->slave; slave; slave = slave->next)
|
949 |
if (slave->page_scan && !bacmp(&slave->bd_addr, bdaddr))
|
950 |
break;
|
951 |
if (!slave)
|
952 |
return -ENODEV;
|
953 |
|
954 |
bt_hci_event_status(hci, HCI_SUCCESS); |
955 |
|
956 |
params.status = HCI_SUCCESS; |
957 |
bacpy(¶ms.bdaddr, &slave->bd_addr); |
958 |
len = snprintf(params.name, sizeof(params.name),
|
959 |
"%s", slave->lmp_name ?: ""); |
960 |
memset(params.name + len, 0, sizeof(params.name) - len); |
961 |
bt_hci_event(hci, EVT_REMOTE_NAME_REQ_COMPLETE, |
962 |
¶ms, EVT_REMOTE_NAME_REQ_COMPLETE_SIZE); |
963 |
|
964 |
return 0; |
965 |
} |
966 |
|
967 |
static int bt_hci_features_req(struct bt_hci_s *hci, uint16_t handle) |
968 |
{ |
969 |
struct bt_device_s *slave;
|
970 |
evt_read_remote_features_complete params; |
971 |
|
972 |
if (bt_hci_handle_bad(hci, handle))
|
973 |
return -ENODEV;
|
974 |
|
975 |
slave = bt_hci_remote_dev(hci, handle); |
976 |
|
977 |
bt_hci_event_status(hci, HCI_SUCCESS); |
978 |
|
979 |
params.status = HCI_SUCCESS; |
980 |
params.handle = HNDL(handle); |
981 |
params.features[0] = (slave->lmp_caps >> 0) & 0xff; |
982 |
params.features[1] = (slave->lmp_caps >> 8) & 0xff; |
983 |
params.features[2] = (slave->lmp_caps >> 16) & 0xff; |
984 |
params.features[3] = (slave->lmp_caps >> 24) & 0xff; |
985 |
params.features[4] = (slave->lmp_caps >> 32) & 0xff; |
986 |
params.features[5] = (slave->lmp_caps >> 40) & 0xff; |
987 |
params.features[6] = (slave->lmp_caps >> 48) & 0xff; |
988 |
params.features[7] = (slave->lmp_caps >> 56) & 0xff; |
989 |
bt_hci_event(hci, EVT_READ_REMOTE_FEATURES_COMPLETE, |
990 |
¶ms, EVT_READ_REMOTE_FEATURES_COMPLETE_SIZE); |
991 |
|
992 |
return 0; |
993 |
} |
994 |
|
995 |
static int bt_hci_version_req(struct bt_hci_s *hci, uint16_t handle) |
996 |
{ |
997 |
evt_read_remote_version_complete params; |
998 |
|
999 |
if (bt_hci_handle_bad(hci, handle))
|
1000 |
return -ENODEV;
|
1001 |
|
1002 |
bt_hci_remote_dev(hci, handle); |
1003 |
|
1004 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1005 |
|
1006 |
params.status = HCI_SUCCESS; |
1007 |
params.handle = HNDL(handle); |
1008 |
params.lmp_ver = 0x03;
|
1009 |
params.manufacturer = cpu_to_le16(0xa000);
|
1010 |
params.lmp_subver = cpu_to_le16(0xa607);
|
1011 |
bt_hci_event(hci, EVT_READ_REMOTE_VERSION_COMPLETE, |
1012 |
¶ms, EVT_READ_REMOTE_VERSION_COMPLETE_SIZE); |
1013 |
|
1014 |
return 0; |
1015 |
} |
1016 |
|
1017 |
static int bt_hci_clkoffset_req(struct bt_hci_s *hci, uint16_t handle) |
1018 |
{ |
1019 |
struct bt_device_s *slave;
|
1020 |
evt_read_clock_offset_complete params; |
1021 |
|
1022 |
if (bt_hci_handle_bad(hci, handle))
|
1023 |
return -ENODEV;
|
1024 |
|
1025 |
slave = bt_hci_remote_dev(hci, handle); |
1026 |
|
1027 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1028 |
|
1029 |
params.status = HCI_SUCCESS; |
1030 |
params.handle = HNDL(handle); |
1031 |
/* TODO: return the clkoff *differenece* */
|
1032 |
params.clock_offset = slave->clkoff; /* Note: no swapping */
|
1033 |
bt_hci_event(hci, EVT_READ_CLOCK_OFFSET_COMPLETE, |
1034 |
¶ms, EVT_READ_CLOCK_OFFSET_COMPLETE_SIZE); |
1035 |
|
1036 |
return 0; |
1037 |
} |
1038 |
|
1039 |
static void bt_hci_event_mode(struct bt_hci_s *hci, struct bt_link_s *link, |
1040 |
uint16_t handle) |
1041 |
{ |
1042 |
evt_mode_change params = { |
1043 |
.status = HCI_SUCCESS, |
1044 |
.handle = HNDL(handle), |
1045 |
.mode = link->acl_mode, |
1046 |
.interval = cpu_to_le16(link->acl_interval), |
1047 |
}; |
1048 |
|
1049 |
bt_hci_event(hci, EVT_MODE_CHANGE, ¶ms, EVT_MODE_CHANGE_SIZE); |
1050 |
} |
1051 |
|
1052 |
static void bt_hci_lmp_mode_change_master(struct bt_hci_s *hci, |
1053 |
struct bt_link_s *link, int mode, uint16_t interval) |
1054 |
{ |
1055 |
link->acl_mode = mode; |
1056 |
link->acl_interval = interval; |
1057 |
|
1058 |
bt_hci_event_mode(hci, link, link->handle); |
1059 |
|
1060 |
link->slave->lmp_mode_change(link); |
1061 |
} |
1062 |
|
1063 |
static void bt_hci_lmp_mode_change_slave(struct bt_link_s *btlink) |
1064 |
{ |
1065 |
struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink; |
1066 |
struct bt_hci_s *hci = hci_from_device(btlink->slave);
|
1067 |
|
1068 |
bt_hci_event_mode(hci, btlink, link->handle); |
1069 |
} |
1070 |
|
1071 |
static int bt_hci_mode_change(struct bt_hci_s *hci, uint16_t handle, |
1072 |
int interval, int mode) |
1073 |
{ |
1074 |
struct bt_hci_master_link_s *link;
|
1075 |
|
1076 |
if (bt_hci_handle_bad(hci, handle) || !bt_hci_role_master(hci, handle))
|
1077 |
return -ENODEV;
|
1078 |
|
1079 |
link = &hci->lm.handle[handle & ~HCI_HANDLE_OFFSET]; |
1080 |
if (link->link->acl_mode != acl_active) {
|
1081 |
bt_hci_event_status(hci, HCI_COMMAND_DISALLOWED); |
1082 |
return 0; |
1083 |
} |
1084 |
|
1085 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1086 |
|
1087 |
qemu_mod_timer(link->acl_mode_timer, qemu_get_clock_ns(vm_clock) + |
1088 |
muldiv64(interval * 625, get_ticks_per_sec(), 1000000)); |
1089 |
bt_hci_lmp_mode_change_master(hci, link->link, mode, interval); |
1090 |
|
1091 |
return 0; |
1092 |
} |
1093 |
|
1094 |
static int bt_hci_mode_cancel(struct bt_hci_s *hci, uint16_t handle, int mode) |
1095 |
{ |
1096 |
struct bt_hci_master_link_s *link;
|
1097 |
|
1098 |
if (bt_hci_handle_bad(hci, handle) || !bt_hci_role_master(hci, handle))
|
1099 |
return -ENODEV;
|
1100 |
|
1101 |
link = &hci->lm.handle[handle & ~HCI_HANDLE_OFFSET]; |
1102 |
if (link->link->acl_mode != mode) {
|
1103 |
bt_hci_event_status(hci, HCI_COMMAND_DISALLOWED); |
1104 |
|
1105 |
return 0; |
1106 |
} |
1107 |
|
1108 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1109 |
|
1110 |
qemu_del_timer(link->acl_mode_timer); |
1111 |
bt_hci_lmp_mode_change_master(hci, link->link, acl_active, 0);
|
1112 |
|
1113 |
return 0; |
1114 |
} |
1115 |
|
1116 |
static void bt_hci_mode_tick(void *opaque) |
1117 |
{ |
1118 |
struct bt_link_s *link = opaque;
|
1119 |
struct bt_hci_s *hci = hci_from_device(link->host);
|
1120 |
|
1121 |
bt_hci_lmp_mode_change_master(hci, link, acl_active, 0);
|
1122 |
} |
1123 |
|
1124 |
static void bt_hci_reset(struct bt_hci_s *hci) |
1125 |
{ |
1126 |
hci->acl_len = 0;
|
1127 |
hci->last_cmd = 0;
|
1128 |
hci->lm.connecting = 0;
|
1129 |
|
1130 |
hci->event_mask[0] = 0xff; |
1131 |
hci->event_mask[1] = 0xff; |
1132 |
hci->event_mask[2] = 0xff; |
1133 |
hci->event_mask[3] = 0xff; |
1134 |
hci->event_mask[4] = 0xff; |
1135 |
hci->event_mask[5] = 0x1f; |
1136 |
hci->event_mask[6] = 0x00; |
1137 |
hci->event_mask[7] = 0x00; |
1138 |
hci->device.inquiry_scan = 0;
|
1139 |
hci->device.page_scan = 0;
|
1140 |
if (hci->device.lmp_name)
|
1141 |
g_free((void *) hci->device.lmp_name);
|
1142 |
hci->device.lmp_name = NULL;
|
1143 |
hci->device.class[0] = 0x00; |
1144 |
hci->device.class[1] = 0x00; |
1145 |
hci->device.class[2] = 0x00; |
1146 |
hci->voice_setting = 0x0000;
|
1147 |
hci->conn_accept_tout = 0x1f40;
|
1148 |
hci->lm.inquiry_mode = 0x00;
|
1149 |
|
1150 |
hci->psb_handle = 0x000;
|
1151 |
hci->asb_handle = 0x000;
|
1152 |
|
1153 |
/* XXX: qemu_del_timer(sl->acl_mode_timer); for all links */
|
1154 |
qemu_del_timer(hci->lm.inquiry_done); |
1155 |
qemu_del_timer(hci->lm.inquiry_next); |
1156 |
qemu_del_timer(hci->conn_accept_timer); |
1157 |
} |
1158 |
|
1159 |
static void bt_hci_read_local_version_rp(struct bt_hci_s *hci) |
1160 |
{ |
1161 |
read_local_version_rp lv = { |
1162 |
.status = HCI_SUCCESS, |
1163 |
.hci_ver = 0x03,
|
1164 |
.hci_rev = cpu_to_le16(0xa607),
|
1165 |
.lmp_ver = 0x03,
|
1166 |
.manufacturer = cpu_to_le16(0xa000),
|
1167 |
.lmp_subver = cpu_to_le16(0xa607),
|
1168 |
}; |
1169 |
|
1170 |
bt_hci_event_complete(hci, &lv, READ_LOCAL_VERSION_RP_SIZE); |
1171 |
} |
1172 |
|
1173 |
static void bt_hci_read_local_commands_rp(struct bt_hci_s *hci) |
1174 |
{ |
1175 |
read_local_commands_rp lc = { |
1176 |
.status = HCI_SUCCESS, |
1177 |
.commands = { |
1178 |
/* Keep updated! */
|
1179 |
/* Also, keep in sync with hci->device.lmp_caps in bt_new_hci */
|
1180 |
0xbf, 0x80, 0xf9, 0x03, 0xb2, 0xc0, 0x03, 0xc3, |
1181 |
0x00, 0x0f, 0x80, 0x00, 0xc0, 0x00, 0xe8, 0x13, |
1182 |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
1183 |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
1184 |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
1185 |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
1186 |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
1187 |
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
1188 |
}, |
1189 |
}; |
1190 |
|
1191 |
bt_hci_event_complete(hci, &lc, READ_LOCAL_COMMANDS_RP_SIZE); |
1192 |
} |
1193 |
|
1194 |
static void bt_hci_read_local_features_rp(struct bt_hci_s *hci) |
1195 |
{ |
1196 |
read_local_features_rp lf = { |
1197 |
.status = HCI_SUCCESS, |
1198 |
.features = { |
1199 |
(hci->device.lmp_caps >> 0) & 0xff, |
1200 |
(hci->device.lmp_caps >> 8) & 0xff, |
1201 |
(hci->device.lmp_caps >> 16) & 0xff, |
1202 |
(hci->device.lmp_caps >> 24) & 0xff, |
1203 |
(hci->device.lmp_caps >> 32) & 0xff, |
1204 |
(hci->device.lmp_caps >> 40) & 0xff, |
1205 |
(hci->device.lmp_caps >> 48) & 0xff, |
1206 |
(hci->device.lmp_caps >> 56) & 0xff, |
1207 |
}, |
1208 |
}; |
1209 |
|
1210 |
bt_hci_event_complete(hci, &lf, READ_LOCAL_FEATURES_RP_SIZE); |
1211 |
} |
1212 |
|
1213 |
static void bt_hci_read_local_ext_features_rp(struct bt_hci_s *hci, int page) |
1214 |
{ |
1215 |
read_local_ext_features_rp lef = { |
1216 |
.status = HCI_SUCCESS, |
1217 |
.page_num = page, |
1218 |
.max_page_num = 0x00,
|
1219 |
.features = { |
1220 |
/* Keep updated! */
|
1221 |
0x5f, 0x35, 0x85, 0x7e, 0x9b, 0x19, 0x00, 0x80, |
1222 |
}, |
1223 |
}; |
1224 |
if (page)
|
1225 |
memset(lef.features, 0, sizeof(lef.features)); |
1226 |
|
1227 |
bt_hci_event_complete(hci, &lef, READ_LOCAL_EXT_FEATURES_RP_SIZE); |
1228 |
} |
1229 |
|
1230 |
static void bt_hci_read_buffer_size_rp(struct bt_hci_s *hci) |
1231 |
{ |
1232 |
read_buffer_size_rp bs = { |
1233 |
/* This can be made configurable, for one standard USB dongle HCI
|
1234 |
* the four values are cpu_to_le16(0x0180), 0x40,
|
1235 |
* cpu_to_le16(0x0008), cpu_to_le16(0x0008). */
|
1236 |
.status = HCI_SUCCESS, |
1237 |
.acl_mtu = cpu_to_le16(0x0200),
|
1238 |
.sco_mtu = 0,
|
1239 |
.acl_max_pkt = cpu_to_le16(0x0001),
|
1240 |
.sco_max_pkt = cpu_to_le16(0x0000),
|
1241 |
}; |
1242 |
|
1243 |
bt_hci_event_complete(hci, &bs, READ_BUFFER_SIZE_RP_SIZE); |
1244 |
} |
1245 |
|
1246 |
/* Deprecated in V2.0 (page 661) */
|
1247 |
static void bt_hci_read_country_code_rp(struct bt_hci_s *hci) |
1248 |
{ |
1249 |
read_country_code_rp cc ={ |
1250 |
.status = HCI_SUCCESS, |
1251 |
.country_code = 0x00, /* North America & Europe^1 and Japan */ |
1252 |
}; |
1253 |
|
1254 |
bt_hci_event_complete(hci, &cc, READ_COUNTRY_CODE_RP_SIZE); |
1255 |
|
1256 |
/* ^1. Except France, sorry */
|
1257 |
} |
1258 |
|
1259 |
static void bt_hci_read_bd_addr_rp(struct bt_hci_s *hci) |
1260 |
{ |
1261 |
read_bd_addr_rp ba = { |
1262 |
.status = HCI_SUCCESS, |
1263 |
.bdaddr = BAINIT(&hci->device.bd_addr), |
1264 |
}; |
1265 |
|
1266 |
bt_hci_event_complete(hci, &ba, READ_BD_ADDR_RP_SIZE); |
1267 |
} |
1268 |
|
1269 |
static int bt_hci_link_quality_rp(struct bt_hci_s *hci, uint16_t handle) |
1270 |
{ |
1271 |
read_link_quality_rp lq = { |
1272 |
.status = HCI_SUCCESS, |
1273 |
.handle = HNDL(handle), |
1274 |
.link_quality = 0xff,
|
1275 |
}; |
1276 |
|
1277 |
if (bt_hci_handle_bad(hci, handle))
|
1278 |
lq.status = HCI_NO_CONNECTION; |
1279 |
|
1280 |
bt_hci_event_complete(hci, &lq, READ_LINK_QUALITY_RP_SIZE); |
1281 |
return 0; |
1282 |
} |
1283 |
|
1284 |
/* Generate a Command Complete event with only the Status parameter */
|
1285 |
static inline void bt_hci_event_complete_status(struct bt_hci_s *hci, |
1286 |
uint8_t status) |
1287 |
{ |
1288 |
bt_hci_event_complete(hci, &status, 1);
|
1289 |
} |
1290 |
|
1291 |
static inline void bt_hci_event_complete_conn_cancel(struct bt_hci_s *hci, |
1292 |
uint8_t status, bdaddr_t *bd_addr) |
1293 |
{ |
1294 |
create_conn_cancel_rp params = { |
1295 |
.status = status, |
1296 |
.bdaddr = BAINIT(bd_addr), |
1297 |
}; |
1298 |
|
1299 |
bt_hci_event_complete(hci, ¶ms, CREATE_CONN_CANCEL_RP_SIZE); |
1300 |
} |
1301 |
|
1302 |
static inline void bt_hci_event_auth_complete(struct bt_hci_s *hci, |
1303 |
uint16_t handle) |
1304 |
{ |
1305 |
evt_auth_complete params = { |
1306 |
.status = HCI_SUCCESS, |
1307 |
.handle = HNDL(handle), |
1308 |
}; |
1309 |
|
1310 |
bt_hci_event(hci, EVT_AUTH_COMPLETE, ¶ms, EVT_AUTH_COMPLETE_SIZE); |
1311 |
} |
1312 |
|
1313 |
static inline void bt_hci_event_encrypt_change(struct bt_hci_s *hci, |
1314 |
uint16_t handle, uint8_t mode) |
1315 |
{ |
1316 |
evt_encrypt_change params = { |
1317 |
.status = HCI_SUCCESS, |
1318 |
.handle = HNDL(handle), |
1319 |
.encrypt = mode, |
1320 |
}; |
1321 |
|
1322 |
bt_hci_event(hci, EVT_ENCRYPT_CHANGE, ¶ms, EVT_ENCRYPT_CHANGE_SIZE); |
1323 |
} |
1324 |
|
1325 |
static inline void bt_hci_event_complete_name_cancel(struct bt_hci_s *hci, |
1326 |
bdaddr_t *bd_addr) |
1327 |
{ |
1328 |
remote_name_req_cancel_rp params = { |
1329 |
.status = HCI_INVALID_PARAMETERS, |
1330 |
.bdaddr = BAINIT(bd_addr), |
1331 |
}; |
1332 |
|
1333 |
bt_hci_event_complete(hci, ¶ms, REMOTE_NAME_REQ_CANCEL_RP_SIZE); |
1334 |
} |
1335 |
|
1336 |
static inline void bt_hci_event_read_remote_ext_features(struct bt_hci_s *hci, |
1337 |
uint16_t handle) |
1338 |
{ |
1339 |
evt_read_remote_ext_features_complete params = { |
1340 |
.status = HCI_UNSUPPORTED_FEATURE, |
1341 |
.handle = HNDL(handle), |
1342 |
/* Rest uninitialised */
|
1343 |
}; |
1344 |
|
1345 |
bt_hci_event(hci, EVT_READ_REMOTE_EXT_FEATURES_COMPLETE, |
1346 |
¶ms, EVT_READ_REMOTE_EXT_FEATURES_COMPLETE_SIZE); |
1347 |
} |
1348 |
|
1349 |
static inline void bt_hci_event_complete_lmp_handle(struct bt_hci_s *hci, |
1350 |
uint16_t handle) |
1351 |
{ |
1352 |
read_lmp_handle_rp params = { |
1353 |
.status = HCI_NO_CONNECTION, |
1354 |
.handle = HNDL(handle), |
1355 |
.reserved = 0,
|
1356 |
/* Rest uninitialised */
|
1357 |
}; |
1358 |
|
1359 |
bt_hci_event_complete(hci, ¶ms, READ_LMP_HANDLE_RP_SIZE); |
1360 |
} |
1361 |
|
1362 |
static inline void bt_hci_event_complete_role_discovery(struct bt_hci_s *hci, |
1363 |
int status, uint16_t handle, int master) |
1364 |
{ |
1365 |
role_discovery_rp params = { |
1366 |
.status = status, |
1367 |
.handle = HNDL(handle), |
1368 |
.role = master ? 0x00 : 0x01, |
1369 |
}; |
1370 |
|
1371 |
bt_hci_event_complete(hci, ¶ms, ROLE_DISCOVERY_RP_SIZE); |
1372 |
} |
1373 |
|
1374 |
static inline void bt_hci_event_complete_flush(struct bt_hci_s *hci, |
1375 |
int status, uint16_t handle)
|
1376 |
{ |
1377 |
flush_rp params = { |
1378 |
.status = status, |
1379 |
.handle = HNDL(handle), |
1380 |
}; |
1381 |
|
1382 |
bt_hci_event_complete(hci, ¶ms, FLUSH_RP_SIZE); |
1383 |
} |
1384 |
|
1385 |
static inline void bt_hci_event_complete_read_local_name(struct bt_hci_s *hci) |
1386 |
{ |
1387 |
read_local_name_rp params; |
1388 |
params.status = HCI_SUCCESS; |
1389 |
memset(params.name, 0, sizeof(params.name)); |
1390 |
if (hci->device.lmp_name)
|
1391 |
strncpy(params.name, hci->device.lmp_name, sizeof(params.name));
|
1392 |
|
1393 |
bt_hci_event_complete(hci, ¶ms, READ_LOCAL_NAME_RP_SIZE); |
1394 |
} |
1395 |
|
1396 |
static inline void bt_hci_event_complete_read_conn_accept_timeout( |
1397 |
struct bt_hci_s *hci)
|
1398 |
{ |
1399 |
read_conn_accept_timeout_rp params = { |
1400 |
.status = HCI_SUCCESS, |
1401 |
.timeout = cpu_to_le16(hci->conn_accept_tout), |
1402 |
}; |
1403 |
|
1404 |
bt_hci_event_complete(hci, ¶ms, READ_CONN_ACCEPT_TIMEOUT_RP_SIZE); |
1405 |
} |
1406 |
|
1407 |
static inline void bt_hci_event_complete_read_scan_enable(struct bt_hci_s *hci) |
1408 |
{ |
1409 |
read_scan_enable_rp params = { |
1410 |
.status = HCI_SUCCESS, |
1411 |
.enable = |
1412 |
(hci->device.inquiry_scan ? SCAN_INQUIRY : 0) |
|
1413 |
(hci->device.page_scan ? SCAN_PAGE : 0),
|
1414 |
}; |
1415 |
|
1416 |
bt_hci_event_complete(hci, ¶ms, READ_SCAN_ENABLE_RP_SIZE); |
1417 |
} |
1418 |
|
1419 |
static inline void bt_hci_event_complete_read_local_class(struct bt_hci_s *hci) |
1420 |
{ |
1421 |
read_class_of_dev_rp params; |
1422 |
|
1423 |
params.status = HCI_SUCCESS; |
1424 |
memcpy(params.dev_class, hci->device.class, sizeof(params.dev_class));
|
1425 |
|
1426 |
bt_hci_event_complete(hci, ¶ms, READ_CLASS_OF_DEV_RP_SIZE); |
1427 |
} |
1428 |
|
1429 |
static inline void bt_hci_event_complete_voice_setting(struct bt_hci_s *hci) |
1430 |
{ |
1431 |
read_voice_setting_rp params = { |
1432 |
.status = HCI_SUCCESS, |
1433 |
.voice_setting = hci->voice_setting, /* Note: no swapping */
|
1434 |
}; |
1435 |
|
1436 |
bt_hci_event_complete(hci, ¶ms, READ_VOICE_SETTING_RP_SIZE); |
1437 |
} |
1438 |
|
1439 |
static inline void bt_hci_event_complete_read_inquiry_mode( |
1440 |
struct bt_hci_s *hci)
|
1441 |
{ |
1442 |
read_inquiry_mode_rp params = { |
1443 |
.status = HCI_SUCCESS, |
1444 |
.mode = hci->lm.inquiry_mode, |
1445 |
}; |
1446 |
|
1447 |
bt_hci_event_complete(hci, ¶ms, READ_INQUIRY_MODE_RP_SIZE); |
1448 |
} |
1449 |
|
1450 |
static inline void bt_hci_event_num_comp_pkts(struct bt_hci_s *hci, |
1451 |
uint16_t handle, int packets)
|
1452 |
{ |
1453 |
uint16_t buf[EVT_NUM_COMP_PKTS_SIZE(1) / 2 + 1]; |
1454 |
evt_num_comp_pkts *params = (void *) ((uint8_t *) buf + 1); |
1455 |
|
1456 |
params->num_hndl = 1;
|
1457 |
params->connection->handle = HNDL(handle); |
1458 |
params->connection->num_packets = cpu_to_le16(packets); |
1459 |
|
1460 |
bt_hci_event(hci, EVT_NUM_COMP_PKTS, params, EVT_NUM_COMP_PKTS_SIZE(1));
|
1461 |
} |
1462 |
|
1463 |
static void bt_submit_hci(struct HCIInfo *info, |
1464 |
const uint8_t *data, int length) |
1465 |
{ |
1466 |
struct bt_hci_s *hci = hci_from_info(info);
|
1467 |
uint16_t cmd; |
1468 |
int paramlen, i;
|
1469 |
|
1470 |
if (length < HCI_COMMAND_HDR_SIZE)
|
1471 |
goto short_hci;
|
1472 |
|
1473 |
memcpy(&hci->last_cmd, data, 2);
|
1474 |
|
1475 |
cmd = (data[1] << 8) | data[0]; |
1476 |
paramlen = data[2];
|
1477 |
if (cmd_opcode_ogf(cmd) == 0 || cmd_opcode_ocf(cmd) == 0) /* NOP */ |
1478 |
return;
|
1479 |
|
1480 |
data += HCI_COMMAND_HDR_SIZE; |
1481 |
length -= HCI_COMMAND_HDR_SIZE; |
1482 |
|
1483 |
if (paramlen > length)
|
1484 |
return;
|
1485 |
|
1486 |
#define PARAM(cmd, param) (((cmd##_cp *) data)->param) |
1487 |
#define PARAM16(cmd, param) le16_to_cpup(&PARAM(cmd, param))
|
1488 |
#define PARAMHANDLE(cmd) HNDL(PARAM(cmd, handle))
|
1489 |
#define LENGTH_CHECK(cmd) if (length < sizeof(cmd##_cp)) goto short_hci |
1490 |
/* Note: the supported commands bitmask in bt_hci_read_local_commands_rp
|
1491 |
* needs to be updated every time a command is implemented here! */
|
1492 |
switch (cmd) {
|
1493 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_INQUIRY):
|
1494 |
LENGTH_CHECK(inquiry); |
1495 |
|
1496 |
if (PARAM(inquiry, length) < 1) { |
1497 |
bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS); |
1498 |
break;
|
1499 |
} |
1500 |
|
1501 |
hci->lm.inquire = 1;
|
1502 |
hci->lm.periodic = 0;
|
1503 |
hci->lm.responses_left = PARAM(inquiry, num_rsp) ?: INT_MAX; |
1504 |
hci->lm.responses = 0;
|
1505 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1506 |
bt_hci_inquiry_start(hci, PARAM(inquiry, length)); |
1507 |
break;
|
1508 |
|
1509 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_INQUIRY_CANCEL):
|
1510 |
if (!hci->lm.inquire || hci->lm.periodic) {
|
1511 |
fprintf(stderr, "%s: Inquiry Cancel should only be issued after "
|
1512 |
"the Inquiry command has been issued, a Command "
|
1513 |
"Status event has been received for the Inquiry "
|
1514 |
"command, and before the Inquiry Complete event "
|
1515 |
"occurs", __FUNCTION__);
|
1516 |
bt_hci_event_complete_status(hci, HCI_COMMAND_DISALLOWED); |
1517 |
break;
|
1518 |
} |
1519 |
|
1520 |
hci->lm.inquire = 0;
|
1521 |
qemu_del_timer(hci->lm.inquiry_done); |
1522 |
bt_hci_event_complete_status(hci, HCI_SUCCESS); |
1523 |
break;
|
1524 |
|
1525 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_PERIODIC_INQUIRY):
|
1526 |
LENGTH_CHECK(periodic_inquiry); |
1527 |
|
1528 |
if (!(PARAM(periodic_inquiry, length) <
|
1529 |
PARAM16(periodic_inquiry, min_period) && |
1530 |
PARAM16(periodic_inquiry, min_period) < |
1531 |
PARAM16(periodic_inquiry, max_period)) || |
1532 |
PARAM(periodic_inquiry, length) < 1 ||
|
1533 |
PARAM16(periodic_inquiry, min_period) < 2 ||
|
1534 |
PARAM16(periodic_inquiry, max_period) < 3) {
|
1535 |
bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS); |
1536 |
break;
|
1537 |
} |
1538 |
|
1539 |
hci->lm.inquire = 1;
|
1540 |
hci->lm.periodic = 1;
|
1541 |
hci->lm.responses_left = PARAM(periodic_inquiry, num_rsp); |
1542 |
hci->lm.responses = 0;
|
1543 |
hci->lm.inquiry_period = PARAM16(periodic_inquiry, max_period); |
1544 |
bt_hci_event_complete_status(hci, HCI_SUCCESS); |
1545 |
bt_hci_inquiry_start(hci, PARAM(periodic_inquiry, length)); |
1546 |
break;
|
1547 |
|
1548 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_EXIT_PERIODIC_INQUIRY):
|
1549 |
if (!hci->lm.inquire || !hci->lm.periodic) {
|
1550 |
fprintf(stderr, "%s: Inquiry Cancel should only be issued after "
|
1551 |
"the Inquiry command has been issued, a Command "
|
1552 |
"Status event has been received for the Inquiry "
|
1553 |
"command, and before the Inquiry Complete event "
|
1554 |
"occurs", __FUNCTION__);
|
1555 |
bt_hci_event_complete_status(hci, HCI_COMMAND_DISALLOWED); |
1556 |
break;
|
1557 |
} |
1558 |
hci->lm.inquire = 0;
|
1559 |
qemu_del_timer(hci->lm.inquiry_done); |
1560 |
qemu_del_timer(hci->lm.inquiry_next); |
1561 |
bt_hci_event_complete_status(hci, HCI_SUCCESS); |
1562 |
break;
|
1563 |
|
1564 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_CREATE_CONN):
|
1565 |
LENGTH_CHECK(create_conn); |
1566 |
|
1567 |
if (hci->lm.connecting >= HCI_HANDLES_MAX) {
|
1568 |
bt_hci_event_status(hci, HCI_REJECTED_LIMITED_RESOURCES); |
1569 |
break;
|
1570 |
} |
1571 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1572 |
|
1573 |
if (bt_hci_connect(hci, &PARAM(create_conn, bdaddr)))
|
1574 |
bt_hci_connection_reject_event(hci, &PARAM(create_conn, bdaddr)); |
1575 |
break;
|
1576 |
|
1577 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_DISCONNECT):
|
1578 |
LENGTH_CHECK(disconnect); |
1579 |
|
1580 |
if (bt_hci_handle_bad(hci, PARAMHANDLE(disconnect))) {
|
1581 |
bt_hci_event_status(hci, HCI_NO_CONNECTION); |
1582 |
break;
|
1583 |
} |
1584 |
|
1585 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1586 |
bt_hci_disconnect(hci, PARAMHANDLE(disconnect), |
1587 |
PARAM(disconnect, reason)); |
1588 |
break;
|
1589 |
|
1590 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_CREATE_CONN_CANCEL):
|
1591 |
LENGTH_CHECK(create_conn_cancel); |
1592 |
|
1593 |
if (bt_hci_lmp_connection_ready(hci,
|
1594 |
&PARAM(create_conn_cancel, bdaddr))) { |
1595 |
for (i = 0; i < HCI_HANDLES_MAX; i ++) |
1596 |
if (bt_hci_role_master(hci, i) && hci->lm.handle[i].link &&
|
1597 |
!bacmp(&hci->lm.handle[i].link->slave->bd_addr, |
1598 |
&PARAM(create_conn_cancel, bdaddr))) |
1599 |
break;
|
1600 |
|
1601 |
bt_hci_event_complete_conn_cancel(hci, i < HCI_HANDLES_MAX ? |
1602 |
HCI_ACL_CONNECTION_EXISTS : HCI_NO_CONNECTION, |
1603 |
&PARAM(create_conn_cancel, bdaddr)); |
1604 |
} else
|
1605 |
bt_hci_event_complete_conn_cancel(hci, HCI_SUCCESS, |
1606 |
&PARAM(create_conn_cancel, bdaddr)); |
1607 |
break;
|
1608 |
|
1609 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_ACCEPT_CONN_REQ):
|
1610 |
LENGTH_CHECK(accept_conn_req); |
1611 |
|
1612 |
if (!hci->conn_req_host ||
|
1613 |
bacmp(&PARAM(accept_conn_req, bdaddr), |
1614 |
&hci->conn_req_host->bd_addr)) { |
1615 |
bt_hci_event_status(hci, HCI_INVALID_PARAMETERS); |
1616 |
break;
|
1617 |
} |
1618 |
|
1619 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1620 |
bt_hci_connection_accept(hci, hci->conn_req_host); |
1621 |
hci->conn_req_host = NULL;
|
1622 |
break;
|
1623 |
|
1624 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_REJECT_CONN_REQ):
|
1625 |
LENGTH_CHECK(reject_conn_req); |
1626 |
|
1627 |
if (!hci->conn_req_host ||
|
1628 |
bacmp(&PARAM(reject_conn_req, bdaddr), |
1629 |
&hci->conn_req_host->bd_addr)) { |
1630 |
bt_hci_event_status(hci, HCI_INVALID_PARAMETERS); |
1631 |
break;
|
1632 |
} |
1633 |
|
1634 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1635 |
bt_hci_connection_reject(hci, hci->conn_req_host, |
1636 |
PARAM(reject_conn_req, reason)); |
1637 |
bt_hci_connection_reject_event(hci, &hci->conn_req_host->bd_addr); |
1638 |
hci->conn_req_host = NULL;
|
1639 |
break;
|
1640 |
|
1641 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_AUTH_REQUESTED):
|
1642 |
LENGTH_CHECK(auth_requested); |
1643 |
|
1644 |
if (bt_hci_handle_bad(hci, PARAMHANDLE(auth_requested)))
|
1645 |
bt_hci_event_status(hci, HCI_NO_CONNECTION); |
1646 |
else {
|
1647 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1648 |
bt_hci_event_auth_complete(hci, PARAMHANDLE(auth_requested)); |
1649 |
} |
1650 |
break;
|
1651 |
|
1652 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_SET_CONN_ENCRYPT):
|
1653 |
LENGTH_CHECK(set_conn_encrypt); |
1654 |
|
1655 |
if (bt_hci_handle_bad(hci, PARAMHANDLE(set_conn_encrypt)))
|
1656 |
bt_hci_event_status(hci, HCI_NO_CONNECTION); |
1657 |
else {
|
1658 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1659 |
bt_hci_event_encrypt_change(hci, |
1660 |
PARAMHANDLE(set_conn_encrypt), |
1661 |
PARAM(set_conn_encrypt, encrypt)); |
1662 |
} |
1663 |
break;
|
1664 |
|
1665 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_REMOTE_NAME_REQ):
|
1666 |
LENGTH_CHECK(remote_name_req); |
1667 |
|
1668 |
if (bt_hci_name_req(hci, &PARAM(remote_name_req, bdaddr)))
|
1669 |
bt_hci_event_status(hci, HCI_NO_CONNECTION); |
1670 |
break;
|
1671 |
|
1672 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_REMOTE_NAME_REQ_CANCEL):
|
1673 |
LENGTH_CHECK(remote_name_req_cancel); |
1674 |
|
1675 |
bt_hci_event_complete_name_cancel(hci, |
1676 |
&PARAM(remote_name_req_cancel, bdaddr)); |
1677 |
break;
|
1678 |
|
1679 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_FEATURES):
|
1680 |
LENGTH_CHECK(read_remote_features); |
1681 |
|
1682 |
if (bt_hci_features_req(hci, PARAMHANDLE(read_remote_features)))
|
1683 |
bt_hci_event_status(hci, HCI_NO_CONNECTION); |
1684 |
break;
|
1685 |
|
1686 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_EXT_FEATURES):
|
1687 |
LENGTH_CHECK(read_remote_ext_features); |
1688 |
|
1689 |
if (bt_hci_handle_bad(hci, PARAMHANDLE(read_remote_ext_features)))
|
1690 |
bt_hci_event_status(hci, HCI_NO_CONNECTION); |
1691 |
else {
|
1692 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1693 |
bt_hci_event_read_remote_ext_features(hci, |
1694 |
PARAMHANDLE(read_remote_ext_features)); |
1695 |
} |
1696 |
break;
|
1697 |
|
1698 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_REMOTE_VERSION):
|
1699 |
LENGTH_CHECK(read_remote_version); |
1700 |
|
1701 |
if (bt_hci_version_req(hci, PARAMHANDLE(read_remote_version)))
|
1702 |
bt_hci_event_status(hci, HCI_NO_CONNECTION); |
1703 |
break;
|
1704 |
|
1705 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_CLOCK_OFFSET):
|
1706 |
LENGTH_CHECK(read_clock_offset); |
1707 |
|
1708 |
if (bt_hci_clkoffset_req(hci, PARAMHANDLE(read_clock_offset)))
|
1709 |
bt_hci_event_status(hci, HCI_NO_CONNECTION); |
1710 |
break;
|
1711 |
|
1712 |
case cmd_opcode_pack(OGF_LINK_CTL, OCF_READ_LMP_HANDLE):
|
1713 |
LENGTH_CHECK(read_lmp_handle); |
1714 |
|
1715 |
/* TODO: */
|
1716 |
bt_hci_event_complete_lmp_handle(hci, PARAMHANDLE(read_lmp_handle)); |
1717 |
break;
|
1718 |
|
1719 |
case cmd_opcode_pack(OGF_LINK_POLICY, OCF_HOLD_MODE):
|
1720 |
LENGTH_CHECK(hold_mode); |
1721 |
|
1722 |
if (PARAM16(hold_mode, min_interval) >
|
1723 |
PARAM16(hold_mode, max_interval) || |
1724 |
PARAM16(hold_mode, min_interval) < 0x0002 ||
|
1725 |
PARAM16(hold_mode, max_interval) > 0xff00 ||
|
1726 |
(PARAM16(hold_mode, min_interval) & 1) ||
|
1727 |
(PARAM16(hold_mode, max_interval) & 1)) {
|
1728 |
bt_hci_event_status(hci, HCI_INVALID_PARAMETERS); |
1729 |
break;
|
1730 |
} |
1731 |
|
1732 |
if (bt_hci_mode_change(hci, PARAMHANDLE(hold_mode),
|
1733 |
PARAM16(hold_mode, max_interval), |
1734 |
acl_hold)) |
1735 |
bt_hci_event_status(hci, HCI_NO_CONNECTION); |
1736 |
break;
|
1737 |
|
1738 |
case cmd_opcode_pack(OGF_LINK_POLICY, OCF_PARK_MODE):
|
1739 |
LENGTH_CHECK(park_mode); |
1740 |
|
1741 |
if (PARAM16(park_mode, min_interval) >
|
1742 |
PARAM16(park_mode, max_interval) || |
1743 |
PARAM16(park_mode, min_interval) < 0x000e ||
|
1744 |
(PARAM16(park_mode, min_interval) & 1) ||
|
1745 |
(PARAM16(park_mode, max_interval) & 1)) {
|
1746 |
bt_hci_event_status(hci, HCI_INVALID_PARAMETERS); |
1747 |
break;
|
1748 |
} |
1749 |
|
1750 |
if (bt_hci_mode_change(hci, PARAMHANDLE(park_mode),
|
1751 |
PARAM16(park_mode, max_interval), |
1752 |
acl_parked)) |
1753 |
bt_hci_event_status(hci, HCI_NO_CONNECTION); |
1754 |
break;
|
1755 |
|
1756 |
case cmd_opcode_pack(OGF_LINK_POLICY, OCF_EXIT_PARK_MODE):
|
1757 |
LENGTH_CHECK(exit_park_mode); |
1758 |
|
1759 |
if (bt_hci_mode_cancel(hci, PARAMHANDLE(exit_park_mode),
|
1760 |
acl_parked)) |
1761 |
bt_hci_event_status(hci, HCI_NO_CONNECTION); |
1762 |
break;
|
1763 |
|
1764 |
case cmd_opcode_pack(OGF_LINK_POLICY, OCF_ROLE_DISCOVERY):
|
1765 |
LENGTH_CHECK(role_discovery); |
1766 |
|
1767 |
if (bt_hci_handle_bad(hci, PARAMHANDLE(role_discovery)))
|
1768 |
bt_hci_event_complete_role_discovery(hci, |
1769 |
HCI_NO_CONNECTION, PARAMHANDLE(role_discovery), 0);
|
1770 |
else
|
1771 |
bt_hci_event_complete_role_discovery(hci, |
1772 |
HCI_SUCCESS, PARAMHANDLE(role_discovery), |
1773 |
bt_hci_role_master(hci, |
1774 |
PARAMHANDLE(role_discovery))); |
1775 |
break;
|
1776 |
|
1777 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_SET_EVENT_MASK):
|
1778 |
LENGTH_CHECK(set_event_mask); |
1779 |
|
1780 |
memcpy(hci->event_mask, PARAM(set_event_mask, mask), 8);
|
1781 |
bt_hci_event_complete_status(hci, HCI_SUCCESS); |
1782 |
break;
|
1783 |
|
1784 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_RESET):
|
1785 |
bt_hci_reset(hci); |
1786 |
bt_hci_event_status(hci, HCI_SUCCESS); |
1787 |
break;
|
1788 |
|
1789 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_SET_EVENT_FLT):
|
1790 |
if (length >= 1 && PARAM(set_event_flt, flt_type) == FLT_CLEAR_ALL) |
1791 |
/* No length check */;
|
1792 |
else
|
1793 |
LENGTH_CHECK(set_event_flt); |
1794 |
|
1795 |
/* Filters are not implemented */
|
1796 |
bt_hci_event_complete_status(hci, HCI_SUCCESS); |
1797 |
break;
|
1798 |
|
1799 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_FLUSH):
|
1800 |
LENGTH_CHECK(flush); |
1801 |
|
1802 |
if (bt_hci_handle_bad(hci, PARAMHANDLE(flush)))
|
1803 |
bt_hci_event_complete_flush(hci, |
1804 |
HCI_NO_CONNECTION, PARAMHANDLE(flush)); |
1805 |
else {
|
1806 |
/* TODO: ordering? */
|
1807 |
bt_hci_event(hci, EVT_FLUSH_OCCURRED, |
1808 |
&PARAM(flush, handle), |
1809 |
EVT_FLUSH_OCCURRED_SIZE); |
1810 |
bt_hci_event_complete_flush(hci, |
1811 |
HCI_SUCCESS, PARAMHANDLE(flush)); |
1812 |
} |
1813 |
break;
|
1814 |
|
1815 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_CHANGE_LOCAL_NAME):
|
1816 |
LENGTH_CHECK(change_local_name); |
1817 |
|
1818 |
if (hci->device.lmp_name)
|
1819 |
g_free((void *) hci->device.lmp_name);
|
1820 |
hci->device.lmp_name = g_strndup(PARAM(change_local_name, name), |
1821 |
sizeof(PARAM(change_local_name, name)));
|
1822 |
bt_hci_event_complete_status(hci, HCI_SUCCESS); |
1823 |
break;
|
1824 |
|
1825 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_LOCAL_NAME):
|
1826 |
bt_hci_event_complete_read_local_name(hci); |
1827 |
break;
|
1828 |
|
1829 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_CONN_ACCEPT_TIMEOUT):
|
1830 |
bt_hci_event_complete_read_conn_accept_timeout(hci); |
1831 |
break;
|
1832 |
|
1833 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_CONN_ACCEPT_TIMEOUT):
|
1834 |
/* TODO */
|
1835 |
LENGTH_CHECK(write_conn_accept_timeout); |
1836 |
|
1837 |
if (PARAM16(write_conn_accept_timeout, timeout) < 0x0001 || |
1838 |
PARAM16(write_conn_accept_timeout, timeout) > 0xb540) {
|
1839 |
bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS); |
1840 |
break;
|
1841 |
} |
1842 |
|
1843 |
hci->conn_accept_tout = PARAM16(write_conn_accept_timeout, timeout); |
1844 |
bt_hci_event_complete_status(hci, HCI_SUCCESS); |
1845 |
break;
|
1846 |
|
1847 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_SCAN_ENABLE):
|
1848 |
bt_hci_event_complete_read_scan_enable(hci); |
1849 |
break;
|
1850 |
|
1851 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE):
|
1852 |
LENGTH_CHECK(write_scan_enable); |
1853 |
|
1854 |
/* TODO: check that the remaining bits are all 0 */
|
1855 |
hci->device.inquiry_scan = |
1856 |
!!(PARAM(write_scan_enable, scan_enable) & SCAN_INQUIRY); |
1857 |
hci->device.page_scan = |
1858 |
!!(PARAM(write_scan_enable, scan_enable) & SCAN_PAGE); |
1859 |
bt_hci_event_complete_status(hci, HCI_SUCCESS); |
1860 |
break;
|
1861 |
|
1862 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_CLASS_OF_DEV):
|
1863 |
bt_hci_event_complete_read_local_class(hci); |
1864 |
break;
|
1865 |
|
1866 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_CLASS_OF_DEV):
|
1867 |
LENGTH_CHECK(write_class_of_dev); |
1868 |
|
1869 |
memcpy(hci->device.class, PARAM(write_class_of_dev, dev_class), |
1870 |
sizeof(PARAM(write_class_of_dev, dev_class)));
|
1871 |
bt_hci_event_complete_status(hci, HCI_SUCCESS); |
1872 |
break;
|
1873 |
|
1874 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_VOICE_SETTING):
|
1875 |
bt_hci_event_complete_voice_setting(hci); |
1876 |
break;
|
1877 |
|
1878 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_VOICE_SETTING):
|
1879 |
LENGTH_CHECK(write_voice_setting); |
1880 |
|
1881 |
hci->voice_setting = PARAM(write_voice_setting, voice_setting); |
1882 |
bt_hci_event_complete_status(hci, HCI_SUCCESS); |
1883 |
break;
|
1884 |
|
1885 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_HOST_NUMBER_OF_COMPLETED_PACKETS):
|
1886 |
if (length < data[0] * 2 + 1) |
1887 |
goto short_hci;
|
1888 |
|
1889 |
for (i = 0; i < data[0]; i ++) |
1890 |
if (bt_hci_handle_bad(hci,
|
1891 |
data[i * 2 + 1] | (data[i * 2 + 2] << 8))) |
1892 |
bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS); |
1893 |
break;
|
1894 |
|
1895 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_READ_INQUIRY_MODE):
|
1896 |
/* Only if (local_features[3] & 0x40) && (local_commands[12] & 0x40)
|
1897 |
* else
|
1898 |
* goto unknown_command */
|
1899 |
bt_hci_event_complete_read_inquiry_mode(hci); |
1900 |
break;
|
1901 |
|
1902 |
case cmd_opcode_pack(OGF_HOST_CTL, OCF_WRITE_INQUIRY_MODE):
|
1903 |
/* Only if (local_features[3] & 0x40) && (local_commands[12] & 0x80)
|
1904 |
* else
|
1905 |
* goto unknown_command */
|
1906 |
LENGTH_CHECK(write_inquiry_mode); |
1907 |
|
1908 |
if (PARAM(write_inquiry_mode, mode) > 0x01) { |
1909 |
bt_hci_event_complete_status(hci, HCI_INVALID_PARAMETERS); |
1910 |
break;
|
1911 |
} |
1912 |
|
1913 |
hci->lm.inquiry_mode = PARAM(write_inquiry_mode, mode); |
1914 |
bt_hci_event_complete_status(hci, HCI_SUCCESS); |
1915 |
break;
|
1916 |
|
1917 |
case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_VERSION):
|
1918 |
bt_hci_read_local_version_rp(hci); |
1919 |
break;
|
1920 |
|
1921 |
case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_COMMANDS):
|
1922 |
bt_hci_read_local_commands_rp(hci); |
1923 |
break;
|
1924 |
|
1925 |
case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_FEATURES):
|
1926 |
bt_hci_read_local_features_rp(hci); |
1927 |
break;
|
1928 |
|
1929 |
case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_LOCAL_EXT_FEATURES):
|
1930 |
LENGTH_CHECK(read_local_ext_features); |
1931 |
|
1932 |
bt_hci_read_local_ext_features_rp(hci, |
1933 |
PARAM(read_local_ext_features, page_num)); |
1934 |
break;
|
1935 |
|
1936 |
case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_BUFFER_SIZE):
|
1937 |
bt_hci_read_buffer_size_rp(hci); |
1938 |
break;
|
1939 |
|
1940 |
case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_COUNTRY_CODE):
|
1941 |
bt_hci_read_country_code_rp(hci); |
1942 |
break;
|
1943 |
|
1944 |
case cmd_opcode_pack(OGF_INFO_PARAM, OCF_READ_BD_ADDR):
|
1945 |
bt_hci_read_bd_addr_rp(hci); |
1946 |
break;
|
1947 |
|
1948 |
case cmd_opcode_pack(OGF_STATUS_PARAM, OCF_READ_LINK_QUALITY):
|
1949 |
LENGTH_CHECK(read_link_quality); |
1950 |
|
1951 |
bt_hci_link_quality_rp(hci, PARAMHANDLE(read_link_quality)); |
1952 |
break;
|
1953 |
|
1954 |
default:
|
1955 |
bt_hci_event_status(hci, HCI_UNKNOWN_COMMAND); |
1956 |
break;
|
1957 |
|
1958 |
short_hci:
|
1959 |
fprintf(stderr, "%s: HCI packet too short (%iB)\n",
|
1960 |
__FUNCTION__, length); |
1961 |
bt_hci_event_status(hci, HCI_INVALID_PARAMETERS); |
1962 |
break;
|
1963 |
} |
1964 |
} |
1965 |
|
1966 |
/* We could perform fragmentation here, we can't do "recombination" because
|
1967 |
* at this layer the length of the payload is not know ahead, so we only
|
1968 |
* know that a packet contained the last fragment of the SDU when the next
|
1969 |
* SDU starts. */
|
1970 |
static inline void bt_hci_lmp_acl_data(struct bt_hci_s *hci, uint16_t handle, |
1971 |
const uint8_t *data, int start, int len) |
1972 |
{ |
1973 |
struct hci_acl_hdr *pkt = (void *) hci->acl_buf; |
1974 |
|
1975 |
/* TODO: packet flags */
|
1976 |
/* TODO: avoid memcpy'ing */
|
1977 |
|
1978 |
if (len + HCI_ACL_HDR_SIZE > sizeof(hci->acl_buf)) { |
1979 |
fprintf(stderr, "%s: can't take ACL packets %i bytes long\n",
|
1980 |
__FUNCTION__, len); |
1981 |
return;
|
1982 |
} |
1983 |
memcpy(hci->acl_buf + HCI_ACL_HDR_SIZE, data, len); |
1984 |
|
1985 |
pkt->handle = cpu_to_le16( |
1986 |
acl_handle_pack(handle, start ? ACL_START : ACL_CONT)); |
1987 |
pkt->dlen = cpu_to_le16(len); |
1988 |
hci->info.acl_recv(hci->info.opaque, |
1989 |
hci->acl_buf, len + HCI_ACL_HDR_SIZE); |
1990 |
} |
1991 |
|
1992 |
static void bt_hci_lmp_acl_data_slave(struct bt_link_s *btlink, |
1993 |
const uint8_t *data, int start, int len) |
1994 |
{ |
1995 |
struct bt_hci_link_s *link = (struct bt_hci_link_s *) btlink; |
1996 |
|
1997 |
bt_hci_lmp_acl_data(hci_from_device(btlink->slave), |
1998 |
link->handle, data, start, len); |
1999 |
} |
2000 |
|
2001 |
static void bt_hci_lmp_acl_data_host(struct bt_link_s *link, |
2002 |
const uint8_t *data, int start, int len) |
2003 |
{ |
2004 |
bt_hci_lmp_acl_data(hci_from_device(link->host), |
2005 |
link->handle, data, start, len); |
2006 |
} |
2007 |
|
2008 |
static void bt_submit_acl(struct HCIInfo *info, |
2009 |
const uint8_t *data, int length) |
2010 |
{ |
2011 |
struct bt_hci_s *hci = hci_from_info(info);
|
2012 |
uint16_t handle; |
2013 |
int datalen, flags;
|
2014 |
struct bt_link_s *link;
|
2015 |
|
2016 |
if (length < HCI_ACL_HDR_SIZE) {
|
2017 |
fprintf(stderr, "%s: ACL packet too short (%iB)\n",
|
2018 |
__FUNCTION__, length); |
2019 |
return;
|
2020 |
} |
2021 |
|
2022 |
handle = acl_handle((data[1] << 8) | data[0]); |
2023 |
flags = acl_flags((data[1] << 8) | data[0]); |
2024 |
datalen = (data[3] << 8) | data[2]; |
2025 |
data += HCI_ACL_HDR_SIZE; |
2026 |
length -= HCI_ACL_HDR_SIZE; |
2027 |
|
2028 |
if (bt_hci_handle_bad(hci, handle)) {
|
2029 |
fprintf(stderr, "%s: invalid ACL handle %03x\n",
|
2030 |
__FUNCTION__, handle); |
2031 |
/* TODO: signal an error */
|
2032 |
return;
|
2033 |
} |
2034 |
handle &= ~HCI_HANDLE_OFFSET; |
2035 |
|
2036 |
if (datalen > length) {
|
2037 |
fprintf(stderr, "%s: ACL packet too short (%iB < %iB)\n",
|
2038 |
__FUNCTION__, length, datalen); |
2039 |
return;
|
2040 |
} |
2041 |
|
2042 |
link = hci->lm.handle[handle].link; |
2043 |
|
2044 |
if ((flags & ~3) == ACL_ACTIVE_BCAST) { |
2045 |
if (!hci->asb_handle)
|
2046 |
hci->asb_handle = handle; |
2047 |
else if (handle != hci->asb_handle) { |
2048 |
fprintf(stderr, "%s: Bad handle %03x in Active Slave Broadcast\n",
|
2049 |
__FUNCTION__, handle); |
2050 |
/* TODO: signal an error */
|
2051 |
return;
|
2052 |
} |
2053 |
|
2054 |
/* TODO */
|
2055 |
} |
2056 |
|
2057 |
if ((flags & ~3) == ACL_PICO_BCAST) { |
2058 |
if (!hci->psb_handle)
|
2059 |
hci->psb_handle = handle; |
2060 |
else if (handle != hci->psb_handle) { |
2061 |
fprintf(stderr, "%s: Bad handle %03x in Parked Slave Broadcast\n",
|
2062 |
__FUNCTION__, handle); |
2063 |
/* TODO: signal an error */
|
2064 |
return;
|
2065 |
} |
2066 |
|
2067 |
/* TODO */
|
2068 |
} |
2069 |
|
2070 |
/* TODO: increase counter and send EVT_NUM_COMP_PKTS */
|
2071 |
bt_hci_event_num_comp_pkts(hci, handle | HCI_HANDLE_OFFSET, 1);
|
2072 |
|
2073 |
/* Do this last as it can trigger further events even in this HCI */
|
2074 |
hci->lm.handle[handle].lmp_acl_data(link, data, |
2075 |
(flags & 3) == ACL_START, length);
|
2076 |
} |
2077 |
|
2078 |
static void bt_submit_sco(struct HCIInfo *info, |
2079 |
const uint8_t *data, int length) |
2080 |
{ |
2081 |
struct bt_hci_s *hci = hci_from_info(info);
|
2082 |
uint16_t handle; |
2083 |
int datalen;
|
2084 |
|
2085 |
if (length < 3) |
2086 |
return;
|
2087 |
|
2088 |
handle = acl_handle((data[1] << 8) | data[0]); |
2089 |
datalen = data[2];
|
2090 |
length -= 3;
|
2091 |
|
2092 |
if (bt_hci_handle_bad(hci, handle)) {
|
2093 |
fprintf(stderr, "%s: invalid SCO handle %03x\n",
|
2094 |
__FUNCTION__, handle); |
2095 |
return;
|
2096 |
} |
2097 |
|
2098 |
if (datalen > length) {
|
2099 |
fprintf(stderr, "%s: SCO packet too short (%iB < %iB)\n",
|
2100 |
__FUNCTION__, length, datalen); |
2101 |
return;
|
2102 |
} |
2103 |
|
2104 |
/* TODO */
|
2105 |
|
2106 |
/* TODO: increase counter and send EVT_NUM_COMP_PKTS if synchronous
|
2107 |
* Flow Control is enabled.
|
2108 |
* (See Read/Write_Synchronous_Flow_Control_Enable on page 513 and
|
2109 |
* page 514.) */
|
2110 |
} |
2111 |
|
2112 |
static uint8_t *bt_hci_evt_packet(void *opaque) |
2113 |
{ |
2114 |
/* TODO: allocate a packet from upper layer */
|
2115 |
struct bt_hci_s *s = opaque;
|
2116 |
|
2117 |
return s->evt_buf;
|
2118 |
} |
2119 |
|
2120 |
static void bt_hci_evt_submit(void *opaque, int len) |
2121 |
{ |
2122 |
/* TODO: notify upper layer */
|
2123 |
struct bt_hci_s *s = opaque;
|
2124 |
|
2125 |
s->info.evt_recv(s->info.opaque, s->evt_buf, len); |
2126 |
} |
2127 |
|
2128 |
static int bt_hci_bdaddr_set(struct HCIInfo *info, const uint8_t *bd_addr) |
2129 |
{ |
2130 |
struct bt_hci_s *hci = hci_from_info(info);
|
2131 |
|
2132 |
bacpy(&hci->device.bd_addr, (const bdaddr_t *) bd_addr);
|
2133 |
return 0; |
2134 |
} |
2135 |
|
2136 |
static void bt_hci_done(struct HCIInfo *info); |
2137 |
static void bt_hci_destroy(struct bt_device_s *dev) |
2138 |
{ |
2139 |
struct bt_hci_s *hci = hci_from_device(dev);
|
2140 |
|
2141 |
bt_hci_done(&hci->info); |
2142 |
} |
2143 |
|
2144 |
struct HCIInfo *bt_new_hci(struct bt_scatternet_s *net) |
2145 |
{ |
2146 |
struct bt_hci_s *s = g_malloc0(sizeof(struct bt_hci_s)); |
2147 |
|
2148 |
s->lm.inquiry_done = qemu_new_timer_ns(vm_clock, bt_hci_inquiry_done, s); |
2149 |
s->lm.inquiry_next = qemu_new_timer_ns(vm_clock, bt_hci_inquiry_next, s); |
2150 |
s->conn_accept_timer = |
2151 |
qemu_new_timer_ns(vm_clock, bt_hci_conn_accept_timeout, s); |
2152 |
|
2153 |
s->evt_packet = bt_hci_evt_packet; |
2154 |
s->evt_submit = bt_hci_evt_submit; |
2155 |
s->opaque = s; |
2156 |
|
2157 |
bt_device_init(&s->device, net); |
2158 |
s->device.lmp_connection_request = bt_hci_lmp_connection_request; |
2159 |
s->device.lmp_connection_complete = bt_hci_lmp_connection_complete; |
2160 |
s->device.lmp_disconnect_master = bt_hci_lmp_disconnect_host; |
2161 |
s->device.lmp_disconnect_slave = bt_hci_lmp_disconnect_slave; |
2162 |
s->device.lmp_acl_data = bt_hci_lmp_acl_data_slave; |
2163 |
s->device.lmp_acl_resp = bt_hci_lmp_acl_data_host; |
2164 |
s->device.lmp_mode_change = bt_hci_lmp_mode_change_slave; |
2165 |
|
2166 |
/* Keep updated! */
|
2167 |
/* Also keep in sync with supported commands bitmask in
|
2168 |
* bt_hci_read_local_commands_rp */
|
2169 |
s->device.lmp_caps = 0x8000199b7e85355fll;
|
2170 |
|
2171 |
bt_hci_reset(s); |
2172 |
|
2173 |
s->info.cmd_send = bt_submit_hci; |
2174 |
s->info.sco_send = bt_submit_sco; |
2175 |
s->info.acl_send = bt_submit_acl; |
2176 |
s->info.bdaddr_set = bt_hci_bdaddr_set; |
2177 |
|
2178 |
s->device.handle_destroy = bt_hci_destroy; |
2179 |
|
2180 |
return &s->info;
|
2181 |
} |
2182 |
|
2183 |
static void bt_hci_done(struct HCIInfo *info) |
2184 |
{ |
2185 |
struct bt_hci_s *hci = hci_from_info(info);
|
2186 |
int handle;
|
2187 |
|
2188 |
bt_device_done(&hci->device); |
2189 |
|
2190 |
if (hci->device.lmp_name)
|
2191 |
g_free((void *) hci->device.lmp_name);
|
2192 |
|
2193 |
/* Be gentle and send DISCONNECT to all connected peers and those
|
2194 |
* currently waiting for us to accept or reject a connection request.
|
2195 |
* This frees the links. */
|
2196 |
if (hci->conn_req_host) {
|
2197 |
bt_hci_connection_reject(hci, |
2198 |
hci->conn_req_host, HCI_OE_POWER_OFF); |
2199 |
return;
|
2200 |
} |
2201 |
|
2202 |
for (handle = HCI_HANDLE_OFFSET;
|
2203 |
handle < (HCI_HANDLE_OFFSET | HCI_HANDLES_MAX); handle ++) |
2204 |
if (!bt_hci_handle_bad(hci, handle))
|
2205 |
bt_hci_disconnect(hci, handle, HCI_OE_POWER_OFF); |
2206 |
|
2207 |
/* TODO: this is not enough actually, there may be slaves from whom
|
2208 |
* we have requested a connection who will soon (or not) respond with
|
2209 |
* an accept or a reject, so we should also check if hci->lm.connecting
|
2210 |
* is non-zero and if so, avoid freeing the hci but otherwise disappear
|
2211 |
* from all qemu social life (e.g. stop scanning and request to be
|
2212 |
* removed from s->device.net) and arrange for
|
2213 |
* s->device.lmp_connection_complete to free the remaining bits once
|
2214 |
* hci->lm.awaiting_bdaddr[] is empty. */
|
2215 |
|
2216 |
qemu_free_timer(hci->lm.inquiry_done); |
2217 |
qemu_free_timer(hci->lm.inquiry_next); |
2218 |
qemu_free_timer(hci->conn_accept_timer); |
2219 |
|
2220 |
g_free(hci); |
2221 |
} |