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