1/*
2 * QEMU Bluetooth L2CAP logic.
3 *
4 * Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.org>
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License as
8 * published by the Free Software Foundation; either version 2 of
9 * the License, or (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, see <http://www.gnu.org/licenses/>.
18 */
19
20#include "qemu/osdep.h"
21#include "qemu/error-report.h"
22#include "qemu/timer.h"
23#include "qemu/bswap.h"
24#include "hw/bt.h"
25
26#define L2CAP_CID_MAX 0x100 /* Between 0x40 and 0x10000 */
27
28struct l2cap_instance_s {
29 struct bt_link_s *link;
30 struct bt_l2cap_device_s *dev;
31 int role;
32
33 uint8_t frame_in[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4)));
34 int frame_in_len;
35
36 uint8_t frame_out[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4)));
37 int frame_out_len;
38
39 /* Signalling channel timers. They exist per-request but we can make
40 * sure we have no more than one outstanding request at any time. */
41 QEMUTimer *rtx;
42 QEMUTimer *ertx;
43
44 int last_id;
45 int next_id;
46
47 struct l2cap_chan_s {
48 struct bt_l2cap_conn_params_s params;
49
50 void (*frame_in)(struct l2cap_chan_s *chan, uint16_t cid,
51 const l2cap_hdr *hdr, int len);
52 int mps;
53 int min_mtu;
54
55 struct l2cap_instance_s *l2cap;
56
57 /* Only allocated channels */
58 uint16_t remote_cid;
59#define L2CAP_CFG_INIT 2
60#define L2CAP_CFG_ACC 1
61 int config_req_id; /* TODO: handle outgoing requests generically */
62 int config;
63
64 /* Only connection-oriented channels. Note: if we allow the tx and
65 * rx traffic to be in different modes at any time, we need two. */
66 int mode;
67
68 /* Only flow-controlled, connection-oriented channels */
69 uint8_t sdu[65536]; /* TODO: dynamically allocate */
70 int len_cur, len_total;
71 int rexmit;
72 int monitor_timeout;
73 QEMUTimer *monitor_timer;
74 QEMUTimer *retransmission_timer;
75 } *cid[L2CAP_CID_MAX];
76 /* The channel state machine states map as following:
77 * CLOSED -> !cid[N]
78 * WAIT_CONNECT -> never occurs
79 * WAIT_CONNECT_RSP -> never occurs
80 * CONFIG -> cid[N] && config < 3
81 * WAIT_CONFIG -> never occurs, cid[N] && config == 0 && !config_r
82 * WAIT_SEND_CONFIG -> never occurs, cid[N] && config == 1 && !config_r
83 * WAIT_CONFIG_REQ_RSP -> cid[N] && config == 0 && config_req_id
84 * WAIT_CONFIG_RSP -> cid[N] && config == 1 && config_req_id
85 * WAIT_CONFIG_REQ -> cid[N] && config == 2
86 * OPEN -> cid[N] && config == 3
87 * WAIT_DISCONNECT -> never occurs
88 */
89
90 struct l2cap_chan_s signalling_ch;
91 struct l2cap_chan_s group_ch;
92};
93
94struct slave_l2cap_instance_s {
95 struct bt_link_s link; /* Underlying logical link (ACL) */
96 struct l2cap_instance_s l2cap;
97};
98
99struct bt_l2cap_psm_s {
100 int psm;
101 int min_mtu;
102 int (*new_channel)(struct bt_l2cap_device_s *device,
103 struct bt_l2cap_conn_params_s *params);
104 struct bt_l2cap_psm_s *next;
105};
106
107static const uint16_t l2cap_fcs16_table[256] = {
108 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241,
109 0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440,
110 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40,
111 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841,
112 0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40,
113 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41,
114 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641,
115 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040,
116 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240,
117 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441,
118 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41,
119 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840,
120 0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41,
121 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40,
122 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640,
123 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041,
124 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240,
125 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441,
126 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41,
127 0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840,
128 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41,
129 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40,
130 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640,
131 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041,
132 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241,
133 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440,
134 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40,
135 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841,
136 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40,
137 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41,
138 0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641,
139 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040,
140};
141
142static uint16_t l2cap_fcs16(const uint8_t *message, int len)
143{
144 uint16_t fcs = 0x0000;
145
146 while (len --)
147#if 0
148 {
149 int i;
150
151 fcs ^= *message ++;
152 for (i = 8; i; -- i)
153 if (fcs & 1)
154 fcs = (fcs >> 1) ^ 0xa001;
155 else
156 fcs = (fcs >> 1);
157 }
158#else
159 fcs = (fcs >> 8) ^ l2cap_fcs16_table[(fcs ^ *message ++) & 0xff];
160#endif
161
162 return fcs;
163}
164
165/* L2CAP layer logic (protocol) */
166
167static void l2cap_retransmission_timer_update(struct l2cap_chan_s *ch)
168{
169#if 0
170 if (ch->mode != L2CAP_MODE_BASIC && ch->rexmit)
171 timer_mod(ch->retransmission_timer);
172 else
173 timer_del(ch->retransmission_timer);
174#endif
175}
176
177static void l2cap_monitor_timer_update(struct l2cap_chan_s *ch)
178{
179#if 0
180 if (ch->mode != L2CAP_MODE_BASIC && !ch->rexmit)
181 timer_mod(ch->monitor_timer);
182 else
183 timer_del(ch->monitor_timer);
184#endif
185}
186
187static void l2cap_command_reject(struct l2cap_instance_s *l2cap, int id,
188 uint16_t reason, const void *data, int plen)
189{
190 uint8_t *pkt;
191 l2cap_cmd_hdr *hdr;
192 l2cap_cmd_rej *params;
193 uint16_t len;
194
195 reason = cpu_to_le16(reason);
196 len = cpu_to_le16(L2CAP_CMD_REJ_SIZE + plen);
197
198 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
199 L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE + plen);
200 hdr = (void *) (pkt + 0);
201 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
202
203 hdr->code = L2CAP_COMMAND_REJ;
204 hdr->ident = id;
205 memcpy(&hdr->len, &len, sizeof(hdr->len));
206 memcpy(&params->reason, &reason, sizeof(reason));
207 if (plen)
208 memcpy(pkt + L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE, data, plen);
209
210 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
211}
212
213static void l2cap_command_reject_cid(struct l2cap_instance_s *l2cap, int id,
214 uint16_t reason, uint16_t dcid, uint16_t scid)
215{
216 l2cap_cmd_rej_cid params = {
217 .dcid = dcid,
218 .scid = scid,
219 };
220
221 l2cap_command_reject(l2cap, id, reason, &params, L2CAP_CMD_REJ_CID_SIZE);
222}
223
224static void l2cap_connection_response(struct l2cap_instance_s *l2cap,
225 int dcid, int scid, int result, int status)
226{
227 uint8_t *pkt;
228 l2cap_cmd_hdr *hdr;
229 l2cap_conn_rsp *params;
230
231 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
232 L2CAP_CMD_HDR_SIZE + L2CAP_CONN_RSP_SIZE);
233 hdr = (void *) (pkt + 0);
234 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
235
236 hdr->code = L2CAP_CONN_RSP;
237 hdr->ident = l2cap->last_id;
238 hdr->len = cpu_to_le16(L2CAP_CONN_RSP_SIZE);
239
240 params->dcid = cpu_to_le16(dcid);
241 params->scid = cpu_to_le16(scid);
242 params->result = cpu_to_le16(result);
243 params->status = cpu_to_le16(status);
244
245 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
246}
247
248static void l2cap_configuration_request(struct l2cap_instance_s *l2cap,
249 int dcid, int flag, const uint8_t *data, int len)
250{
251 uint8_t *pkt;
252 l2cap_cmd_hdr *hdr;
253 l2cap_conf_req *params;
254
255 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
256 L2CAP_CMD_HDR_SIZE + L2CAP_CONF_REQ_SIZE(len));
257 hdr = (void *) (pkt + 0);
258 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
259
260 /* TODO: unify the id sequencing */
261 l2cap->last_id = l2cap->next_id;
262 l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1;
263
264 hdr->code = L2CAP_CONF_REQ;
265 hdr->ident = l2cap->last_id;
266 hdr->len = cpu_to_le16(L2CAP_CONF_REQ_SIZE(len));
267
268 params->dcid = cpu_to_le16(dcid);
269 params->flags = cpu_to_le16(flag);
270 if (len)
271 memcpy(params->data, data, len);
272
273 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
274}
275
276static void l2cap_configuration_response(struct l2cap_instance_s *l2cap,
277 int scid, int flag, int result, const uint8_t *data, int len)
278{
279 uint8_t *pkt;
280 l2cap_cmd_hdr *hdr;
281 l2cap_conf_rsp *params;
282
283 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
284 L2CAP_CMD_HDR_SIZE + L2CAP_CONF_RSP_SIZE(len));
285 hdr = (void *) (pkt + 0);
286 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
287
288 hdr->code = L2CAP_CONF_RSP;
289 hdr->ident = l2cap->last_id;
290 hdr->len = cpu_to_le16(L2CAP_CONF_RSP_SIZE(len));
291
292 params->scid = cpu_to_le16(scid);
293 params->flags = cpu_to_le16(flag);
294 params->result = cpu_to_le16(result);
295 if (len)
296 memcpy(params->data, data, len);
297
298 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
299}
300
301static void l2cap_disconnection_response(struct l2cap_instance_s *l2cap,
302 int dcid, int scid)
303{
304 uint8_t *pkt;
305 l2cap_cmd_hdr *hdr;
306 l2cap_disconn_rsp *params;
307
308 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
309 L2CAP_CMD_HDR_SIZE + L2CAP_DISCONN_RSP_SIZE);
310 hdr = (void *) (pkt + 0);
311 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
312
313 hdr->code = L2CAP_DISCONN_RSP;
314 hdr->ident = l2cap->last_id;
315 hdr->len = cpu_to_le16(L2CAP_DISCONN_RSP_SIZE);
316
317 params->dcid = cpu_to_le16(dcid);
318 params->scid = cpu_to_le16(scid);
319
320 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
321}
322
323static void l2cap_echo_response(struct l2cap_instance_s *l2cap,
324 const uint8_t *data, int len)
325{
326 uint8_t *pkt;
327 l2cap_cmd_hdr *hdr;
328 uint8_t *params;
329
330 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
331 L2CAP_CMD_HDR_SIZE + len);
332 hdr = (void *) (pkt + 0);
333 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
334
335 hdr->code = L2CAP_ECHO_RSP;
336 hdr->ident = l2cap->last_id;
337 hdr->len = cpu_to_le16(len);
338
339 memcpy(params, data, len);
340
341 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
342}
343
344static void l2cap_info_response(struct l2cap_instance_s *l2cap, int type,
345 int result, const uint8_t *data, int len)
346{
347 uint8_t *pkt;
348 l2cap_cmd_hdr *hdr;
349 l2cap_info_rsp *params;
350
351 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
352 L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + len);
353 hdr = (void *) (pkt + 0);
354 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
355
356 hdr->code = L2CAP_INFO_RSP;
357 hdr->ident = l2cap->last_id;
358 hdr->len = cpu_to_le16(L2CAP_INFO_RSP_SIZE + len);
359
360 params->type = cpu_to_le16(type);
361 params->result = cpu_to_le16(result);
362 if (len)
363 memcpy(params->data, data, len);
364
365 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
366}
367
368static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len);
369static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms);
370#if 0
371static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len);
372static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm);
373#endif
374static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid,
375 const l2cap_hdr *hdr, int len);
376static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid,
377 const l2cap_hdr *hdr, int len);
378
379static int l2cap_cid_new(struct l2cap_instance_s *l2cap)
380{
381 int i;
382
383 for (i = L2CAP_CID_ALLOC; i < L2CAP_CID_MAX; i ++)
384 if (!l2cap->cid[i])
385 return i;
386
387 return L2CAP_CID_INVALID;
388}
389
390static inline struct bt_l2cap_psm_s *l2cap_psm(
391 struct bt_l2cap_device_s *device, int psm)
392{
393 struct bt_l2cap_psm_s *ret = device->first_psm;
394
395 while (ret && ret->psm != psm)
396 ret = ret->next;
397
398 return ret;
399}
400
401static struct l2cap_chan_s *l2cap_channel_open(struct l2cap_instance_s *l2cap,
402 int psm, int source_cid)
403{
404 struct l2cap_chan_s *ch = NULL;
405 struct bt_l2cap_psm_s *psm_info;
406 int result, status;
407 int cid = l2cap_cid_new(l2cap);
408
409 if (cid) {
410 /* See what the channel is to be used for.. */
411 psm_info = l2cap_psm(l2cap->dev, psm);
412
413 if (psm_info) {
414 /* Device supports this use-case. */
415 ch = g_malloc0(sizeof(*ch));
416 ch->params.sdu_out = l2cap_bframe_out;
417 ch->params.sdu_submit = l2cap_bframe_submit;
418 ch->frame_in = l2cap_bframe_in;
419 ch->mps = 65536;
420 ch->min_mtu = MAX(48, psm_info->min_mtu);
421 ch->params.remote_mtu = MAX(672, ch->min_mtu);
422 ch->remote_cid = source_cid;
423 ch->mode = L2CAP_MODE_BASIC;
424 ch->l2cap = l2cap;
425
426 /* Does it feel like opening yet another channel though? */
427 if (!psm_info->new_channel(l2cap->dev, &ch->params)) {
428 l2cap->cid[cid] = ch;
429
430 result = L2CAP_CR_SUCCESS;
431 status = L2CAP_CS_NO_INFO;
432 } else {
433 g_free(ch);
434 ch = NULL;
435 result = L2CAP_CR_NO_MEM;
436 status = L2CAP_CS_NO_INFO;
437 }
438 } else {
439 result = L2CAP_CR_BAD_PSM;
440 status = L2CAP_CS_NO_INFO;
441 }
442 } else {
443 result = L2CAP_CR_NO_MEM;
444 status = L2CAP_CS_NO_INFO;
445 }
446
447 l2cap_connection_response(l2cap, cid, source_cid, result, status);
448
449 return ch;
450}
451
452static void l2cap_channel_close(struct l2cap_instance_s *l2cap,
453 int cid, int source_cid)
454{
455 struct l2cap_chan_s *ch = NULL;
456
457 /* According to Volume 3, section 6.1.1, pg 1048 of BT Core V2.0, a
458 * connection in CLOSED state still responds with a L2CAP_DisconnectRsp
459 * message on an L2CAP_DisconnectReq event. */
460 if (unlikely(cid < L2CAP_CID_ALLOC)) {
461 l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
462 cid, source_cid);
463 return;
464 }
465 if (likely(cid >= L2CAP_CID_ALLOC && cid < L2CAP_CID_MAX))
466 ch = l2cap->cid[cid];
467
468 if (likely(ch)) {
469 if (ch->remote_cid != source_cid) {
470 error_report("%s: Ignoring a Disconnection Request with the "
471 "invalid SCID %04x.", __func__, source_cid);
472 return;
473 }
474
475 l2cap->cid[cid] = NULL;
476
477 ch->params.close(ch->params.opaque);
478 g_free(ch);
479 }
480
481 l2cap_disconnection_response(l2cap, cid, source_cid);
482}
483
484static void l2cap_channel_config_null(struct l2cap_instance_s *l2cap,
485 struct l2cap_chan_s *ch)
486{
487 l2cap_configuration_request(l2cap, ch->remote_cid, 0, NULL, 0);
488 ch->config_req_id = l2cap->last_id;
489 ch->config &= ~L2CAP_CFG_INIT;
490}
491
492static void l2cap_channel_config_req_event(struct l2cap_instance_s *l2cap,
493 struct l2cap_chan_s *ch)
494{
495 /* Use all default channel options and terminate negotiation. */
496 l2cap_channel_config_null(l2cap, ch);
497}
498
499static int l2cap_channel_config(struct l2cap_instance_s *l2cap,
500 struct l2cap_chan_s *ch, int flag,
501 const uint8_t *data, int len)
502{
503 l2cap_conf_opt *opt;
504 l2cap_conf_opt_qos *qos;
505 uint32_t val;
506 uint8_t rsp[len];
507 int result = L2CAP_CONF_SUCCESS;
508
509 data = memcpy(rsp, data, len);
510 while (len) {
511 opt = (void *) data;
512
513 if (len < L2CAP_CONF_OPT_SIZE ||
514 len < L2CAP_CONF_OPT_SIZE + opt->len) {
515 result = L2CAP_CONF_REJECT;
516 break;
517 }
518 data += L2CAP_CONF_OPT_SIZE + opt->len;
519 len -= L2CAP_CONF_OPT_SIZE + opt->len;
520
521 switch (opt->type & 0x7f) {
522 case L2CAP_CONF_MTU:
523 if (opt->len != 2) {
524 result = L2CAP_CONF_REJECT;
525 break;
526 }
527
528 /* MTU */
529 val = lduw_le_p(opt->val);
530 if (val < ch->min_mtu) {
531 stw_le_p(opt->val, ch->min_mtu);
532 result = L2CAP_CONF_UNACCEPT;
533 break;
534 }
535
536 ch->params.remote_mtu = val;
537 break;
538
539 case L2CAP_CONF_FLUSH_TO:
540 if (opt->len != 2) {
541 result = L2CAP_CONF_REJECT;
542 break;
543 }
544
545 /* Flush Timeout */
546 val = lduw_le_p(opt->val);
547 if (val < 0x0001) {
548 opt->val[0] = 0xff;
549 opt->val[1] = 0xff;
550 result = L2CAP_CONF_UNACCEPT;
551 break;
552 }
553 break;
554
555 case L2CAP_CONF_QOS:
556 if (opt->len != L2CAP_CONF_OPT_QOS_SIZE) {
557 result = L2CAP_CONF_REJECT;
558 break;
559 }
560 qos = (void *) opt->val;
561
562 /* Flags */
563 val = qos->flags;
564 if (val) {
565 qos->flags = 0;
566 result = L2CAP_CONF_UNACCEPT;
567 }
568
569 /* Service type */
570 val = qos->service_type;
571 if (val != L2CAP_CONF_QOS_BEST_EFFORT &&
572 val != L2CAP_CONF_QOS_NO_TRAFFIC) {
573 qos->service_type = L2CAP_CONF_QOS_BEST_EFFORT;
574 result = L2CAP_CONF_UNACCEPT;
575 }
576
577 if (val != L2CAP_CONF_QOS_NO_TRAFFIC) {
578 /* XXX: These values should possibly be calculated
579 * based on LM / baseband properties also. */
580
581 /* Token rate */
582 val = le32_to_cpu(qos->token_rate);
583 if (val == L2CAP_CONF_QOS_WILDCARD)
584 qos->token_rate = cpu_to_le32(0x100000);
585
586 /* Token bucket size */
587 val = le32_to_cpu(qos->token_bucket_size);
588 if (val == L2CAP_CONF_QOS_WILDCARD)
589 qos->token_bucket_size = cpu_to_le32(65500);
590
591 /* Any Peak bandwidth value is correct to return as-is */
592 /* Any Access latency value is correct to return as-is */
593 /* Any Delay variation value is correct to return as-is */
594 }
595 break;
596
597 case L2CAP_CONF_RFC:
598 if (opt->len != 9) {
599 result = L2CAP_CONF_REJECT;
600 break;
601 }
602
603 /* Mode */
604 val = opt->val[0];
605 switch (val) {
606 case L2CAP_MODE_BASIC:
607 ch->mode = val;
608 ch->frame_in = l2cap_bframe_in;
609
610 /* All other parameters shall be ignored */
611 break;
612
613 case L2CAP_MODE_RETRANS:
614 case L2CAP_MODE_FLOWCTL:
615 ch->mode = val;
616 ch->frame_in = l2cap_iframe_in;
617 /* Note: most of these parameters refer to incoming traffic
618 * so we don't need to save them as long as we can accept
619 * incoming PDUs at any values of the parameters. */
620
621 /* TxWindow size */
622 val = opt->val[1];
623 if (val < 1 || val > 32) {
624 opt->val[1] = 32;
625 result = L2CAP_CONF_UNACCEPT;
626 break;
627 }
628
629 /* MaxTransmit */
630 val = opt->val[2];
631 if (val < 1) {
632 opt->val[2] = 1;
633 result = L2CAP_CONF_UNACCEPT;
634 break;
635 }
636
637 /* Remote Retransmission time-out shouldn't affect local
638 * operation (?) */
639
640 /* The Monitor time-out drives the local Monitor timer (?),
641 * so save the value. */
642 val = (opt->val[6] << 8) | opt->val[5];
643 if (val < 30) {
644 opt->val[5] = 100 & 0xff;
645 opt->val[6] = 100 >> 8;
646 result = L2CAP_CONF_UNACCEPT;
647 break;
648 }
649 ch->monitor_timeout = val;
650 l2cap_monitor_timer_update(ch);
651
652 /* MPS */
653 val = (opt->val[8] << 8) | opt->val[7];
654 if (val < ch->min_mtu) {
655 opt->val[7] = ch->min_mtu & 0xff;
656 opt->val[8] = ch->min_mtu >> 8;
657 result = L2CAP_CONF_UNACCEPT;
658 break;
659 }
660 ch->mps = val;
661 break;
662
663 default:
664 result = L2CAP_CONF_UNACCEPT;
665 break;
666 }
667 break;
668
669 default:
670 if (!(opt->type >> 7))
671 result = L2CAP_CONF_UNKNOWN;
672 break;
673 }
674
675 if (result != L2CAP_CONF_SUCCESS)
676 break; /* XXX: should continue? */
677 }
678
679 l2cap_configuration_response(l2cap, ch->remote_cid,
680 flag, result, rsp, len);
681
682 return result == L2CAP_CONF_SUCCESS && !flag;
683}
684
685static void l2cap_channel_config_req_msg(struct l2cap_instance_s *l2cap,
686 int flag, int cid, const uint8_t *data, int len)
687{
688 struct l2cap_chan_s *ch;
689
690 if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
691 l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
692 cid, 0x0000);
693 return;
694 }
695 ch = l2cap->cid[cid];
696
697 /* From OPEN go to WAIT_CONFIG_REQ and from WAIT_CONFIG_REQ_RSP to
698 * WAIT_CONFIG_REQ_RSP. This is assuming the transition chart for OPEN
699 * on pg 1053, section 6.1.5, volume 3 of BT Core V2.0 has a mistake
700 * and on options-acceptable we go back to OPEN and otherwise to
701 * WAIT_CONFIG_REQ and not the other way. */
702 ch->config &= ~L2CAP_CFG_ACC;
703
704 if (l2cap_channel_config(l2cap, ch, flag, data, len))
705 /* Go to OPEN or WAIT_CONFIG_RSP */
706 ch->config |= L2CAP_CFG_ACC;
707
708 /* TODO: if the incoming traffic flow control or retransmission mode
709 * changed then we probably need to also generate the
710 * ConfigureChannel_Req event and set the outgoing traffic to the same
711 * mode. */
712 if (!(ch->config & L2CAP_CFG_INIT) && (ch->config & L2CAP_CFG_ACC) &&
713 !ch->config_req_id)
714 l2cap_channel_config_req_event(l2cap, ch);
715}
716
717static int l2cap_channel_config_rsp_msg(struct l2cap_instance_s *l2cap,
718 int result, int flag, int cid, const uint8_t *data, int len)
719{
720 struct l2cap_chan_s *ch;
721
722 if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
723 l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
724 cid, 0x0000);
725 return 0;
726 }
727 ch = l2cap->cid[cid];
728
729 if (ch->config_req_id != l2cap->last_id)
730 return 1;
731 ch->config_req_id = 0;
732
733 if (result == L2CAP_CONF_SUCCESS) {
734 if (!flag)
735 ch->config |= L2CAP_CFG_INIT;
736 else
737 l2cap_channel_config_null(l2cap, ch);
738 } else
739 /* Retry until we succeed */
740 l2cap_channel_config_req_event(l2cap, ch);
741
742 return 0;
743}
744
745static void l2cap_channel_open_req_msg(struct l2cap_instance_s *l2cap,
746 int psm, int source_cid)
747{
748 struct l2cap_chan_s *ch = l2cap_channel_open(l2cap, psm, source_cid);
749
750 if (!ch)
751 return;
752
753 /* Optional */
754 if (!(ch->config & L2CAP_CFG_INIT) && !ch->config_req_id)
755 l2cap_channel_config_req_event(l2cap, ch);
756}
757
758static void l2cap_info(struct l2cap_instance_s *l2cap, int type)
759{
760 uint8_t data[4];
761 int len = 0;
762 int result = L2CAP_IR_SUCCESS;
763
764 switch (type) {
765 case L2CAP_IT_CL_MTU:
766 data[len ++] = l2cap->group_ch.mps & 0xff;
767 data[len ++] = l2cap->group_ch.mps >> 8;
768 break;
769
770 case L2CAP_IT_FEAT_MASK:
771 /* (Prematurely) report Flow control and Retransmission modes. */
772 data[len ++] = 0x03;
773 data[len ++] = 0x00;
774 data[len ++] = 0x00;
775 data[len ++] = 0x00;
776 break;
777
778 default:
779 result = L2CAP_IR_NOTSUPP;
780 }
781
782 l2cap_info_response(l2cap, type, result, data, len);
783}
784
785static void l2cap_command(struct l2cap_instance_s *l2cap, int code, int id,
786 const uint8_t *params, int len)
787{
788 int err;
789
790#if 0
791 /* TODO: do the IDs really have to be in sequence? */
792 if (!id || (id != l2cap->last_id && id != l2cap->next_id)) {
793 error_report("%s: out of sequence command packet ignored.",
794 __func__);
795 return;
796 }
797#else
798 l2cap->next_id = id;
799#endif
800 if (id == l2cap->next_id) {
801 l2cap->last_id = l2cap->next_id;
802 l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1;
803 } else {
804 /* TODO: Need to re-send the same response, without re-executing
805 * the corresponding command! */
806 }
807
808 switch (code) {
809 case L2CAP_COMMAND_REJ:
810 if (unlikely(len != 2 && len != 4 && len != 6)) {
811 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
812 goto reject;
813 }
814
815 /* We never issue commands other than Command Reject currently. */
816 error_report("%s: stray Command Reject (%02x, %04x) "
817 "packet, ignoring.", __func__, id,
818 le16_to_cpu(((l2cap_cmd_rej *) params)->reason));
819 break;
820
821 case L2CAP_CONN_REQ:
822 if (unlikely(len != L2CAP_CONN_REQ_SIZE)) {
823 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
824 goto reject;
825 }
826
827 l2cap_channel_open_req_msg(l2cap,
828 le16_to_cpu(((l2cap_conn_req *) params)->psm),
829 le16_to_cpu(((l2cap_conn_req *) params)->scid));
830 break;
831
832 case L2CAP_CONN_RSP:
833 if (unlikely(len != L2CAP_CONN_RSP_SIZE)) {
834 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
835 goto reject;
836 }
837
838 /* We never issue Connection Requests currently. TODO */
839 error_report("%s: unexpected Connection Response (%02x) "
840 "packet, ignoring.", __func__, id);
841 break;
842
843 case L2CAP_CONF_REQ:
844 if (unlikely(len < L2CAP_CONF_REQ_SIZE(0))) {
845 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
846 goto reject;
847 }
848
849 l2cap_channel_config_req_msg(l2cap,
850 le16_to_cpu(((l2cap_conf_req *) params)->flags) & 1,
851 le16_to_cpu(((l2cap_conf_req *) params)->dcid),
852 ((l2cap_conf_req *) params)->data,
853 len - L2CAP_CONF_REQ_SIZE(0));
854 break;
855
856 case L2CAP_CONF_RSP:
857 if (unlikely(len < L2CAP_CONF_RSP_SIZE(0))) {
858 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
859 goto reject;
860 }
861
862 if (l2cap_channel_config_rsp_msg(l2cap,
863 le16_to_cpu(((l2cap_conf_rsp *) params)->result),
864 le16_to_cpu(((l2cap_conf_rsp *) params)->flags) & 1,
865 le16_to_cpu(((l2cap_conf_rsp *) params)->scid),
866 ((l2cap_conf_rsp *) params)->data,
867 len - L2CAP_CONF_RSP_SIZE(0)))
868 error_report("%s: unexpected Configure Response (%02x) "
869 "packet, ignoring.", __func__, id);
870 break;
871
872 case L2CAP_DISCONN_REQ:
873 if (unlikely(len != L2CAP_DISCONN_REQ_SIZE)) {
874 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
875 goto reject;
876 }
877
878 l2cap_channel_close(l2cap,
879 le16_to_cpu(((l2cap_disconn_req *) params)->dcid),
880 le16_to_cpu(((l2cap_disconn_req *) params)->scid));
881 break;
882
883 case L2CAP_DISCONN_RSP:
884 if (unlikely(len != L2CAP_DISCONN_RSP_SIZE)) {
885 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
886 goto reject;
887 }
888
889 /* We never issue Disconnection Requests currently. TODO */
890 error_report("%s: unexpected Disconnection Response (%02x) "
891 "packet, ignoring.", __func__, id);
892 break;
893
894 case L2CAP_ECHO_REQ:
895 l2cap_echo_response(l2cap, params, len);
896 break;
897
898 case L2CAP_ECHO_RSP:
899 /* We never issue Echo Requests currently. TODO */
900 error_report("%s: unexpected Echo Response (%02x) "
901 "packet, ignoring.", __func__, id);
902 break;
903
904 case L2CAP_INFO_REQ:
905 if (unlikely(len != L2CAP_INFO_REQ_SIZE)) {
906 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
907 goto reject;
908 }
909
910 l2cap_info(l2cap, le16_to_cpu(((l2cap_info_req *) params)->type));
911 break;
912
913 case L2CAP_INFO_RSP:
914 if (unlikely(len != L2CAP_INFO_RSP_SIZE)) {
915 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
916 goto reject;
917 }
918
919 /* We never issue Information Requests currently. TODO */
920 error_report("%s: unexpected Information Response (%02x) "
921 "packet, ignoring.", __func__, id);
922 break;
923
924 default:
925 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
926 reject:
927 l2cap_command_reject(l2cap, id, err, 0, 0);
928 break;
929 }
930}
931
932static void l2cap_rexmit_enable(struct l2cap_chan_s *ch, int enable)
933{
934 ch->rexmit = enable;
935
936 l2cap_retransmission_timer_update(ch);
937 l2cap_monitor_timer_update(ch);
938}
939
940/* Command frame SDU */
941static void l2cap_cframe_in(void *opaque, const uint8_t *data, int len)
942{
943 struct l2cap_instance_s *l2cap = opaque;
944 const l2cap_cmd_hdr *hdr;
945 int clen;
946
947 while (len) {
948 hdr = (void *) data;
949 if (len < L2CAP_CMD_HDR_SIZE)
950 /* TODO: signal an error */
951 return;
952 len -= L2CAP_CMD_HDR_SIZE;
953 data += L2CAP_CMD_HDR_SIZE;
954
955 clen = le16_to_cpu(hdr->len);
956 if (len < clen) {
957 l2cap_command_reject(l2cap, hdr->ident,
958 L2CAP_REJ_CMD_NOT_UNDERSTOOD, 0, 0);
959 break;
960 }
961
962 l2cap_command(l2cap, hdr->code, hdr->ident, data, clen);
963 len -= clen;
964 data += clen;
965 }
966}
967
968/* Group frame SDU */
969static void l2cap_gframe_in(void *opaque, const uint8_t *data, int len)
970{
971}
972
973/* Supervisory frame */
974static void l2cap_sframe_in(struct l2cap_chan_s *ch, uint16_t ctrl)
975{
976}
977
978/* Basic L2CAP mode Information frame */
979static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid,
980 const l2cap_hdr *hdr, int len)
981{
982 /* We have a full SDU, no further processing */
983 ch->params.sdu_in(ch->params.opaque, hdr->data, len);
984}
985
986/* Flow Control and Retransmission mode frame */
987static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid,
988 const l2cap_hdr *hdr, int len)
989{
990 uint16_t fcs = lduw_le_p(hdr->data + len - 2);
991
992 if (len < 4)
993 goto len_error;
994 if (l2cap_fcs16((const uint8_t *) hdr, L2CAP_HDR_SIZE + len - 2) != fcs)
995 goto fcs_error;
996
997 if ((hdr->data[0] >> 7) == ch->rexmit)
998 l2cap_rexmit_enable(ch, !(hdr->data[0] >> 7));
999
1000 if (hdr->data[0] & 1) {
1001 if (len != 4) {
1002 /* TODO: Signal an error? */
1003 return;
1004 }
1005 l2cap_sframe_in(ch, lduw_le_p(hdr->data));
1006 return;
1007 }
1008
1009 switch (hdr->data[1] >> 6) { /* SAR */
1010 case L2CAP_SAR_NO_SEG:
1011 if (ch->len_total)
1012 goto seg_error;
1013 if (len - 4 > ch->mps)
1014 goto len_error;
1015
1016 ch->params.sdu_in(ch->params.opaque, hdr->data + 2, len - 4);
1017 break;
1018
1019 case L2CAP_SAR_START:
1020 if (ch->len_total || len < 6)
1021 goto seg_error;
1022 if (len - 6 > ch->mps)
1023 goto len_error;
1024
1025 ch->len_total = lduw_le_p(hdr->data + 2);
1026 if (len >= 6 + ch->len_total)
1027 goto seg_error;
1028
1029 ch->len_cur = len - 6;
1030 memcpy(ch->sdu, hdr->data + 4, ch->len_cur);
1031 break;
1032
1033 case L2CAP_SAR_END:
1034 if (!ch->len_total || ch->len_cur + len - 4 < ch->len_total)
1035 goto seg_error;
1036 if (len - 4 > ch->mps)
1037 goto len_error;
1038
1039 memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
1040 ch->params.sdu_in(ch->params.opaque, ch->sdu, ch->len_total);
1041 break;
1042
1043 case L2CAP_SAR_CONT:
1044 if (!ch->len_total || ch->len_cur + len - 4 >= ch->len_total)
1045 goto seg_error;
1046 if (len - 4 > ch->mps)
1047 goto len_error;
1048
1049 memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
1050 ch->len_cur += len - 4;
1051 break;
1052
1053 seg_error:
1054 len_error: /* TODO */
1055 fcs_error: /* TODO */
1056 ch->len_cur = 0;
1057 ch->len_total = 0;
1058 break;
1059 }
1060}
1061
1062static void l2cap_frame_in(struct l2cap_instance_s *l2cap,
1063 const l2cap_hdr *frame)
1064{
1065 uint16_t cid = le16_to_cpu(frame->cid);
1066 uint16_t len = le16_to_cpu(frame->len);
1067
1068 if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
1069 error_report("%s: frame addressed to a non-existent L2CAP "
1070 "channel %04x received.", __func__, cid);
1071 return;
1072 }
1073
1074 l2cap->cid[cid]->frame_in(l2cap->cid[cid], cid, frame, len);
1075}
1076
1077/* "Recombination" */
1078static void l2cap_pdu_in(struct l2cap_instance_s *l2cap,
1079 const uint8_t *data, int len)
1080{
1081 const l2cap_hdr *hdr = (void *) l2cap->frame_in;
1082
1083 if (unlikely(len + l2cap->frame_in_len > sizeof(l2cap->frame_in))) {
1084 if (l2cap->frame_in_len < sizeof(l2cap->frame_in)) {
1085 memcpy(l2cap->frame_in + l2cap->frame_in_len, data,
1086 sizeof(l2cap->frame_in) - l2cap->frame_in_len);
1087 l2cap->frame_in_len = sizeof(l2cap->frame_in);
1088 /* TODO: truncate */
1089 l2cap_frame_in(l2cap, hdr);
1090 }
1091
1092 return;
1093 }
1094
1095 memcpy(l2cap->frame_in + l2cap->frame_in_len, data, len);
1096 l2cap->frame_in_len += len;
1097
1098 if (len >= L2CAP_HDR_SIZE)
1099 if (len >= L2CAP_HDR_SIZE + le16_to_cpu(hdr->len))
1100 l2cap_frame_in(l2cap, hdr);
1101 /* There is never a start of a new PDU in the same ACL packet, so
1102 * no need to memmove the remaining payload and loop. */
1103}
1104
1105static inline uint8_t *l2cap_pdu_out(struct l2cap_instance_s *l2cap,
1106 uint16_t cid, uint16_t len)
1107{
1108 l2cap_hdr *hdr = (void *) l2cap->frame_out;
1109
1110 l2cap->frame_out_len = len + L2CAP_HDR_SIZE;
1111
1112 hdr->cid = cpu_to_le16(cid);
1113 hdr->len = cpu_to_le16(len);
1114
1115 return l2cap->frame_out + L2CAP_HDR_SIZE;
1116}
1117
1118static inline void l2cap_pdu_submit(struct l2cap_instance_s *l2cap)
1119{
1120 /* TODO: Fragmentation */
1121 (l2cap->role ?
1122 l2cap->link->slave->lmp_acl_data : l2cap->link->host->lmp_acl_resp)
1123 (l2cap->link, l2cap->frame_out, 1, l2cap->frame_out_len);
1124}
1125
1126static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len)
1127{
1128 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
1129
1130 if (len > chan->params.remote_mtu) {
1131 error_report("%s: B-Frame for CID %04x longer than %i octets.",
1132 __func__,
1133 chan->remote_cid, chan->params.remote_mtu);
1134 exit(-1);
1135 }
1136
1137 return l2cap_pdu_out(chan->l2cap, chan->remote_cid, len);
1138}
1139
1140static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms)
1141{
1142 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parms;
1143
1144 l2cap_pdu_submit(chan->l2cap);
1145}
1146
1147#if 0
1148/* Stub: Only used if an emulated device requests outgoing flow control */
1149static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len)
1150{
1151 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
1152
1153 if (len > chan->params.remote_mtu) {
1154 /* TODO: slice into segments and queue each segment as a separate
1155 * I-Frame in a FIFO of I-Frames, local to the CID. */
1156 } else {
1157 /* TODO: add to the FIFO of I-Frames, local to the CID. */
1158 /* Possibly we need to return a pointer to a contiguous buffer
1159 * for now and then memcpy from it into FIFOs in l2cap_iframe_submit
1160 * while segmenting at the same time. */
1161 }
1162 return 0;
1163}
1164
1165static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm)
1166{
1167 /* TODO: If flow control indicates clear to send, start submitting the
1168 * invidual I-Frames from the FIFO, but don't remove them from there.
1169 * Kick the appropriate timer until we get an S-Frame, and only then
1170 * remove from FIFO or resubmit and re-kick the timer if the timer
1171 * expired. */
1172}
1173#endif
1174
1175static void l2cap_init(struct l2cap_instance_s *l2cap,
1176 struct bt_link_s *link, int role)
1177{
1178 l2cap->link = link;
1179 l2cap->role = role;
1180 l2cap->dev = (struct bt_l2cap_device_s *)
1181 (role ? link->host : link->slave);
1182
1183 l2cap->next_id = 1;
1184
1185 /* Establish the signalling channel */
1186 l2cap->signalling_ch.params.sdu_in = l2cap_cframe_in;
1187 l2cap->signalling_ch.params.sdu_out = l2cap_bframe_out;
1188 l2cap->signalling_ch.params.sdu_submit = l2cap_bframe_submit;
1189 l2cap->signalling_ch.params.opaque = l2cap;
1190 l2cap->signalling_ch.params.remote_mtu = 48;
1191 l2cap->signalling_ch.remote_cid = L2CAP_CID_SIGNALLING;
1192 l2cap->signalling_ch.frame_in = l2cap_bframe_in;
1193 l2cap->signalling_ch.mps = 65536;
1194 l2cap->signalling_ch.min_mtu = 48;
1195 l2cap->signalling_ch.mode = L2CAP_MODE_BASIC;
1196 l2cap->signalling_ch.l2cap = l2cap;
1197 l2cap->cid[L2CAP_CID_SIGNALLING] = &l2cap->signalling_ch;
1198
1199 /* Establish the connection-less data channel */
1200 l2cap->group_ch.params.sdu_in = l2cap_gframe_in;
1201 l2cap->group_ch.params.opaque = l2cap;
1202 l2cap->group_ch.frame_in = l2cap_bframe_in;
1203 l2cap->group_ch.mps = 65533;
1204 l2cap->group_ch.l2cap = l2cap;
1205 l2cap->group_ch.remote_cid = L2CAP_CID_INVALID;
1206 l2cap->cid[L2CAP_CID_GROUP] = &l2cap->group_ch;
1207}
1208
1209static void l2cap_teardown(struct l2cap_instance_s *l2cap, int send_disconnect)
1210{
1211 int cid;
1212
1213 /* Don't send DISCONNECT if we are currently handling a DISCONNECT
1214 * sent from the other side. */
1215 if (send_disconnect) {
1216 if (l2cap->role)
1217 l2cap->dev->device.lmp_disconnect_slave(l2cap->link);
1218 /* l2cap->link is invalid from now on. */
1219 else
1220 l2cap->dev->device.lmp_disconnect_master(l2cap->link);
1221 }
1222
1223 for (cid = L2CAP_CID_ALLOC; cid < L2CAP_CID_MAX; cid ++)
1224 if (l2cap->cid[cid]) {
1225 l2cap->cid[cid]->params.close(l2cap->cid[cid]->params.opaque);
1226 g_free(l2cap->cid[cid]);
1227 }
1228
1229 if (l2cap->role)
1230 g_free(l2cap);
1231 else
1232 g_free(l2cap->link);
1233}
1234
1235/* L2CAP glue to lower layers in bluetooth stack (LMP) */
1236
1237static void l2cap_lmp_connection_request(struct bt_link_s *link)
1238{
1239 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->slave;
1240 struct slave_l2cap_instance_s *l2cap;
1241
1242 /* Always accept - we only get called if (dev->device->page_scan). */
1243
1244 l2cap = g_malloc0(sizeof(struct slave_l2cap_instance_s));
1245 l2cap->link.slave = &dev->device;
1246 l2cap->link.host = link->host;
1247 l2cap_init(&l2cap->l2cap, &l2cap->link, 0);
1248
1249 /* Always at the end */
1250 link->host->reject_reason = 0;
1251 link->host->lmp_connection_complete(&l2cap->link);
1252}
1253
1254/* Stub */
1255static void l2cap_lmp_connection_complete(struct bt_link_s *link)
1256{
1257 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1258 struct l2cap_instance_s *l2cap;
1259
1260 if (dev->device.reject_reason) {
1261 /* Signal to upper layer */
1262 return;
1263 }
1264
1265 l2cap = g_malloc0(sizeof(struct l2cap_instance_s));
1266 l2cap_init(l2cap, link, 1);
1267
1268 link->acl_mode = acl_active;
1269
1270 /* Signal to upper layer */
1271}
1272
1273/* Stub */
1274static void l2cap_lmp_disconnect_host(struct bt_link_s *link)
1275{
1276 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1277 struct l2cap_instance_s *l2cap =
1278 /* TODO: Retrieve from upper layer */ (void *) dev;
1279
1280 /* Signal to upper layer */
1281
1282 l2cap_teardown(l2cap, 0);
1283}
1284
1285static void l2cap_lmp_disconnect_slave(struct bt_link_s *link)
1286{
1287 struct slave_l2cap_instance_s *l2cap =
1288 (struct slave_l2cap_instance_s *) link;
1289
1290 l2cap_teardown(&l2cap->l2cap, 0);
1291}
1292
1293static void l2cap_lmp_acl_data_slave(struct bt_link_s *link,
1294 const uint8_t *data, int start, int len)
1295{
1296 struct slave_l2cap_instance_s *l2cap =
1297 (struct slave_l2cap_instance_s *) link;
1298
1299 if (start)
1300 l2cap->l2cap.frame_in_len = 0;
1301
1302 l2cap_pdu_in(&l2cap->l2cap, data, len);
1303}
1304
1305/* Stub */
1306static void l2cap_lmp_acl_data_host(struct bt_link_s *link,
1307 const uint8_t *data, int start, int len)
1308{
1309 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1310 struct l2cap_instance_s *l2cap =
1311 /* TODO: Retrieve from upper layer */ (void *) dev;
1312
1313 if (start)
1314 l2cap->frame_in_len = 0;
1315
1316 l2cap_pdu_in(l2cap, data, len);
1317}
1318
1319static void l2cap_dummy_destroy(struct bt_device_s *dev)
1320{
1321 struct bt_l2cap_device_s *l2cap_dev = (struct bt_l2cap_device_s *) dev;
1322
1323 bt_l2cap_device_done(l2cap_dev);
1324}
1325
1326void bt_l2cap_device_init(struct bt_l2cap_device_s *dev,
1327 struct bt_scatternet_s *net)
1328{
1329 bt_device_init(&dev->device, net);
1330
1331 dev->device.lmp_connection_request = l2cap_lmp_connection_request;
1332 dev->device.lmp_connection_complete = l2cap_lmp_connection_complete;
1333 dev->device.lmp_disconnect_master = l2cap_lmp_disconnect_host;
1334 dev->device.lmp_disconnect_slave = l2cap_lmp_disconnect_slave;
1335 dev->device.lmp_acl_data = l2cap_lmp_acl_data_slave;
1336 dev->device.lmp_acl_resp = l2cap_lmp_acl_data_host;
1337
1338 dev->device.handle_destroy = l2cap_dummy_destroy;
1339}
1340
1341void bt_l2cap_device_done(struct bt_l2cap_device_s *dev)
1342{
1343 bt_device_done(&dev->device);
1344
1345 /* Should keep a list of all instances and go through it and
1346 * invoke l2cap_teardown() for each. */
1347}
1348
1349void bt_l2cap_psm_register(struct bt_l2cap_device_s *dev, int psm, int min_mtu,
1350 int (*new_channel)(struct bt_l2cap_device_s *dev,
1351 struct bt_l2cap_conn_params_s *params))
1352{
1353 struct bt_l2cap_psm_s *new_psm = l2cap_psm(dev, psm);
1354
1355 if (new_psm) {
1356 error_report("%s: PSM %04x already registered for device `%s'.",
1357 __func__, psm, dev->device.lmp_name);
1358 exit(-1);
1359 }
1360
1361 new_psm = g_malloc0(sizeof(*new_psm));
1362 new_psm->psm = psm;
1363 new_psm->min_mtu = min_mtu;
1364 new_psm->new_channel = new_channel;
1365 new_psm->next = dev->first_psm;
1366 dev->first_psm = new_psm;
1367}
1368