1 | /* $NetBSD: rfcomm_session.c,v 1.23 2014/08/05 07:55:32 rtr Exp $ */ |
2 | |
3 | /*- |
4 | * Copyright (c) 2006 Itronix Inc. |
5 | * All rights reserved. |
6 | * |
7 | * Written by Iain Hibbert for Itronix Inc. |
8 | * |
9 | * Redistribution and use in source and binary forms, with or without |
10 | * modification, are permitted provided that the following conditions |
11 | * are met: |
12 | * 1. Redistributions of source code must retain the above copyright |
13 | * notice, this list of conditions and the following disclaimer. |
14 | * 2. Redistributions in binary form must reproduce the above copyright |
15 | * notice, this list of conditions and the following disclaimer in the |
16 | * documentation and/or other materials provided with the distribution. |
17 | * 3. The name of Itronix Inc. may not be used to endorse |
18 | * or promote products derived from this software without specific |
19 | * prior written permission. |
20 | * |
21 | * THIS SOFTWARE IS PROVIDED BY ITRONIX INC. ``AS IS'' AND |
22 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
23 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR |
24 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL ITRONIX INC. BE LIABLE FOR ANY |
25 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES |
26 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND |
28 | * ON ANY THEORY OF LIABILITY, WHETHER IN |
29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
31 | * POSSIBILITY OF SUCH DAMAGE. |
32 | */ |
33 | |
34 | #include <sys/cdefs.h> |
35 | __KERNEL_RCSID(0, "$NetBSD: rfcomm_session.c,v 1.23 2014/08/05 07:55:32 rtr Exp $" ); |
36 | |
37 | #include <sys/param.h> |
38 | #include <sys/kernel.h> |
39 | #include <sys/mbuf.h> |
40 | #include <sys/proc.h> |
41 | #include <sys/socketvar.h> |
42 | #include <sys/systm.h> |
43 | #include <sys/types.h> |
44 | |
45 | #include <netbt/bluetooth.h> |
46 | #include <netbt/hci.h> |
47 | #include <netbt/l2cap.h> |
48 | #include <netbt/rfcomm.h> |
49 | |
50 | /****************************************************************************** |
51 | * |
52 | * RFCOMM Multiplexer Sessions sit directly on L2CAP channels, and can |
53 | * multiplex up to 30 incoming and 30 outgoing connections. |
54 | * Only one Multiplexer is allowed between any two devices. |
55 | */ |
56 | |
57 | static void rfcomm_session_timeout(void *); |
58 | static void rfcomm_session_recv_sabm(struct rfcomm_session *, int); |
59 | static void rfcomm_session_recv_disc(struct rfcomm_session *, int); |
60 | static void rfcomm_session_recv_ua(struct rfcomm_session *, int); |
61 | static void rfcomm_session_recv_dm(struct rfcomm_session *, int); |
62 | static void rfcomm_session_recv_uih(struct rfcomm_session *, int, int, struct mbuf *, int); |
63 | static void rfcomm_session_recv_mcc(struct rfcomm_session *, struct mbuf *); |
64 | static void rfcomm_session_recv_mcc_test(struct rfcomm_session *, int, struct mbuf *); |
65 | static void rfcomm_session_recv_mcc_fcon(struct rfcomm_session *, int); |
66 | static void rfcomm_session_recv_mcc_fcoff(struct rfcomm_session *, int); |
67 | static void rfcomm_session_recv_mcc_msc(struct rfcomm_session *, int, struct mbuf *); |
68 | static void rfcomm_session_recv_mcc_rpn(struct rfcomm_session *, int, struct mbuf *); |
69 | static void rfcomm_session_recv_mcc_rls(struct rfcomm_session *, int, struct mbuf *); |
70 | static void rfcomm_session_recv_mcc_pn(struct rfcomm_session *, int, struct mbuf *); |
71 | static void rfcomm_session_recv_mcc_nsc(struct rfcomm_session *, int, struct mbuf *); |
72 | |
73 | /* L2CAP callbacks */ |
74 | static void rfcomm_session_connecting(void *); |
75 | static void rfcomm_session_connected(void *); |
76 | static void rfcomm_session_disconnected(void *, int); |
77 | static void *rfcomm_session_newconn(void *, struct sockaddr_bt *, struct sockaddr_bt *); |
78 | static void rfcomm_session_complete(void *, int); |
79 | static void rfcomm_session_linkmode(void *, int); |
80 | static void rfcomm_session_input(void *, struct mbuf *); |
81 | |
82 | static const struct btproto rfcomm_session_proto = { |
83 | rfcomm_session_connecting, |
84 | rfcomm_session_connected, |
85 | rfcomm_session_disconnected, |
86 | rfcomm_session_newconn, |
87 | rfcomm_session_complete, |
88 | rfcomm_session_linkmode, |
89 | rfcomm_session_input, |
90 | }; |
91 | |
92 | struct rfcomm_session_list |
93 | rfcomm_session_active = LIST_HEAD_INITIALIZER(rfcomm_session_active); |
94 | |
95 | struct rfcomm_session_list |
96 | rfcomm_session_listen = LIST_HEAD_INITIALIZER(rfcomm_session_listen); |
97 | |
98 | static struct pool rfcomm_credit_pool; |
99 | |
100 | /* |
101 | * RFCOMM System Parameters (see section 5.3) |
102 | */ |
103 | int rfcomm_mtu_default = 127; /* bytes */ |
104 | int rfcomm_ack_timeout = 20; /* seconds */ |
105 | int rfcomm_mcc_timeout = 20; /* seconds */ |
106 | |
107 | /* |
108 | * Reversed CRC table as per TS 07.10 Annex B.3.5 |
109 | */ |
110 | static const uint8_t crctable[256] = { /* reversed, 8-bit, poly=0x07 */ |
111 | 0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75, |
112 | 0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b, |
113 | 0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69, |
114 | 0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67, |
115 | |
116 | 0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d, |
117 | 0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43, |
118 | 0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51, |
119 | 0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f, |
120 | |
121 | 0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05, |
122 | 0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b, |
123 | 0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19, |
124 | 0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17, |
125 | |
126 | 0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d, |
127 | 0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33, |
128 | 0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21, |
129 | 0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f, |
130 | |
131 | 0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95, |
132 | 0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b, |
133 | 0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89, |
134 | 0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87, |
135 | |
136 | 0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad, |
137 | 0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3, |
138 | 0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1, |
139 | 0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf, |
140 | |
141 | 0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5, |
142 | 0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb, |
143 | 0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9, |
144 | 0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7, |
145 | |
146 | 0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd, |
147 | 0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3, |
148 | 0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1, |
149 | 0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf |
150 | }; |
151 | |
152 | #define FCS(f, d) crctable[(f) ^ (d)] |
153 | |
154 | void |
155 | rfcomm_init(void) |
156 | { |
157 | |
158 | pool_init(&rfcomm_credit_pool, sizeof(struct rfcomm_credit), |
159 | 0, 0, 0, "rfcomm_credit" , NULL, IPL_SOFTNET); |
160 | } |
161 | |
162 | /* |
163 | * rfcomm_session_alloc(list, sockaddr) |
164 | * |
165 | * allocate a new session and fill in the blanks, then |
166 | * attach session to front of specified list (active or listen) |
167 | */ |
168 | struct rfcomm_session * |
169 | rfcomm_session_alloc(struct rfcomm_session_list *list, |
170 | struct sockaddr_bt *laddr) |
171 | { |
172 | struct rfcomm_session *rs; |
173 | struct sockopt sopt; |
174 | int err; |
175 | |
176 | rs = malloc(sizeof(*rs), M_BLUETOOTH, M_NOWAIT | M_ZERO); |
177 | if (rs == NULL) |
178 | return NULL; |
179 | |
180 | rs->rs_state = RFCOMM_SESSION_CLOSED; |
181 | |
182 | callout_init(&rs->rs_timeout, 0); |
183 | callout_setfunc(&rs->rs_timeout, rfcomm_session_timeout, rs); |
184 | |
185 | SIMPLEQ_INIT(&rs->rs_credits); |
186 | LIST_INIT(&rs->rs_dlcs); |
187 | |
188 | err = l2cap_attach_pcb(&rs->rs_l2cap, &rfcomm_session_proto, rs); |
189 | if (err) { |
190 | free(rs, M_BLUETOOTH); |
191 | return NULL; |
192 | } |
193 | |
194 | sockopt_init(&sopt, BTPROTO_L2CAP, SO_L2CAP_OMTU, 0); |
195 | (void)l2cap_getopt(rs->rs_l2cap, &sopt); |
196 | (void)sockopt_get(&sopt, &rs->rs_mtu, sizeof(rs->rs_mtu)); |
197 | sockopt_destroy(&sopt); |
198 | |
199 | if (laddr->bt_psm == L2CAP_PSM_ANY) |
200 | laddr->bt_psm = L2CAP_PSM_RFCOMM; |
201 | |
202 | (void)l2cap_bind_pcb(rs->rs_l2cap, laddr); |
203 | |
204 | LIST_INSERT_HEAD(list, rs, rs_next); |
205 | |
206 | return rs; |
207 | } |
208 | |
209 | /* |
210 | * rfcomm_session_free(rfcomm_session) |
211 | * |
212 | * release a session, including any cleanup |
213 | */ |
214 | void |
215 | rfcomm_session_free(struct rfcomm_session *rs) |
216 | { |
217 | struct rfcomm_credit *credit; |
218 | |
219 | KASSERT(rs != NULL); |
220 | KASSERT(LIST_EMPTY(&rs->rs_dlcs)); |
221 | |
222 | rs->rs_state = RFCOMM_SESSION_CLOSED; |
223 | |
224 | /* |
225 | * If the callout is already invoked we have no way to stop it, |
226 | * but it will call us back right away (there are no DLC's) so |
227 | * not to worry. |
228 | */ |
229 | callout_stop(&rs->rs_timeout); |
230 | if (callout_invoking(&rs->rs_timeout)) |
231 | return; |
232 | |
233 | /* |
234 | * Take care that rfcomm_session_disconnected() doesnt call |
235 | * us back either as it will do if the l2cap_channel has not |
236 | * been closed when we detach it.. |
237 | */ |
238 | if (rs->rs_flags & RFCOMM_SESSION_FREE) |
239 | return; |
240 | |
241 | rs->rs_flags |= RFCOMM_SESSION_FREE; |
242 | |
243 | /* throw away any remaining credit notes */ |
244 | while ((credit = SIMPLEQ_FIRST(&rs->rs_credits)) != NULL) { |
245 | SIMPLEQ_REMOVE_HEAD(&rs->rs_credits, rc_next); |
246 | pool_put(&rfcomm_credit_pool, credit); |
247 | } |
248 | |
249 | KASSERT(SIMPLEQ_EMPTY(&rs->rs_credits)); |
250 | |
251 | /* Goodbye! */ |
252 | LIST_REMOVE(rs, rs_next); |
253 | l2cap_detach_pcb(&rs->rs_l2cap); |
254 | callout_destroy(&rs->rs_timeout); |
255 | free(rs, M_BLUETOOTH); |
256 | } |
257 | |
258 | /* |
259 | * rfcomm_session_lookup(sockaddr, sockaddr) |
260 | * |
261 | * Find active rfcomm session matching src and dest addresses |
262 | * when src is BDADDR_ANY match any local address |
263 | */ |
264 | struct rfcomm_session * |
265 | rfcomm_session_lookup(struct sockaddr_bt *src, struct sockaddr_bt *dest) |
266 | { |
267 | struct rfcomm_session *rs; |
268 | struct sockaddr_bt addr; |
269 | |
270 | LIST_FOREACH(rs, &rfcomm_session_active, rs_next) { |
271 | if (rs->rs_state == RFCOMM_SESSION_CLOSED) |
272 | continue; |
273 | |
274 | l2cap_sockaddr_pcb(rs->rs_l2cap, &addr); |
275 | |
276 | if (bdaddr_same(&src->bt_bdaddr, &addr.bt_bdaddr) == 0) |
277 | if (bdaddr_any(&src->bt_bdaddr) == 0) |
278 | continue; |
279 | |
280 | l2cap_peeraddr_pcb(rs->rs_l2cap, &addr); |
281 | |
282 | if (addr.bt_psm != dest->bt_psm) |
283 | continue; |
284 | |
285 | if (bdaddr_same(&dest->bt_bdaddr, &addr.bt_bdaddr)) |
286 | break; |
287 | } |
288 | |
289 | return rs; |
290 | } |
291 | |
292 | /* |
293 | * rfcomm_session_timeout(rfcomm_session) |
294 | * |
295 | * Session timeouts are scheduled when a session is left or |
296 | * created with no DLCs, and when SABM(0) or DISC(0) are |
297 | * sent. |
298 | * |
299 | * So, if it is in an open state with DLC's attached then |
300 | * we leave it alone, otherwise the session is lost. |
301 | */ |
302 | static void |
303 | rfcomm_session_timeout(void *arg) |
304 | { |
305 | struct rfcomm_session *rs = arg; |
306 | struct rfcomm_dlc *dlc; |
307 | |
308 | KASSERT(rs != NULL); |
309 | |
310 | mutex_enter(bt_lock); |
311 | callout_ack(&rs->rs_timeout); |
312 | |
313 | if (rs->rs_state != RFCOMM_SESSION_OPEN) { |
314 | DPRINTF("timeout\n" ); |
315 | rs->rs_state = RFCOMM_SESSION_CLOSED; |
316 | |
317 | while (!LIST_EMPTY(&rs->rs_dlcs)) { |
318 | dlc = LIST_FIRST(&rs->rs_dlcs); |
319 | |
320 | rfcomm_dlc_close(dlc, ETIMEDOUT); |
321 | } |
322 | } |
323 | |
324 | if (LIST_EMPTY(&rs->rs_dlcs)) { |
325 | DPRINTF("expiring\n" ); |
326 | rfcomm_session_free(rs); |
327 | } |
328 | mutex_exit(bt_lock); |
329 | } |
330 | |
331 | /*********************************************************************** |
332 | * |
333 | * RFCOMM Session L2CAP protocol callbacks |
334 | * |
335 | */ |
336 | |
337 | static void |
338 | rfcomm_session_connecting(void *arg) |
339 | { |
340 | /* struct rfcomm_session *rs = arg; */ |
341 | |
342 | DPRINTF("Connecting\n" ); |
343 | } |
344 | |
345 | static void |
346 | rfcomm_session_connected(void *arg) |
347 | { |
348 | struct rfcomm_session *rs = arg; |
349 | struct sockopt sopt; |
350 | |
351 | DPRINTF("Connected\n" ); |
352 | |
353 | /* |
354 | * L2CAP is open. |
355 | * |
356 | * If we are initiator, we can send our SABM(0) |
357 | * a timeout should be active? |
358 | * |
359 | * We must take note of the L2CAP MTU because currently |
360 | * the L2CAP implementation can only do Basic Mode. |
361 | */ |
362 | sockopt_init(&sopt, BTPROTO_L2CAP, SO_L2CAP_OMTU, 0); |
363 | (void)l2cap_getopt(rs->rs_l2cap, &sopt); |
364 | (void)sockopt_get(&sopt, &rs->rs_mtu, sizeof(rs->rs_mtu)); |
365 | sockopt_destroy(&sopt); |
366 | |
367 | rs->rs_mtu -= 6; /* (RFCOMM overhead could be this big) */ |
368 | if (rs->rs_mtu < RFCOMM_MTU_MIN) { |
369 | rfcomm_session_disconnected(rs, EINVAL); |
370 | return; |
371 | } |
372 | |
373 | if (IS_INITIATOR(rs)) { |
374 | int err; |
375 | |
376 | err = rfcomm_session_send_frame(rs, RFCOMM_FRAME_SABM, 0); |
377 | if (err) |
378 | rfcomm_session_disconnected(rs, err); |
379 | |
380 | callout_schedule(&rs->rs_timeout, rfcomm_ack_timeout * hz); |
381 | } |
382 | } |
383 | |
384 | static void |
385 | rfcomm_session_disconnected(void *arg, int err) |
386 | { |
387 | struct rfcomm_session *rs = arg; |
388 | struct rfcomm_dlc *dlc; |
389 | |
390 | DPRINTF("Disconnected\n" ); |
391 | |
392 | /* |
393 | * If we have any DLCs outstanding in the unlikely case that the |
394 | * L2CAP channel disconnected normally, close them with an error |
395 | */ |
396 | if (err == 0) |
397 | err = ECONNRESET; |
398 | |
399 | rs->rs_state = RFCOMM_SESSION_CLOSED; |
400 | |
401 | while (!LIST_EMPTY(&rs->rs_dlcs)) { |
402 | dlc = LIST_FIRST(&rs->rs_dlcs); |
403 | |
404 | rfcomm_dlc_close(dlc, err); |
405 | } |
406 | |
407 | rfcomm_session_free(rs); |
408 | } |
409 | |
410 | static void * |
411 | rfcomm_session_newconn(void *arg, struct sockaddr_bt *laddr, |
412 | struct sockaddr_bt *raddr) |
413 | { |
414 | struct rfcomm_session *new, *rs = arg; |
415 | |
416 | DPRINTF("New Connection\n" ); |
417 | |
418 | /* |
419 | * Incoming session connect request. We should return a new |
420 | * session pointer if this is acceptable. The L2CAP layer |
421 | * passes local and remote addresses, which we must check as |
422 | * only one RFCOMM session is allowed between any two devices |
423 | */ |
424 | new = rfcomm_session_lookup(laddr, raddr); |
425 | if (new != NULL) |
426 | return NULL; |
427 | |
428 | new = rfcomm_session_alloc(&rfcomm_session_active, laddr); |
429 | if (new == NULL) |
430 | return NULL; |
431 | |
432 | new->rs_mtu = rs->rs_mtu; |
433 | new->rs_state = RFCOMM_SESSION_WAIT_CONNECT; |
434 | |
435 | /* |
436 | * schedule an expiry so that if nothing comes of it we |
437 | * can punt. |
438 | */ |
439 | callout_schedule(&new->rs_timeout, rfcomm_mcc_timeout * hz); |
440 | |
441 | return new->rs_l2cap; |
442 | } |
443 | |
444 | static void |
445 | rfcomm_session_complete(void *arg, int count) |
446 | { |
447 | struct rfcomm_session *rs = arg; |
448 | struct rfcomm_credit *credit; |
449 | struct rfcomm_dlc *dlc; |
450 | |
451 | /* |
452 | * count L2CAP packets are 'complete', meaning that they are cleared |
453 | * our buffers (for best effort) or arrived safe (for guaranteed) so |
454 | * we can take it off our list and pass the message on, so that |
455 | * eventually the data can be removed from the sockbuf |
456 | */ |
457 | while (count-- > 0) { |
458 | credit = SIMPLEQ_FIRST(&rs->rs_credits); |
459 | if (credit == NULL) { |
460 | printf("%s: too many packets completed!\n" , __func__); |
461 | break; |
462 | } |
463 | |
464 | dlc = credit->rc_dlc; |
465 | if (dlc != NULL) { |
466 | dlc->rd_pending--; |
467 | (*dlc->rd_proto->complete) |
468 | (dlc->rd_upper, credit->rc_len); |
469 | |
470 | /* |
471 | * if not using credit flow control, we may push |
472 | * more data now |
473 | */ |
474 | if ((rs->rs_flags & RFCOMM_SESSION_CFC) == 0 |
475 | && dlc->rd_state == RFCOMM_DLC_OPEN) { |
476 | rfcomm_dlc_start(dlc); |
477 | } |
478 | |
479 | /* |
480 | * When shutdown is indicated, we are just waiting to |
481 | * clear outgoing data. |
482 | */ |
483 | if ((dlc->rd_flags & RFCOMM_DLC_SHUTDOWN) |
484 | && dlc->rd_txbuf == NULL && dlc->rd_pending == 0) { |
485 | dlc->rd_state = RFCOMM_DLC_WAIT_DISCONNECT; |
486 | rfcomm_session_send_frame(rs, RFCOMM_FRAME_DISC, |
487 | dlc->rd_dlci); |
488 | callout_schedule(&dlc->rd_timeout, |
489 | rfcomm_ack_timeout * hz); |
490 | } |
491 | } |
492 | |
493 | SIMPLEQ_REMOVE_HEAD(&rs->rs_credits, rc_next); |
494 | pool_put(&rfcomm_credit_pool, credit); |
495 | } |
496 | |
497 | /* |
498 | * If session is closed, we are just waiting to clear the queue |
499 | */ |
500 | if (rs->rs_state == RFCOMM_SESSION_CLOSED) { |
501 | if (SIMPLEQ_EMPTY(&rs->rs_credits)) |
502 | l2cap_disconnect_pcb(rs->rs_l2cap, 0); |
503 | } |
504 | } |
505 | |
506 | /* |
507 | * Link Mode changed |
508 | * |
509 | * This is called when a mode change is complete. Proceed with connections |
510 | * where appropriate, or pass the new mode to any active DLCs. |
511 | */ |
512 | static void |
513 | rfcomm_session_linkmode(void *arg, int new) |
514 | { |
515 | struct rfcomm_session *rs = arg; |
516 | struct rfcomm_dlc *dlc, *next; |
517 | int err, mode = 0; |
518 | |
519 | DPRINTF("auth %s, encrypt %s, secure %s\n" , |
520 | (new & L2CAP_LM_AUTH ? "on" : "off" ), |
521 | (new & L2CAP_LM_ENCRYPT ? "on" : "off" ), |
522 | (new & L2CAP_LM_SECURE ? "on" : "off" )); |
523 | |
524 | if (new & L2CAP_LM_AUTH) |
525 | mode |= RFCOMM_LM_AUTH; |
526 | |
527 | if (new & L2CAP_LM_ENCRYPT) |
528 | mode |= RFCOMM_LM_ENCRYPT; |
529 | |
530 | if (new & L2CAP_LM_SECURE) |
531 | mode |= RFCOMM_LM_SECURE; |
532 | |
533 | next = LIST_FIRST(&rs->rs_dlcs); |
534 | while ((dlc = next) != NULL) { |
535 | next = LIST_NEXT(dlc, rd_next); |
536 | |
537 | switch (dlc->rd_state) { |
538 | case RFCOMM_DLC_WAIT_SEND_SABM: /* we are connecting */ |
539 | if ((mode & dlc->rd_mode) != dlc->rd_mode) { |
540 | rfcomm_dlc_close(dlc, ECONNABORTED); |
541 | } else { |
542 | err = rfcomm_session_send_frame(rs, |
543 | RFCOMM_FRAME_SABM, dlc->rd_dlci); |
544 | if (err) { |
545 | rfcomm_dlc_close(dlc, err); |
546 | } else { |
547 | dlc->rd_state = RFCOMM_DLC_WAIT_RECV_UA; |
548 | callout_schedule(&dlc->rd_timeout, |
549 | rfcomm_ack_timeout * hz); |
550 | break; |
551 | } |
552 | } |
553 | |
554 | /* |
555 | * If we aborted the connection and there are no more DLCs |
556 | * on the session, it is our responsibility to disconnect. |
557 | */ |
558 | if (!LIST_EMPTY(&rs->rs_dlcs)) |
559 | break; |
560 | |
561 | rs->rs_state = RFCOMM_SESSION_WAIT_DISCONNECT; |
562 | rfcomm_session_send_frame(rs, RFCOMM_FRAME_DISC, 0); |
563 | callout_schedule(&rs->rs_timeout, rfcomm_ack_timeout * hz); |
564 | break; |
565 | |
566 | case RFCOMM_DLC_WAIT_SEND_UA: /* they are connecting */ |
567 | if ((mode & dlc->rd_mode) != dlc->rd_mode) { |
568 | rfcomm_session_send_frame(rs, |
569 | RFCOMM_FRAME_DM, dlc->rd_dlci); |
570 | rfcomm_dlc_close(dlc, ECONNABORTED); |
571 | break; |
572 | } |
573 | |
574 | err = rfcomm_session_send_frame(rs, |
575 | RFCOMM_FRAME_UA, dlc->rd_dlci); |
576 | if (err) { |
577 | rfcomm_session_send_frame(rs, |
578 | RFCOMM_FRAME_DM, dlc->rd_dlci); |
579 | rfcomm_dlc_close(dlc, err); |
580 | break; |
581 | } |
582 | |
583 | err = rfcomm_dlc_open(dlc); |
584 | if (err) { |
585 | rfcomm_session_send_frame(rs, |
586 | RFCOMM_FRAME_DM, dlc->rd_dlci); |
587 | rfcomm_dlc_close(dlc, err); |
588 | break; |
589 | } |
590 | |
591 | break; |
592 | |
593 | case RFCOMM_DLC_WAIT_RECV_UA: |
594 | case RFCOMM_DLC_OPEN: /* already established */ |
595 | (*dlc->rd_proto->linkmode)(dlc->rd_upper, mode); |
596 | break; |
597 | |
598 | default: |
599 | break; |
600 | } |
601 | } |
602 | } |
603 | |
604 | /* |
605 | * Receive data from L2CAP layer for session. There is always exactly one |
606 | * RFCOMM frame contained in each L2CAP frame. |
607 | */ |
608 | static void |
609 | rfcomm_session_input(void *arg, struct mbuf *m) |
610 | { |
611 | struct rfcomm_session *rs = arg; |
612 | int dlci, len, type, pf; |
613 | uint8_t fcs, b; |
614 | |
615 | KASSERT(m != NULL); |
616 | KASSERT(rs != NULL); |
617 | |
618 | /* |
619 | * UIH frames: FCS is only calculated on address and control fields |
620 | * For other frames: FCS is calculated on address, control and length |
621 | * Length may extend to two octets |
622 | */ |
623 | fcs = 0xff; |
624 | |
625 | if (m->m_pkthdr.len < 4) { |
626 | DPRINTF("short frame (%d), discarded\n" , m->m_pkthdr.len); |
627 | goto done; |
628 | } |
629 | |
630 | /* address - one octet */ |
631 | m_copydata(m, 0, 1, &b); |
632 | m_adj(m, 1); |
633 | fcs = FCS(fcs, b); |
634 | dlci = RFCOMM_DLCI(b); |
635 | |
636 | /* control - one octet */ |
637 | m_copydata(m, 0, 1, &b); |
638 | m_adj(m, 1); |
639 | fcs = FCS(fcs, b); |
640 | type = RFCOMM_TYPE(b); |
641 | pf = RFCOMM_PF(b); |
642 | |
643 | /* length - may be two octets */ |
644 | m_copydata(m, 0, 1, &b); |
645 | m_adj(m, 1); |
646 | if (type != RFCOMM_FRAME_UIH) |
647 | fcs = FCS(fcs, b); |
648 | len = (b >> 1) & 0x7f; |
649 | |
650 | if (RFCOMM_EA(b) == 0) { |
651 | if (m->m_pkthdr.len < 2) { |
652 | DPRINTF("short frame (%d, EA = 0), discarded\n" , |
653 | m->m_pkthdr.len); |
654 | goto done; |
655 | } |
656 | |
657 | m_copydata(m, 0, 1, &b); |
658 | m_adj(m, 1); |
659 | if (type != RFCOMM_FRAME_UIH) |
660 | fcs = FCS(fcs, b); |
661 | |
662 | len |= (b << 7); |
663 | } |
664 | |
665 | /* FCS byte is last octet in frame */ |
666 | m_copydata(m, m->m_pkthdr.len - 1, 1, &b); |
667 | m_adj(m, -1); |
668 | fcs = FCS(fcs, b); |
669 | |
670 | if (fcs != 0xcf) { |
671 | DPRINTF("Bad FCS value (%#2.2x), frame discarded\n" , fcs); |
672 | goto done; |
673 | } |
674 | |
675 | DPRINTFN(10, "dlci %d, type %2.2x, len = %d\n" , dlci, type, len); |
676 | |
677 | switch (type) { |
678 | case RFCOMM_FRAME_SABM: |
679 | if (pf) |
680 | rfcomm_session_recv_sabm(rs, dlci); |
681 | break; |
682 | |
683 | case RFCOMM_FRAME_DISC: |
684 | if (pf) |
685 | rfcomm_session_recv_disc(rs, dlci); |
686 | break; |
687 | |
688 | case RFCOMM_FRAME_UA: |
689 | if (pf) |
690 | rfcomm_session_recv_ua(rs, dlci); |
691 | break; |
692 | |
693 | case RFCOMM_FRAME_DM: |
694 | rfcomm_session_recv_dm(rs, dlci); |
695 | break; |
696 | |
697 | case RFCOMM_FRAME_UIH: |
698 | rfcomm_session_recv_uih(rs, dlci, pf, m, len); |
699 | return; /* (no release) */ |
700 | |
701 | default: |
702 | UNKNOWN(type); |
703 | break; |
704 | } |
705 | |
706 | done: |
707 | m_freem(m); |
708 | } |
709 | |
710 | /*********************************************************************** |
711 | * |
712 | * RFCOMM Session receive processing |
713 | */ |
714 | |
715 | /* |
716 | * rfcomm_session_recv_sabm(rfcomm_session, dlci) |
717 | * |
718 | * Set Asyncrhonous Balanced Mode - open the channel. |
719 | */ |
720 | static void |
721 | rfcomm_session_recv_sabm(struct rfcomm_session *rs, int dlci) |
722 | { |
723 | struct rfcomm_dlc *dlc; |
724 | int err; |
725 | |
726 | DPRINTFN(5, "SABM(%d)\n" , dlci); |
727 | |
728 | if (dlci == 0) { /* Open Session */ |
729 | rs->rs_state = RFCOMM_SESSION_OPEN; |
730 | rfcomm_session_send_frame(rs, RFCOMM_FRAME_UA, 0); |
731 | LIST_FOREACH(dlc, &rs->rs_dlcs, rd_next) { |
732 | if (dlc->rd_state == RFCOMM_DLC_WAIT_SESSION) |
733 | rfcomm_dlc_connect(dlc); |
734 | } |
735 | return; |
736 | } |
737 | |
738 | if (rs->rs_state != RFCOMM_SESSION_OPEN) { |
739 | DPRINTF("session was not even open!\n" ); |
740 | return; |
741 | } |
742 | |
743 | /* validate direction bit */ |
744 | if ((IS_INITIATOR(rs) && !RFCOMM_DIRECTION(dlci)) |
745 | || (!IS_INITIATOR(rs) && RFCOMM_DIRECTION(dlci))) { |
746 | DPRINTF("Invalid direction bit on DLCI\n" ); |
747 | return; |
748 | } |
749 | |
750 | /* |
751 | * look for our DLC - this may exist if we received PN |
752 | * already, or we may have to fabricate a new one. |
753 | */ |
754 | dlc = rfcomm_dlc_lookup(rs, dlci); |
755 | if (dlc == NULL) { |
756 | dlc = rfcomm_dlc_newconn(rs, dlci); |
757 | if (dlc == NULL) |
758 | return; /* (DM is sent) */ |
759 | } |
760 | |
761 | /* |
762 | * ..but if this DLC is not waiting to connect, they did |
763 | * something wrong, ignore it. |
764 | */ |
765 | if (dlc->rd_state != RFCOMM_DLC_WAIT_CONNECT) |
766 | return; |
767 | |
768 | /* set link mode */ |
769 | err = rfcomm_dlc_setmode(dlc); |
770 | if (err == EINPROGRESS) { |
771 | dlc->rd_state = RFCOMM_DLC_WAIT_SEND_UA; |
772 | (*dlc->rd_proto->connecting)(dlc->rd_upper); |
773 | return; |
774 | } |
775 | if (err) |
776 | goto close; |
777 | |
778 | err = rfcomm_session_send_frame(rs, RFCOMM_FRAME_UA, dlci); |
779 | if (err) |
780 | goto close; |
781 | |
782 | /* and mark it open */ |
783 | err = rfcomm_dlc_open(dlc); |
784 | if (err) |
785 | goto close; |
786 | |
787 | return; |
788 | |
789 | close: |
790 | rfcomm_dlc_close(dlc, err); |
791 | } |
792 | |
793 | /* |
794 | * Receive Disconnect Command |
795 | */ |
796 | static void |
797 | rfcomm_session_recv_disc(struct rfcomm_session *rs, int dlci) |
798 | { |
799 | struct rfcomm_dlc *dlc; |
800 | |
801 | DPRINTFN(5, "DISC(%d)\n" , dlci); |
802 | |
803 | if (dlci == 0) { |
804 | /* |
805 | * Disconnect Session |
806 | * |
807 | * We set the session state to CLOSED so that when |
808 | * the UA frame is clear the session will be closed |
809 | * automatically. We wont bother to close any DLC's |
810 | * just yet as there should be none. In the unlikely |
811 | * event that something is left, it will get flushed |
812 | * out as the session goes down. |
813 | */ |
814 | rfcomm_session_send_frame(rs, RFCOMM_FRAME_UA, 0); |
815 | rs->rs_state = RFCOMM_SESSION_CLOSED; |
816 | return; |
817 | } |
818 | |
819 | dlc = rfcomm_dlc_lookup(rs, dlci); |
820 | if (dlc == NULL) { |
821 | rfcomm_session_send_frame(rs, RFCOMM_FRAME_DM, dlci); |
822 | return; |
823 | } |
824 | |
825 | rfcomm_dlc_close(dlc, 0); |
826 | rfcomm_session_send_frame(rs, RFCOMM_FRAME_UA, dlci); |
827 | } |
828 | |
829 | /* |
830 | * Receive Unnumbered Acknowledgement Response |
831 | * |
832 | * This should be a response to a DISC or SABM frame that we |
833 | * have previously sent. If unexpected, ignore it. |
834 | */ |
835 | static void |
836 | rfcomm_session_recv_ua(struct rfcomm_session *rs, int dlci) |
837 | { |
838 | struct rfcomm_dlc *dlc; |
839 | |
840 | DPRINTFN(5, "UA(%d)\n" , dlci); |
841 | |
842 | if (dlci == 0) { |
843 | switch (rs->rs_state) { |
844 | case RFCOMM_SESSION_WAIT_CONNECT: /* We sent SABM */ |
845 | callout_stop(&rs->rs_timeout); |
846 | rs->rs_state = RFCOMM_SESSION_OPEN; |
847 | LIST_FOREACH(dlc, &rs->rs_dlcs, rd_next) { |
848 | if (dlc->rd_state == RFCOMM_DLC_WAIT_SESSION) |
849 | rfcomm_dlc_connect(dlc); |
850 | } |
851 | break; |
852 | |
853 | case RFCOMM_SESSION_WAIT_DISCONNECT: /* We sent DISC */ |
854 | callout_stop(&rs->rs_timeout); |
855 | rs->rs_state = RFCOMM_SESSION_CLOSED; |
856 | l2cap_disconnect_pcb(rs->rs_l2cap, 0); |
857 | break; |
858 | |
859 | default: |
860 | DPRINTF("Received spurious UA(0)!\n" ); |
861 | break; |
862 | } |
863 | |
864 | return; |
865 | } |
866 | |
867 | /* |
868 | * If we have no DLC on this dlci, we may have aborted |
869 | * without shutting down properly, so check if the session |
870 | * needs disconnecting. |
871 | */ |
872 | dlc = rfcomm_dlc_lookup(rs, dlci); |
873 | if (dlc == NULL) |
874 | goto check; |
875 | |
876 | switch (dlc->rd_state) { |
877 | case RFCOMM_DLC_WAIT_RECV_UA: /* We sent SABM */ |
878 | rfcomm_dlc_open(dlc); |
879 | return; |
880 | |
881 | case RFCOMM_DLC_WAIT_DISCONNECT: /* We sent DISC */ |
882 | rfcomm_dlc_close(dlc, 0); |
883 | break; |
884 | |
885 | default: |
886 | DPRINTF("Received spurious UA(%d)!\n" , dlci); |
887 | return; |
888 | } |
889 | |
890 | check: /* last one out turns out the light */ |
891 | if (LIST_EMPTY(&rs->rs_dlcs)) { |
892 | rs->rs_state = RFCOMM_SESSION_WAIT_DISCONNECT; |
893 | rfcomm_session_send_frame(rs, RFCOMM_FRAME_DISC, 0); |
894 | callout_schedule(&rs->rs_timeout, rfcomm_ack_timeout * hz); |
895 | } |
896 | } |
897 | |
898 | /* |
899 | * Receive Disconnected Mode Response |
900 | * |
901 | * If this does not apply to a known DLC then we may ignore it. |
902 | */ |
903 | static void |
904 | rfcomm_session_recv_dm(struct rfcomm_session *rs, int dlci) |
905 | { |
906 | struct rfcomm_dlc *dlc; |
907 | |
908 | DPRINTFN(5, "DM(%d)\n" , dlci); |
909 | |
910 | dlc = rfcomm_dlc_lookup(rs, dlci); |
911 | if (dlc == NULL) |
912 | return; |
913 | |
914 | if (dlc->rd_state == RFCOMM_DLC_WAIT_CONNECT) |
915 | rfcomm_dlc_close(dlc, ECONNREFUSED); |
916 | else |
917 | rfcomm_dlc_close(dlc, ECONNRESET); |
918 | } |
919 | |
920 | /* |
921 | * Receive Unnumbered Information with Header check (MCC or data packet) |
922 | */ |
923 | static void |
924 | rfcomm_session_recv_uih(struct rfcomm_session *rs, int dlci, |
925 | int pf, struct mbuf *m, int len) |
926 | { |
927 | struct rfcomm_dlc *dlc; |
928 | uint8_t credits = 0; |
929 | |
930 | DPRINTFN(10, "UIH(%d)\n" , dlci); |
931 | |
932 | if (dlci == 0) { |
933 | rfcomm_session_recv_mcc(rs, m); |
934 | return; |
935 | } |
936 | |
937 | if (m->m_pkthdr.len != len + pf) { |
938 | DPRINTF("Bad Frame Length (%d), frame discarded\n" , |
939 | m->m_pkthdr.len); |
940 | |
941 | goto discard; |
942 | } |
943 | |
944 | dlc = rfcomm_dlc_lookup(rs, dlci); |
945 | if (dlc == NULL) { |
946 | DPRINTF("UIH received for non existent DLC, discarded\n" ); |
947 | rfcomm_session_send_frame(rs, RFCOMM_FRAME_DM, dlci); |
948 | goto discard; |
949 | } |
950 | |
951 | if (dlc->rd_state != RFCOMM_DLC_OPEN) { |
952 | DPRINTF("non-open DLC (state = %d), discarded\n" , |
953 | dlc->rd_state); |
954 | goto discard; |
955 | } |
956 | |
957 | /* if PF is set, credits were included */ |
958 | if (rs->rs_flags & RFCOMM_SESSION_CFC) { |
959 | if (pf != 0) { |
960 | if (m->m_pkthdr.len < sizeof(credits)) { |
961 | DPRINTF("Bad PF value, UIH discarded\n" ); |
962 | goto discard; |
963 | } |
964 | |
965 | m_copydata(m, 0, sizeof(credits), &credits); |
966 | m_adj(m, sizeof(credits)); |
967 | |
968 | dlc->rd_txcred += credits; |
969 | |
970 | if (credits > 0 && dlc->rd_txbuf != NULL) |
971 | rfcomm_dlc_start(dlc); |
972 | } |
973 | |
974 | if (len == 0) |
975 | goto discard; |
976 | |
977 | if (dlc->rd_rxcred == 0) { |
978 | DPRINTF("Credit limit reached, UIH discarded\n" ); |
979 | goto discard; |
980 | } |
981 | |
982 | if (len > dlc->rd_rxsize) { |
983 | DPRINTF("UIH frame exceeds rxsize, discarded\n" ); |
984 | goto discard; |
985 | } |
986 | |
987 | dlc->rd_rxcred--; |
988 | dlc->rd_rxsize -= len; |
989 | } |
990 | |
991 | (*dlc->rd_proto->input)(dlc->rd_upper, m); |
992 | return; |
993 | |
994 | discard: |
995 | m_freem(m); |
996 | } |
997 | |
998 | /* |
999 | * Receive Multiplexer Control Command |
1000 | */ |
1001 | static void |
1002 | rfcomm_session_recv_mcc(struct rfcomm_session *rs, struct mbuf *m) |
1003 | { |
1004 | int type, cr, len; |
1005 | uint8_t b; |
1006 | |
1007 | /* |
1008 | * Extract MCC header. |
1009 | * |
1010 | * Fields are variable length using extension bit = 1 to signify the |
1011 | * last octet in the sequence. |
1012 | * |
1013 | * Only single octet types are defined in TS 07.10/RFCOMM spec |
1014 | * |
1015 | * Length can realistically only use 15 bits (max RFCOMM MTU) |
1016 | */ |
1017 | if (m->m_pkthdr.len < sizeof(b)) { |
1018 | DPRINTF("Short MCC header, discarded\n" ); |
1019 | goto release; |
1020 | } |
1021 | |
1022 | m_copydata(m, 0, sizeof(b), &b); |
1023 | m_adj(m, sizeof(b)); |
1024 | |
1025 | if (RFCOMM_EA(b) == 0) { /* verify no extensions */ |
1026 | DPRINTF("MCC type EA = 0, discarded\n" ); |
1027 | goto release; |
1028 | } |
1029 | |
1030 | type = RFCOMM_MCC_TYPE(b); |
1031 | cr = RFCOMM_CR(b); |
1032 | |
1033 | len = 0; |
1034 | do { |
1035 | if (m->m_pkthdr.len < sizeof(b)) { |
1036 | DPRINTF("Short MCC header, discarded\n" ); |
1037 | goto release; |
1038 | } |
1039 | |
1040 | m_copydata(m, 0, sizeof(b), &b); |
1041 | m_adj(m, sizeof(b)); |
1042 | |
1043 | len = (len << 7) | (b >> 1); |
1044 | len = min(len, RFCOMM_MTU_MAX); |
1045 | } while (RFCOMM_EA(b) == 0); |
1046 | |
1047 | if (len != m->m_pkthdr.len) { |
1048 | DPRINTF("Incorrect MCC length, discarded\n" ); |
1049 | goto release; |
1050 | } |
1051 | |
1052 | DPRINTFN(2, "MCC %s type %2.2x (%d bytes)\n" , |
1053 | (cr ? "command" : "response" ), type, len); |
1054 | |
1055 | /* |
1056 | * pass to command handler |
1057 | */ |
1058 | switch(type) { |
1059 | case RFCOMM_MCC_TEST: /* Test */ |
1060 | rfcomm_session_recv_mcc_test(rs, cr, m); |
1061 | break; |
1062 | |
1063 | case RFCOMM_MCC_FCON: /* Flow Control On */ |
1064 | rfcomm_session_recv_mcc_fcon(rs, cr); |
1065 | break; |
1066 | |
1067 | case RFCOMM_MCC_FCOFF: /* Flow Control Off */ |
1068 | rfcomm_session_recv_mcc_fcoff(rs, cr); |
1069 | break; |
1070 | |
1071 | case RFCOMM_MCC_MSC: /* Modem Status Command */ |
1072 | rfcomm_session_recv_mcc_msc(rs, cr, m); |
1073 | break; |
1074 | |
1075 | case RFCOMM_MCC_RPN: /* Remote Port Negotiation */ |
1076 | rfcomm_session_recv_mcc_rpn(rs, cr, m); |
1077 | break; |
1078 | |
1079 | case RFCOMM_MCC_RLS: /* Remote Line Status */ |
1080 | rfcomm_session_recv_mcc_rls(rs, cr, m); |
1081 | break; |
1082 | |
1083 | case RFCOMM_MCC_PN: /* Parameter Negotiation */ |
1084 | rfcomm_session_recv_mcc_pn(rs, cr, m); |
1085 | break; |
1086 | |
1087 | case RFCOMM_MCC_NSC: /* Non Supported Command */ |
1088 | rfcomm_session_recv_mcc_nsc(rs, cr, m); |
1089 | break; |
1090 | |
1091 | default: |
1092 | b = RFCOMM_MKMCC_TYPE(cr, type); |
1093 | rfcomm_session_send_mcc(rs, 0, RFCOMM_MCC_NSC, &b, sizeof(b)); |
1094 | } |
1095 | |
1096 | release: |
1097 | m_freem(m); |
1098 | } |
1099 | |
1100 | /* |
1101 | * process TEST command/response |
1102 | */ |
1103 | static void |
1104 | rfcomm_session_recv_mcc_test(struct rfcomm_session *rs, int cr, struct mbuf *m) |
1105 | { |
1106 | void *data; |
1107 | int len; |
1108 | |
1109 | if (cr == 0) /* ignore ack */ |
1110 | return; |
1111 | |
1112 | /* |
1113 | * we must send all the data they included back as is |
1114 | */ |
1115 | |
1116 | len = m->m_pkthdr.len; |
1117 | if (len > RFCOMM_MTU_MAX) |
1118 | return; |
1119 | |
1120 | data = malloc(len, M_BLUETOOTH, M_NOWAIT); |
1121 | if (data == NULL) |
1122 | return; |
1123 | |
1124 | m_copydata(m, 0, len, data); |
1125 | rfcomm_session_send_mcc(rs, 0, RFCOMM_MCC_TEST, data, len); |
1126 | free(data, M_BLUETOOTH); |
1127 | } |
1128 | |
1129 | /* |
1130 | * process Flow Control ON command/response |
1131 | */ |
1132 | static void |
1133 | rfcomm_session_recv_mcc_fcon(struct rfcomm_session *rs, int cr) |
1134 | { |
1135 | |
1136 | if (cr == 0) /* ignore ack */ |
1137 | return; |
1138 | |
1139 | rs->rs_flags |= RFCOMM_SESSION_RFC; |
1140 | rfcomm_session_send_mcc(rs, 0, RFCOMM_MCC_FCON, NULL, 0); |
1141 | } |
1142 | |
1143 | /* |
1144 | * process Flow Control OFF command/response |
1145 | */ |
1146 | static void |
1147 | rfcomm_session_recv_mcc_fcoff(struct rfcomm_session *rs, int cr) |
1148 | { |
1149 | |
1150 | if (cr == 0) /* ignore ack */ |
1151 | return; |
1152 | |
1153 | rs->rs_flags &= ~RFCOMM_SESSION_RFC; |
1154 | rfcomm_session_send_mcc(rs, 0, RFCOMM_MCC_FCOFF, NULL, 0); |
1155 | } |
1156 | |
1157 | /* |
1158 | * process Modem Status Command command/response |
1159 | */ |
1160 | static void |
1161 | rfcomm_session_recv_mcc_msc(struct rfcomm_session *rs, int cr, struct mbuf *m) |
1162 | { |
1163 | struct rfcomm_mcc_msc msc; /* (3 octets) */ |
1164 | struct rfcomm_dlc *dlc; |
1165 | int len = 0; |
1166 | |
1167 | /* [ADDRESS] */ |
1168 | if (m->m_pkthdr.len < sizeof(msc.address)) |
1169 | return; |
1170 | |
1171 | m_copydata(m, 0, sizeof(msc.address), &msc.address); |
1172 | m_adj(m, sizeof(msc.address)); |
1173 | len += sizeof(msc.address); |
1174 | |
1175 | dlc = rfcomm_dlc_lookup(rs, RFCOMM_DLCI(msc.address)); |
1176 | |
1177 | if (cr == 0) { /* ignore acks */ |
1178 | if (dlc != NULL) |
1179 | callout_stop(&dlc->rd_timeout); |
1180 | |
1181 | return; |
1182 | } |
1183 | |
1184 | if (dlc == NULL) { |
1185 | rfcomm_session_send_frame(rs, RFCOMM_FRAME_DM, |
1186 | RFCOMM_DLCI(msc.address)); |
1187 | return; |
1188 | } |
1189 | |
1190 | /* [SIGNALS] */ |
1191 | if (m->m_pkthdr.len < sizeof(msc.modem)) |
1192 | return; |
1193 | |
1194 | m_copydata(m, 0, sizeof(msc.modem), &msc.modem); |
1195 | m_adj(m, sizeof(msc.modem)); |
1196 | len += sizeof(msc.modem); |
1197 | |
1198 | dlc->rd_rmodem = msc.modem; |
1199 | /* XXX how do we signal this upstream? */ |
1200 | |
1201 | if (RFCOMM_EA(msc.modem) == 0) { |
1202 | if (m->m_pkthdr.len < sizeof(msc.brk)) |
1203 | return; |
1204 | |
1205 | m_copydata(m, 0, sizeof(msc.brk), &msc.brk); |
1206 | m_adj(m, sizeof(msc.brk)); |
1207 | len += sizeof(msc.brk); |
1208 | |
1209 | /* XXX how do we signal this upstream? */ |
1210 | } |
1211 | |
1212 | rfcomm_session_send_mcc(rs, 0, RFCOMM_MCC_MSC, &msc, len); |
1213 | } |
1214 | |
1215 | /* |
1216 | * process Remote Port Negotiation command/response |
1217 | */ |
1218 | static void |
1219 | rfcomm_session_recv_mcc_rpn(struct rfcomm_session *rs, int cr, struct mbuf *m) |
1220 | { |
1221 | struct rfcomm_mcc_rpn rpn; |
1222 | uint16_t mask; |
1223 | |
1224 | if (cr == 0) /* ignore ack */ |
1225 | return; |
1226 | |
1227 | /* default values */ |
1228 | rpn.bit_rate = RFCOMM_RPN_BR_9600; |
1229 | rpn.line_settings = RFCOMM_RPN_8_N_1; |
1230 | rpn.flow_control = RFCOMM_RPN_FLOW_NONE; |
1231 | rpn.xon_char = RFCOMM_RPN_XON_CHAR; |
1232 | rpn.xoff_char = RFCOMM_RPN_XOFF_CHAR; |
1233 | |
1234 | if (m->m_pkthdr.len == sizeof(rpn)) { |
1235 | m_copydata(m, 0, sizeof(rpn), &rpn); |
1236 | rpn.param_mask = RFCOMM_RPN_PM_ALL; |
1237 | } else if (m->m_pkthdr.len == 1) { |
1238 | m_copydata(m, 0, 1, &rpn); |
1239 | rpn.param_mask = le16toh(rpn.param_mask); |
1240 | } else { |
1241 | DPRINTF("Bad RPN length (%d)\n" , m->m_pkthdr.len); |
1242 | return; |
1243 | } |
1244 | |
1245 | mask = 0; |
1246 | |
1247 | if (rpn.param_mask & RFCOMM_RPN_PM_RATE) |
1248 | mask |= RFCOMM_RPN_PM_RATE; |
1249 | |
1250 | if (rpn.param_mask & RFCOMM_RPN_PM_DATA |
1251 | && RFCOMM_RPN_DATA_BITS(rpn.line_settings) == RFCOMM_RPN_DATA_8) |
1252 | mask |= RFCOMM_RPN_PM_DATA; |
1253 | |
1254 | if (rpn.param_mask & RFCOMM_RPN_PM_STOP |
1255 | && RFCOMM_RPN_STOP_BITS(rpn.line_settings) == RFCOMM_RPN_STOP_1) |
1256 | mask |= RFCOMM_RPN_PM_STOP; |
1257 | |
1258 | if (rpn.param_mask & RFCOMM_RPN_PM_PARITY |
1259 | && RFCOMM_RPN_PARITY(rpn.line_settings) == RFCOMM_RPN_PARITY_NONE) |
1260 | mask |= RFCOMM_RPN_PM_PARITY; |
1261 | |
1262 | if (rpn.param_mask & RFCOMM_RPN_PM_XON |
1263 | && rpn.xon_char == RFCOMM_RPN_XON_CHAR) |
1264 | mask |= RFCOMM_RPN_PM_XON; |
1265 | |
1266 | if (rpn.param_mask & RFCOMM_RPN_PM_XOFF |
1267 | && rpn.xoff_char == RFCOMM_RPN_XOFF_CHAR) |
1268 | mask |= RFCOMM_RPN_PM_XOFF; |
1269 | |
1270 | if (rpn.param_mask & RFCOMM_RPN_PM_FLOW |
1271 | && rpn.flow_control == RFCOMM_RPN_FLOW_NONE) |
1272 | mask |= RFCOMM_RPN_PM_FLOW; |
1273 | |
1274 | rpn.param_mask = htole16(mask); |
1275 | |
1276 | rfcomm_session_send_mcc(rs, 0, RFCOMM_MCC_RPN, &rpn, sizeof(rpn)); |
1277 | } |
1278 | |
1279 | /* |
1280 | * process Remote Line Status command/response |
1281 | */ |
1282 | static void |
1283 | rfcomm_session_recv_mcc_rls(struct rfcomm_session *rs, int cr, struct mbuf *m) |
1284 | { |
1285 | struct rfcomm_mcc_rls rls; |
1286 | |
1287 | if (cr == 0) /* ignore ack */ |
1288 | return; |
1289 | |
1290 | if (m->m_pkthdr.len != sizeof(rls)) { |
1291 | DPRINTF("Bad RLS length %d\n" , m->m_pkthdr.len); |
1292 | return; |
1293 | } |
1294 | |
1295 | m_copydata(m, 0, sizeof(rls), &rls); |
1296 | |
1297 | /* |
1298 | * So far as I can tell, we just send back what |
1299 | * they sent us. This signifies errors that seem |
1300 | * irrelevent for RFCOMM over L2CAP. |
1301 | */ |
1302 | rls.address |= 0x03; /* EA = 1, CR = 1 */ |
1303 | rls.status &= 0x0f; /* only 4 bits valid */ |
1304 | |
1305 | rfcomm_session_send_mcc(rs, 0, RFCOMM_MCC_RLS, &rls, sizeof(rls)); |
1306 | } |
1307 | |
1308 | /* |
1309 | * process Parameter Negotiation command/response |
1310 | */ |
1311 | static void |
1312 | rfcomm_session_recv_mcc_pn(struct rfcomm_session *rs, int cr, struct mbuf *m) |
1313 | { |
1314 | struct rfcomm_dlc *dlc; |
1315 | struct rfcomm_mcc_pn pn; |
1316 | int err; |
1317 | |
1318 | if (m->m_pkthdr.len != sizeof(pn)) { |
1319 | DPRINTF("Bad PN length %d\n" , m->m_pkthdr.len); |
1320 | return; |
1321 | } |
1322 | |
1323 | m_copydata(m, 0, sizeof(pn), &pn); |
1324 | |
1325 | pn.dlci &= 0x3f; |
1326 | pn.mtu = le16toh(pn.mtu); |
1327 | |
1328 | dlc = rfcomm_dlc_lookup(rs, pn.dlci); |
1329 | if (cr) { /* Command */ |
1330 | /* |
1331 | * If there is no DLC present, this is a new |
1332 | * connection so attempt to make one |
1333 | */ |
1334 | if (dlc == NULL) { |
1335 | dlc = rfcomm_dlc_newconn(rs, pn.dlci); |
1336 | if (dlc == NULL) |
1337 | return; /* (DM is sent) */ |
1338 | } |
1339 | |
1340 | /* accept any valid MTU, and offer it back */ |
1341 | pn.mtu = min(pn.mtu, RFCOMM_MTU_MAX); |
1342 | pn.mtu = min(pn.mtu, rs->rs_mtu); |
1343 | pn.mtu = max(pn.mtu, RFCOMM_MTU_MIN); |
1344 | dlc->rd_mtu = pn.mtu; |
1345 | pn.mtu = htole16(pn.mtu); |
1346 | |
1347 | /* credits are only set before DLC is open */ |
1348 | if (dlc->rd_state == RFCOMM_DLC_WAIT_CONNECT |
1349 | && (pn.flow_control & 0xf0) == 0xf0) { |
1350 | rs->rs_flags |= RFCOMM_SESSION_CFC; |
1351 | dlc->rd_txcred = pn.credits & 0x07; |
1352 | |
1353 | dlc->rd_rxcred = (dlc->rd_rxsize / dlc->rd_mtu); |
1354 | dlc->rd_rxcred = min(dlc->rd_rxcred, |
1355 | RFCOMM_CREDITS_DEFAULT); |
1356 | |
1357 | pn.flow_control = 0xe0; |
1358 | pn.credits = dlc->rd_rxcred; |
1359 | } else { |
1360 | pn.flow_control = 0x00; |
1361 | pn.credits = 0x00; |
1362 | } |
1363 | |
1364 | /* unused fields must be ignored and set to zero */ |
1365 | pn.ack_timer = 0; |
1366 | pn.max_retrans = 0; |
1367 | |
1368 | /* send our response */ |
1369 | err = rfcomm_session_send_mcc(rs, 0, |
1370 | RFCOMM_MCC_PN, &pn, sizeof(pn)); |
1371 | if (err) |
1372 | goto close; |
1373 | |
1374 | } else { /* Response */ |
1375 | /* ignore responses with no matching DLC */ |
1376 | if (dlc == NULL) |
1377 | return; |
1378 | |
1379 | callout_stop(&dlc->rd_timeout); |
1380 | |
1381 | /* reject invalid or unacceptable MTU */ |
1382 | if (pn.mtu < RFCOMM_MTU_MIN || pn.mtu > dlc->rd_mtu) { |
1383 | dlc->rd_state = RFCOMM_DLC_WAIT_DISCONNECT; |
1384 | err = rfcomm_session_send_frame(rs, RFCOMM_FRAME_DISC, |
1385 | pn.dlci); |
1386 | if (err) |
1387 | goto close; |
1388 | |
1389 | callout_schedule(&dlc->rd_timeout, |
1390 | rfcomm_ack_timeout * hz); |
1391 | return; |
1392 | } |
1393 | dlc->rd_mtu = pn.mtu; |
1394 | |
1395 | /* if DLC is not waiting to connect, we are done */ |
1396 | if (dlc->rd_state != RFCOMM_DLC_WAIT_CONNECT) |
1397 | return; |
1398 | |
1399 | /* set initial credits according to RFCOMM spec */ |
1400 | if ((pn.flow_control & 0xf0) == 0xe0) { |
1401 | rs->rs_flags |= RFCOMM_SESSION_CFC; |
1402 | dlc->rd_txcred = (pn.credits & 0x07); |
1403 | } |
1404 | |
1405 | callout_schedule(&dlc->rd_timeout, rfcomm_ack_timeout * hz); |
1406 | |
1407 | /* set link mode */ |
1408 | err = rfcomm_dlc_setmode(dlc); |
1409 | if (err == EINPROGRESS) { |
1410 | dlc->rd_state = RFCOMM_DLC_WAIT_SEND_SABM; |
1411 | (*dlc->rd_proto->connecting)(dlc->rd_upper); |
1412 | return; |
1413 | } |
1414 | if (err) |
1415 | goto close; |
1416 | |
1417 | /* we can proceed now */ |
1418 | err = rfcomm_session_send_frame(rs, RFCOMM_FRAME_SABM, pn.dlci); |
1419 | if (err) |
1420 | goto close; |
1421 | |
1422 | dlc->rd_state = RFCOMM_DLC_WAIT_RECV_UA; |
1423 | } |
1424 | return; |
1425 | |
1426 | close: |
1427 | rfcomm_dlc_close(dlc, err); |
1428 | } |
1429 | |
1430 | /* |
1431 | * process Non Supported Command command/response |
1432 | */ |
1433 | static void |
1434 | rfcomm_session_recv_mcc_nsc(struct rfcomm_session *rs, |
1435 | int cr, struct mbuf *m) |
1436 | { |
1437 | struct rfcomm_dlc *dlc, *next; |
1438 | |
1439 | /* |
1440 | * Since we did nothing that is not mandatory, |
1441 | * we just abort the whole session.. |
1442 | */ |
1443 | |
1444 | next = LIST_FIRST(&rs->rs_dlcs); |
1445 | while ((dlc = next) != NULL) { |
1446 | next = LIST_NEXT(dlc, rd_next); |
1447 | rfcomm_dlc_close(dlc, ECONNABORTED); |
1448 | } |
1449 | |
1450 | rfcomm_session_free(rs); |
1451 | } |
1452 | |
1453 | /*********************************************************************** |
1454 | * |
1455 | * RFCOMM Session outward frame/uih/mcc building |
1456 | */ |
1457 | |
1458 | /* |
1459 | * SABM/DISC/DM/UA frames are all minimal and mostly identical. |
1460 | */ |
1461 | int |
1462 | rfcomm_session_send_frame(struct rfcomm_session *rs, int type, int dlci) |
1463 | { |
1464 | struct rfcomm_cmd_hdr *hdr; |
1465 | struct rfcomm_credit *credit; |
1466 | struct mbuf *m; |
1467 | uint8_t fcs, cr; |
1468 | |
1469 | credit = pool_get(&rfcomm_credit_pool, PR_NOWAIT); |
1470 | if (credit == NULL) |
1471 | return ENOMEM; |
1472 | |
1473 | m = m_gethdr(M_DONTWAIT, MT_DATA); |
1474 | if (m == NULL) { |
1475 | pool_put(&rfcomm_credit_pool, credit); |
1476 | return ENOMEM; |
1477 | } |
1478 | |
1479 | /* |
1480 | * The CR (command/response) bit identifies the frame either as a |
1481 | * commmand or a response and is used along with the DLCI to form |
1482 | * the address. Commands contain the non-initiator address, whereas |
1483 | * responses contain the initiator address, so the CR value is |
1484 | * also dependent on the session direction. |
1485 | */ |
1486 | if (type == RFCOMM_FRAME_UA || type == RFCOMM_FRAME_DM) |
1487 | cr = IS_INITIATOR(rs) ? 0 : 1; |
1488 | else |
1489 | cr = IS_INITIATOR(rs) ? 1 : 0; |
1490 | |
1491 | hdr = mtod(m, struct rfcomm_cmd_hdr *); |
1492 | hdr->address = RFCOMM_MKADDRESS(cr, dlci); |
1493 | hdr->control = RFCOMM_MKCONTROL(type, 1); /* PF = 1 */ |
1494 | hdr->length = (0x00 << 1) | 0x01; /* len = 0x00, EA = 1 */ |
1495 | |
1496 | fcs = 0xff; |
1497 | fcs = FCS(fcs, hdr->address); |
1498 | fcs = FCS(fcs, hdr->control); |
1499 | fcs = FCS(fcs, hdr->length); |
1500 | fcs = 0xff - fcs; /* ones complement */ |
1501 | hdr->fcs = fcs; |
1502 | |
1503 | m->m_pkthdr.len = m->m_len = sizeof(struct rfcomm_cmd_hdr); |
1504 | |
1505 | /* empty credit note */ |
1506 | credit->rc_dlc = NULL; |
1507 | credit->rc_len = m->m_pkthdr.len; |
1508 | SIMPLEQ_INSERT_TAIL(&rs->rs_credits, credit, rc_next); |
1509 | |
1510 | DPRINTFN(5, "dlci %d type %2.2x (%d bytes, fcs=%#2.2x)\n" , |
1511 | dlci, type, m->m_pkthdr.len, fcs); |
1512 | |
1513 | return l2cap_send_pcb(rs->rs_l2cap, m); |
1514 | } |
1515 | |
1516 | /* |
1517 | * rfcomm_session_send_uih(rfcomm_session, rfcomm_dlc, credits, mbuf) |
1518 | * |
1519 | * UIH frame is per DLC data or Multiplexer Control Commands |
1520 | * when no DLC is given. Data mbuf is optional (just credits |
1521 | * will be sent in that case) |
1522 | */ |
1523 | int |
1524 | rfcomm_session_send_uih(struct rfcomm_session *rs, struct rfcomm_dlc *dlc, |
1525 | int credits, struct mbuf *m) |
1526 | { |
1527 | struct rfcomm_credit *credit; |
1528 | struct mbuf *m0 = NULL; |
1529 | int err, len; |
1530 | uint8_t fcs, *hdr; |
1531 | |
1532 | KASSERT(rs != NULL); |
1533 | |
1534 | len = (m == NULL) ? 0 : m->m_pkthdr.len; |
1535 | KASSERT(!(credits == 0 && len == 0)); |
1536 | |
1537 | /* |
1538 | * Make a credit note for the completion notification |
1539 | */ |
1540 | credit = pool_get(&rfcomm_credit_pool, PR_NOWAIT); |
1541 | if (credit == NULL) |
1542 | goto nomem; |
1543 | |
1544 | credit->rc_len = len; |
1545 | credit->rc_dlc = dlc; |
1546 | |
1547 | /* |
1548 | * Wrap UIH frame information around payload. |
1549 | * |
1550 | * [ADDRESS] [CONTROL] [LENGTH] [CREDITS] [...] [FCS] |
1551 | * |
1552 | * Address is one octet. |
1553 | * Control is one octet. |
1554 | * Length is one or two octets. |
1555 | * Credits may be one octet. |
1556 | * |
1557 | * FCS is one octet and calculated on address and |
1558 | * control octets only. |
1559 | * |
1560 | * If there are credits to be sent, we will set the PF |
1561 | * flag and include them in the frame. |
1562 | */ |
1563 | m0 = m_gethdr(M_DONTWAIT, MT_DATA); |
1564 | if (m0 == NULL) |
1565 | goto nomem; |
1566 | |
1567 | MH_ALIGN(m0, 5); /* (max 5 header octets) */ |
1568 | hdr = mtod(m0, uint8_t *); |
1569 | |
1570 | /* CR bit is set according to the initiator of the session */ |
1571 | *hdr = RFCOMM_MKADDRESS((IS_INITIATOR(rs) ? 1 : 0), |
1572 | (dlc ? dlc->rd_dlci : 0)); |
1573 | fcs = FCS(0xff, *hdr); |
1574 | hdr++; |
1575 | |
1576 | /* PF bit is set if credits are being sent */ |
1577 | *hdr = RFCOMM_MKCONTROL(RFCOMM_FRAME_UIH, (credits > 0 ? 1 : 0)); |
1578 | fcs = FCS(fcs, *hdr); |
1579 | hdr++; |
1580 | |
1581 | if (len < (1 << 7)) { |
1582 | *hdr++ = ((len << 1) & 0xfe) | 0x01; /* 7 bits, EA = 1 */ |
1583 | } else { |
1584 | *hdr++ = ((len << 1) & 0xfe); /* 7 bits, EA = 0 */ |
1585 | *hdr++ = ((len >> 7) & 0xff); /* 8 bits, no EA */ |
1586 | } |
1587 | |
1588 | if (credits > 0) |
1589 | *hdr++ = (uint8_t)credits; |
1590 | |
1591 | m0->m_len = hdr - mtod(m0, uint8_t *); |
1592 | |
1593 | /* Append payload */ |
1594 | m0->m_next = m; |
1595 | m = NULL; |
1596 | |
1597 | m0->m_pkthdr.len = m0->m_len + len; |
1598 | |
1599 | /* Append FCS */ |
1600 | fcs = 0xff - fcs; /* ones complement */ |
1601 | len = m0->m_pkthdr.len; |
1602 | m_copyback(m0, len, sizeof(fcs), &fcs); |
1603 | if (m0->m_pkthdr.len != len + sizeof(fcs)) |
1604 | goto nomem; |
1605 | |
1606 | DPRINTFN(10, "dlci %d, pktlen %d (%d data, %d credits), fcs=%#2.2x\n" , |
1607 | dlc ? dlc->rd_dlci : 0, m0->m_pkthdr.len, credit->rc_len, |
1608 | credits, fcs); |
1609 | |
1610 | /* |
1611 | * UIH frame ready to go.. |
1612 | */ |
1613 | err = l2cap_send_pcb(rs->rs_l2cap, m0); |
1614 | if (err) |
1615 | goto fail; |
1616 | |
1617 | SIMPLEQ_INSERT_TAIL(&rs->rs_credits, credit, rc_next); |
1618 | return 0; |
1619 | |
1620 | nomem: |
1621 | err = ENOMEM; |
1622 | |
1623 | if (m0 != NULL) |
1624 | m_freem(m0); |
1625 | |
1626 | if (m != NULL) |
1627 | m_freem(m); |
1628 | |
1629 | fail: |
1630 | if (credit != NULL) |
1631 | pool_put(&rfcomm_credit_pool, credit); |
1632 | |
1633 | return err; |
1634 | } |
1635 | |
1636 | /* |
1637 | * send Multiplexer Control Command (or Response) on session |
1638 | */ |
1639 | int |
1640 | rfcomm_session_send_mcc(struct rfcomm_session *rs, int cr, |
1641 | uint8_t type, void *data, int len) |
1642 | { |
1643 | struct mbuf *m; |
1644 | uint8_t *hdr; |
1645 | int hlen; |
1646 | |
1647 | m = m_gethdr(M_DONTWAIT, MT_DATA); |
1648 | if (m == NULL) |
1649 | return ENOMEM; |
1650 | |
1651 | hdr = mtod(m, uint8_t *); |
1652 | |
1653 | /* |
1654 | * Technically the type field can extend past one octet, but none |
1655 | * currently defined will do that. |
1656 | */ |
1657 | *hdr++ = RFCOMM_MKMCC_TYPE(cr, type); |
1658 | |
1659 | /* |
1660 | * In the frame, the max length size is 2 octets (15 bits) whereas |
1661 | * no max length size is specified for MCC commands. We must allow |
1662 | * for 3 octets since for MCC frames we use 7 bits + EA in each. |
1663 | * |
1664 | * Only test data can possibly be that big. |
1665 | * |
1666 | * XXX Should we check this against the MTU? |
1667 | */ |
1668 | if (len < (1 << 7)) { |
1669 | *hdr++ = ((len << 1) & 0xfe) | 0x01; /* 7 bits, EA = 1 */ |
1670 | } else if (len < (1 << 14)) { |
1671 | *hdr++ = ((len << 1) & 0xfe); /* 7 bits, EA = 0 */ |
1672 | *hdr++ = ((len >> 6) & 0xfe) | 0x01; /* 7 bits, EA = 1 */ |
1673 | } else if (len < (1 << 15)) { |
1674 | *hdr++ = ((len << 1) & 0xfe); /* 7 bits, EA = 0 */ |
1675 | *hdr++ = ((len >> 6) & 0xfe); /* 7 bits, EA = 0 */ |
1676 | *hdr++ = ((len >> 13) & 0x02) | 0x01; /* 1 bit, EA = 1 */ |
1677 | } else { |
1678 | DPRINTF("incredible length! (%d)\n" , len); |
1679 | m_freem(m); |
1680 | return EMSGSIZE; |
1681 | } |
1682 | |
1683 | /* |
1684 | * add command data (to same mbuf if possible) |
1685 | */ |
1686 | hlen = hdr - mtod(m, uint8_t *); |
1687 | |
1688 | if (len > 0) { |
1689 | m->m_pkthdr.len = m->m_len = MHLEN; |
1690 | m_copyback(m, hlen, len, data); |
1691 | if (m->m_pkthdr.len != max(MHLEN, hlen + len)) { |
1692 | m_freem(m); |
1693 | return ENOMEM; |
1694 | } |
1695 | } |
1696 | |
1697 | m->m_pkthdr.len = hlen + len; |
1698 | m->m_len = min(MHLEN, m->m_pkthdr.len); |
1699 | |
1700 | DPRINTFN(5, "%s type %2.2x len %d\n" , |
1701 | (cr ? "command" : "response" ), type, m->m_pkthdr.len); |
1702 | |
1703 | return rfcomm_session_send_uih(rs, NULL, 0, m); |
1704 | } |
1705 | |