net/caif/cfctrl.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Copyright (C) ST-Ericsson AB 2010
0004  * Author:  Sjur Brendeland
0005  */
0006
0007 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
0008
0009 #include <linux/stddef.h>
0010 #include <linux/spinlock.h>
0011 #include <linux/slab.h>
0012 #include <linux/pkt_sched.h>
0013 #include <net/caif/caif_layer.h>
0014 #include <net/caif/cfpkt.h>
0015 #include <net/caif/cfctrl.h>
0016
0017 #define container_obj(layr) container_of(layr, struct cfctrl, serv.layer)
0018 #define UTILITY_NAME_LENGTH 16
0019 #define CFPKT_CTRL_PKT_LEN 20
0020
0021 #ifdef CAIF_NO_LOOP
0022 static int handle_loop(struct cfctrl *ctrl,
0023                int cmd, struct cfpkt *pkt){
0024     return -1;
0025 }
0026 #else
0027 static int handle_loop(struct cfctrl *ctrl,
0028                int cmd, struct cfpkt *pkt);
0029 #endif
0030 static int cfctrl_recv(struct cflayer *layr, struct cfpkt *pkt);
0031 static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
0032                int phyid);
0033
0034
0035 struct cflayer *cfctrl_create(void)
0036 {
0037     struct dev_info dev_info;
0038     struct cfctrl *this =
0039         kzalloc(sizeof(struct cfctrl), GFP_ATOMIC);
0040     if (!this)
0041         return NULL;
0042     caif_assert(offsetof(struct cfctrl, serv.layer) == 0);
0043     memset(&dev_info, 0, sizeof(dev_info));
0044     dev_info.id = 0xff;
0045     cfsrvl_init(&this->serv, 0, &dev_info, false);
0046     atomic_set(&this->req_seq_no, 1);
0047     atomic_set(&this->rsp_seq_no, 1);
0048     this->serv.layer.receive = cfctrl_recv;
0049     sprintf(this->serv.layer.name, "ctrl");
0050     this->serv.layer.ctrlcmd = cfctrl_ctrlcmd;
0051 #ifndef CAIF_NO_LOOP
0052     spin_lock_init(&this->loop_linkid_lock);
0053     this->loop_linkid = 1;
0054 #endif
0055     spin_lock_init(&this->info_list_lock);
0056     INIT_LIST_HEAD(&this->list);
0057     return &this->serv.layer;
0058 }
0059
0060 void cfctrl_remove(struct cflayer *layer)
0061 {
0062     struct cfctrl_request_info *p, *tmp;
0063     struct cfctrl *ctrl = container_obj(layer);
0064
0065     spin_lock_bh(&ctrl->info_list_lock);
0066     list_for_each_entry_safe(p, tmp, &ctrl->list, list) {
0067         list_del(&p->list);
0068         kfree(p);
0069     }
0070     spin_unlock_bh(&ctrl->info_list_lock);
0071     kfree(layer);
0072 }
0073
0074 static bool param_eq(const struct cfctrl_link_param *p1,
0075              const struct cfctrl_link_param *p2)
0076 {
0077     bool eq =
0078         p1->linktype == p2->linktype &&
0079         p1->priority == p2->priority &&
0080         p1->phyid == p2->phyid &&
0081         p1->endpoint == p2->endpoint && p1->chtype == p2->chtype;
0082
0083     if (!eq)
0084         return false;
0085
0086     switch (p1->linktype) {
0087     case CFCTRL_SRV_VEI:
0088         return true;
0089     case CFCTRL_SRV_DATAGRAM:
0090         return p1->u.datagram.connid == p2->u.datagram.connid;
0091     case CFCTRL_SRV_RFM:
0092         return
0093             p1->u.rfm.connid == p2->u.rfm.connid &&
0094             strcmp(p1->u.rfm.volume, p2->u.rfm.volume) == 0;
0095     case CFCTRL_SRV_UTIL:
0096         return
0097             p1->u.utility.fifosize_kb == p2->u.utility.fifosize_kb
0098             && p1->u.utility.fifosize_bufs ==
0099             p2->u.utility.fifosize_bufs
0100             && strcmp(p1->u.utility.name, p2->u.utility.name) == 0
0101             && p1->u.utility.paramlen == p2->u.utility.paramlen
0102             && memcmp(p1->u.utility.params, p2->u.utility.params,
0103                   p1->u.utility.paramlen) == 0;
0104
0105     case CFCTRL_SRV_VIDEO:
0106         return p1->u.video.connid == p2->u.video.connid;
0107     case CFCTRL_SRV_DBG:
0108         return true;
0109     case CFCTRL_SRV_DECM:
0110         return false;
0111     default:
0112         return false;
0113     }
0114     return false;
0115 }
0116
0117 static bool cfctrl_req_eq(const struct cfctrl_request_info *r1,
0118               const struct cfctrl_request_info *r2)
0119 {
0120     if (r1->cmd != r2->cmd)
0121         return false;
0122     if (r1->cmd == CFCTRL_CMD_LINK_SETUP)
0123         return param_eq(&r1->param, &r2->param);
0124     else
0125         return r1->channel_id == r2->channel_id;
0126 }
0127
0128 /* Insert request at the end */
0129 static void cfctrl_insert_req(struct cfctrl *ctrl,
0130                   struct cfctrl_request_info *req)
0131 {
0132     spin_lock_bh(&ctrl->info_list_lock);
0133     atomic_inc(&ctrl->req_seq_no);
0134     req->sequence_no = atomic_read(&ctrl->req_seq_no);
0135     list_add_tail(&req->list, &ctrl->list);
0136     spin_unlock_bh(&ctrl->info_list_lock);
0137 }
0138
0139 /* Compare and remove request */
0140 static struct cfctrl_request_info *cfctrl_remove_req(struct cfctrl *ctrl,
0141                         struct cfctrl_request_info *req)
0142 {
0143     struct cfctrl_request_info *p, *tmp, *first;
0144
0145     first = list_first_entry(&ctrl->list, struct cfctrl_request_info, list);
0146
0147     list_for_each_entry_safe(p, tmp, &ctrl->list, list) {
0148         if (cfctrl_req_eq(req, p)) {
0149             if (p != first)
0150                 pr_warn("Requests are not received in order\n");
0151
0152             atomic_set(&ctrl->rsp_seq_no,
0153                      p->sequence_no);
0154             list_del(&p->list);
0155             goto out;
0156         }
0157     }
0158     p = NULL;
0159 out:
0160     return p;
0161 }
0162
0163 struct cfctrl_rsp *cfctrl_get_respfuncs(struct cflayer *layer)
0164 {
0165     struct cfctrl *this = container_obj(layer);
0166     return &this->res;
0167 }
0168
0169 static void init_info(struct caif_payload_info *info, struct cfctrl *cfctrl)
0170 {
0171     info->hdr_len = 0;
0172     info->channel_id = cfctrl->serv.layer.id;
0173     info->dev_info = &cfctrl->serv.dev_info;
0174 }
0175
0176 void cfctrl_enum_req(struct cflayer *layer, u8 physlinkid)
0177 {
0178     struct cfpkt *pkt;
0179     struct cfctrl *cfctrl = container_obj(layer);
0180     struct cflayer *dn = cfctrl->serv.layer.dn;
0181
0182     if (!dn) {
0183         pr_debug("not able to send enum request\n");
0184         return;
0185     }
0186     pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
0187     if (!pkt)
0188         return;
0189     caif_assert(offsetof(struct cfctrl, serv.layer) == 0);
0190     init_info(cfpkt_info(pkt), cfctrl);
0191     cfpkt_info(pkt)->dev_info->id = physlinkid;
0192     cfctrl->serv.dev_info.id = physlinkid;
0193     cfpkt_addbdy(pkt, CFCTRL_CMD_ENUM);
0194     cfpkt_addbdy(pkt, physlinkid);
0195     cfpkt_set_prio(pkt, TC_PRIO_CONTROL);
0196     dn->transmit(dn, pkt);
0197 }
0198
0199 int cfctrl_linkup_request(struct cflayer *layer,
0200               struct cfctrl_link_param *param,
0201               struct cflayer *user_layer)
0202 {
0203     struct cfctrl *cfctrl = container_obj(layer);
0204     u32 tmp32;
0205     u16 tmp16;
0206     u8 tmp8;
0207     struct cfctrl_request_info *req;
0208     int ret;
0209     char utility_name[16];
0210     struct cfpkt *pkt;
0211     struct cflayer *dn = cfctrl->serv.layer.dn;
0212
0213     if (!dn) {
0214         pr_debug("not able to send linkup request\n");
0215         return -ENODEV;
0216     }
0217
0218     if (cfctrl_cancel_req(layer, user_layer) > 0) {
0219         /* Slight Paranoia, check if already connecting */
0220         pr_err("Duplicate connect request for same client\n");
0221         WARN_ON(1);
0222         return -EALREADY;
0223     }
0224
0225     pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
0226     if (!pkt)
0227         return -ENOMEM;
0228     cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_SETUP);
0229     cfpkt_addbdy(pkt, (param->chtype << 4) | param->linktype);
0230     cfpkt_addbdy(pkt, (param->priority << 3) | param->phyid);
0231     cfpkt_addbdy(pkt, param->endpoint & 0x03);
0232
0233     switch (param->linktype) {
0234     case CFCTRL_SRV_VEI:
0235         break;
0236     case CFCTRL_SRV_VIDEO:
0237         cfpkt_addbdy(pkt, (u8) param->u.video.connid);
0238         break;
0239     case CFCTRL_SRV_DBG:
0240         break;
0241     case CFCTRL_SRV_DATAGRAM:
0242         tmp32 = cpu_to_le32(param->u.datagram.connid);
0243         cfpkt_add_body(pkt, &tmp32, 4);
0244         break;
0245     case CFCTRL_SRV_RFM:
0246         /* Construct a frame, convert DatagramConnectionID to network
0247          * format long and copy it out...
0248          */
0249         tmp32 = cpu_to_le32(param->u.rfm.connid);
0250         cfpkt_add_body(pkt, &tmp32, 4);
0251         /* Add volume name, including zero termination... */
0252         cfpkt_add_body(pkt, param->u.rfm.volume,
0253                    strlen(param->u.rfm.volume) + 1);
0254         break;
0255     case CFCTRL_SRV_UTIL:
0256         tmp16 = cpu_to_le16(param->u.utility.fifosize_kb);
0257         cfpkt_add_body(pkt, &tmp16, 2);
0258         tmp16 = cpu_to_le16(param->u.utility.fifosize_bufs);
0259         cfpkt_add_body(pkt, &tmp16, 2);
0260         memset(utility_name, 0, sizeof(utility_name));
0261         strlcpy(utility_name, param->u.utility.name,
0262             UTILITY_NAME_LENGTH);
0263         cfpkt_add_body(pkt, utility_name, UTILITY_NAME_LENGTH);
0264         tmp8 = param->u.utility.paramlen;
0265         cfpkt_add_body(pkt, &tmp8, 1);
0266         cfpkt_add_body(pkt, param->u.utility.params,
0267                    param->u.utility.paramlen);
0268         break;
0269     default:
0270         pr_warn("Request setup of bad link type = %d\n",
0271             param->linktype);
0272         return -EINVAL;
0273     }
0274     req = kzalloc(sizeof(*req), GFP_KERNEL);
0275     if (!req)
0276         return -ENOMEM;
0277     req->client_layer = user_layer;
0278     req->cmd = CFCTRL_CMD_LINK_SETUP;
0279     req->param = *param;
0280     cfctrl_insert_req(cfctrl, req);
0281     init_info(cfpkt_info(pkt), cfctrl);
0282     /*
0283      * NOTE:Always send linkup and linkdown request on the same
0284      *  device as the payload. Otherwise old queued up payload
0285      *  might arrive with the newly allocated channel ID.
0286      */
0287     cfpkt_info(pkt)->dev_info->id = param->phyid;
0288     cfpkt_set_prio(pkt, TC_PRIO_CONTROL);
0289     ret =
0290         dn->transmit(dn, pkt);
0291     if (ret < 0) {
0292         int count;
0293
0294         count = cfctrl_cancel_req(&cfctrl->serv.layer,
0295                         user_layer);
0296         if (count != 1) {
0297             pr_err("Could not remove request (%d)", count);
0298             return -ENODEV;
0299         }
0300     }
0301     return 0;
0302 }
0303
0304 int cfctrl_linkdown_req(struct cflayer *layer, u8 channelid,
0305             struct cflayer *client)
0306 {
0307     int ret;
0308     struct cfpkt *pkt;
0309     struct cfctrl *cfctrl = container_obj(layer);
0310     struct cflayer *dn = cfctrl->serv.layer.dn;
0311
0312     if (!dn) {
0313         pr_debug("not able to send link-down request\n");
0314         return -ENODEV;
0315     }
0316     pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
0317     if (!pkt)
0318         return -ENOMEM;
0319     cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_DESTROY);
0320     cfpkt_addbdy(pkt, channelid);
0321     init_info(cfpkt_info(pkt), cfctrl);
0322     cfpkt_set_prio(pkt, TC_PRIO_CONTROL);
0323     ret =
0324         dn->transmit(dn, pkt);
0325 #ifndef CAIF_NO_LOOP
0326     cfctrl->loop_linkused[channelid] = 0;
0327 #endif
0328     return ret;
0329 }
0330
0331 int cfctrl_cancel_req(struct cflayer *layr, struct cflayer *adap_layer)
0332 {
0333     struct cfctrl_request_info *p, *tmp;
0334     struct cfctrl *ctrl = container_obj(layr);
0335     int found = 0;
0336     spin_lock_bh(&ctrl->info_list_lock);
0337
0338     list_for_each_entry_safe(p, tmp, &ctrl->list, list) {
0339         if (p->client_layer == adap_layer) {
0340             list_del(&p->list);
0341             kfree(p);
0342             found++;
0343         }
0344     }
0345
0346     spin_unlock_bh(&ctrl->info_list_lock);
0347     return found;
0348 }
0349
0350 static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt)
0351 {
0352     u8 cmdrsp;
0353     u8 cmd;
0354     int ret = -1;
0355     u8 len;
0356     u8 param[255];
0357     u8 linkid = 0;
0358     struct cfctrl *cfctrl = container_obj(layer);
0359     struct cfctrl_request_info rsp, *req;
0360
0361
0362     cmdrsp = cfpkt_extr_head_u8(pkt);
0363     cmd = cmdrsp & CFCTRL_CMD_MASK;
0364     if (cmd != CFCTRL_CMD_LINK_ERR
0365         && CFCTRL_RSP_BIT != (CFCTRL_RSP_BIT & cmdrsp)
0366         && CFCTRL_ERR_BIT != (CFCTRL_ERR_BIT & cmdrsp)) {
0367         if (handle_loop(cfctrl, cmd, pkt) != 0)
0368             cmdrsp |= CFCTRL_ERR_BIT;
0369     }
0370
0371     switch (cmd) {
0372     case CFCTRL_CMD_LINK_SETUP:
0373         {
0374             enum cfctrl_srv serv;
0375             enum cfctrl_srv servtype;
0376             u8 endpoint;
0377             u8 physlinkid;
0378             u8 prio;
0379             u8 tmp;
0380             u8 *cp;
0381             int i;
0382             struct cfctrl_link_param linkparam;
0383             memset(&linkparam, 0, sizeof(linkparam));
0384
0385             tmp = cfpkt_extr_head_u8(pkt);
0386
0387             serv = tmp & CFCTRL_SRV_MASK;
0388             linkparam.linktype = serv;
0389
0390             servtype = tmp >> 4;
0391             linkparam.chtype = servtype;
0392
0393             tmp = cfpkt_extr_head_u8(pkt);
0394             physlinkid = tmp & 0x07;
0395             prio = tmp >> 3;
0396
0397             linkparam.priority = prio;
0398             linkparam.phyid = physlinkid;
0399             endpoint = cfpkt_extr_head_u8(pkt);
0400             linkparam.endpoint = endpoint & 0x03;
0401
0402             switch (serv) {
0403             case CFCTRL_SRV_VEI:
0404             case CFCTRL_SRV_DBG:
0405                 if (CFCTRL_ERR_BIT & cmdrsp)
0406                     break;
0407                 /* Link ID */
0408                 linkid = cfpkt_extr_head_u8(pkt);
0409                 break;
0410             case CFCTRL_SRV_VIDEO:
0411                 tmp = cfpkt_extr_head_u8(pkt);
0412                 linkparam.u.video.connid = tmp;
0413                 if (CFCTRL_ERR_BIT & cmdrsp)
0414                     break;
0415                 /* Link ID */
0416                 linkid = cfpkt_extr_head_u8(pkt);
0417                 break;
0418
0419             case CFCTRL_SRV_DATAGRAM:
0420                 linkparam.u.datagram.connid =
0421                     cfpkt_extr_head_u32(pkt);
0422                 if (CFCTRL_ERR_BIT & cmdrsp)
0423                     break;
0424                 /* Link ID */
0425                 linkid = cfpkt_extr_head_u8(pkt);
0426                 break;
0427             case CFCTRL_SRV_RFM:
0428                 /* Construct a frame, convert
0429                  * DatagramConnectionID
0430                  * to network format long and copy it out...
0431                  */
0432                 linkparam.u.rfm.connid =
0433                     cfpkt_extr_head_u32(pkt);
0434                 cp = (u8 *) linkparam.u.rfm.volume;
0435                 for (tmp = cfpkt_extr_head_u8(pkt);
0436                      cfpkt_more(pkt) && tmp != '\0';
0437                      tmp = cfpkt_extr_head_u8(pkt))
0438                     *cp++ = tmp;
0439                 *cp = '\0';
0440
0441                 if (CFCTRL_ERR_BIT & cmdrsp)
0442                     break;
0443                 /* Link ID */
0444                 linkid = cfpkt_extr_head_u8(pkt);
0445
0446                 break;
0447             case CFCTRL_SRV_UTIL:
0448                 /* Construct a frame, convert
0449                  * DatagramConnectionID
0450                  * to network format long and copy it out...
0451                  */
0452                 /* Fifosize KB */
0453                 linkparam.u.utility.fifosize_kb =
0454                     cfpkt_extr_head_u16(pkt);
0455                 /* Fifosize bufs */
0456                 linkparam.u.utility.fifosize_bufs =
0457                     cfpkt_extr_head_u16(pkt);
0458                 /* name */
0459                 cp = (u8 *) linkparam.u.utility.name;
0460                 caif_assert(sizeof(linkparam.u.utility.name)
0461                          >= UTILITY_NAME_LENGTH);
0462                 for (i = 0;
0463                      i < UTILITY_NAME_LENGTH
0464                      && cfpkt_more(pkt); i++) {
0465                     tmp = cfpkt_extr_head_u8(pkt);
0466                     *cp++ = tmp;
0467                 }
0468                 /* Length */
0469                 len = cfpkt_extr_head_u8(pkt);
0470                 linkparam.u.utility.paramlen = len;
0471                 /* Param Data */
0472                 cp = linkparam.u.utility.params;
0473                 while (cfpkt_more(pkt) && len--) {
0474                     tmp = cfpkt_extr_head_u8(pkt);
0475                     *cp++ = tmp;
0476                 }
0477                 if (CFCTRL_ERR_BIT & cmdrsp)
0478                     break;
0479                 /* Link ID */
0480                 linkid = cfpkt_extr_head_u8(pkt);
0481                 /* Length */
0482                 len = cfpkt_extr_head_u8(pkt);
0483                 /* Param Data */
0484                 cfpkt_extr_head(pkt, &param, len);
0485                 break;
0486             default:
0487                 pr_warn("Request setup, invalid type (%d)\n",
0488                     serv);
0489                 goto error;
0490             }
0491
0492             rsp.cmd = cmd;
0493             rsp.param = linkparam;
0494             spin_lock_bh(&cfctrl->info_list_lock);
0495             req = cfctrl_remove_req(cfctrl, &rsp);
0496
0497             if (CFCTRL_ERR_BIT == (CFCTRL_ERR_BIT & cmdrsp) ||
0498                 cfpkt_erroneous(pkt)) {
0499                 pr_err("Invalid O/E bit or parse error "
0500                         "on CAIF control channel\n");
0501                 cfctrl->res.reject_rsp(cfctrl->serv.layer.up,
0502                                0,
0503                                req ? req->client_layer
0504                                : NULL);
0505             } else {
0506                 cfctrl->res.linksetup_rsp(cfctrl->serv.
0507                               layer.up, linkid,
0508                               serv, physlinkid,
0509                               req ? req->
0510                               client_layer : NULL);
0511             }
0512
0513             kfree(req);
0514
0515             spin_unlock_bh(&cfctrl->info_list_lock);
0516         }
0517         break;
0518     case CFCTRL_CMD_LINK_DESTROY:
0519         linkid = cfpkt_extr_head_u8(pkt);
0520         cfctrl->res.linkdestroy_rsp(cfctrl->serv.layer.up, linkid);
0521         break;
0522     case CFCTRL_CMD_LINK_ERR:
0523         pr_err("Frame Error Indication received\n");
0524         cfctrl->res.linkerror_ind();
0525         break;
0526     case CFCTRL_CMD_ENUM:
0527         cfctrl->res.enum_rsp();
0528         break;
0529     case CFCTRL_CMD_SLEEP:
0530         cfctrl->res.sleep_rsp();
0531         break;
0532     case CFCTRL_CMD_WAKE:
0533         cfctrl->res.wake_rsp();
0534         break;
0535     case CFCTRL_CMD_LINK_RECONF:
0536         cfctrl->res.restart_rsp();
0537         break;
0538     case CFCTRL_CMD_RADIO_SET:
0539         cfctrl->res.radioset_rsp();
0540         break;
0541     default:
0542         pr_err("Unrecognized Control Frame\n");
0543         goto error;
0544     }
0545     ret = 0;
0546 error:
0547     cfpkt_destroy(pkt);
0548     return ret;
0549 }
0550
0551 static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
0552                int phyid)
0553 {
0554     struct cfctrl *this = container_obj(layr);
0555     switch (ctrl) {
0556     case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND:
0557     case CAIF_CTRLCMD_FLOW_OFF_IND:
0558         spin_lock_bh(&this->info_list_lock);
0559         if (!list_empty(&this->list))
0560             pr_debug("Received flow off in control layer\n");
0561         spin_unlock_bh(&this->info_list_lock);
0562         break;
0563     case _CAIF_CTRLCMD_PHYIF_DOWN_IND: {
0564         struct cfctrl_request_info *p, *tmp;
0565
0566         /* Find all connect request and report failure */
0567         spin_lock_bh(&this->info_list_lock);
0568         list_for_each_entry_safe(p, tmp, &this->list, list) {
0569             if (p->param.phyid == phyid) {
0570                 list_del(&p->list);
0571                 p->client_layer->ctrlcmd(p->client_layer,
0572                         CAIF_CTRLCMD_INIT_FAIL_RSP,
0573                         phyid);
0574                 kfree(p);
0575             }
0576         }
0577         spin_unlock_bh(&this->info_list_lock);
0578         break;
0579     }
0580     default:
0581         break;
0582     }
0583 }
0584
0585 #ifndef CAIF_NO_LOOP
0586 static int handle_loop(struct cfctrl *ctrl, int cmd, struct cfpkt *pkt)
0587 {
0588     static int last_linkid;
0589     static int dec;
0590     u8 linkid, linktype, tmp;
0591     switch (cmd) {
0592     case CFCTRL_CMD_LINK_SETUP:
0593         spin_lock_bh(&ctrl->loop_linkid_lock);
0594         if (!dec) {
0595             for (linkid = last_linkid + 1; linkid < 254; linkid++)
0596                 if (!ctrl->loop_linkused[linkid])
0597                     goto found;
0598         }
0599         dec = 1;
0600         for (linkid = last_linkid - 1; linkid > 1; linkid--)
0601             if (!ctrl->loop_linkused[linkid])
0602                 goto found;
0603         spin_unlock_bh(&ctrl->loop_linkid_lock);
0604         return -1;
0605 found:
0606         if (linkid < 10)
0607             dec = 0;
0608
0609         if (!ctrl->loop_linkused[linkid])
0610             ctrl->loop_linkused[linkid] = 1;
0611
0612         last_linkid = linkid;
0613
0614         cfpkt_add_trail(pkt, &linkid, 1);
0615         spin_unlock_bh(&ctrl->loop_linkid_lock);
0616         cfpkt_peek_head(pkt, &linktype, 1);
0617         if (linktype ==  CFCTRL_SRV_UTIL) {
0618             tmp = 0x01;
0619             cfpkt_add_trail(pkt, &tmp, 1);
0620             cfpkt_add_trail(pkt, &tmp, 1);
0621         }
0622         break;
0623
0624     case CFCTRL_CMD_LINK_DESTROY:
0625         spin_lock_bh(&ctrl->loop_linkid_lock);
0626         cfpkt_peek_head(pkt, &linkid, 1);
0627         ctrl->loop_linkused[linkid] = 0;
0628         spin_unlock_bh(&ctrl->loop_linkid_lock);
0629         break;
0630     default:
0631         break;
0632     }
0633     return 0;
0634 }
0635 #endif