SAFE public projects git trees. - safe/jmp/linux-2.6/blob - 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 = 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, &param, 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