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