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