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