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