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