xref: /illumos-kvm-cmd/hw/bt-l2cap.c (revision 68396ea9)
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 
l2cap_fcs16(const uint8_t * message,int len)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 
l2cap_retransmission_timer_update(struct l2cap_chan_s * ch)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 
l2cap_monitor_timer_update(struct l2cap_chan_s * ch)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 
l2cap_command_reject(struct l2cap_instance_s * l2cap,int id,uint16_t reason,const void * data,int plen)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 
l2cap_command_reject_cid(struct l2cap_instance_s * l2cap,int id,uint16_t reason,uint16_t dcid,uint16_t scid)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 
l2cap_connection_response(struct l2cap_instance_s * l2cap,int dcid,int scid,int result,int status)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 
l2cap_configuration_request(struct l2cap_instance_s * l2cap,int dcid,int flag,const uint8_t * data,int len)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 
l2cap_configuration_response(struct l2cap_instance_s * l2cap,int scid,int flag,int result,const uint8_t * data,int len)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 
l2cap_disconnection_response(struct l2cap_instance_s * l2cap,int dcid,int scid)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 
l2cap_echo_response(struct l2cap_instance_s * l2cap,const uint8_t * data,int len)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 
l2cap_info_response(struct l2cap_instance_s * l2cap,int type,int result,const uint8_t * data,int len)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 
l2cap_cid_new(struct l2cap_instance_s * l2cap)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 
l2cap_psm(struct bt_l2cap_device_s * device,int psm)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 
l2cap_channel_open(struct l2cap_instance_s * l2cap,int psm,int source_cid)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 = qemu_mallocz(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                 qemu_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 
l2cap_channel_close(struct l2cap_instance_s * l2cap,int cid,int source_cid)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         qemu_free(ch);
477     }
478 
479     l2cap_disconnection_response(l2cap, cid, source_cid);
480 }
481 
l2cap_channel_config_null(struct l2cap_instance_s * l2cap,struct l2cap_chan_s * ch)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 
l2cap_channel_config_req_event(struct l2cap_instance_s * l2cap,struct l2cap_chan_s * ch)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 
l2cap_channel_config(struct l2cap_instance_s * l2cap,struct l2cap_chan_s * ch,int flag,const uint8_t * data,int len)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 
l2cap_channel_config_req_msg(struct l2cap_instance_s * l2cap,int flag,int cid,const uint8_t * data,int len)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 
l2cap_channel_config_rsp_msg(struct l2cap_instance_s * l2cap,int result,int flag,int cid,const uint8_t * data,int len)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 
l2cap_channel_open_req_msg(struct l2cap_instance_s * l2cap,int psm,int source_cid)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 
l2cap_info(struct l2cap_instance_s * l2cap,int type)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 
l2cap_command(struct l2cap_instance_s * l2cap,int code,int id,const uint8_t * params,int len)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 
l2cap_rexmit_enable(struct l2cap_chan_s * ch,int enable)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 */
l2cap_cframe_in(void * opaque,const uint8_t * data,int len)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 */
l2cap_gframe_in(void * opaque,const uint8_t * data,int len)967 static void l2cap_gframe_in(void *opaque, const uint8_t *data, int len)
968 {
969 }
970 
971 /* Supervisory frame */
l2cap_sframe_in(struct l2cap_chan_s * ch,uint16_t ctrl)972 static void l2cap_sframe_in(struct l2cap_chan_s *ch, uint16_t ctrl)
973 {
974 }
975 
976 /* Basic L2CAP mode Information frame */
l2cap_bframe_in(struct l2cap_chan_s * ch,uint16_t cid,const l2cap_hdr * hdr,int len)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 */
l2cap_iframe_in(struct l2cap_chan_s * ch,uint16_t cid,const l2cap_hdr * hdr,int len)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 
l2cap_frame_in(struct l2cap_instance_s * l2cap,const l2cap_hdr * frame)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" */
l2cap_pdu_in(struct l2cap_instance_s * l2cap,const uint8_t * data,int len)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 
l2cap_pdu_out(struct l2cap_instance_s * l2cap,uint16_t cid,uint16_t len)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 
l2cap_pdu_submit(struct l2cap_instance_s * l2cap)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 
l2cap_bframe_out(struct bt_l2cap_conn_params_s * parm,int len)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 
l2cap_bframe_submit(struct bt_l2cap_conn_params_s * parms)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 
l2cap_init(struct l2cap_instance_s * l2cap,struct bt_link_s * link,int role)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 
l2cap_teardown(struct l2cap_instance_s * l2cap,int send_disconnect)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             qemu_free(l2cap->cid[cid]);
1222         }
1223 
1224     if (l2cap->role)
1225         qemu_free(l2cap);
1226     else
1227         qemu_free(l2cap->link);
1228 }
1229 
1230 /* L2CAP glue to lower layers in bluetooth stack (LMP) */
1231 
l2cap_lmp_connection_request(struct bt_link_s * link)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 = qemu_mallocz(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 */
l2cap_lmp_connection_complete(struct bt_link_s * link)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 = qemu_mallocz(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 */
l2cap_lmp_disconnect_host(struct bt_link_s * link)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 
l2cap_lmp_disconnect_slave(struct bt_link_s * link)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 
l2cap_lmp_acl_data_slave(struct bt_link_s * link,const uint8_t * data,int start,int len)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 */
l2cap_lmp_acl_data_host(struct bt_link_s * link,const uint8_t * data,int start,int len)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 
l2cap_dummy_destroy(struct bt_device_s * dev)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 
bt_l2cap_device_init(struct bt_l2cap_device_s * dev,struct bt_scatternet_s * net)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 
bt_l2cap_device_done(struct bt_l2cap_device_s * dev)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 
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))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 = qemu_mallocz(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 }
1363