1 /* 2 * Copyright 2007 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com 3 * All rights reserved. Distributed under the terms of the MIT License. 4 * 5 */ 6 7 /*- 8 * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com> 9 * All rights reserved. 10 * 11 * Redistribution and use in source and binary forms, with or without 12 * modification, are permitted provided that the following conditions 13 * are met: 14 * 1. Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * 2. Redistributions in binary form must reproduce the above copyright 17 * notice, this list of conditions and the following disclaimer in the 18 * documentation and/or other materials provided with the distribution. 19 */ 20 21 22 #include <NetBufferUtilities.h> 23 24 #include "l2cap_command.h" 25 26 /* 27 * Note: All L2CAP implementations are required to support minimal signaling 28 * MTU of 48 bytes. In order to simplify things we will send one command 29 * per one L2CAP packet. Given evrything above we can assume that one 30 * signaling packet will fit into single mbuf. 31 */ 32 33 34 /* Private types */ 35 struct _cmd_rej { 36 l2cap_cmd_hdr_t hdr; 37 l2cap_cmd_rej_cp param; 38 l2cap_cmd_rej_data_t data; 39 } __attribute__ ((packed)) ; 40 41 struct _con_req { 42 l2cap_cmd_hdr_t hdr; 43 l2cap_con_req_cp param; 44 } __attribute__ ((packed)); 45 46 struct _con_rsp { 47 l2cap_cmd_hdr_t hdr; 48 l2cap_con_rsp_cp param; 49 } __attribute__ ((packed)); 50 51 struct _cfg_req { 52 l2cap_cmd_hdr_t hdr; 53 l2cap_cfg_req_cp param; 54 } __attribute__ ((packed)); 55 56 struct _cfg_rsp { 57 l2cap_cmd_hdr_t hdr; 58 l2cap_cfg_rsp_cp param; 59 } __attribute__ ((packed)); 60 61 struct _discon_req { 62 l2cap_cmd_hdr_t hdr; 63 l2cap_discon_req_cp param; 64 } __attribute__ ((packed)); 65 66 struct _discon_rsp { 67 l2cap_cmd_hdr_t hdr; 68 l2cap_discon_rsp_cp param; 69 } __attribute__ ((packed)); 70 71 struct _info_req { 72 l2cap_cmd_hdr_t hdr; 73 l2cap_info_req_cp param; 74 } __attribute__ ((packed)); 75 76 struct _info_rsp { 77 l2cap_cmd_hdr_t hdr; 78 l2cap_info_rsp_cp param; 79 l2cap_info_rsp_data_t data; 80 } __attribute__ ((packed)); 81 82 // Configuration options 83 struct _cfg_opt_flow { 84 l2cap_cfg_opt_t hdr; 85 l2cap_flow_t val; 86 } __attribute__ ((packed)); 87 88 89 struct _cfg_opt_flush { 90 l2cap_cfg_opt_t hdr; 91 uint16 val; 92 } __attribute__ ((packed)); 93 94 struct _cfg_opt_mtu { 95 l2cap_cfg_opt_t hdr; 96 uint16 val; 97 } __attribute__ ((packed)); 98 99 100 101 102 /* L2CAP_CommandRej */ 103 net_buffer* 104 l2cap_cmd_rej(uint8 _ident, uint16 _reason, uint16 _mtu, uint16 _scid, uint16 _dcid) 105 { 106 107 net_buffer* _m = gBufferModule->create(sizeof(struct _cmd_rej)); 108 if ((_m) == NULL) 109 return NULL; 110 111 NetBufferPrepend<struct _cmd_rej> bufferHeader(_m); 112 status_t status = bufferHeader.Status(); 113 if (status < B_OK) { 114 // free the buffer 115 return NULL; 116 } 117 118 bufferHeader->hdr.code = L2CAP_CMD_REJ; 119 bufferHeader->hdr.ident = (_ident); 120 bufferHeader->hdr.length = sizeof(bufferHeader->param); 121 122 bufferHeader->param.reason = htole16((_reason)); 123 124 if ((_reason) == L2CAP_REJ_MTU_EXCEEDED) { 125 bufferHeader->data.mtu.mtu = htole16((_mtu)); 126 bufferHeader->hdr.length += sizeof(bufferHeader->data.mtu); 127 } else if ((_reason) == L2CAP_REJ_INVALID_CID) { 128 bufferHeader->data.cid.scid = htole16((_scid)); 129 bufferHeader->data.cid.dcid = htole16((_dcid)); 130 bufferHeader->hdr.length += sizeof(bufferHeader->data.cid); 131 } 132 133 bufferHeader->hdr.length = htole16(bufferHeader->hdr.length); 134 135 bufferHeader.Sync(); 136 137 return _m; 138 } 139 140 141 /* L2CAP_ConnectReq */ 142 net_buffer* 143 l2cap_con_req(uint8 _ident, uint16 _psm, uint16 _scid) 144 { 145 146 net_buffer* _m = gBufferModule->create(sizeof(struct _con_req)); 147 if ((_m) == NULL) 148 return NULL; 149 150 NetBufferPrepend<struct _con_req> bufferHeader(_m); 151 status_t status = bufferHeader.Status(); 152 if (status < B_OK) { 153 /* TODO free the buffer */ 154 return NULL; 155 } 156 157 bufferHeader->hdr.code = L2CAP_CON_REQ; 158 bufferHeader->hdr.ident = (_ident); 159 bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param)); 160 161 bufferHeader->param.psm = htole16((_psm)); 162 bufferHeader->param.scid = htole16((_scid)); 163 164 bufferHeader.Sync(); 165 166 return _m; 167 } 168 169 170 /* L2CAP_ConnectRsp */ 171 net_buffer* 172 l2cap_con_rsp(uint8 _ident, uint16 _dcid, uint16 _scid, uint16 _result, uint16 _status) 173 { 174 175 net_buffer* _m = gBufferModule->create(sizeof(struct _con_rsp)); 176 if ((_m) == NULL) 177 return NULL; 178 179 NetBufferPrepend<struct _con_rsp> bufferHeader(_m); 180 status_t status = bufferHeader.Status(); 181 if (status < B_OK) { 182 /* TODO free the buffer */ 183 return NULL; 184 } 185 186 bufferHeader->hdr.code = L2CAP_CON_RSP; 187 bufferHeader->hdr.ident = (_ident); 188 bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param)); 189 190 bufferHeader->param.dcid = htole16((_dcid)); 191 bufferHeader->param.scid = htole16((_scid)); 192 bufferHeader->param.result = htole16((_result)); 193 bufferHeader->param.status = htole16((_status)); /* reason */ 194 195 bufferHeader.Sync(); 196 197 return _m; 198 } 199 200 201 /* L2CAP_ConfigReq */ 202 net_buffer* 203 l2cap_cfg_req(uint8 _ident, uint16 _dcid, uint16 _flags, net_buffer* _data) 204 { 205 206 net_buffer* _m = gBufferModule->create(sizeof(struct _cfg_req)); 207 if ((_m) == NULL){ 208 /* TODO free the _data buffer? */ 209 return NULL; 210 } 211 212 NetBufferPrepend<struct _cfg_req> bufferHeader(_m); 213 status_t status = bufferHeader.Status(); 214 if (status < B_OK) { 215 /* TODO free the buffer */ 216 return NULL; 217 } 218 219 bufferHeader->hdr.code = L2CAP_CFG_REQ; 220 bufferHeader->hdr.ident = (_ident); 221 bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param)); 222 223 bufferHeader->param.dcid = htole16((_dcid)); 224 bufferHeader->param.flags = htole16((_flags)); 225 226 bufferHeader.Sync(); 227 228 /* Add the given data */ 229 // TODO: given data can be freed... merge does it? 230 if (_data != NULL) 231 gBufferModule->merge(_m, _data, true); 232 233 return _m; 234 } 235 236 237 /* L2CAP_ConfigRsp */ 238 net_buffer* 239 l2cap_cfg_rsp(uint8 _ident, uint16 _scid, uint16 _flags, uint16 _result, net_buffer* _data) 240 { 241 242 net_buffer* _m = gBufferModule->create(sizeof(struct _cfg_rsp)); 243 if ((_m) == NULL){ 244 /* TODO free the _data buffer */ 245 return NULL; 246 } 247 248 NetBufferPrepend<struct _cfg_rsp> bufferHeader(_m); 249 status_t status = bufferHeader.Status(); 250 if (status < B_OK) { 251 /* TODO free the buffer */ 252 return NULL; 253 } 254 255 bufferHeader->hdr.code = L2CAP_CFG_RSP; 256 bufferHeader->hdr.ident = (_ident); 257 bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param)); 258 259 bufferHeader->param.scid = htole16((_scid)); 260 bufferHeader->param.flags = htole16((_flags)); 261 bufferHeader->param.result = htole16((_result)); 262 263 bufferHeader.Sync(); 264 265 if (_data != NULL) 266 gBufferModule->merge(_m, _data, true); 267 268 return _m; 269 270 } 271 272 273 /* L2CAP_DisconnectReq */ 274 net_buffer* 275 l2cap_discon_req(uint8 _ident, uint16 _dcid, uint16 _scid) 276 { 277 278 net_buffer* _m = gBufferModule->create(sizeof(struct _discon_req)); 279 if ((_m) == NULL){ 280 return NULL; 281 } 282 283 NetBufferPrepend<struct _discon_req> bufferHeader(_m); 284 status_t status = bufferHeader.Status(); 285 if (status < B_OK) { 286 /* TODO free the buffer */ 287 return NULL; 288 } 289 290 bufferHeader->hdr.code = L2CAP_DISCON_REQ; 291 bufferHeader->hdr.ident = (_ident); 292 bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param)); 293 294 bufferHeader->param.dcid = htole16((_dcid)); 295 bufferHeader->param.scid = htole16((_scid)); 296 297 bufferHeader.Sync(); 298 299 return _m; 300 } 301 302 303 /* L2CA_DisconnectRsp */ 304 net_buffer* 305 l2cap_discon_rsp(uint8 _ident, uint16 _dcid, uint16 _scid) 306 { 307 308 net_buffer* _m = gBufferModule->create(sizeof(struct _discon_rsp)); 309 if ((_m) == NULL){ 310 return NULL; 311 } 312 313 NetBufferPrepend<struct _discon_rsp> bufferHeader(_m); 314 status_t status = bufferHeader.Status(); 315 if (status < B_OK) { 316 /* TODO free the buffer */ 317 return NULL; 318 } 319 320 bufferHeader->hdr.code = L2CAP_DISCON_RSP; 321 bufferHeader->hdr.ident = (_ident); 322 bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param)); 323 324 bufferHeader->param.dcid = htole16((_dcid)); 325 bufferHeader->param.scid = htole16((_scid)); 326 327 bufferHeader.Sync(); 328 329 return _m; 330 } 331 332 333 /* L2CAP_EchoReq */ 334 net_buffer* 335 l2cap_echo_req(uint8 _ident, void* _data, size_t _size) 336 { 337 net_buffer* _m = gBufferModule->create(sizeof(l2cap_cmd_hdr_t)); 338 if ((_m) == NULL){ 339 /* TODO free the _data buffer */ 340 return NULL; 341 } 342 343 344 345 if ((_data) != NULL) { 346 gBufferModule->append(_m, _data, _size); 347 } 348 349 return _m; 350 } 351 352 353 /* L2CAP_InfoReq */ 354 net_buffer* 355 l2cap_info_req(uint8 _ident, uint16 _type) 356 { 357 358 net_buffer* _m = gBufferModule->create(sizeof(struct _info_req)); 359 if ((_m) == NULL){ 360 return NULL; 361 } 362 363 NetBufferPrepend<struct _info_req> bufferHeader(_m); 364 status_t status = bufferHeader.Status(); 365 if (status < B_OK) { 366 /* TODO free the buffer */ 367 return NULL; 368 } 369 370 bufferHeader->hdr.code = L2CAP_INFO_REQ; 371 bufferHeader->hdr.ident = (_ident); 372 bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param)); 373 374 bufferHeader->param.type = htole16((_type)); 375 376 bufferHeader.Sync(); 377 378 return _m; 379 } 380 381 382 /* L2CAP_InfoRsp */ 383 net_buffer* 384 l2cap_info_rsp(uint8 _ident, uint16 _type, uint16 _result, uint16 _mtu) 385 { 386 387 net_buffer* _m = gBufferModule->create(sizeof(struct _info_rsp)); 388 if ((_m) == NULL){ 389 return NULL; 390 } 391 392 NetBufferPrepend<struct _info_rsp> bufferHeader(_m); 393 status_t status = bufferHeader.Status(); 394 if (status < B_OK) { 395 /* TODO free the buffer */ 396 return NULL; 397 } 398 399 bufferHeader->hdr.code = L2CAP_INFO_REQ; 400 bufferHeader->hdr.ident = (_ident); 401 bufferHeader->hdr.length = sizeof(bufferHeader->param); 402 403 bufferHeader->param.type = htole16((_type)); 404 bufferHeader->param.result = htole16((_result)); 405 406 if ((_result) == L2CAP_SUCCESS) { 407 switch ((_type)) { 408 case L2CAP_CONNLESS_MTU: 409 bufferHeader->data.mtu.mtu = htole16((_mtu)); 410 bufferHeader->hdr.length += sizeof((bufferHeader->data.mtu.mtu)); 411 break; 412 } 413 } 414 415 bufferHeader->hdr.length = htole16(bufferHeader->hdr.length); 416 417 bufferHeader.Sync(); 418 419 return _m; 420 421 } 422 423 #if 0 424 #pragma mark - 425 #endif 426 427 428 /* Build configuration options */ 429 net_buffer* 430 l2cap_build_cfg_options(uint16* _mtu, uint16* _flush_timo, l2cap_flow_t* _flow) 431 { 432 size_t requestedSize = 0; 433 434 if (_mtu != NULL) 435 requestedSize+=sizeof(*_mtu); 436 437 438 if (_flush_timo != NULL) 439 requestedSize+=sizeof(*_flush_timo); 440 441 if (_flow != NULL) 442 requestedSize+=sizeof(*_flow); 443 444 net_buffer* _m = gBufferModule->create(sizeof(requestedSize)); 445 446 if (_m == NULL) 447 return NULL; 448 449 if (_mtu != NULL) { 450 NetBufferPrepend<struct _cfg_opt_mtu> bufferHeader(_m); 451 status_t status = bufferHeader.Status(); 452 if (status < B_OK) { 453 /* TODO free the buffer ?? */ 454 return NULL; 455 } 456 457 bufferHeader->hdr.type = L2CAP_OPT_MTU; 458 bufferHeader->hdr.length = sizeof(bufferHeader->val); 459 bufferHeader->val = htole16(*(uint16 *)(_mtu)); 460 461 bufferHeader.Sync(); 462 } 463 464 if (_flush_timo != NULL) { 465 466 NetBufferPrepend<struct _cfg_opt_flush> bufferHeader(_m); 467 status_t status = bufferHeader.Status(); 468 if (status < B_OK) { 469 /* TODO free the buffer ?? */ 470 return NULL; 471 } 472 473 bufferHeader->hdr.type = L2CAP_OPT_FLUSH_TIMO; 474 bufferHeader->hdr.length = sizeof(bufferHeader->val); 475 bufferHeader->val = htole16(*(int16 *)(_flush_timo)); 476 477 bufferHeader.Sync(); 478 } 479 480 if (_flow != NULL) { 481 482 NetBufferPrepend<struct _cfg_opt_flow> bufferHeader(_m); 483 status_t status = bufferHeader.Status(); 484 if (status < B_OK) { 485 /* TODO free the buffer ?? */ 486 return NULL; 487 } 488 489 bufferHeader->hdr.type = L2CAP_OPT_QOS; 490 bufferHeader->hdr.length = sizeof(bufferHeader->val); 491 bufferHeader->val.flags = _flow->flags; 492 bufferHeader->val.service_type = _flow->service_type; 493 bufferHeader->val.token_rate = htole32(_flow->token_rate); 494 bufferHeader->val.token_bucket_size = htole32(_flow->token_bucket_size); 495 bufferHeader->val.peak_bandwidth = htole32(_flow->peak_bandwidth); 496 bufferHeader->val.latency = htole32(_flow->latency); 497 bufferHeader->val.delay_variation = htole32(_flow->delay_variation); 498 499 bufferHeader.Sync(); 500 } 501 502 return _m; 503 } 504