1 /* 2 * Copyright (C) ST-Ericsson AB 2010 3 * Author: Sjur Brendeland/sjur.brandeland@stericsson.com 4 * License terms: GNU General Public License (GPL) version 2 5 */ 6 7 #include <linux/stddef.h> 8 #include <linux/spinlock.h> 9 #include <linux/slab.h> 10 #include <net/caif/caif_layer.h> 11 #include <net/caif/cfpkt.h> 12 #include <net/caif/cfctrl.h> 13 14 #define container_obj(layr) container_of(layr, struct cfctrl, serv.layer) 15 #define UTILITY_NAME_LENGTH 16 16 #define CFPKT_CTRL_PKT_LEN 20 17 18 19 #ifdef CAIF_NO_LOOP 20 static int handle_loop(struct cfctrl *ctrl, 21 int cmd, struct cfpkt *pkt){ 22 return CAIF_FAILURE; 23 } 24 #else 25 static int handle_loop(struct cfctrl *ctrl, 26 int cmd, struct cfpkt *pkt); 27 #endif 28 static int cfctrl_recv(struct cflayer *layr, struct cfpkt *pkt); 29 static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, 30 int phyid); 31 32 33 struct cflayer *cfctrl_create(void) 34 { 35 struct dev_info dev_info; 36 struct cfctrl *this = 37 kmalloc(sizeof(struct cfctrl), GFP_ATOMIC); 38 if (!this) { 39 pr_warning("CAIF: %s(): Out of memory\n", __func__); 40 return NULL; 41 } 42 caif_assert(offsetof(struct cfctrl, serv.layer) == 0); 43 memset(&dev_info, 0, sizeof(dev_info)); 44 dev_info.id = 0xff; 45 memset(this, 0, sizeof(*this)); 46 cfsrvl_init(&this->serv, 0, &dev_info); 47 spin_lock_init(&this->info_list_lock); 48 atomic_set(&this->req_seq_no, 1); 49 atomic_set(&this->rsp_seq_no, 1); 50 this->serv.layer.receive = cfctrl_recv; 51 sprintf(this->serv.layer.name, "ctrl"); 52 this->serv.layer.ctrlcmd = cfctrl_ctrlcmd; 53 spin_lock_init(&this->loop_linkid_lock); 54 this->loop_linkid = 1; 55 return &this->serv.layer; 56 } 57 58 static bool param_eq(struct cfctrl_link_param *p1, struct cfctrl_link_param *p2) 59 { 60 bool eq = 61 p1->linktype == p2->linktype && 62 p1->priority == p2->priority && 63 p1->phyid == p2->phyid && 64 p1->endpoint == p2->endpoint && p1->chtype == p2->chtype; 65 66 if (!eq) 67 return false; 68 69 switch (p1->linktype) { 70 case CFCTRL_SRV_VEI: 71 return true; 72 case CFCTRL_SRV_DATAGRAM: 73 return p1->u.datagram.connid == p2->u.datagram.connid; 74 case CFCTRL_SRV_RFM: 75 return 76 p1->u.rfm.connid == p2->u.rfm.connid && 77 strcmp(p1->u.rfm.volume, p2->u.rfm.volume) == 0; 78 case CFCTRL_SRV_UTIL: 79 return 80 p1->u.utility.fifosize_kb == p2->u.utility.fifosize_kb 81 && p1->u.utility.fifosize_bufs == 82 p2->u.utility.fifosize_bufs 83 && strcmp(p1->u.utility.name, p2->u.utility.name) == 0 84 && p1->u.utility.paramlen == p2->u.utility.paramlen 85 && memcmp(p1->u.utility.params, p2->u.utility.params, 86 p1->u.utility.paramlen) == 0; 87 88 case CFCTRL_SRV_VIDEO: 89 return p1->u.video.connid == p2->u.video.connid; 90 case CFCTRL_SRV_DBG: 91 return true; 92 case CFCTRL_SRV_DECM: 93 return false; 94 default: 95 return false; 96 } 97 return false; 98 } 99 100 bool cfctrl_req_eq(struct cfctrl_request_info *r1, 101 struct cfctrl_request_info *r2) 102 { 103 if (r1->cmd != r2->cmd) 104 return false; 105 if (r1->cmd == CFCTRL_CMD_LINK_SETUP) 106 return param_eq(&r1->param, &r2->param); 107 else 108 return r1->channel_id == r2->channel_id; 109 } 110 111 /* Insert request at the end */ 112 void cfctrl_insert_req(struct cfctrl *ctrl, 113 struct cfctrl_request_info *req) 114 { 115 struct cfctrl_request_info *p; 116 spin_lock(&ctrl->info_list_lock); 117 req->next = NULL; 118 atomic_inc(&ctrl->req_seq_no); 119 req->sequence_no = atomic_read(&ctrl->req_seq_no); 120 if (ctrl->first_req == NULL) { 121 ctrl->first_req = req; 122 spin_unlock(&ctrl->info_list_lock); 123 return; 124 } 125 p = ctrl->first_req; 126 while (p->next != NULL) 127 p = p->next; 128 p->next = req; 129 spin_unlock(&ctrl->info_list_lock); 130 } 131 132 /* Compare and remove request */ 133 struct cfctrl_request_info *cfctrl_remove_req(struct cfctrl *ctrl, 134 struct cfctrl_request_info *req) 135 { 136 struct cfctrl_request_info *p; 137 struct cfctrl_request_info *ret; 138 139 spin_lock(&ctrl->info_list_lock); 140 if (ctrl->first_req == NULL) { 141 spin_unlock(&ctrl->info_list_lock); 142 return NULL; 143 } 144 145 if (cfctrl_req_eq(req, ctrl->first_req)) { 146 ret = ctrl->first_req; 147 caif_assert(ctrl->first_req); 148 atomic_set(&ctrl->rsp_seq_no, 149 ctrl->first_req->sequence_no); 150 ctrl->first_req = ctrl->first_req->next; 151 spin_unlock(&ctrl->info_list_lock); 152 return ret; 153 } 154 155 p = ctrl->first_req; 156 157 while (p->next != NULL) { 158 if (cfctrl_req_eq(req, p->next)) { 159 pr_warning("CAIF: %s(): Requests are not " 160 "received in order\n", 161 __func__); 162 ret = p->next; 163 atomic_set(&ctrl->rsp_seq_no, 164 p->next->sequence_no); 165 p->next = p->next->next; 166 spin_unlock(&ctrl->info_list_lock); 167 return ret; 168 } 169 p = p->next; 170 } 171 spin_unlock(&ctrl->info_list_lock); 172 173 pr_warning("CAIF: %s(): Request does not match\n", 174 __func__); 175 return NULL; 176 } 177 178 struct cfctrl_rsp *cfctrl_get_respfuncs(struct cflayer *layer) 179 { 180 struct cfctrl *this = container_obj(layer); 181 return &this->res; 182 } 183 184 void cfctrl_set_dnlayer(struct cflayer *this, struct cflayer *dn) 185 { 186 this->dn = dn; 187 } 188 189 void cfctrl_set_uplayer(struct cflayer *this, struct cflayer *up) 190 { 191 this->up = up; 192 } 193 194 static void init_info(struct caif_payload_info *info, struct cfctrl *cfctrl) 195 { 196 info->hdr_len = 0; 197 info->channel_id = cfctrl->serv.layer.id; 198 info->dev_info = &cfctrl->serv.dev_info; 199 } 200 201 void cfctrl_enum_req(struct cflayer *layer, u8 physlinkid) 202 { 203 struct cfctrl *cfctrl = container_obj(layer); 204 int ret; 205 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 206 if (!pkt) { 207 pr_warning("CAIF: %s(): Out of memory\n", __func__); 208 return; 209 } 210 caif_assert(offsetof(struct cfctrl, serv.layer) == 0); 211 init_info(cfpkt_info(pkt), cfctrl); 212 cfpkt_info(pkt)->dev_info->id = physlinkid; 213 cfctrl->serv.dev_info.id = physlinkid; 214 cfpkt_addbdy(pkt, CFCTRL_CMD_ENUM); 215 cfpkt_addbdy(pkt, physlinkid); 216 ret = 217 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 218 if (ret < 0) { 219 pr_err("CAIF: %s(): Could not transmit enum message\n", 220 __func__); 221 cfpkt_destroy(pkt); 222 } 223 } 224 225 int cfctrl_linkup_request(struct cflayer *layer, 226 struct cfctrl_link_param *param, 227 struct cflayer *user_layer) 228 { 229 struct cfctrl *cfctrl = container_obj(layer); 230 u32 tmp32; 231 u16 tmp16; 232 u8 tmp8; 233 struct cfctrl_request_info *req; 234 int ret; 235 char utility_name[16]; 236 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 237 if (!pkt) { 238 pr_warning("CAIF: %s(): Out of memory\n", __func__); 239 return -ENOMEM; 240 } 241 cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_SETUP); 242 cfpkt_addbdy(pkt, (param->chtype << 4) + param->linktype); 243 cfpkt_addbdy(pkt, (param->priority << 3) + param->phyid); 244 cfpkt_addbdy(pkt, param->endpoint & 0x03); 245 246 switch (param->linktype) { 247 case CFCTRL_SRV_VEI: 248 break; 249 case CFCTRL_SRV_VIDEO: 250 cfpkt_addbdy(pkt, (u8) param->u.video.connid); 251 break; 252 case CFCTRL_SRV_DBG: 253 break; 254 case CFCTRL_SRV_DATAGRAM: 255 tmp32 = cpu_to_le32(param->u.datagram.connid); 256 cfpkt_add_body(pkt, &tmp32, 4); 257 break; 258 case CFCTRL_SRV_RFM: 259 /* Construct a frame, convert DatagramConnectionID to network 260 * format long and copy it out... 261 */ 262 tmp32 = cpu_to_le32(param->u.rfm.connid); 263 cfpkt_add_body(pkt, &tmp32, 4); 264 /* Add volume name, including zero termination... */ 265 cfpkt_add_body(pkt, param->u.rfm.volume, 266 strlen(param->u.rfm.volume) + 1); 267 break; 268 case CFCTRL_SRV_UTIL: 269 tmp16 = cpu_to_le16(param->u.utility.fifosize_kb); 270 cfpkt_add_body(pkt, &tmp16, 2); 271 tmp16 = cpu_to_le16(param->u.utility.fifosize_bufs); 272 cfpkt_add_body(pkt, &tmp16, 2); 273 memset(utility_name, 0, sizeof(utility_name)); 274 strncpy(utility_name, param->u.utility.name, 275 UTILITY_NAME_LENGTH - 1); 276 cfpkt_add_body(pkt, utility_name, UTILITY_NAME_LENGTH); 277 tmp8 = param->u.utility.paramlen; 278 cfpkt_add_body(pkt, &tmp8, 1); 279 cfpkt_add_body(pkt, param->u.utility.params, 280 param->u.utility.paramlen); 281 break; 282 default: 283 pr_warning("CAIF: %s():Request setup of bad link type = %d\n", 284 __func__, param->linktype); 285 return -EINVAL; 286 } 287 req = kzalloc(sizeof(*req), GFP_KERNEL); 288 if (!req) { 289 pr_warning("CAIF: %s(): Out of memory\n", __func__); 290 return -ENOMEM; 291 } 292 req->client_layer = user_layer; 293 req->cmd = CFCTRL_CMD_LINK_SETUP; 294 req->param = *param; 295 cfctrl_insert_req(cfctrl, req); 296 init_info(cfpkt_info(pkt), cfctrl); 297 /* 298 * NOTE:Always send linkup and linkdown request on the same 299 * device as the payload. Otherwise old queued up payload 300 * might arrive with the newly allocated channel ID. 301 */ 302 cfpkt_info(pkt)->dev_info->id = param->phyid; 303 ret = 304 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 305 if (ret < 0) { 306 pr_err("CAIF: %s(): Could not transmit linksetup request\n", 307 __func__); 308 cfpkt_destroy(pkt); 309 return -ENODEV; 310 } 311 return 0; 312 } 313 314 int cfctrl_linkdown_req(struct cflayer *layer, u8 channelid, 315 struct cflayer *client) 316 { 317 int ret; 318 struct cfctrl *cfctrl = container_obj(layer); 319 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 320 if (!pkt) { 321 pr_warning("CAIF: %s(): Out of memory\n", __func__); 322 return -ENOMEM; 323 } 324 cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_DESTROY); 325 cfpkt_addbdy(pkt, channelid); 326 init_info(cfpkt_info(pkt), cfctrl); 327 ret = 328 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 329 if (ret < 0) { 330 pr_err("CAIF: %s(): Could not transmit link-down request\n", 331 __func__); 332 cfpkt_destroy(pkt); 333 } 334 return ret; 335 } 336 337 void cfctrl_sleep_req(struct cflayer *layer) 338 { 339 int ret; 340 struct cfctrl *cfctrl = container_obj(layer); 341 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 342 if (!pkt) { 343 pr_warning("CAIF: %s(): Out of memory\n", __func__); 344 return; 345 } 346 cfpkt_addbdy(pkt, CFCTRL_CMD_SLEEP); 347 init_info(cfpkt_info(pkt), cfctrl); 348 ret = 349 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 350 if (ret < 0) 351 cfpkt_destroy(pkt); 352 } 353 354 void cfctrl_wake_req(struct cflayer *layer) 355 { 356 int ret; 357 struct cfctrl *cfctrl = container_obj(layer); 358 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 359 if (!pkt) { 360 pr_warning("CAIF: %s(): Out of memory\n", __func__); 361 return; 362 } 363 cfpkt_addbdy(pkt, CFCTRL_CMD_WAKE); 364 init_info(cfpkt_info(pkt), cfctrl); 365 ret = 366 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 367 if (ret < 0) 368 cfpkt_destroy(pkt); 369 } 370 371 void cfctrl_getstartreason_req(struct cflayer *layer) 372 { 373 int ret; 374 struct cfctrl *cfctrl = container_obj(layer); 375 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 376 if (!pkt) { 377 pr_warning("CAIF: %s(): Out of memory\n", __func__); 378 return; 379 } 380 cfpkt_addbdy(pkt, CFCTRL_CMD_START_REASON); 381 init_info(cfpkt_info(pkt), cfctrl); 382 ret = 383 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 384 if (ret < 0) 385 cfpkt_destroy(pkt); 386 } 387 388 389 void cfctrl_cancel_req(struct cflayer *layr, struct cflayer *adap_layer) 390 { 391 struct cfctrl_request_info *p, *req; 392 struct cfctrl *ctrl = container_obj(layr); 393 spin_lock(&ctrl->info_list_lock); 394 395 if (ctrl->first_req == NULL) { 396 spin_unlock(&ctrl->info_list_lock); 397 return; 398 } 399 400 if (ctrl->first_req->client_layer == adap_layer) { 401 402 req = ctrl->first_req; 403 ctrl->first_req = ctrl->first_req->next; 404 kfree(req); 405 } 406 407 p = ctrl->first_req; 408 while (p != NULL && p->next != NULL) { 409 if (p->next->client_layer == adap_layer) { 410 411 req = p->next; 412 p->next = p->next->next; 413 kfree(p->next); 414 } 415 p = p->next; 416 } 417 418 spin_unlock(&ctrl->info_list_lock); 419 } 420 421 static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt) 422 { 423 u8 cmdrsp; 424 u8 cmd; 425 int ret = -1; 426 u16 tmp16; 427 u8 len; 428 u8 param[255]; 429 u8 linkid; 430 struct cfctrl *cfctrl = container_obj(layer); 431 struct cfctrl_request_info rsp, *req; 432 433 434 cfpkt_extr_head(pkt, &cmdrsp, 1); 435 cmd = cmdrsp & CFCTRL_CMD_MASK; 436 if (cmd != CFCTRL_CMD_LINK_ERR 437 && CFCTRL_RSP_BIT != (CFCTRL_RSP_BIT & cmdrsp)) { 438 if (handle_loop(cfctrl, cmd, pkt) == CAIF_FAILURE) 439 cmdrsp |= CFCTRL_ERR_BIT; 440 } 441 442 switch (cmd) { 443 case CFCTRL_CMD_LINK_SETUP: 444 { 445 enum cfctrl_srv serv; 446 enum cfctrl_srv servtype; 447 u8 endpoint; 448 u8 physlinkid; 449 u8 prio; 450 u8 tmp; 451 u32 tmp32; 452 u8 *cp; 453 int i; 454 struct cfctrl_link_param linkparam; 455 memset(&linkparam, 0, sizeof(linkparam)); 456 457 cfpkt_extr_head(pkt, &tmp, 1); 458 459 serv = tmp & CFCTRL_SRV_MASK; 460 linkparam.linktype = serv; 461 462 servtype = tmp >> 4; 463 linkparam.chtype = servtype; 464 465 cfpkt_extr_head(pkt, &tmp, 1); 466 physlinkid = tmp & 0x07; 467 prio = tmp >> 3; 468 469 linkparam.priority = prio; 470 linkparam.phyid = physlinkid; 471 cfpkt_extr_head(pkt, &endpoint, 1); 472 linkparam.endpoint = endpoint & 0x03; 473 474 switch (serv) { 475 case CFCTRL_SRV_VEI: 476 case CFCTRL_SRV_DBG: 477 if (CFCTRL_ERR_BIT & cmdrsp) 478 break; 479 /* Link ID */ 480 cfpkt_extr_head(pkt, &linkid, 1); 481 break; 482 case CFCTRL_SRV_VIDEO: 483 cfpkt_extr_head(pkt, &tmp, 1); 484 linkparam.u.video.connid = tmp; 485 if (CFCTRL_ERR_BIT & cmdrsp) 486 break; 487 /* Link ID */ 488 cfpkt_extr_head(pkt, &linkid, 1); 489 break; 490 491 case CFCTRL_SRV_DATAGRAM: 492 cfpkt_extr_head(pkt, &tmp32, 4); 493 linkparam.u.datagram.connid = 494 le32_to_cpu(tmp32); 495 if (CFCTRL_ERR_BIT & cmdrsp) 496 break; 497 /* Link ID */ 498 cfpkt_extr_head(pkt, &linkid, 1); 499 break; 500 case CFCTRL_SRV_RFM: 501 /* Construct a frame, convert 502 * DatagramConnectionID 503 * to network format long and copy it out... 504 */ 505 cfpkt_extr_head(pkt, &tmp32, 4); 506 linkparam.u.rfm.connid = 507 le32_to_cpu(tmp32); 508 cp = (u8 *) linkparam.u.rfm.volume; 509 for (cfpkt_extr_head(pkt, &tmp, 1); 510 cfpkt_more(pkt) && tmp != '\0'; 511 cfpkt_extr_head(pkt, &tmp, 1)) 512 *cp++ = tmp; 513 *cp = '\0'; 514 515 if (CFCTRL_ERR_BIT & cmdrsp) 516 break; 517 /* Link ID */ 518 cfpkt_extr_head(pkt, &linkid, 1); 519 520 break; 521 case CFCTRL_SRV_UTIL: 522 /* Construct a frame, convert 523 * DatagramConnectionID 524 * to network format long and copy it out... 525 */ 526 /* Fifosize KB */ 527 cfpkt_extr_head(pkt, &tmp16, 2); 528 linkparam.u.utility.fifosize_kb = 529 le16_to_cpu(tmp16); 530 /* Fifosize bufs */ 531 cfpkt_extr_head(pkt, &tmp16, 2); 532 linkparam.u.utility.fifosize_bufs = 533 le16_to_cpu(tmp16); 534 /* name */ 535 cp = (u8 *) linkparam.u.utility.name; 536 caif_assert(sizeof(linkparam.u.utility.name) 537 >= UTILITY_NAME_LENGTH); 538 for (i = 0; 539 i < UTILITY_NAME_LENGTH 540 && cfpkt_more(pkt); i++) { 541 cfpkt_extr_head(pkt, &tmp, 1); 542 *cp++ = tmp; 543 } 544 /* Length */ 545 cfpkt_extr_head(pkt, &len, 1); 546 linkparam.u.utility.paramlen = len; 547 /* Param Data */ 548 cp = linkparam.u.utility.params; 549 while (cfpkt_more(pkt) && len--) { 550 cfpkt_extr_head(pkt, &tmp, 1); 551 *cp++ = tmp; 552 } 553 if (CFCTRL_ERR_BIT & cmdrsp) 554 break; 555 /* Link ID */ 556 cfpkt_extr_head(pkt, &linkid, 1); 557 /* Length */ 558 cfpkt_extr_head(pkt, &len, 1); 559 /* Param Data */ 560 cfpkt_extr_head(pkt, ¶m, len); 561 break; 562 default: 563 pr_warning("CAIF: %s(): Request setup " 564 "- invalid link type (%d)", 565 __func__, serv); 566 goto error; 567 } 568 569 rsp.cmd = cmd; 570 rsp.param = linkparam; 571 req = cfctrl_remove_req(cfctrl, &rsp); 572 573 if (CFCTRL_ERR_BIT == (CFCTRL_ERR_BIT & cmdrsp) || 574 cfpkt_erroneous(pkt)) { 575 pr_err("CAIF: %s(): Invalid O/E bit or parse " 576 "error on CAIF control channel", 577 __func__); 578 cfctrl->res.reject_rsp(cfctrl->serv.layer.up, 579 0, 580 req ? req->client_layer 581 : NULL); 582 } else { 583 cfctrl->res.linksetup_rsp(cfctrl->serv. 584 layer.up, linkid, 585 serv, physlinkid, 586 req ? req-> 587 client_layer : NULL); 588 } 589 590 if (req != NULL) 591 kfree(req); 592 } 593 break; 594 case CFCTRL_CMD_LINK_DESTROY: 595 cfpkt_extr_head(pkt, &linkid, 1); 596 cfctrl->res.linkdestroy_rsp(cfctrl->serv.layer.up, linkid); 597 break; 598 case CFCTRL_CMD_LINK_ERR: 599 pr_err("CAIF: %s(): Frame Error Indication received\n", 600 __func__); 601 cfctrl->res.linkerror_ind(); 602 break; 603 case CFCTRL_CMD_ENUM: 604 cfctrl->res.enum_rsp(); 605 break; 606 case CFCTRL_CMD_SLEEP: 607 cfctrl->res.sleep_rsp(); 608 break; 609 case CFCTRL_CMD_WAKE: 610 cfctrl->res.wake_rsp(); 611 break; 612 case CFCTRL_CMD_LINK_RECONF: 613 cfctrl->res.restart_rsp(); 614 break; 615 case CFCTRL_CMD_RADIO_SET: 616 cfctrl->res.radioset_rsp(); 617 break; 618 default: 619 pr_err("CAIF: %s(): Unrecognized Control Frame\n", __func__); 620 goto error; 621 break; 622 } 623 ret = 0; 624 error: 625 cfpkt_destroy(pkt); 626 return ret; 627 } 628 629 static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, 630 int phyid) 631 { 632 struct cfctrl *this = container_obj(layr); 633 switch (ctrl) { 634 case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND: 635 case CAIF_CTRLCMD_FLOW_OFF_IND: 636 spin_lock(&this->info_list_lock); 637 if (this->first_req != NULL) { 638 pr_debug("CAIF: %s(): Received flow off in " 639 "control layer", __func__); 640 } 641 spin_unlock(&this->info_list_lock); 642 break; 643 default: 644 break; 645 } 646 } 647 648 #ifndef CAIF_NO_LOOP 649 static int handle_loop(struct cfctrl *ctrl, int cmd, struct cfpkt *pkt) 650 { 651 static int last_linkid; 652 u8 linkid, linktype, tmp; 653 switch (cmd) { 654 case CFCTRL_CMD_LINK_SETUP: 655 spin_lock(&ctrl->loop_linkid_lock); 656 for (linkid = last_linkid + 1; linkid < 255; linkid++) 657 if (!ctrl->loop_linkused[linkid]) 658 goto found; 659 for (linkid = last_linkid - 1; linkid > 0; linkid--) 660 if (!ctrl->loop_linkused[linkid]) 661 goto found; 662 spin_unlock(&ctrl->loop_linkid_lock); 663 pr_err("CAIF: %s(): Out of link-ids\n", __func__); 664 return -EINVAL; 665 found: 666 if (!ctrl->loop_linkused[linkid]) 667 ctrl->loop_linkused[linkid] = 1; 668 669 last_linkid = linkid; 670 671 cfpkt_add_trail(pkt, &linkid, 1); 672 spin_unlock(&ctrl->loop_linkid_lock); 673 cfpkt_peek_head(pkt, &linktype, 1); 674 if (linktype == CFCTRL_SRV_UTIL) { 675 tmp = 0x01; 676 cfpkt_add_trail(pkt, &tmp, 1); 677 cfpkt_add_trail(pkt, &tmp, 1); 678 } 679 break; 680 681 case CFCTRL_CMD_LINK_DESTROY: 682 spin_lock(&ctrl->loop_linkid_lock); 683 cfpkt_peek_head(pkt, &linkid, 1); 684 ctrl->loop_linkused[linkid] = 0; 685 spin_unlock(&ctrl->loop_linkid_lock); 686 break; 687 default: 688 break; 689 } 690 return CAIF_SUCCESS; 691 } 692 #endif 693