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