net/caif/cfctrl.c

   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, &param, 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