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