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