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 = kmalloc(sizeof(*req), GFP_KERNEL); 288 if (!req) { 289 pr_warning("CAIF: %s(): Out of memory\n", __func__); 290 return -ENOMEM; 291 } 292 memset(req, 0, sizeof(*req)); 293 req->client_layer = user_layer; 294 req->cmd = CFCTRL_CMD_LINK_SETUP; 295 req->param = *param; 296 cfctrl_insert_req(cfctrl, req); 297 init_info(cfpkt_info(pkt), cfctrl); 298 /* 299 * NOTE:Always send linkup and linkdown request on the same 300 * device as the payload. Otherwise old queued up payload 301 * might arrive with the newly allocated channel ID. 302 */ 303 cfpkt_info(pkt)->dev_info->id = param->phyid; 304 ret = 305 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 306 if (ret < 0) { 307 pr_err("CAIF: %s(): Could not transmit linksetup request\n", 308 __func__); 309 cfpkt_destroy(pkt); 310 return -ENODEV; 311 } 312 return 0; 313 } 314 315 int cfctrl_linkdown_req(struct cflayer *layer, u8 channelid, 316 struct cflayer *client) 317 { 318 int ret; 319 struct cfctrl *cfctrl = container_obj(layer); 320 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 321 if (!pkt) { 322 pr_warning("CAIF: %s(): Out of memory\n", __func__); 323 return -ENOMEM; 324 } 325 cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_DESTROY); 326 cfpkt_addbdy(pkt, channelid); 327 init_info(cfpkt_info(pkt), cfctrl); 328 ret = 329 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 330 if (ret < 0) { 331 pr_err("CAIF: %s(): Could not transmit link-down request\n", 332 __func__); 333 cfpkt_destroy(pkt); 334 } 335 return ret; 336 } 337 338 void cfctrl_sleep_req(struct cflayer *layer) 339 { 340 int ret; 341 struct cfctrl *cfctrl = container_obj(layer); 342 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 343 if (!pkt) { 344 pr_warning("CAIF: %s(): Out of memory\n", __func__); 345 return; 346 } 347 cfpkt_addbdy(pkt, CFCTRL_CMD_SLEEP); 348 init_info(cfpkt_info(pkt), cfctrl); 349 ret = 350 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 351 if (ret < 0) 352 cfpkt_destroy(pkt); 353 } 354 355 void cfctrl_wake_req(struct cflayer *layer) 356 { 357 int ret; 358 struct cfctrl *cfctrl = container_obj(layer); 359 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 360 if (!pkt) { 361 pr_warning("CAIF: %s(): Out of memory\n", __func__); 362 return; 363 } 364 cfpkt_addbdy(pkt, CFCTRL_CMD_WAKE); 365 init_info(cfpkt_info(pkt), cfctrl); 366 ret = 367 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 368 if (ret < 0) 369 cfpkt_destroy(pkt); 370 } 371 372 void cfctrl_getstartreason_req(struct cflayer *layer) 373 { 374 int ret; 375 struct cfctrl *cfctrl = container_obj(layer); 376 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 377 if (!pkt) { 378 pr_warning("CAIF: %s(): Out of memory\n", __func__); 379 return; 380 } 381 cfpkt_addbdy(pkt, CFCTRL_CMD_START_REASON); 382 init_info(cfpkt_info(pkt), cfctrl); 383 ret = 384 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 385 if (ret < 0) 386 cfpkt_destroy(pkt); 387 } 388 389 390 void cfctrl_cancel_req(struct cflayer *layr, struct cflayer *adap_layer) 391 { 392 struct cfctrl_request_info *p, *req; 393 struct cfctrl *ctrl = container_obj(layr); 394 spin_lock(&ctrl->info_list_lock); 395 396 if (ctrl->first_req == NULL) { 397 spin_unlock(&ctrl->info_list_lock); 398 return; 399 } 400 401 if (ctrl->first_req->client_layer == adap_layer) { 402 403 req = ctrl->first_req; 404 ctrl->first_req = ctrl->first_req->next; 405 kfree(req); 406 } 407 408 p = ctrl->first_req; 409 while (p != NULL && p->next != NULL) { 410 if (p->next->client_layer == adap_layer) { 411 412 req = p->next; 413 p->next = p->next->next; 414 kfree(p->next); 415 } 416 p = p->next; 417 } 418 419 spin_unlock(&ctrl->info_list_lock); 420 } 421 422 static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt) 423 { 424 u8 cmdrsp; 425 u8 cmd; 426 int ret = -1; 427 u16 tmp16; 428 u8 len; 429 u8 param[255]; 430 u8 linkid; 431 struct cfctrl *cfctrl = container_obj(layer); 432 struct cfctrl_request_info rsp, *req; 433 434 435 cfpkt_extr_head(pkt, &cmdrsp, 1); 436 cmd = cmdrsp & CFCTRL_CMD_MASK; 437 if (cmd != CFCTRL_CMD_LINK_ERR 438 && CFCTRL_RSP_BIT != (CFCTRL_RSP_BIT & cmdrsp)) { 439 if (handle_loop(cfctrl, cmd, pkt) == CAIF_FAILURE) 440 cmdrsp |= CFCTRL_ERR_BIT; 441 } 442 443 switch (cmd) { 444 case CFCTRL_CMD_LINK_SETUP: 445 { 446 enum cfctrl_srv serv; 447 enum cfctrl_srv servtype; 448 u8 endpoint; 449 u8 physlinkid; 450 u8 prio; 451 u8 tmp; 452 u32 tmp32; 453 u8 *cp; 454 int i; 455 struct cfctrl_link_param linkparam; 456 memset(&linkparam, 0, sizeof(linkparam)); 457 458 cfpkt_extr_head(pkt, &tmp, 1); 459 460 serv = tmp & CFCTRL_SRV_MASK; 461 linkparam.linktype = serv; 462 463 servtype = tmp >> 4; 464 linkparam.chtype = servtype; 465 466 cfpkt_extr_head(pkt, &tmp, 1); 467 physlinkid = tmp & 0x07; 468 prio = tmp >> 3; 469 470 linkparam.priority = prio; 471 linkparam.phyid = physlinkid; 472 cfpkt_extr_head(pkt, &endpoint, 1); 473 linkparam.endpoint = endpoint & 0x03; 474 475 switch (serv) { 476 case CFCTRL_SRV_VEI: 477 case CFCTRL_SRV_DBG: 478 if (CFCTRL_ERR_BIT & cmdrsp) 479 break; 480 /* Link ID */ 481 cfpkt_extr_head(pkt, &linkid, 1); 482 break; 483 case CFCTRL_SRV_VIDEO: 484 cfpkt_extr_head(pkt, &tmp, 1); 485 linkparam.u.video.connid = tmp; 486 if (CFCTRL_ERR_BIT & cmdrsp) 487 break; 488 /* Link ID */ 489 cfpkt_extr_head(pkt, &linkid, 1); 490 break; 491 492 case CFCTRL_SRV_DATAGRAM: 493 cfpkt_extr_head(pkt, &tmp32, 4); 494 linkparam.u.datagram.connid = 495 le32_to_cpu(tmp32); 496 if (CFCTRL_ERR_BIT & cmdrsp) 497 break; 498 /* Link ID */ 499 cfpkt_extr_head(pkt, &linkid, 1); 500 break; 501 case CFCTRL_SRV_RFM: 502 /* Construct a frame, convert 503 * DatagramConnectionID 504 * to network format long and copy it out... 505 */ 506 cfpkt_extr_head(pkt, &tmp32, 4); 507 linkparam.u.rfm.connid = 508 le32_to_cpu(tmp32); 509 cp = (u8 *) linkparam.u.rfm.volume; 510 for (cfpkt_extr_head(pkt, &tmp, 1); 511 cfpkt_more(pkt) && tmp != '\0'; 512 cfpkt_extr_head(pkt, &tmp, 1)) 513 *cp++ = tmp; 514 *cp = '\0'; 515 516 if (CFCTRL_ERR_BIT & cmdrsp) 517 break; 518 /* Link ID */ 519 cfpkt_extr_head(pkt, &linkid, 1); 520 521 break; 522 case CFCTRL_SRV_UTIL: 523 /* Construct a frame, convert 524 * DatagramConnectionID 525 * to network format long and copy it out... 526 */ 527 /* Fifosize KB */ 528 cfpkt_extr_head(pkt, &tmp16, 2); 529 linkparam.u.utility.fifosize_kb = 530 le16_to_cpu(tmp16); 531 /* Fifosize bufs */ 532 cfpkt_extr_head(pkt, &tmp16, 2); 533 linkparam.u.utility.fifosize_bufs = 534 le16_to_cpu(tmp16); 535 /* name */ 536 cp = (u8 *) linkparam.u.utility.name; 537 caif_assert(sizeof(linkparam.u.utility.name) 538 >= UTILITY_NAME_LENGTH); 539 for (i = 0; 540 i < UTILITY_NAME_LENGTH 541 && cfpkt_more(pkt); i++) { 542 cfpkt_extr_head(pkt, &tmp, 1); 543 *cp++ = tmp; 544 } 545 /* Length */ 546 cfpkt_extr_head(pkt, &len, 1); 547 linkparam.u.utility.paramlen = len; 548 /* Param Data */ 549 cp = linkparam.u.utility.params; 550 while (cfpkt_more(pkt) && len--) { 551 cfpkt_extr_head(pkt, &tmp, 1); 552 *cp++ = tmp; 553 } 554 if (CFCTRL_ERR_BIT & cmdrsp) 555 break; 556 /* Link ID */ 557 cfpkt_extr_head(pkt, &linkid, 1); 558 /* Length */ 559 cfpkt_extr_head(pkt, &len, 1); 560 /* Param Data */ 561 cfpkt_extr_head(pkt, ¶m, len); 562 break; 563 default: 564 pr_warning("CAIF: %s(): Request setup " 565 "- invalid link type (%d)", 566 __func__, serv); 567 goto error; 568 } 569 570 rsp.cmd = cmd; 571 rsp.param = linkparam; 572 req = cfctrl_remove_req(cfctrl, &rsp); 573 574 if (CFCTRL_ERR_BIT == (CFCTRL_ERR_BIT & cmdrsp) || 575 cfpkt_erroneous(pkt)) { 576 pr_err("CAIF: %s(): Invalid O/E bit or parse " 577 "error on CAIF control channel", 578 __func__); 579 cfctrl->res.reject_rsp(cfctrl->serv.layer.up, 580 0, 581 req ? req->client_layer 582 : NULL); 583 } else { 584 cfctrl->res.linksetup_rsp(cfctrl->serv. 585 layer.up, linkid, 586 serv, physlinkid, 587 req ? req-> 588 client_layer : NULL); 589 } 590 591 if (req != NULL) 592 kfree(req); 593 } 594 break; 595 case CFCTRL_CMD_LINK_DESTROY: 596 cfpkt_extr_head(pkt, &linkid, 1); 597 cfctrl->res.linkdestroy_rsp(cfctrl->serv.layer.up, linkid); 598 break; 599 case CFCTRL_CMD_LINK_ERR: 600 pr_err("CAIF: %s(): Frame Error Indication received\n", 601 __func__); 602 cfctrl->res.linkerror_ind(); 603 break; 604 case CFCTRL_CMD_ENUM: 605 cfctrl->res.enum_rsp(); 606 break; 607 case CFCTRL_CMD_SLEEP: 608 cfctrl->res.sleep_rsp(); 609 break; 610 case CFCTRL_CMD_WAKE: 611 cfctrl->res.wake_rsp(); 612 break; 613 case CFCTRL_CMD_LINK_RECONF: 614 cfctrl->res.restart_rsp(); 615 break; 616 case CFCTRL_CMD_RADIO_SET: 617 cfctrl->res.radioset_rsp(); 618 break; 619 default: 620 pr_err("CAIF: %s(): Unrecognized Control Frame\n", __func__); 621 goto error; 622 break; 623 } 624 ret = 0; 625 error: 626 cfpkt_destroy(pkt); 627 return ret; 628 } 629 630 static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, 631 int phyid) 632 { 633 struct cfctrl *this = container_obj(layr); 634 switch (ctrl) { 635 case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND: 636 case CAIF_CTRLCMD_FLOW_OFF_IND: 637 spin_lock(&this->info_list_lock); 638 if (this->first_req != NULL) { 639 pr_debug("CAIF: %s(): Received flow off in " 640 "control layer", __func__); 641 } 642 spin_unlock(&this->info_list_lock); 643 break; 644 default: 645 break; 646 } 647 } 648 649 #ifndef CAIF_NO_LOOP 650 static int handle_loop(struct cfctrl *ctrl, int cmd, struct cfpkt *pkt) 651 { 652 static int last_linkid; 653 u8 linkid, linktype, tmp; 654 switch (cmd) { 655 case CFCTRL_CMD_LINK_SETUP: 656 spin_lock(&ctrl->loop_linkid_lock); 657 for (linkid = last_linkid + 1; linkid < 255; linkid++) 658 if (!ctrl->loop_linkused[linkid]) 659 goto found; 660 for (linkid = last_linkid - 1; linkid > 0; linkid--) 661 if (!ctrl->loop_linkused[linkid]) 662 goto found; 663 spin_unlock(&ctrl->loop_linkid_lock); 664 pr_err("CAIF: %s(): Out of link-ids\n", __func__); 665 return -EINVAL; 666 found: 667 if (!ctrl->loop_linkused[linkid]) 668 ctrl->loop_linkused[linkid] = 1; 669 670 last_linkid = linkid; 671 672 cfpkt_add_trail(pkt, &linkid, 1); 673 spin_unlock(&ctrl->loop_linkid_lock); 674 cfpkt_peek_head(pkt, &linktype, 1); 675 if (linktype == CFCTRL_SRV_UTIL) { 676 tmp = 0x01; 677 cfpkt_add_trail(pkt, &tmp, 1); 678 cfpkt_add_trail(pkt, &tmp, 1); 679 } 680 break; 681 682 case CFCTRL_CMD_LINK_DESTROY: 683 spin_lock(&ctrl->loop_linkid_lock); 684 cfpkt_peek_head(pkt, &linkid, 1); 685 ctrl->loop_linkused[linkid] = 0; 686 spin_unlock(&ctrl->loop_linkid_lock); 687 break; 688 default: 689 break; 690 } 691 return CAIF_SUCCESS; 692 } 693 #endif 694