PPC: e500mc: add missing IVORs to bitmap
[qemu.git] / hw / bt-l2cap.c
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-common.h"
21 #include "qemu-timer.h"
22 #include "bt.h"
23
24 #define L2CAP_CID_MAX 0x100 /* Between 0x40 and 0x10000 */
25
26 struct l2cap_instance_s {
27 struct bt_link_s *link;
28 struct bt_l2cap_device_s *dev;
29 int role;
30
31 uint8_t frame_in[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4)));
32 int frame_in_len;
33
34 uint8_t frame_out[65535 + L2CAP_HDR_SIZE] __attribute__ ((aligned (4)));
35 int frame_out_len;
36
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;
41
42 int last_id;
43 int next_id;
44
45 struct l2cap_chan_s {
46 struct bt_l2cap_conn_params_s params;
47
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;
52
53 struct l2cap_instance_s *l2cap;
54
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;
61
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;
65
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
86 */
87
88 struct l2cap_chan_s signalling_ch;
89 struct l2cap_chan_s group_ch;
90 };
91
92 struct slave_l2cap_instance_s {
93 struct bt_link_s link; /* Underlying logical link (ACL) */
94 struct l2cap_instance_s l2cap;
95 };
96
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;
103 };
104
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,
138 };
139
140 static uint16_t l2cap_fcs16(const uint8_t *message, int len)
141 {
142 uint16_t fcs = 0x0000;
143
144 while (len --)
145 #if 0
146 {
147 int i;
148
149 fcs ^= *message ++;
150 for (i = 8; i; -- i)
151 if (fcs & 1)
152 fcs = (fcs >> 1) ^ 0xa001;
153 else
154 fcs = (fcs >> 1);
155 }
156 #else
157 fcs = (fcs >> 8) ^ l2cap_fcs16_table[(fcs ^ *message ++) & 0xff];
158 #endif
159
160 return fcs;
161 }
162
163 /* L2CAP layer logic (protocol) */
164
165 static void l2cap_retransmission_timer_update(struct l2cap_chan_s *ch)
166 {
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
173 }
174
175 static void l2cap_monitor_timer_update(struct l2cap_chan_s *ch)
176 {
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
183 }
184
185 static void l2cap_command_reject(struct l2cap_instance_s *l2cap, int id,
186 uint16_t reason, const void *data, int plen)
187 {
188 uint8_t *pkt;
189 l2cap_cmd_hdr *hdr;
190 l2cap_cmd_rej *params;
191 uint16_t len;
192
193 reason = cpu_to_le16(reason);
194 len = cpu_to_le16(L2CAP_CMD_REJ_SIZE + plen);
195
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);
200
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);
207
208 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
209 }
210
211 static void l2cap_command_reject_cid(struct l2cap_instance_s *l2cap, int id,
212 uint16_t reason, uint16_t dcid, uint16_t scid)
213 {
214 l2cap_cmd_rej_cid params = {
215 .dcid = dcid,
216 .scid = scid,
217 };
218
219 l2cap_command_reject(l2cap, id, reason, &params, L2CAP_CMD_REJ_CID_SIZE);
220 }
221
222 static void l2cap_connection_response(struct l2cap_instance_s *l2cap,
223 int dcid, int scid, int result, int status)
224 {
225 uint8_t *pkt;
226 l2cap_cmd_hdr *hdr;
227 l2cap_conn_rsp *params;
228
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);
233
234 hdr->code = L2CAP_CONN_RSP;
235 hdr->ident = l2cap->last_id;
236 hdr->len = cpu_to_le16(L2CAP_CONN_RSP_SIZE);
237
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);
242
243 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
244 }
245
246 static void l2cap_configuration_request(struct l2cap_instance_s *l2cap,
247 int dcid, int flag, const uint8_t *data, int len)
248 {
249 uint8_t *pkt;
250 l2cap_cmd_hdr *hdr;
251 l2cap_conf_req *params;
252
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);
257
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;
261
262 hdr->code = L2CAP_CONF_REQ;
263 hdr->ident = l2cap->last_id;
264 hdr->len = cpu_to_le16(L2CAP_CONF_REQ_SIZE(len));
265
266 params->dcid = cpu_to_le16(dcid);
267 params->flags = cpu_to_le16(flag);
268 if (len)
269 memcpy(params->data, data, len);
270
271 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
272 }
273
274 static void l2cap_configuration_response(struct l2cap_instance_s *l2cap,
275 int scid, int flag, int result, const uint8_t *data, int len)
276 {
277 uint8_t *pkt;
278 l2cap_cmd_hdr *hdr;
279 l2cap_conf_rsp *params;
280
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);
285
286 hdr->code = L2CAP_CONF_RSP;
287 hdr->ident = l2cap->last_id;
288 hdr->len = cpu_to_le16(L2CAP_CONF_RSP_SIZE(len));
289
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);
295
296 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
297 }
298
299 static void l2cap_disconnection_response(struct l2cap_instance_s *l2cap,
300 int dcid, int scid)
301 {
302 uint8_t *pkt;
303 l2cap_cmd_hdr *hdr;
304 l2cap_disconn_rsp *params;
305
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);
310
311 hdr->code = L2CAP_DISCONN_RSP;
312 hdr->ident = l2cap->last_id;
313 hdr->len = cpu_to_le16(L2CAP_DISCONN_RSP_SIZE);
314
315 params->dcid = cpu_to_le16(dcid);
316 params->scid = cpu_to_le16(scid);
317
318 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
319 }
320
321 static void l2cap_echo_response(struct l2cap_instance_s *l2cap,
322 const uint8_t *data, int len)
323 {
324 uint8_t *pkt;
325 l2cap_cmd_hdr *hdr;
326 uint8_t *params;
327
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);
332
333 hdr->code = L2CAP_ECHO_RSP;
334 hdr->ident = l2cap->last_id;
335 hdr->len = cpu_to_le16(len);
336
337 memcpy(params, data, len);
338
339 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
340 }
341
342 static void l2cap_info_response(struct l2cap_instance_s *l2cap, int type,
343 int result, const uint8_t *data, int len)
344 {
345 uint8_t *pkt;
346 l2cap_cmd_hdr *hdr;
347 l2cap_info_rsp *params;
348
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);
353
354 hdr->code = L2CAP_INFO_RSP;
355 hdr->ident = l2cap->last_id;
356 hdr->len = cpu_to_le16(L2CAP_INFO_RSP_SIZE + len);
357
358 params->type = cpu_to_le16(type);
359 params->result = cpu_to_le16(result);
360 if (len)
361 memcpy(params->data, data, len);
362
363 l2cap->signalling_ch.params.sdu_submit(&l2cap->signalling_ch.params);
364 }
365
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);
376
377 static int l2cap_cid_new(struct l2cap_instance_s *l2cap)
378 {
379 int i;
380
381 for (i = L2CAP_CID_ALLOC; i < L2CAP_CID_MAX; i ++)
382 if (!l2cap->cid[i])
383 return i;
384
385 return L2CAP_CID_INVALID;
386 }
387
388 static inline struct bt_l2cap_psm_s *l2cap_psm(
389 struct bt_l2cap_device_s *device, int psm)
390 {
391 struct bt_l2cap_psm_s *ret = device->first_psm;
392
393 while (ret && ret->psm != psm)
394 ret = ret->next;
395
396 return ret;
397 }
398
399 static struct l2cap_chan_s *l2cap_channel_open(struct l2cap_instance_s *l2cap,
400 int psm, int source_cid)
401 {
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);
406
407 if (cid) {
408 /* See what the channel is to be used for.. */
409 psm_info = l2cap_psm(l2cap->dev, psm);
410
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;
423
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;
427
428 result = L2CAP_CR_SUCCESS;
429 status = L2CAP_CS_NO_INFO;
430 } else {
431 g_free(ch);
432
433 result = L2CAP_CR_NO_MEM;
434 status = L2CAP_CS_NO_INFO;
435 }
436 } else {
437 result = L2CAP_CR_BAD_PSM;
438 status = L2CAP_CS_NO_INFO;
439 }
440 } else {
441 result = L2CAP_CR_NO_MEM;
442 status = L2CAP_CS_NO_INFO;
443 }
444
445 l2cap_connection_response(l2cap, cid, source_cid, result, status);
446
447 return ch;
448 }
449
450 static void l2cap_channel_close(struct l2cap_instance_s *l2cap,
451 int cid, int source_cid)
452 {
453 struct l2cap_chan_s *ch = NULL;
454
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;
462 }
463 if (likely(cid >= L2CAP_CID_ALLOC && cid < L2CAP_CID_MAX))
464 ch = l2cap->cid[cid];
465
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;
471 }
472
473 l2cap->cid[cid] = NULL;
474
475 ch->params.close(ch->params.opaque);
476 g_free(ch);
477 }
478
479 l2cap_disconnection_response(l2cap, cid, source_cid);
480 }
481
482 static void l2cap_channel_config_null(struct l2cap_instance_s *l2cap,
483 struct l2cap_chan_s *ch)
484 {
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;
488 }
489
490 static void l2cap_channel_config_req_event(struct l2cap_instance_s *l2cap,
491 struct l2cap_chan_s *ch)
492 {
493 /* Use all default channel options and terminate negotiation. */
494 l2cap_channel_config_null(l2cap, ch);
495 }
496
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)
500 {
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;
506
507 data = memcpy(rsp, data, len);
508 while (len) {
509 opt = (void *) data;
510
511 if (len < L2CAP_CONF_OPT_SIZE ||
512 len < L2CAP_CONF_OPT_SIZE + opt->len) {
513 result = L2CAP_CONF_REJECT;
514 break;
515 }
516 data += L2CAP_CONF_OPT_SIZE + opt->len;
517 len -= L2CAP_CONF_OPT_SIZE + opt->len;
518
519 switch (opt->type & 0x7f) {
520 case L2CAP_CONF_MTU:
521 if (opt->len != 2) {
522 result = L2CAP_CONF_REJECT;
523 break;
524 }
525
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;
532 }
533
534 ch->params.remote_mtu = val;
535 break;
536
537 case L2CAP_CONF_FLUSH_TO:
538 if (opt->len != 2) {
539 result = L2CAP_CONF_REJECT;
540 break;
541 }
542
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;
550 }
551 break;
552
553 case L2CAP_CONF_QOS:
554 if (opt->len != L2CAP_CONF_OPT_QOS_SIZE) {
555 result = L2CAP_CONF_REJECT;
556 break;
557 }
558 qos = (void *) opt->val;
559
560 /* Flags */
561 val = qos->flags;
562 if (val) {
563 qos->flags = 0;
564 result = L2CAP_CONF_UNACCEPT;
565 }
566
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;
573 }
574
575 if (val != L2CAP_CONF_QOS_NO_TRAFFIC) {
576 /* XXX: These values should possibly be calculated
577 * based on LM / baseband properties also. */
578
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);
583
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);
588
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 */
592 }
593 break;
594
595 case L2CAP_CONF_RFC:
596 if (opt->len != 9) {
597 result = L2CAP_CONF_REJECT;
598 break;
599 }
600
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;
607
608 /* All other parameters shall be ignored */
609 break;
610
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. */
618
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;
625 }
626
627 /* MaxTransmit */
628 val = opt->val[2];
629 if (val < 1) {
630 opt->val[2] = 1;
631 result = L2CAP_CONF_UNACCEPT;
632 break;
633 }
634
635 /* Remote Retransmission time-out shouldn't affect local
636 * operation (?) */
637
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;
646 }
647 ch->monitor_timeout = val;
648 l2cap_monitor_timer_update(ch);
649
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;
657 }
658 ch->mps = val;
659 break;
660
661 default:
662 result = L2CAP_CONF_UNACCEPT;
663 break;
664 }
665 break;
666
667 default:
668 if (!(opt->type >> 7))
669 result = L2CAP_CONF_UNKNOWN;
670 break;
671 }
672
673 if (result != L2CAP_CONF_SUCCESS)
674 break; /* XXX: should continue? */
675 }
676
677 l2cap_configuration_response(l2cap, ch->remote_cid,
678 flag, result, rsp, len);
679
680 return result == L2CAP_CONF_SUCCESS && !flag;
681 }
682
683 static void l2cap_channel_config_req_msg(struct l2cap_instance_s *l2cap,
684 int flag, int cid, const uint8_t *data, int len)
685 {
686 struct l2cap_chan_s *ch;
687
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;
692 }
693 ch = l2cap->cid[cid];
694
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;
701
702 if (l2cap_channel_config(l2cap, ch, flag, data, len))
703 /* Go to OPEN or WAIT_CONFIG_RSP */
704 ch->config |= L2CAP_CFG_ACC;
705
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);
713 }
714
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)
717 {
718 struct l2cap_chan_s *ch;
719
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;
724 }
725 ch = l2cap->cid[cid];
726
727 if (ch->config_req_id != l2cap->last_id)
728 return 1;
729 ch->config_req_id = 0;
730
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);
739
740 return 0;
741 }
742
743 static void l2cap_channel_open_req_msg(struct l2cap_instance_s *l2cap,
744 int psm, int source_cid)
745 {
746 struct l2cap_chan_s *ch = l2cap_channel_open(l2cap, psm, source_cid);
747
748 if (!ch)
749 return;
750
751 /* Optional */
752 if (!(ch->config & L2CAP_CFG_INIT) && !ch->config_req_id)
753 l2cap_channel_config_req_event(l2cap, ch);
754 }
755
756 static void l2cap_info(struct l2cap_instance_s *l2cap, int type)
757 {
758 uint8_t data[4];
759 int len = 0;
760 int result = L2CAP_IR_SUCCESS;
761
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;
767
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;
775
776 default:
777 result = L2CAP_IR_NOTSUPP;
778 }
779
780 l2cap_info_response(l2cap, type, result, data, len);
781 }
782
783 static void l2cap_command(struct l2cap_instance_s *l2cap, int code, int id,
784 const uint8_t *params, int len)
785 {
786 int err;
787
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;
794 }
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! */
804 }
805
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;
811 }
812
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;
818
819 case L2CAP_CONN_REQ:
820 if (unlikely(len != L2CAP_CONN_REQ_SIZE)) {
821 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
822 goto reject;
823 }
824
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;
829
830 case L2CAP_CONN_RSP:
831 if (unlikely(len != L2CAP_CONN_RSP_SIZE)) {
832 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
833 goto reject;
834 }
835
836 /* We never issue Connection Requests currently. TODO */
837 fprintf(stderr, "%s: unexpected Connection Response (%02x) "
838 "packet, ignoring.\n", __FUNCTION__, id);
839 break;
840
841 case L2CAP_CONF_REQ:
842 if (unlikely(len < L2CAP_CONF_REQ_SIZE(0))) {
843 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
844 goto reject;
845 }
846
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;
853
854 case L2CAP_CONF_RSP:
855 if (unlikely(len < L2CAP_CONF_RSP_SIZE(0))) {
856 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
857 goto reject;
858 }
859
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;
869
870 case L2CAP_DISCONN_REQ:
871 if (unlikely(len != L2CAP_DISCONN_REQ_SIZE)) {
872 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
873 goto reject;
874 }
875
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;
880
881 case L2CAP_DISCONN_RSP:
882 if (unlikely(len != L2CAP_DISCONN_RSP_SIZE)) {
883 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
884 goto reject;
885 }
886
887 /* We never issue Disconnection Requests currently. TODO */
888 fprintf(stderr, "%s: unexpected Disconnection Response (%02x) "
889 "packet, ignoring.\n", __FUNCTION__, id);
890 break;
891
892 case L2CAP_ECHO_REQ:
893 l2cap_echo_response(l2cap, params, len);
894 break;
895
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;
901
902 case L2CAP_INFO_REQ:
903 if (unlikely(len != L2CAP_INFO_REQ_SIZE)) {
904 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
905 goto reject;
906 }
907
908 l2cap_info(l2cap, le16_to_cpu(((l2cap_info_req *) params)->type));
909 break;
910
911 case L2CAP_INFO_RSP:
912 if (unlikely(len != L2CAP_INFO_RSP_SIZE)) {
913 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
914 goto reject;
915 }
916
917 /* We never issue Information Requests currently. TODO */
918 fprintf(stderr, "%s: unexpected Information Response (%02x) "
919 "packet, ignoring.\n", __FUNCTION__, id);
920 break;
921
922 default:
923 err = L2CAP_REJ_CMD_NOT_UNDERSTOOD;
924 reject:
925 l2cap_command_reject(l2cap, id, err, 0, 0);
926 break;
927 }
928 }
929
930 static void l2cap_rexmit_enable(struct l2cap_chan_s *ch, int enable)
931 {
932 ch->rexmit = enable;
933
934 l2cap_retransmission_timer_update(ch);
935 l2cap_monitor_timer_update(ch);
936 }
937
938 /* Command frame SDU */
939 static void l2cap_cframe_in(void *opaque, const uint8_t *data, int len)
940 {
941 struct l2cap_instance_s *l2cap = opaque;
942 const l2cap_cmd_hdr *hdr;
943 int clen;
944
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;
952
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;
958 }
959
960 l2cap_command(l2cap, hdr->code, hdr->ident, data, clen);
961 len -= clen;
962 data += clen;
963 }
964 }
965
966 /* Group frame SDU */
967 static void l2cap_gframe_in(void *opaque, const uint8_t *data, int len)
968 {
969 }
970
971 /* Supervisory frame */
972 static void l2cap_sframe_in(struct l2cap_chan_s *ch, uint16_t ctrl)
973 {
974 }
975
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)
979 {
980 /* We have a full SDU, no further processing */
981 ch->params.sdu_in(ch->params.opaque, hdr->data, len);
982 }
983
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)
987 {
988 uint16_t fcs = le16_to_cpup((void *) (hdr->data + len - 2));
989
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;
994
995 if ((hdr->data[0] >> 7) == ch->rexmit)
996 l2cap_rexmit_enable(ch, !(hdr->data[0] >> 7));
997
998 if (hdr->data[0] & 1) {
999 if (len != 4) {
1000 /* TODO: Signal an error? */
1001 return;
1002 }
1003 return l2cap_sframe_in(ch, le16_to_cpup((void *) hdr->data));
1004 }
1005
1006 switch (hdr->data[1] >> 6) { /* SAR */
1007 case L2CAP_SAR_NO_SEG:
1008 if (ch->len_total)
1009 goto seg_error;
1010 if (len - 4 > ch->mps)
1011 goto len_error;
1012
1013 return ch->params.sdu_in(ch->params.opaque, hdr->data + 2, len - 4);
1014
1015 case L2CAP_SAR_START:
1016 if (ch->len_total || len < 6)
1017 goto seg_error;
1018 if (len - 6 > ch->mps)
1019 goto len_error;
1020
1021 ch->len_total = le16_to_cpup((void *) (hdr->data + 2));
1022 if (len >= 6 + ch->len_total)
1023 goto seg_error;
1024
1025 ch->len_cur = len - 6;
1026 memcpy(ch->sdu, hdr->data + 4, ch->len_cur);
1027 break;
1028
1029 case L2CAP_SAR_END:
1030 if (!ch->len_total || ch->len_cur + len - 4 < ch->len_total)
1031 goto seg_error;
1032 if (len - 4 > ch->mps)
1033 goto len_error;
1034
1035 memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
1036 return ch->params.sdu_in(ch->params.opaque, ch->sdu, ch->len_total);
1037
1038 case L2CAP_SAR_CONT:
1039 if (!ch->len_total || ch->len_cur + len - 4 >= ch->len_total)
1040 goto seg_error;
1041 if (len - 4 > ch->mps)
1042 goto len_error;
1043
1044 memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4);
1045 ch->len_cur += len - 4;
1046 break;
1047
1048 seg_error:
1049 len_error: /* TODO */
1050 fcs_error: /* TODO */
1051 ch->len_cur = 0;
1052 ch->len_total = 0;
1053 break;
1054 }
1055 }
1056
1057 static void l2cap_frame_in(struct l2cap_instance_s *l2cap,
1058 const l2cap_hdr *frame)
1059 {
1060 uint16_t cid = le16_to_cpu(frame->cid);
1061 uint16_t len = le16_to_cpu(frame->len);
1062
1063 if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) {
1064 fprintf(stderr, "%s: frame addressed to a non-existent L2CAP "
1065 "channel %04x received.\n", __FUNCTION__, cid);
1066 return;
1067 }
1068
1069 l2cap->cid[cid]->frame_in(l2cap->cid[cid], cid, frame, len);
1070 }
1071
1072 /* "Recombination" */
1073 static void l2cap_pdu_in(struct l2cap_instance_s *l2cap,
1074 const uint8_t *data, int len)
1075 {
1076 const l2cap_hdr *hdr = (void *) l2cap->frame_in;
1077
1078 if (unlikely(len + l2cap->frame_in_len > sizeof(l2cap->frame_in))) {
1079 if (l2cap->frame_in_len < sizeof(l2cap->frame_in)) {
1080 memcpy(l2cap->frame_in + l2cap->frame_in_len, data,
1081 sizeof(l2cap->frame_in) - l2cap->frame_in_len);
1082 l2cap->frame_in_len = sizeof(l2cap->frame_in);
1083 /* TODO: truncate */
1084 l2cap_frame_in(l2cap, hdr);
1085 }
1086
1087 return;
1088 }
1089
1090 memcpy(l2cap->frame_in + l2cap->frame_in_len, data, len);
1091 l2cap->frame_in_len += len;
1092
1093 if (len >= L2CAP_HDR_SIZE)
1094 if (len >= L2CAP_HDR_SIZE + le16_to_cpu(hdr->len))
1095 l2cap_frame_in(l2cap, hdr);
1096 /* There is never a start of a new PDU in the same ACL packet, so
1097 * no need to memmove the remaining payload and loop. */
1098 }
1099
1100 static inline uint8_t *l2cap_pdu_out(struct l2cap_instance_s *l2cap,
1101 uint16_t cid, uint16_t len)
1102 {
1103 l2cap_hdr *hdr = (void *) l2cap->frame_out;
1104
1105 l2cap->frame_out_len = len + L2CAP_HDR_SIZE;
1106
1107 hdr->cid = cpu_to_le16(cid);
1108 hdr->len = cpu_to_le16(len);
1109
1110 return l2cap->frame_out + L2CAP_HDR_SIZE;
1111 }
1112
1113 static inline void l2cap_pdu_submit(struct l2cap_instance_s *l2cap)
1114 {
1115 /* TODO: Fragmentation */
1116 (l2cap->role ?
1117 l2cap->link->slave->lmp_acl_data : l2cap->link->host->lmp_acl_resp)
1118 (l2cap->link, l2cap->frame_out, 1, l2cap->frame_out_len);
1119 }
1120
1121 static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len)
1122 {
1123 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
1124
1125 if (len > chan->params.remote_mtu) {
1126 fprintf(stderr, "%s: B-Frame for CID %04x longer than %i octets.\n",
1127 __FUNCTION__,
1128 chan->remote_cid, chan->params.remote_mtu);
1129 exit(-1);
1130 }
1131
1132 return l2cap_pdu_out(chan->l2cap, chan->remote_cid, len);
1133 }
1134
1135 static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms)
1136 {
1137 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parms;
1138
1139 return l2cap_pdu_submit(chan->l2cap);
1140 }
1141
1142 #if 0
1143 /* Stub: Only used if an emulated device requests outgoing flow control */
1144 static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len)
1145 {
1146 struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm;
1147
1148 if (len > chan->params.remote_mtu) {
1149 /* TODO: slice into segments and queue each segment as a separate
1150 * I-Frame in a FIFO of I-Frames, local to the CID. */
1151 } else {
1152 /* TODO: add to the FIFO of I-Frames, local to the CID. */
1153 /* Possibly we need to return a pointer to a contiguous buffer
1154 * for now and then memcpy from it into FIFOs in l2cap_iframe_submit
1155 * while segmenting at the same time. */
1156 }
1157 return 0;
1158 }
1159
1160 static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm)
1161 {
1162 /* TODO: If flow control indicates clear to send, start submitting the
1163 * invidual I-Frames from the FIFO, but don't remove them from there.
1164 * Kick the appropriate timer until we get an S-Frame, and only then
1165 * remove from FIFO or resubmit and re-kick the timer if the timer
1166 * expired. */
1167 }
1168 #endif
1169
1170 static void l2cap_init(struct l2cap_instance_s *l2cap,
1171 struct bt_link_s *link, int role)
1172 {
1173 l2cap->link = link;
1174 l2cap->role = role;
1175 l2cap->dev = (struct bt_l2cap_device_s *)
1176 (role ? link->host : link->slave);
1177
1178 l2cap->next_id = 1;
1179
1180 /* Establish the signalling channel */
1181 l2cap->signalling_ch.params.sdu_in = l2cap_cframe_in;
1182 l2cap->signalling_ch.params.sdu_out = l2cap_bframe_out;
1183 l2cap->signalling_ch.params.sdu_submit = l2cap_bframe_submit;
1184 l2cap->signalling_ch.params.opaque = l2cap;
1185 l2cap->signalling_ch.params.remote_mtu = 48;
1186 l2cap->signalling_ch.remote_cid = L2CAP_CID_SIGNALLING;
1187 l2cap->signalling_ch.frame_in = l2cap_bframe_in;
1188 l2cap->signalling_ch.mps = 65536;
1189 l2cap->signalling_ch.min_mtu = 48;
1190 l2cap->signalling_ch.mode = L2CAP_MODE_BASIC;
1191 l2cap->signalling_ch.l2cap = l2cap;
1192 l2cap->cid[L2CAP_CID_SIGNALLING] = &l2cap->signalling_ch;
1193
1194 /* Establish the connection-less data channel */
1195 l2cap->group_ch.params.sdu_in = l2cap_gframe_in;
1196 l2cap->group_ch.params.opaque = l2cap;
1197 l2cap->group_ch.frame_in = l2cap_bframe_in;
1198 l2cap->group_ch.mps = 65533;
1199 l2cap->group_ch.l2cap = l2cap;
1200 l2cap->group_ch.remote_cid = L2CAP_CID_INVALID;
1201 l2cap->cid[L2CAP_CID_GROUP] = &l2cap->group_ch;
1202 }
1203
1204 static void l2cap_teardown(struct l2cap_instance_s *l2cap, int send_disconnect)
1205 {
1206 int cid;
1207
1208 /* Don't send DISCONNECT if we are currently handling a DISCONNECT
1209 * sent from the other side. */
1210 if (send_disconnect) {
1211 if (l2cap->role)
1212 l2cap->dev->device.lmp_disconnect_slave(l2cap->link);
1213 /* l2cap->link is invalid from now on. */
1214 else
1215 l2cap->dev->device.lmp_disconnect_master(l2cap->link);
1216 }
1217
1218 for (cid = L2CAP_CID_ALLOC; cid < L2CAP_CID_MAX; cid ++)
1219 if (l2cap->cid[cid]) {
1220 l2cap->cid[cid]->params.close(l2cap->cid[cid]->params.opaque);
1221 g_free(l2cap->cid[cid]);
1222 }
1223
1224 if (l2cap->role)
1225 g_free(l2cap);
1226 else
1227 g_free(l2cap->link);
1228 }
1229
1230 /* L2CAP glue to lower layers in bluetooth stack (LMP) */
1231
1232 static void l2cap_lmp_connection_request(struct bt_link_s *link)
1233 {
1234 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->slave;
1235 struct slave_l2cap_instance_s *l2cap;
1236
1237 /* Always accept - we only get called if (dev->device->page_scan). */
1238
1239 l2cap = g_malloc0(sizeof(struct slave_l2cap_instance_s));
1240 l2cap->link.slave = &dev->device;
1241 l2cap->link.host = link->host;
1242 l2cap_init(&l2cap->l2cap, &l2cap->link, 0);
1243
1244 /* Always at the end */
1245 link->host->reject_reason = 0;
1246 link->host->lmp_connection_complete(&l2cap->link);
1247 }
1248
1249 /* Stub */
1250 static void l2cap_lmp_connection_complete(struct bt_link_s *link)
1251 {
1252 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1253 struct l2cap_instance_s *l2cap;
1254
1255 if (dev->device.reject_reason) {
1256 /* Signal to upper layer */
1257 return;
1258 }
1259
1260 l2cap = g_malloc0(sizeof(struct l2cap_instance_s));
1261 l2cap_init(l2cap, link, 1);
1262
1263 link->acl_mode = acl_active;
1264
1265 /* Signal to upper layer */
1266 }
1267
1268 /* Stub */
1269 static void l2cap_lmp_disconnect_host(struct bt_link_s *link)
1270 {
1271 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1272 struct l2cap_instance_s *l2cap =
1273 /* TODO: Retrieve from upper layer */ (void *) dev;
1274
1275 /* Signal to upper layer */
1276
1277 l2cap_teardown(l2cap, 0);
1278 }
1279
1280 static void l2cap_lmp_disconnect_slave(struct bt_link_s *link)
1281 {
1282 struct slave_l2cap_instance_s *l2cap =
1283 (struct slave_l2cap_instance_s *) link;
1284
1285 l2cap_teardown(&l2cap->l2cap, 0);
1286 }
1287
1288 static void l2cap_lmp_acl_data_slave(struct bt_link_s *link,
1289 const uint8_t *data, int start, int len)
1290 {
1291 struct slave_l2cap_instance_s *l2cap =
1292 (struct slave_l2cap_instance_s *) link;
1293
1294 if (start)
1295 l2cap->l2cap.frame_in_len = 0;
1296
1297 l2cap_pdu_in(&l2cap->l2cap, data, len);
1298 }
1299
1300 /* Stub */
1301 static void l2cap_lmp_acl_data_host(struct bt_link_s *link,
1302 const uint8_t *data, int start, int len)
1303 {
1304 struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host;
1305 struct l2cap_instance_s *l2cap =
1306 /* TODO: Retrieve from upper layer */ (void *) dev;
1307
1308 if (start)
1309 l2cap->frame_in_len = 0;
1310
1311 l2cap_pdu_in(l2cap, data, len);
1312 }
1313
1314 static void l2cap_dummy_destroy(struct bt_device_s *dev)
1315 {
1316 struct bt_l2cap_device_s *l2cap_dev = (struct bt_l2cap_device_s *) dev;
1317
1318 bt_l2cap_device_done(l2cap_dev);
1319 }
1320
1321 void bt_l2cap_device_init(struct bt_l2cap_device_s *dev,
1322 struct bt_scatternet_s *net)
1323 {
1324 bt_device_init(&dev->device, net);
1325
1326 dev->device.lmp_connection_request = l2cap_lmp_connection_request;
1327 dev->device.lmp_connection_complete = l2cap_lmp_connection_complete;
1328 dev->device.lmp_disconnect_master = l2cap_lmp_disconnect_host;
1329 dev->device.lmp_disconnect_slave = l2cap_lmp_disconnect_slave;
1330 dev->device.lmp_acl_data = l2cap_lmp_acl_data_slave;
1331 dev->device.lmp_acl_resp = l2cap_lmp_acl_data_host;
1332
1333 dev->device.handle_destroy = l2cap_dummy_destroy;
1334 }
1335
1336 void bt_l2cap_device_done(struct bt_l2cap_device_s *dev)
1337 {
1338 bt_device_done(&dev->device);
1339
1340 /* Should keep a list of all instances and go through it and
1341 * invoke l2cap_teardown() for each. */
1342 }
1343
1344 void bt_l2cap_psm_register(struct bt_l2cap_device_s *dev, int psm, int min_mtu,
1345 int (*new_channel)(struct bt_l2cap_device_s *dev,
1346 struct bt_l2cap_conn_params_s *params))
1347 {
1348 struct bt_l2cap_psm_s *new_psm = l2cap_psm(dev, psm);
1349
1350 if (new_psm) {
1351 fprintf(stderr, "%s: PSM %04x already registered for device `%s'.\n",
1352 __FUNCTION__, psm, dev->device.lmp_name);
1353 exit(-1);
1354 }
1355
1356 new_psm = g_malloc0(sizeof(*new_psm));
1357 new_psm->psm = psm;
1358 new_psm->min_mtu = min_mtu;
1359 new_psm->new_channel = new_channel;
1360 new_psm->next = dev->first_psm;
1361 dev->first_psm = new_psm;
1362 }