Replace all setjmp()/longjmp() with sigsetjmp()/siglongjmp()
[qemu/pbrook.git] / hw / bt-l2cap.c
blobba061c0da32762297fe23fbe8ca04c19157a3833
1 /*
2 * QEMU Bluetooth L2CAP logic.
4 * Copyright (C) 2008 Andrzej Zaborowski <balrog@zabor.org>
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.
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.
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/>.
20 #include "qemu-common.h"
21 #include "qemu/timer.h"
22 #include "bt.h"
24 #define L2CAP_CID_MAX 0x100 /* Between 0x40 and 0x10000 */
26 struct l2cap_instance_s {
27 struct bt_link_s *link;
28 struct bt_l2cap_device_s *dev;
29 int role;
31 uint8_t frame_in[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4)));
32 int frame_in_len;
34 uint8_t frame_out[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4)));
35 int frame_out_len;
37 /* Signalling channel timers. They exist per-request but we can make
38 * sure we have no more than one outstanding request at any time. */
39 QEMUTimer *rtx;
40 QEMUTimer *ertx;
42 int last_id;
43 int next_id;
45 struct l2cap_chan_s {
46 struct bt_l2cap_conn_params_s params;
48 void (*frame_in)(struct l2cap_chan_s *chan, uint16_t cid,
49 const l2cap_hdr *hdr, int len);
50 int mps;
51 int min_mtu;
53 struct l2cap_instance_s *l2cap;
55 /* Only allocated channels */
56 uint16_t remote_cid;
57 #define L2CAP_CFG_INIT 2
58 #define L2CAP_CFG_ACC 1
59 int config_req_id; /* TODO: handle outgoing requests generically */
60 int config;
62 /* Only connection-oriented channels. Note: if we allow the tx and
63 * rx traffic to be in different modes at any time, we need two. */
64 int mode;
66 /* Only flow-controlled, connection-oriented channels */
67 uint8_t sdu[65536]; /* TODO: dynamically allocate */
68 int len_cur, len_total;
69 int rexmit;
70 int monitor_timeout;
71 QEMUTimer *monitor_timer;
72 QEMUTimer *retransmission_timer;
73 } *cid[L2CAP_CID_MAX];
74 /* The channel state machine states map as following:
75 * CLOSED -> !cid[N]
76 * WAIT_CONNECT -> never occurs
77 * WAIT_CONNECT_RSP -> never occurs
78 * CONFIG -> cid[N] && config < 3
79 * WAIT_CONFIG -> never occurs, cid[N] && config == 0 && !config_r
80 * WAIT_SEND_CONFIG -> never occurs, cid[N] && config == 1 && !config_r
81 * WAIT_CONFIG_REQ_RSP -> cid[N] && config == 0 && config_req_id
82 * WAIT_CONFIG_RSP -> cid[N] && config == 1 && config_req_id
83 * WAIT_CONFIG_REQ -> cid[N] && config == 2
84 * OPEN -> cid[N] && config == 3
85 * WAIT_DISCONNECT -> never occurs
88 struct l2cap_chan_s signalling_ch;
89 struct l2cap_chan_s group_ch;
92 struct slave_l2cap_instance_s {
93 struct bt_link_s link; /* Underlying logical link (ACL) */
94 struct l2cap_instance_s l2cap;
97 struct bt_l2cap_psm_s {
98 int psm;
99 int min_mtu;
100 int (*new_channel)(struct bt_l2cap_device_s *device,
101 struct bt_l2cap_conn_params_s *params);
102 struct bt_l2cap_psm_s *next;
105 static const uint16_t l2cap_fcs16_table[256] = {
106 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241,
107 0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440,
108 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40,
109 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841,
110 0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40,
111 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41,
112 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641,
113 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040,
114 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240,
115 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441,
116 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41,
117 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840,
118 0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41,
119 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40,
120 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640,
121 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041,
122 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240,
123 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441,
124 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41,
125 0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840,
126 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41,
127 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40,
128 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640,
129 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041,
130 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241,
131 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440,
132 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40,
133 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841,
134 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40,
135 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41,
136 0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641,
137 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040,
140 static uint16_t l2cap_fcs16(const uint8_t *message, int len)
142 uint16_t fcs = 0x0000;
144 while (len --)
145 #if 0
147 int i;
149 fcs ^= *message ++;
150 for (i = 8; i; -- i)
151 if (fcs & 1)
152 fcs = (fcs >> 1) ^ 0xa001;
153 else
154 fcs = (fcs >> 1);
156 #else
157 fcs = (fcs >> 8) ^ l2cap_fcs16_table[(fcs ^ *message ++) & 0xff];
158 #endif
160 return fcs;
163 /* L2CAP layer logic (protocol) */
165 static void l2cap_retransmission_timer_update(struct l2cap_chan_s *ch)
167 #if 0
168 if (ch->mode != L2CAP_MODE_BASIC && ch->rexmit)
169 qemu_mod_timer(ch->retransmission_timer);
170 else
171 qemu_del_timer(ch->retransmission_timer);
172 #endif
175 static void l2cap_monitor_timer_update(struct l2cap_chan_s *ch)
177 #if 0
178 if (ch->mode != L2CAP_MODE_BASIC && !ch->rexmit)
179 qemu_mod_timer(ch->monitor_timer);
180 else
181 qemu_del_timer(ch->monitor_timer);
182 #endif
185 static void l2cap_command_reject(struct l2cap_instance_s *l2cap, int id,
186 uint16_t reason, const void *data, int plen)
188 uint8_t *pkt;
189 l2cap_cmd_hdr *hdr;
190 l2cap_cmd_rej *params;
191 uint16_t len;
193 reason = cpu_to_le16(reason);
194 len = cpu_to_le16(L2CAP_CMD_REJ_SIZE + plen);
196 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
197 L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE + plen);
198 hdr = (void *) (pkt + 0);
199 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
201 hdr->code = L2CAP_COMMAND_REJ;
202 hdr->ident = id;
203 memcpy(&hdr->len, &len, sizeof(hdr->len));
204 memcpy(&params->reason, &reason, sizeof(reason));
205 if (plen)
206 memcpy(pkt + L2CAP_CMD_HDR_SIZE + L2CAP_CMD_REJ_SIZE, data, plen);
208 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
211 static void l2cap_command_reject_cid(struct l2cap_instance_s *l2cap, int id,
212 uint16_t reason, uint16_t dcid, uint16_t scid)
214 l2cap_cmd_rej_cid params = {
215 .dcid = dcid,
216 .scid = scid,
219 l2cap_command_reject(l2cap, id, reason, &params, L2CAP_CMD_REJ_CID_SIZE);
222 static void l2cap_connection_response(struct l2cap_instance_s *l2cap,
223 int dcid, int scid, int result, int status)
225 uint8_t *pkt;
226 l2cap_cmd_hdr *hdr;
227 l2cap_conn_rsp *params;
229 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
230 L2CAP_CMD_HDR_SIZE + L2CAP_CONN_RSP_SIZE);
231 hdr = (void *) (pkt + 0);
232 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
234 hdr->code = L2CAP_CONN_RSP;
235 hdr->ident = l2cap->last_id;
236 hdr->len = cpu_to_le16(L2CAP_CONN_RSP_SIZE);
238 params->dcid = cpu_to_le16(dcid);
239 params->scid = cpu_to_le16(scid);
240 params->result = cpu_to_le16(result);
241 params->status = cpu_to_le16(status);
243 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
246 static void l2cap_configuration_request(struct l2cap_instance_s *l2cap,
247 int dcid, int flag, const uint8_t *data, int len)
249 uint8_t *pkt;
250 l2cap_cmd_hdr *hdr;
251 l2cap_conf_req *params;
253 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
254 L2CAP_CMD_HDR_SIZE + L2CAP_CONF_REQ_SIZE(len));
255 hdr = (void *) (pkt + 0);
256 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
258 /* TODO: unify the id sequencing */
259 l2cap->last_id = l2cap->next_id;
260 l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1;
262 hdr->code = L2CAP_CONF_REQ;
263 hdr->ident = l2cap->last_id;
264 hdr->len = cpu_to_le16(L2CAP_CONF_REQ_SIZE(len));
266 params->dcid = cpu_to_le16(dcid);
267 params->flags = cpu_to_le16(flag);
268 if (len)
269 memcpy(params->data, data, len);
271 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
274 static void l2cap_configuration_response(struct l2cap_instance_s *l2cap,
275 int scid, int flag, int result, const uint8_t *data, int len)
277 uint8_t *pkt;
278 l2cap_cmd_hdr *hdr;
279 l2cap_conf_rsp *params;
281 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
282 L2CAP_CMD_HDR_SIZE + L2CAP_CONF_RSP_SIZE(len));
283 hdr = (void *) (pkt + 0);
284 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
286 hdr->code = L2CAP_CONF_RSP;
287 hdr->ident = l2cap->last_id;
288 hdr->len = cpu_to_le16(L2CAP_CONF_RSP_SIZE(len));
290 params->scid = cpu_to_le16(scid);
291 params->flags = cpu_to_le16(flag);
292 params->result = cpu_to_le16(result);
293 if (len)
294 memcpy(params->data, data, len);
296 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
299 static void l2cap_disconnection_response(struct l2cap_instance_s *l2cap,
300 int dcid, int scid)
302 uint8_t *pkt;
303 l2cap_cmd_hdr *hdr;
304 l2cap_disconn_rsp *params;
306 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
307 L2CAP_CMD_HDR_SIZE + L2CAP_DISCONN_RSP_SIZE);
308 hdr = (void *) (pkt + 0);
309 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
311 hdr->code = L2CAP_DISCONN_RSP;
312 hdr->ident = l2cap->last_id;
313 hdr->len = cpu_to_le16(L2CAP_DISCONN_RSP_SIZE);
315 params->dcid = cpu_to_le16(dcid);
316 params->scid = cpu_to_le16(scid);
318 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
321 static void l2cap_echo_response(struct l2cap_instance_s *l2cap,
322 const uint8_t *data, int len)
324 uint8_t *pkt;
325 l2cap_cmd_hdr *hdr;
326 uint8_t *params;
328 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
329 L2CAP_CMD_HDR_SIZE + len);
330 hdr = (void *) (pkt + 0);
331 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
333 hdr->code = L2CAP_ECHO_RSP;
334 hdr->ident = l2cap->last_id;
335 hdr->len = cpu_to_le16(len);
337 memcpy(params, data, len);
339 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
342 static void l2cap_info_response(struct l2cap_instance_s *l2cap, int type,
343 int result, const uint8_t *data, int len)
345 uint8_t *pkt;
346 l2cap_cmd_hdr *hdr;
347 l2cap_info_rsp *params;
349 pkt = l2cap->signalling_ch.params.sdu_out(&l2cap->signalling_ch.params,
350 L2CAP_CMD_HDR_SIZE + L2CAP_INFO_RSP_SIZE + len);
351 hdr = (void *) (pkt + 0);
352 params = (void *) (pkt + L2CAP_CMD_HDR_SIZE);
354 hdr->code = L2CAP_INFO_RSP;
355 hdr->ident = l2cap->last_id;
356 hdr->len = cpu_to_le16(L2CAP_INFO_RSP_SIZE + len);
358 params->type = cpu_to_le16(type);
359 params->result = cpu_to_le16(result);
360 if (len)
361 memcpy(params->data, data, len);
363 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
366 static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len);
367 static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms);
368 #if 0
369 static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len);
370 static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm);
371 #endif
372 static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid,
373 const l2cap_hdr *hdr, int len);
374 static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid,
375 const l2cap_hdr *hdr, int len);
377 static int l2cap_cid_new(struct l2cap_instance_s *l2cap)
379 int i;
381 for (i = L2CAP_CID_ALLOC; i < L2CAP_CID_MAX; i ++)
382 if (!l2cap->cid[i])
383 return i;
385 return L2CAP_CID_INVALID;
388 static inline struct bt_l2cap_psm_s *l2cap_psm(
389 struct bt_l2cap_device_s *device, int psm)
391 struct bt_l2cap_psm_s *ret = device->first_psm;
393 while (ret && ret->psm != psm)
394 ret = ret->next;
396 return ret;
399 static struct l2cap_chan_s *l2cap_channel_open(struct l2cap_instance_s *l2cap,
400 int psm, int source_cid)
402 struct l2cap_chan_s *ch = NULL;
403 struct bt_l2cap_psm_s *psm_info;
404 int result, status;
405 int cid = l2cap_cid_new(l2cap);
407 if (cid) {
408 /* See what the channel is to be used for.. */
409 psm_info = l2cap_psm(l2cap->dev, psm);
411 if (psm_info) {
412 /* Device supports this use-case. */
413 ch = g_malloc0(sizeof(*ch));
414 ch->params.sdu_out = l2cap_bframe_out;
415 ch->params.sdu_submit = l2cap_bframe_submit;
416 ch->frame_in = l2cap_bframe_in;
417 ch->mps = 65536;
418 ch->min_mtu = MAX(48, psm_info->min_mtu);
419 ch->params.remote_mtu = MAX(672, ch->min_mtu);
420 ch->remote_cid = source_cid;
421 ch->mode = L2CAP_MODE_BASIC;
422 ch->l2cap = l2cap;
424 /* Does it feel like opening yet another channel though? */
425 if (!psm_info->new_channel(l2cap->dev, &ch->params)) {
426 l2cap->cid[cid] = ch;
428 result = L2CAP_CR_SUCCESS;
429 status = L2CAP_CS_NO_INFO;
430 } else {
431 g_free(ch);
433 result = L2CAP_CR_NO_MEM;
434 status = L2CAP_CS_NO_INFO;
436 } else {
437 result = L2CAP_CR_BAD_PSM;
438 status = L2CAP_CS_NO_INFO;
440 } else {
441 result = L2CAP_CR_NO_MEM;
442 status = L2CAP_CS_NO_INFO;
445 l2cap_connection_response(l2cap, cid, source_cid, result, status);
447 return ch;
450 static void l2cap_channel_close(struct l2cap_instance_s *l2cap,
451 int cid, int source_cid)
453 struct l2cap_chan_s *ch = NULL;
455 /* According to Volume 3, section 6.1.1, pg 1048 of BT Core V2.0, a
456 * connection in CLOSED state still responds with a L2CAP_DisconnectRsp
457 * message on an L2CAP_DisconnectReq event. */
458 if (unlikely(cid < L2CAP_CID_ALLOC)) {
459 l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
460 cid, source_cid);
461 return;
463 if (likely(cid >= L2CAP_CID_ALLOC && cid < L2CAP_CID_MAX))
464 ch = l2cap->cid[cid];
466 if (likely(ch)) {
467 if (ch->remote_cid != source_cid) {
468 fprintf(stderr, "%s: Ignoring a Disconnection Request with the "
469 "invalid SCID %04x.\n", __FUNCTION__, source_cid);
470 return;
473 l2cap->cid[cid] = NULL;
475 ch->params.close(ch->params.opaque);
476 g_free(ch);
479 l2cap_disconnection_response(l2cap, cid, source_cid);
482 static void l2cap_channel_config_null(struct l2cap_instance_s *l2cap,
483 struct l2cap_chan_s *ch)
485 l2cap_configuration_request(l2cap, ch->remote_cid, 0, NULL, 0);
486 ch->config_req_id = l2cap->last_id;
487 ch->config &= ~L2CAP_CFG_INIT;
490 static void l2cap_channel_config_req_event(struct l2cap_instance_s *l2cap,
491 struct l2cap_chan_s *ch)
493 /* Use all default channel options and terminate negotiation. */
494 l2cap_channel_config_null(l2cap, ch);
497 static int l2cap_channel_config(struct l2cap_instance_s *l2cap,
498 struct l2cap_chan_s *ch, int flag,
499 const uint8_t *data, int len)
501 l2cap_conf_opt *opt;
502 l2cap_conf_opt_qos *qos;
503 uint32_t val;
504 uint8_t rsp[len];
505 int result = L2CAP_CONF_SUCCESS;
507 data = memcpy(rsp, data, len);
508 while (len) {
509 opt = (void *) data;
511 if (len < L2CAP_CONF_OPT_SIZE ||
512 len < L2CAP_CONF_OPT_SIZE + opt->len) {
513 result = L2CAP_CONF_REJECT;
514 break;
516 data += L2CAP_CONF_OPT_SIZE + opt->len;
517 len -= L2CAP_CONF_OPT_SIZE + opt->len;
519 switch (opt->type & 0x7f) {
520 case L2CAP_CONF_MTU:
521 if (opt->len != 2) {
522 result = L2CAP_CONF_REJECT;
523 break;
526 /* MTU */
527 val = le16_to_cpup((void *) opt->val);
528 if (val < ch->min_mtu) {
529 cpu_to_le16w((void *) opt->val, ch->min_mtu);
530 result = L2CAP_CONF_UNACCEPT;
531 break;
534 ch->params.remote_mtu = val;
535 break;
537 case L2CAP_CONF_FLUSH_TO:
538 if (opt->len != 2) {
539 result = L2CAP_CONF_REJECT;
540 break;
543 /* Flush Timeout */
544 val = le16_to_cpup((void *) opt->val);
545 if (val < 0x0001) {
546 opt->val[0] = 0xff;
547 opt->val[1] = 0xff;
548 result = L2CAP_CONF_UNACCEPT;
549 break;
551 break;
553 case L2CAP_CONF_QOS:
554 if (opt->len != L2CAP_CONF_OPT_QOS_SIZE) {
555 result = L2CAP_CONF_REJECT;
556 break;
558 qos = (void *) opt->val;
560 /* Flags */
561 val = qos->flags;
562 if (val) {
563 qos->flags = 0;
564 result = L2CAP_CONF_UNACCEPT;
567 /* Service type */
568 val = qos->service_type;
569 if (val != L2CAP_CONF_QOS_BEST_EFFORT &&
570 val != L2CAP_CONF_QOS_NO_TRAFFIC) {
571 qos->service_type = L2CAP_CONF_QOS_BEST_EFFORT;
572 result = L2CAP_CONF_UNACCEPT;
575 if (val != L2CAP_CONF_QOS_NO_TRAFFIC) {
576 /* XXX: These values should possibly be calculated
577 * based on LM / baseband properties also. */
579 /* Token rate */
580 val = le32_to_cpu(qos->token_rate);
581 if (val == L2CAP_CONF_QOS_WILDCARD)
582 qos->token_rate = cpu_to_le32(0x100000);
584 /* Token bucket size */
585 val = le32_to_cpu(qos->token_bucket_size);
586 if (val == L2CAP_CONF_QOS_WILDCARD)
587 qos->token_bucket_size = cpu_to_le32(65500);
589 /* Any Peak bandwidth value is correct to return as-is */
590 /* Any Access latency value is correct to return as-is */
591 /* Any Delay variation value is correct to return as-is */
593 break;
595 case L2CAP_CONF_RFC:
596 if (opt->len != 9) {
597 result = L2CAP_CONF_REJECT;
598 break;
601 /* Mode */
602 val = opt->val[0];
603 switch (val) {
604 case L2CAP_MODE_BASIC:
605 ch->mode = val;
606 ch->frame_in = l2cap_bframe_in;
608 /* All other parameters shall be ignored */
609 break;
611 case L2CAP_MODE_RETRANS:
612 case L2CAP_MODE_FLOWCTL:
613 ch->mode = val;
614 ch->frame_in = l2cap_iframe_in;
615 /* Note: most of these parameters refer to incoming traffic
616 * so we don't need to save them as long as we can accept
617 * incoming PDUs at any values of the parameters. */
619 /* TxWindow size */
620 val = opt->val[1];
621 if (val < 1 || val > 32) {
622 opt->val[1] = 32;
623 result = L2CAP_CONF_UNACCEPT;
624 break;
627 /* MaxTransmit */
628 val = opt->val[2];
629 if (val < 1) {
630 opt->val[2] = 1;
631 result = L2CAP_CONF_UNACCEPT;
632 break;
635 /* Remote Retransmission time-out shouldn't affect local
636 * operation (?) */
638 /* The Monitor time-out drives the local Monitor timer (?),
639 * so save the value. */
640 val = (opt->val[6] << 8) | opt->val[5];
641 if (val < 30) {
642 opt->val[5] = 100 & 0xff;
643 opt->val[6] = 100 >> 8;
644 result = L2CAP_CONF_UNACCEPT;
645 break;
647 ch->monitor_timeout = val;
648 l2cap_monitor_timer_update(ch);
650 /* MPS */
651 val = (opt->val[8] << 8) | opt->val[7];
652 if (val < ch->min_mtu) {
653 opt->val[7] = ch->min_mtu & 0xff;
654 opt->val[8] = ch->min_mtu >> 8;
655 result = L2CAP_CONF_UNACCEPT;
656 break;
658 ch->mps = val;
659 break;
661 default:
662 result = L2CAP_CONF_UNACCEPT;
663 break;
665 break;
667 default:
668 if (!(opt->type >> 7))
669 result = L2CAP_CONF_UNKNOWN;
670 break;
673 if (result != L2CAP_CONF_SUCCESS)
674 break; /* XXX: should continue? */
677 l2cap_configuration_response(l2cap, ch->remote_cid,
678 flag, result, rsp, len);
680 return result == L2CAP_CONF_SUCCESS && !flag;
683 static void l2cap_channel_config_req_msg(struct l2cap_instance_s *l2cap,
684 int flag, int cid, const uint8_t *data, int len)
686 struct l2cap_chan_s *ch;
688 if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
689 l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
690 cid, 0x0000);
691 return;
693 ch = l2cap->cid[cid];
695 /* From OPEN go to WAIT_CONFIG_REQ and from WAIT_CONFIG_REQ_RSP to
696 * WAIT_CONFIG_REQ_RSP. This is assuming the transition chart for OPEN
697 * on pg 1053, section 6.1.5, volume 3 of BT Core V2.0 has a mistake
698 * and on options-acceptable we go back to OPEN and otherwise to
699 * WAIT_CONFIG_REQ and not the other way. */
700 ch->config &= ~L2CAP_CFG_ACC;
702 if (l2cap_channel_config(l2cap, ch, flag, data, len))
703 /* Go to OPEN or WAIT_CONFIG_RSP */
704 ch->config |= L2CAP_CFG_ACC;
706 /* TODO: if the incoming traffic flow control or retransmission mode
707 * changed then we probably need to also generate the
708 * ConfigureChannel_Req event and set the outgoing traffic to the same
709 * mode. */
710 if (!(ch->config & L2CAP_CFG_INIT) && (ch->config & L2CAP_CFG_ACC) &&
711 !ch->config_req_id)
712 l2cap_channel_config_req_event(l2cap, ch);
715 static int l2cap_channel_config_rsp_msg(struct l2cap_instance_s *l2cap,
716 int result, int flag, int cid, const uint8_t *data, int len)
718 struct l2cap_chan_s *ch;
720 if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
721 l2cap_command_reject_cid(l2cap, l2cap->last_id, L2CAP_REJ_CID_INVAL,
722 cid, 0x0000);
723 return 0;
725 ch = l2cap->cid[cid];
727 if (ch->config_req_id != l2cap->last_id)
728 return 1;
729 ch->config_req_id = 0;
731 if (result == L2CAP_CONF_SUCCESS) {
732 if (!flag)
733 ch->config |= L2CAP_CFG_INIT;
734 else
735 l2cap_channel_config_null(l2cap, ch);
736 } else
737 /* Retry until we succeed */
738 l2cap_channel_config_req_event(l2cap, ch);
740 return 0;
743 static void l2cap_channel_open_req_msg(struct l2cap_instance_s *l2cap,
744 int psm, int source_cid)
746 struct l2cap_chan_s *ch = l2cap_channel_open(l2cap, psm, source_cid);
748 if (!ch)
749 return;
751 /* Optional */
752 if (!(ch->config & L2CAP_CFG_INIT) && !ch->config_req_id)
753 l2cap_channel_config_req_event(l2cap, ch);
756 static void l2cap_info(struct l2cap_instance_s *l2cap, int type)
758 uint8_t data[4];
759 int len = 0;
760 int result = L2CAP_IR_SUCCESS;
762 switch (type) {
763 case L2CAP_IT_CL_MTU:
764 data[len ++] = l2cap->group_ch.mps & 0xff;
765 data[len ++] = l2cap->group_ch.mps >> 8;
766 break;
768 case L2CAP_IT_FEAT_MASK:
769 /* (Prematurely) report Flow control and Retransmission modes. */
770 data[len ++] = 0x03;
771 data[len ++] = 0x00;
772 data[len ++] = 0x00;
773 data[len ++] = 0x00;
774 break;
776 default:
777 result = L2CAP_IR_NOTSUPP;
780 l2cap_info_response(l2cap, type, result, data, len);
783 static void l2cap_command(struct l2cap_instance_s *l2cap, int code, int id,
784 const uint8_t *params, int len)
786 int err;
788 #if 0
789 /* TODO: do the IDs really have to be in sequence? */
790 if (!id || (id != l2cap->last_id && id != l2cap->next_id)) {
791 fprintf(stderr, "%s: out of sequence command packet ignored.\n",
792 __FUNCTION__);
793 return;
795 #else
796 l2cap->next_id = id;
797 #endif
798 if (id == l2cap->next_id) {
799 l2cap->last_id = l2cap->next_id;
800 l2cap->next_id = l2cap->next_id == 255 ? 1 : l2cap->next_id + 1;
801 } else {
802 /* TODO: Need to re-send the same response, without re-executing
803 * the corresponding command! */
806 switch (code) {
807 case L2CAP_COMMAND_REJ:
808 if (unlikely(len != 2 && len != 4 && len != 6)) {
809 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
810 goto reject;
813 /* We never issue commands other than Command Reject currently. */
814 fprintf(stderr, "%s: stray Command Reject (%02x, %04x) "
815 "packet, ignoring.\n", __FUNCTION__, id,
816 le16_to_cpu(((l2cap_cmd_rej *) params)->reason));
817 break;
819 case L2CAP_CONN_REQ:
820 if (unlikely(len != L2CAP_CONN_REQ_SIZE)) {
821 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
822 goto reject;
825 l2cap_channel_open_req_msg(l2cap,
826 le16_to_cpu(((l2cap_conn_req *) params)->psm),
827 le16_to_cpu(((l2cap_conn_req *) params)->scid));
828 break;
830 case L2CAP_CONN_RSP:
831 if (unlikely(len != L2CAP_CONN_RSP_SIZE)) {
832 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
833 goto reject;
836 /* We never issue Connection Requests currently. TODO */
837 fprintf(stderr, "%s: unexpected Connection Response (%02x) "
838 "packet, ignoring.\n", __FUNCTION__, id);
839 break;
841 case L2CAP_CONF_REQ:
842 if (unlikely(len < L2CAP_CONF_REQ_SIZE(0))) {
843 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
844 goto reject;
847 l2cap_channel_config_req_msg(l2cap,
848 le16_to_cpu(((l2cap_conf_req *) params)->flags) & 1,
849 le16_to_cpu(((l2cap_conf_req *) params)->dcid),
850 ((l2cap_conf_req *) params)->data,
851 len - L2CAP_CONF_REQ_SIZE(0));
852 break;
854 case L2CAP_CONF_RSP:
855 if (unlikely(len < L2CAP_CONF_RSP_SIZE(0))) {
856 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
857 goto reject;
860 if (l2cap_channel_config_rsp_msg(l2cap,
861 le16_to_cpu(((l2cap_conf_rsp *) params)->result),
862 le16_to_cpu(((l2cap_conf_rsp *) params)->flags) & 1,
863 le16_to_cpu(((l2cap_conf_rsp *) params)->scid),
864 ((l2cap_conf_rsp *) params)->data,
865 len - L2CAP_CONF_RSP_SIZE(0)))
866 fprintf(stderr, "%s: unexpected Configure Response (%02x) "
867 "packet, ignoring.\n", __FUNCTION__, id);
868 break;
870 case L2CAP_DISCONN_REQ:
871 if (unlikely(len != L2CAP_DISCONN_REQ_SIZE)) {
872 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
873 goto reject;
876 l2cap_channel_close(l2cap,
877 le16_to_cpu(((l2cap_disconn_req *) params)->dcid),
878 le16_to_cpu(((l2cap_disconn_req *) params)->scid));
879 break;
881 case L2CAP_DISCONN_RSP:
882 if (unlikely(len != L2CAP_DISCONN_RSP_SIZE)) {
883 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
884 goto reject;
887 /* We never issue Disconnection Requests currently. TODO */
888 fprintf(stderr, "%s: unexpected Disconnection Response (%02x) "
889 "packet, ignoring.\n", __FUNCTION__, id);
890 break;
892 case L2CAP_ECHO_REQ:
893 l2cap_echo_response(l2cap, params, len);
894 break;
896 case L2CAP_ECHO_RSP:
897 /* We never issue Echo Requests currently. TODO */
898 fprintf(stderr, "%s: unexpected Echo Response (%02x) "
899 "packet, ignoring.\n", __FUNCTION__, id);
900 break;
902 case L2CAP_INFO_REQ:
903 if (unlikely(len != L2CAP_INFO_REQ_SIZE)) {
904 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
905 goto reject;
908 l2cap_info(l2cap, le16_to_cpu(((l2cap_info_req *) params)->type));
909 break;
911 case L2CAP_INFO_RSP:
912 if (unlikely(len != L2CAP_INFO_RSP_SIZE)) {
913 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
914 goto reject;
917 /* We never issue Information Requests currently. TODO */
918 fprintf(stderr, "%s: unexpected Information Response (%02x) "
919 "packet, ignoring.\n", __FUNCTION__, id);
920 break;
922 default:
923 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
924 reject:
925 l2cap_command_reject(l2cap, id, err, 0, 0);
926 break;
930 static void l2cap_rexmit_enable(struct l2cap_chan_s *ch, int enable)
932 ch->rexmit = enable;
934 l2cap_retransmission_timer_update(ch);
935 l2cap_monitor_timer_update(ch);
938 /* Command frame SDU */
939 static void l2cap_cframe_in(void *opaque, const uint8_t *data, int len)
941 struct l2cap_instance_s *l2cap = opaque;
942 const l2cap_cmd_hdr *hdr;
943 int clen;
945 while (len) {
946 hdr = (void *) data;
947 if (len < L2CAP_CMD_HDR_SIZE)
948 /* TODO: signal an error */
949 return;
950 len -= L2CAP_CMD_HDR_SIZE;
951 data += L2CAP_CMD_HDR_SIZE;
953 clen = le16_to_cpu(hdr->len);
954 if (len < clen) {
955 l2cap_command_reject(l2cap, hdr->ident,
956 L2CAP_REJ_CMD_NOT_UNDERSTOOD, 0, 0);
957 break;
960 l2cap_command(l2cap, hdr->code, hdr->ident, data, clen);
961 len -= clen;
962 data += clen;
966 /* Group frame SDU */
967 static void l2cap_gframe_in(void *opaque, const uint8_t *data, int len)
971 /* Supervisory frame */
972 static void l2cap_sframe_in(struct l2cap_chan_s *ch, uint16_t ctrl)
976 /* Basic L2CAP mode Information frame */
977 static void l2cap_bframe_in(struct l2cap_chan_s *ch, uint16_t cid,
978 const l2cap_hdr *hdr, int len)
980 /* We have a full SDU, no further processing */
981 ch->params.sdu_in(ch->params.opaque, hdr->data, len);
984 /* Flow Control and Retransmission mode frame */
985 static void l2cap_iframe_in(struct l2cap_chan_s *ch, uint16_t cid,
986 const l2cap_hdr *hdr, int len)
988 uint16_t fcs = le16_to_cpup((void *) (hdr->data + len - 2));
990 if (len < 4)
991 goto len_error;
992 if (l2cap_fcs16((const uint8_t *) hdr, L2CAP_HDR_SIZE + len - 2) != fcs)
993 goto fcs_error;
995 if ((hdr->data[0] >> 7) == ch->rexmit)
996 l2cap_rexmit_enable(ch, !(hdr->data[0] >> 7));
998 if (hdr->data[0] & 1) {
999 if (len != 4) {
1000 /* TODO: Signal an error? */
1001 return;
1003 l2cap_sframe_in(ch, le16_to_cpup((void *) hdr->data));
1004 return;
1007 switch (hdr->data[1] >> 6) { /* SAR */
1008 case L2CAP_SAR_NO_SEG:
1009 if (ch->len_total)
1010 goto seg_error;
1011 if (len - 4 > ch->mps)
1012 goto len_error;
1014 ch->params.sdu_in(ch->params.opaque, hdr->data + 2, len - 4);
1015 break;
1017 case L2CAP_SAR_START:
1018 if (ch->len_total || len < 6)
1019 goto seg_error;
1020 if (len - 6 > ch->mps)
1021 goto len_error;
1023 ch->len_total = le16_to_cpup((void *) (hdr->data + 2));
1024 if (len >= 6 + ch->len_total)
1025 goto seg_error;
1027 ch->len_cur = len - 6;
1028 memcpy(ch->sdu, hdr->data + 4, ch->len_cur);
1029 break;
1031 case L2CAP_SAR_END:
1032 if (!ch->len_total || ch->len_cur + len - 4 < ch->len_total)
1033 goto seg_error;
1034 if (len - 4 > ch->mps)
1035 goto len_error;
1037 memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
1038 ch->params.sdu_in(ch->params.opaque, ch->sdu, ch->len_total);
1039 break;
1041 case L2CAP_SAR_CONT:
1042 if (!ch->len_total || ch->len_cur + len - 4 >= ch->len_total)
1043 goto seg_error;
1044 if (len - 4 > ch->mps)
1045 goto len_error;
1047 memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
1048 ch->len_cur += len - 4;
1049 break;
1051 seg_error:
1052 len_error: /* TODO */
1053 fcs_error: /* TODO */
1054 ch->len_cur = 0;
1055 ch->len_total = 0;
1056 break;
1060 static void l2cap_frame_in(struct l2cap_instance_s *l2cap,
1061 const l2cap_hdr *frame)
1063 uint16_t cid = le16_to_cpu(frame->cid);
1064 uint16_t len = le16_to_cpu(frame->len);
1066 if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
1067 fprintf(stderr, "%s: frame addressed to a non-existent L2CAP "
1068 "channel %04x received.\n", __FUNCTION__, cid);
1069 return;
1072 l2cap->cid[cid]->frame_in(l2cap->cid[cid], cid, frame, len);
1075 /* "Recombination" */
1076 static void l2cap_pdu_in(struct l2cap_instance_s *l2cap,
1077 const uint8_t *data, int len)
1079 const l2cap_hdr *hdr = (void *) l2cap->frame_in;
1081 if (unlikely(len + l2cap->frame_in_len > sizeof(l2cap->frame_in))) {
1082 if (l2cap->frame_in_len < sizeof(l2cap->frame_in)) {
1083 memcpy(l2cap->frame_in + l2cap->frame_in_len, data,
1084 sizeof(l2cap->frame_in) - l2cap->frame_in_len);
1085 l2cap->frame_in_len = sizeof(l2cap->frame_in);
1086 /* TODO: truncate */
1087 l2cap_frame_in(l2cap, hdr);
1090 return;
1093 memcpy(l2cap->frame_in + l2cap->frame_in_len, data, len);
1094 l2cap->frame_in_len += len;
1096 if (len >= L2CAP_HDR_SIZE)
1097 if (len >= L2CAP_HDR_SIZE + le16_to_cpu(hdr->len))
1098 l2cap_frame_in(l2cap, hdr);
1099 /* There is never a start of a new PDU in the same ACL packet, so
1100 * no need to memmove the remaining payload and loop. */
1103 static inline uint8_t *l2cap_pdu_out(struct l2cap_instance_s *l2cap,
1104 uint16_t cid, uint16_t len)
1106 l2cap_hdr *hdr = (void *) l2cap->frame_out;
1108 l2cap->frame_out_len = len + L2CAP_HDR_SIZE;
1110 hdr->cid = cpu_to_le16(cid);
1111 hdr->len = cpu_to_le16(len);
1113 return l2cap->frame_out + L2CAP_HDR_SIZE;
1116 static inline void l2cap_pdu_submit(struct l2cap_instance_s *l2cap)
1118 /* TODO: Fragmentation */
1119 (l2cap->role ?
1120 l2cap->link->slave->lmp_acl_data : l2cap->link->host->lmp_acl_resp)
1121 (l2cap->link, l2cap->frame_out, 1, l2cap->frame_out_len);
1124 static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len)
1126 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
1128 if (len > chan->params.remote_mtu) {
1129 fprintf(stderr, "%s: B-Frame for CID %04x longer than %i octets.\n",
1130 __FUNCTION__,
1131 chan->remote_cid, chan->params.remote_mtu);
1132 exit(-1);
1135 return l2cap_pdu_out(chan->l2cap, chan->remote_cid, len);
1138 static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms)
1140 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parms;
1142 l2cap_pdu_submit(chan->l2cap);
1145 #if 0
1146 /* Stub: Only used if an emulated device requests outgoing flow control */
1147 static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len)
1149 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
1151 if (len > chan->params.remote_mtu) {
1152 /* TODO: slice into segments and queue each segment as a separate
1153 * I-Frame in a FIFO of I-Frames, local to the CID. */
1154 } else {
1155 /* TODO: add to the FIFO of I-Frames, local to the CID. */
1156 /* Possibly we need to return a pointer to a contiguous buffer
1157 * for now and then memcpy from it into FIFOs in l2cap_iframe_submit
1158 * while segmenting at the same time. */
1160 return 0;
1163 static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm)
1165 /* TODO: If flow control indicates clear to send, start submitting the
1166 * invidual I-Frames from the FIFO, but don't remove them from there.
1167 * Kick the appropriate timer until we get an S-Frame, and only then
1168 * remove from FIFO or resubmit and re-kick the timer if the timer
1169 * expired. */
1171 #endif
1173 static void l2cap_init(struct l2cap_instance_s *l2cap,
1174 struct bt_link_s *link, int role)
1176 l2cap->link = link;
1177 l2cap->role = role;
1178 l2cap->dev = (struct bt_l2cap_device_s *)
1179 (role ? link->host : link->slave);
1181 l2cap->next_id = 1;
1183 /* Establish the signalling channel */
1184 l2cap->signalling_ch.params.sdu_in = l2cap_cframe_in;
1185 l2cap->signalling_ch.params.sdu_out = l2cap_bframe_out;
1186 l2cap->signalling_ch.params.sdu_submit = l2cap_bframe_submit;
1187 l2cap->signalling_ch.params.opaque = l2cap;
1188 l2cap->signalling_ch.params.remote_mtu = 48;
1189 l2cap->signalling_ch.remote_cid = L2CAP_CID_SIGNALLING;
1190 l2cap->signalling_ch.frame_in = l2cap_bframe_in;
1191 l2cap->signalling_ch.mps = 65536;
1192 l2cap->signalling_ch.min_mtu = 48;
1193 l2cap->signalling_ch.mode = L2CAP_MODE_BASIC;
1194 l2cap->signalling_ch.l2cap = l2cap;
1195 l2cap->cid[L2CAP_CID_SIGNALLING] = &l2cap->signalling_ch;
1197 /* Establish the connection-less data channel */
1198 l2cap->group_ch.params.sdu_in = l2cap_gframe_in;
1199 l2cap->group_ch.params.opaque = l2cap;
1200 l2cap->group_ch.frame_in = l2cap_bframe_in;
1201 l2cap->group_ch.mps = 65533;
1202 l2cap->group_ch.l2cap = l2cap;
1203 l2cap->group_ch.remote_cid = L2CAP_CID_INVALID;
1204 l2cap->cid[L2CAP_CID_GROUP] = &l2cap->group_ch;
1207 static void l2cap_teardown(struct l2cap_instance_s *l2cap, int send_disconnect)
1209 int cid;
1211 /* Don't send DISCONNECT if we are currently handling a DISCONNECT
1212 * sent from the other side. */
1213 if (send_disconnect) {
1214 if (l2cap->role)
1215 l2cap->dev->device.lmp_disconnect_slave(l2cap->link);
1216 /* l2cap->link is invalid from now on. */
1217 else
1218 l2cap->dev->device.lmp_disconnect_master(l2cap->link);
1221 for (cid = L2CAP_CID_ALLOC; cid < L2CAP_CID_MAX; cid ++)
1222 if (l2cap->cid[cid]) {
1223 l2cap->cid[cid]->params.close(l2cap->cid[cid]->params.opaque);
1224 g_free(l2cap->cid[cid]);
1227 if (l2cap->role)
1228 g_free(l2cap);
1229 else
1230 g_free(l2cap->link);
1233 /* L2CAP glue to lower layers in bluetooth stack (LMP) */
1235 static void l2cap_lmp_connection_request(struct bt_link_s *link)
1237 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->slave;
1238 struct slave_l2cap_instance_s *l2cap;
1240 /* Always accept - we only get called if (dev->device->page_scan). */
1242 l2cap = g_malloc0(sizeof(struct slave_l2cap_instance_s));
1243 l2cap->link.slave = &dev->device;
1244 l2cap->link.host = link->host;
1245 l2cap_init(&l2cap->l2cap, &l2cap->link, 0);
1247 /* Always at the end */
1248 link->host->reject_reason = 0;
1249 link->host->lmp_connection_complete(&l2cap->link);
1252 /* Stub */
1253 static void l2cap_lmp_connection_complete(struct bt_link_s *link)
1255 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1256 struct l2cap_instance_s *l2cap;
1258 if (dev->device.reject_reason) {
1259 /* Signal to upper layer */
1260 return;
1263 l2cap = g_malloc0(sizeof(struct l2cap_instance_s));
1264 l2cap_init(l2cap, link, 1);
1266 link->acl_mode = acl_active;
1268 /* Signal to upper layer */
1271 /* Stub */
1272 static void l2cap_lmp_disconnect_host(struct bt_link_s *link)
1274 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1275 struct l2cap_instance_s *l2cap =
1276 /* TODO: Retrieve from upper layer */ (void *) dev;
1278 /* Signal to upper layer */
1280 l2cap_teardown(l2cap, 0);
1283 static void l2cap_lmp_disconnect_slave(struct bt_link_s *link)
1285 struct slave_l2cap_instance_s *l2cap =
1286 (struct slave_l2cap_instance_s *) link;
1288 l2cap_teardown(&l2cap->l2cap, 0);
1291 static void l2cap_lmp_acl_data_slave(struct bt_link_s *link,
1292 const uint8_t *data, int start, int len)
1294 struct slave_l2cap_instance_s *l2cap =
1295 (struct slave_l2cap_instance_s *) link;
1297 if (start)
1298 l2cap->l2cap.frame_in_len = 0;
1300 l2cap_pdu_in(&l2cap->l2cap, data, len);
1303 /* Stub */
1304 static void l2cap_lmp_acl_data_host(struct bt_link_s *link,
1305 const uint8_t *data, int start, int len)
1307 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1308 struct l2cap_instance_s *l2cap =
1309 /* TODO: Retrieve from upper layer */ (void *) dev;
1311 if (start)
1312 l2cap->frame_in_len = 0;
1314 l2cap_pdu_in(l2cap, data, len);
1317 static void l2cap_dummy_destroy(struct bt_device_s *dev)
1319 struct bt_l2cap_device_s *l2cap_dev = (struct bt_l2cap_device_s *) dev;
1321 bt_l2cap_device_done(l2cap_dev);
1324 void bt_l2cap_device_init(struct bt_l2cap_device_s *dev,
1325 struct bt_scatternet_s *net)
1327 bt_device_init(&dev->device, net);
1329 dev->device.lmp_connection_request = l2cap_lmp_connection_request;
1330 dev->device.lmp_connection_complete = l2cap_lmp_connection_complete;
1331 dev->device.lmp_disconnect_master = l2cap_lmp_disconnect_host;
1332 dev->device.lmp_disconnect_slave = l2cap_lmp_disconnect_slave;
1333 dev->device.lmp_acl_data = l2cap_lmp_acl_data_slave;
1334 dev->device.lmp_acl_resp = l2cap_lmp_acl_data_host;
1336 dev->device.handle_destroy = l2cap_dummy_destroy;
1339 void bt_l2cap_device_done(struct bt_l2cap_device_s *dev)
1341 bt_device_done(&dev->device);
1343 /* Should keep a list of all instances and go through it and
1344 * invoke l2cap_teardown() for each. */
1347 void bt_l2cap_psm_register(struct bt_l2cap_device_s *dev, int psm, int min_mtu,
1348 int (*new_channel)(struct bt_l2cap_device_s *dev,
1349 struct bt_l2cap_conn_params_s *params))
1351 struct bt_l2cap_psm_s *new_psm = l2cap_psm(dev, psm);
1353 if (new_psm) {
1354 fprintf(stderr, "%s: PSM %04x already registered for device `%s'.\n",
1355 __FUNCTION__, psm, dev->device.lmp_name);
1356 exit(-1);
1359 new_psm = g_malloc0(sizeof(*new_psm));
1360 new_psm->psm = psm;
1361 new_psm->min_mtu = min_mtu;
1362 new_psm->new_channel = new_channel;
1363 new_psm->next = dev->first_psm;
1364 dev->first_psm = new_psm;