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