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/osdep.h" |
21 | #include "qemu/error-report.h" |
22 | #include "qemu/timer.h" |
23 | #include "qemu/bswap.h" |
24 | #include "hw/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 | |
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 | |
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 | timer_mod(ch->retransmission_timer); |
172 | else |
173 | timer_del(ch->retransmission_timer); |
174 | #endif |
175 | } |
176 | |
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 | timer_mod(ch->monitor_timer); |
182 | else |
183 | timer_del(ch->monitor_timer); |
184 | #endif |
185 | } |
186 | |
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 | |
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 | |
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 | |
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 | |
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 | |
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 | |
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 | |
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 | |
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 | |
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 | |
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 = g_malloc0(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 | g_free(ch); |
434 | ch = NULL; |
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 | |
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 | error_report("%s: Ignoring a Disconnection Request with the " |
471 | "invalid SCID %04x." , __func__, source_cid); |
472 | return; |
473 | } |
474 | |
475 | l2cap->cid[cid] = NULL; |
476 | |
477 | ch->params.close(ch->params.opaque); |
478 | g_free(ch); |
479 | } |
480 | |
481 | l2cap_disconnection_response(l2cap, cid, source_cid); |
482 | } |
483 | |
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 | |
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 | |
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 = lduw_le_p(opt->val); |
530 | if (val < ch->min_mtu) { |
531 | stw_le_p(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 = lduw_le_p(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 | |
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 | |
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 | |
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 | |
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 | |
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 | error_report("%s: out of sequence command packet ignored." , |
794 | __func__); |
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 | error_report("%s: stray Command Reject (%02x, %04x) " |
817 | "packet, ignoring." , __func__, 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 | error_report("%s: unexpected Connection Response (%02x) " |
840 | "packet, ignoring." , __func__, 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 | error_report("%s: unexpected Configure Response (%02x) " |
869 | "packet, ignoring." , __func__, 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 | error_report("%s: unexpected Disconnection Response (%02x) " |
891 | "packet, ignoring." , __func__, 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 | error_report("%s: unexpected Echo Response (%02x) " |
901 | "packet, ignoring." , __func__, 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 | error_report("%s: unexpected Information Response (%02x) " |
921 | "packet, ignoring." , __func__, 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 | |
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 */ |
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 */ |
969 | static void l2cap_gframe_in(void *opaque, const uint8_t *data, int len) |
970 | { |
971 | } |
972 | |
973 | /* Supervisory frame */ |
974 | static void l2cap_sframe_in(struct l2cap_chan_s *ch, uint16_t ctrl) |
975 | { |
976 | } |
977 | |
978 | /* Basic L2CAP mode Information frame */ |
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 */ |
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 = lduw_le_p(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 | l2cap_sframe_in(ch, lduw_le_p(hdr->data)); |
1006 | return; |
1007 | } |
1008 | |
1009 | switch (hdr->data[1] >> 6) { /* SAR */ |
1010 | case L2CAP_SAR_NO_SEG: |
1011 | if (ch->len_total) |
1012 | goto seg_error; |
1013 | if (len - 4 > ch->mps) |
1014 | goto len_error; |
1015 | |
1016 | ch->params.sdu_in(ch->params.opaque, hdr->data + 2, len - 4); |
1017 | break; |
1018 | |
1019 | case L2CAP_SAR_START: |
1020 | if (ch->len_total || len < 6) |
1021 | goto seg_error; |
1022 | if (len - 6 > ch->mps) |
1023 | goto len_error; |
1024 | |
1025 | ch->len_total = lduw_le_p(hdr->data + 2); |
1026 | if (len >= 6 + ch->len_total) |
1027 | goto seg_error; |
1028 | |
1029 | ch->len_cur = len - 6; |
1030 | memcpy(ch->sdu, hdr->data + 4, ch->len_cur); |
1031 | break; |
1032 | |
1033 | case L2CAP_SAR_END: |
1034 | if (!ch->len_total || ch->len_cur + len - 4 < ch->len_total) |
1035 | goto seg_error; |
1036 | if (len - 4 > ch->mps) |
1037 | goto len_error; |
1038 | |
1039 | memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4); |
1040 | ch->params.sdu_in(ch->params.opaque, ch->sdu, ch->len_total); |
1041 | break; |
1042 | |
1043 | case L2CAP_SAR_CONT: |
1044 | if (!ch->len_total || ch->len_cur + len - 4 >= ch->len_total) |
1045 | goto seg_error; |
1046 | if (len - 4 > ch->mps) |
1047 | goto len_error; |
1048 | |
1049 | memcpy(ch->sdu + ch->len_cur, hdr->data + 2, len - 4); |
1050 | ch->len_cur += len - 4; |
1051 | break; |
1052 | |
1053 | seg_error: |
1054 | len_error: /* TODO */ |
1055 | fcs_error: /* TODO */ |
1056 | ch->len_cur = 0; |
1057 | ch->len_total = 0; |
1058 | break; |
1059 | } |
1060 | } |
1061 | |
1062 | static void l2cap_frame_in(struct l2cap_instance_s *l2cap, |
1063 | const l2cap_hdr *frame) |
1064 | { |
1065 | uint16_t cid = le16_to_cpu(frame->cid); |
1066 | uint16_t len = le16_to_cpu(frame->len); |
1067 | |
1068 | if (unlikely(cid >= L2CAP_CID_MAX || !l2cap->cid[cid])) { |
1069 | error_report("%s: frame addressed to a non-existent L2CAP " |
1070 | "channel %04x received." , __func__, cid); |
1071 | return; |
1072 | } |
1073 | |
1074 | l2cap->cid[cid]->frame_in(l2cap->cid[cid], cid, frame, len); |
1075 | } |
1076 | |
1077 | /* "Recombination" */ |
1078 | static void l2cap_pdu_in(struct l2cap_instance_s *l2cap, |
1079 | const uint8_t *data, int len) |
1080 | { |
1081 | const l2cap_hdr *hdr = (void *) l2cap->frame_in; |
1082 | |
1083 | if (unlikely(len + l2cap->frame_in_len > sizeof(l2cap->frame_in))) { |
1084 | if (l2cap->frame_in_len < sizeof(l2cap->frame_in)) { |
1085 | memcpy(l2cap->frame_in + l2cap->frame_in_len, data, |
1086 | sizeof(l2cap->frame_in) - l2cap->frame_in_len); |
1087 | l2cap->frame_in_len = sizeof(l2cap->frame_in); |
1088 | /* TODO: truncate */ |
1089 | l2cap_frame_in(l2cap, hdr); |
1090 | } |
1091 | |
1092 | return; |
1093 | } |
1094 | |
1095 | memcpy(l2cap->frame_in + l2cap->frame_in_len, data, len); |
1096 | l2cap->frame_in_len += len; |
1097 | |
1098 | if (len >= L2CAP_HDR_SIZE) |
1099 | if (len >= L2CAP_HDR_SIZE + le16_to_cpu(hdr->len)) |
1100 | l2cap_frame_in(l2cap, hdr); |
1101 | /* There is never a start of a new PDU in the same ACL packet, so |
1102 | * no need to memmove the remaining payload and loop. */ |
1103 | } |
1104 | |
1105 | static inline uint8_t *l2cap_pdu_out(struct l2cap_instance_s *l2cap, |
1106 | uint16_t cid, uint16_t len) |
1107 | { |
1108 | l2cap_hdr *hdr = (void *) l2cap->frame_out; |
1109 | |
1110 | l2cap->frame_out_len = len + L2CAP_HDR_SIZE; |
1111 | |
1112 | hdr->cid = cpu_to_le16(cid); |
1113 | hdr->len = cpu_to_le16(len); |
1114 | |
1115 | return l2cap->frame_out + L2CAP_HDR_SIZE; |
1116 | } |
1117 | |
1118 | static inline void l2cap_pdu_submit(struct l2cap_instance_s *l2cap) |
1119 | { |
1120 | /* TODO: Fragmentation */ |
1121 | (l2cap->role ? |
1122 | l2cap->link->slave->lmp_acl_data : l2cap->link->host->lmp_acl_resp) |
1123 | (l2cap->link, l2cap->frame_out, 1, l2cap->frame_out_len); |
1124 | } |
1125 | |
1126 | static uint8_t *l2cap_bframe_out(struct bt_l2cap_conn_params_s *parm, int len) |
1127 | { |
1128 | struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm; |
1129 | |
1130 | if (len > chan->params.remote_mtu) { |
1131 | error_report("%s: B-Frame for CID %04x longer than %i octets." , |
1132 | __func__, |
1133 | chan->remote_cid, chan->params.remote_mtu); |
1134 | exit(-1); |
1135 | } |
1136 | |
1137 | return l2cap_pdu_out(chan->l2cap, chan->remote_cid, len); |
1138 | } |
1139 | |
1140 | static void l2cap_bframe_submit(struct bt_l2cap_conn_params_s *parms) |
1141 | { |
1142 | struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parms; |
1143 | |
1144 | l2cap_pdu_submit(chan->l2cap); |
1145 | } |
1146 | |
1147 | #if 0 |
1148 | /* Stub: Only used if an emulated device requests outgoing flow control */ |
1149 | static uint8_t *l2cap_iframe_out(struct bt_l2cap_conn_params_s *parm, int len) |
1150 | { |
1151 | struct l2cap_chan_s *chan = (struct l2cap_chan_s *) parm; |
1152 | |
1153 | if (len > chan->params.remote_mtu) { |
1154 | /* TODO: slice into segments and queue each segment as a separate |
1155 | * I-Frame in a FIFO of I-Frames, local to the CID. */ |
1156 | } else { |
1157 | /* TODO: add to the FIFO of I-Frames, local to the CID. */ |
1158 | /* Possibly we need to return a pointer to a contiguous buffer |
1159 | * for now and then memcpy from it into FIFOs in l2cap_iframe_submit |
1160 | * while segmenting at the same time. */ |
1161 | } |
1162 | return 0; |
1163 | } |
1164 | |
1165 | static void l2cap_iframe_submit(struct bt_l2cap_conn_params_s *parm) |
1166 | { |
1167 | /* TODO: If flow control indicates clear to send, start submitting the |
1168 | * invidual I-Frames from the FIFO, but don't remove them from there. |
1169 | * Kick the appropriate timer until we get an S-Frame, and only then |
1170 | * remove from FIFO or resubmit and re-kick the timer if the timer |
1171 | * expired. */ |
1172 | } |
1173 | #endif |
1174 | |
1175 | static void l2cap_init(struct l2cap_instance_s *l2cap, |
1176 | struct bt_link_s *link, int role) |
1177 | { |
1178 | l2cap->link = link; |
1179 | l2cap->role = role; |
1180 | l2cap->dev = (struct bt_l2cap_device_s *) |
1181 | (role ? link->host : link->slave); |
1182 | |
1183 | l2cap->next_id = 1; |
1184 | |
1185 | /* Establish the signalling channel */ |
1186 | l2cap->signalling_ch.params.sdu_in = l2cap_cframe_in; |
1187 | l2cap->signalling_ch.params.sdu_out = l2cap_bframe_out; |
1188 | l2cap->signalling_ch.params.sdu_submit = l2cap_bframe_submit; |
1189 | l2cap->signalling_ch.params.opaque = l2cap; |
1190 | l2cap->signalling_ch.params.remote_mtu = 48; |
1191 | l2cap->signalling_ch.remote_cid = L2CAP_CID_SIGNALLING; |
1192 | l2cap->signalling_ch.frame_in = l2cap_bframe_in; |
1193 | l2cap->signalling_ch.mps = 65536; |
1194 | l2cap->signalling_ch.min_mtu = 48; |
1195 | l2cap->signalling_ch.mode = L2CAP_MODE_BASIC; |
1196 | l2cap->signalling_ch.l2cap = l2cap; |
1197 | l2cap->cid[L2CAP_CID_SIGNALLING] = &l2cap->signalling_ch; |
1198 | |
1199 | /* Establish the connection-less data channel */ |
1200 | l2cap->group_ch.params.sdu_in = l2cap_gframe_in; |
1201 | l2cap->group_ch.params.opaque = l2cap; |
1202 | l2cap->group_ch.frame_in = l2cap_bframe_in; |
1203 | l2cap->group_ch.mps = 65533; |
1204 | l2cap->group_ch.l2cap = l2cap; |
1205 | l2cap->group_ch.remote_cid = L2CAP_CID_INVALID; |
1206 | l2cap->cid[L2CAP_CID_GROUP] = &l2cap->group_ch; |
1207 | } |
1208 | |
1209 | static void l2cap_teardown(struct l2cap_instance_s *l2cap, int send_disconnect) |
1210 | { |
1211 | int cid; |
1212 | |
1213 | /* Don't send DISCONNECT if we are currently handling a DISCONNECT |
1214 | * sent from the other side. */ |
1215 | if (send_disconnect) { |
1216 | if (l2cap->role) |
1217 | l2cap->dev->device.lmp_disconnect_slave(l2cap->link); |
1218 | /* l2cap->link is invalid from now on. */ |
1219 | else |
1220 | l2cap->dev->device.lmp_disconnect_master(l2cap->link); |
1221 | } |
1222 | |
1223 | for (cid = L2CAP_CID_ALLOC; cid < L2CAP_CID_MAX; cid ++) |
1224 | if (l2cap->cid[cid]) { |
1225 | l2cap->cid[cid]->params.close(l2cap->cid[cid]->params.opaque); |
1226 | g_free(l2cap->cid[cid]); |
1227 | } |
1228 | |
1229 | if (l2cap->role) |
1230 | g_free(l2cap); |
1231 | else |
1232 | g_free(l2cap->link); |
1233 | } |
1234 | |
1235 | /* L2CAP glue to lower layers in bluetooth stack (LMP) */ |
1236 | |
1237 | static void l2cap_lmp_connection_request(struct bt_link_s *link) |
1238 | { |
1239 | struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->slave; |
1240 | struct slave_l2cap_instance_s *l2cap; |
1241 | |
1242 | /* Always accept - we only get called if (dev->device->page_scan). */ |
1243 | |
1244 | l2cap = g_malloc0(sizeof(struct slave_l2cap_instance_s)); |
1245 | l2cap->link.slave = &dev->device; |
1246 | l2cap->link.host = link->host; |
1247 | l2cap_init(&l2cap->l2cap, &l2cap->link, 0); |
1248 | |
1249 | /* Always at the end */ |
1250 | link->host->reject_reason = 0; |
1251 | link->host->lmp_connection_complete(&l2cap->link); |
1252 | } |
1253 | |
1254 | /* Stub */ |
1255 | static void l2cap_lmp_connection_complete(struct bt_link_s *link) |
1256 | { |
1257 | struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host; |
1258 | struct l2cap_instance_s *l2cap; |
1259 | |
1260 | if (dev->device.reject_reason) { |
1261 | /* Signal to upper layer */ |
1262 | return; |
1263 | } |
1264 | |
1265 | l2cap = g_malloc0(sizeof(struct l2cap_instance_s)); |
1266 | l2cap_init(l2cap, link, 1); |
1267 | |
1268 | link->acl_mode = acl_active; |
1269 | |
1270 | /* Signal to upper layer */ |
1271 | } |
1272 | |
1273 | /* Stub */ |
1274 | static void l2cap_lmp_disconnect_host(struct bt_link_s *link) |
1275 | { |
1276 | struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host; |
1277 | struct l2cap_instance_s *l2cap = |
1278 | /* TODO: Retrieve from upper layer */ (void *) dev; |
1279 | |
1280 | /* Signal to upper layer */ |
1281 | |
1282 | l2cap_teardown(l2cap, 0); |
1283 | } |
1284 | |
1285 | static void l2cap_lmp_disconnect_slave(struct bt_link_s *link) |
1286 | { |
1287 | struct slave_l2cap_instance_s *l2cap = |
1288 | (struct slave_l2cap_instance_s *) link; |
1289 | |
1290 | l2cap_teardown(&l2cap->l2cap, 0); |
1291 | } |
1292 | |
1293 | static void l2cap_lmp_acl_data_slave(struct bt_link_s *link, |
1294 | const uint8_t *data, int start, int len) |
1295 | { |
1296 | struct slave_l2cap_instance_s *l2cap = |
1297 | (struct slave_l2cap_instance_s *) link; |
1298 | |
1299 | if (start) |
1300 | l2cap->l2cap.frame_in_len = 0; |
1301 | |
1302 | l2cap_pdu_in(&l2cap->l2cap, data, len); |
1303 | } |
1304 | |
1305 | /* Stub */ |
1306 | static void l2cap_lmp_acl_data_host(struct bt_link_s *link, |
1307 | const uint8_t *data, int start, int len) |
1308 | { |
1309 | struct bt_l2cap_device_s *dev = (struct bt_l2cap_device_s *) link->host; |
1310 | struct l2cap_instance_s *l2cap = |
1311 | /* TODO: Retrieve from upper layer */ (void *) dev; |
1312 | |
1313 | if (start) |
1314 | l2cap->frame_in_len = 0; |
1315 | |
1316 | l2cap_pdu_in(l2cap, data, len); |
1317 | } |
1318 | |
1319 | static void l2cap_dummy_destroy(struct bt_device_s *dev) |
1320 | { |
1321 | struct bt_l2cap_device_s *l2cap_dev = (struct bt_l2cap_device_s *) dev; |
1322 | |
1323 | bt_l2cap_device_done(l2cap_dev); |
1324 | } |
1325 | |
1326 | void bt_l2cap_device_init(struct bt_l2cap_device_s *dev, |
1327 | struct bt_scatternet_s *net) |
1328 | { |
1329 | bt_device_init(&dev->device, net); |
1330 | |
1331 | dev->device.lmp_connection_request = l2cap_lmp_connection_request; |
1332 | dev->device.lmp_connection_complete = l2cap_lmp_connection_complete; |
1333 | dev->device.lmp_disconnect_master = l2cap_lmp_disconnect_host; |
1334 | dev->device.lmp_disconnect_slave = l2cap_lmp_disconnect_slave; |
1335 | dev->device.lmp_acl_data = l2cap_lmp_acl_data_slave; |
1336 | dev->device.lmp_acl_resp = l2cap_lmp_acl_data_host; |
1337 | |
1338 | dev->device.handle_destroy = l2cap_dummy_destroy; |
1339 | } |
1340 | |
1341 | void bt_l2cap_device_done(struct bt_l2cap_device_s *dev) |
1342 | { |
1343 | bt_device_done(&dev->device); |
1344 | |
1345 | /* Should keep a list of all instances and go through it and |
1346 | * invoke l2cap_teardown() for each. */ |
1347 | } |
1348 | |
1349 | void bt_l2cap_psm_register(struct bt_l2cap_device_s *dev, int psm, int min_mtu, |
1350 | int (*new_channel)(struct bt_l2cap_device_s *dev, |
1351 | struct bt_l2cap_conn_params_s *params)) |
1352 | { |
1353 | struct bt_l2cap_psm_s *new_psm = l2cap_psm(dev, psm); |
1354 | |
1355 | if (new_psm) { |
1356 | error_report("%s: PSM %04x already registered for device `%s'." , |
1357 | __func__, psm, dev->device.lmp_name); |
1358 | exit(-1); |
1359 | } |
1360 | |
1361 | new_psm = g_malloc0(sizeof(*new_psm)); |
1362 | new_psm->psm = psm; |
1363 | new_psm->min_mtu = min_mtu; |
1364 | new_psm->new_channel = new_channel; |
1365 | new_psm->next = dev->first_psm; |
1366 | dev->first_psm = new_psm; |
1367 | } |
1368 | |