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