Back to home page

OSCL-LXR
 
 

     

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/kernel.h>
0010 #include <linux/stddef.h>
0011 #include <linux/slab.h>
0012 #include <linux/netdevice.h>
0013 #include <linux/module.h>
0014 #include <net/caif/caif_layer.h>
0015 #include <net/caif/cfpkt.h>
0016 #include <net/caif/cfcnfg.h>
0017 #include <net/caif/cfctrl.h>
0018 #include <net/caif/cfmuxl.h>
0019 #include <net/caif/cffrml.h>
0020 #include <net/caif/cfserl.h>
0021 #include <net/caif/cfsrvl.h>
0022 #include <net/caif/caif_dev.h>
0023 
0024 #define container_obj(layr) container_of(layr, struct cfcnfg, layer)
0025 
0026 /* Information about CAIF physical interfaces held by Config Module in order
0027  * to manage physical interfaces
0028  */
0029 struct cfcnfg_phyinfo {
0030     struct list_head node;
0031     bool up;
0032 
0033     /* Pointer to the layer below the MUX (framing layer) */
0034     struct cflayer *frm_layer;
0035     /* Pointer to the lowest actual physical layer */
0036     struct cflayer *phy_layer;
0037     /* Unique identifier of the physical interface */
0038     unsigned int id;
0039     /* Preference of the physical in interface */
0040     enum cfcnfg_phy_preference pref;
0041 
0042     /* Information about the physical device */
0043     struct dev_info dev_info;
0044 
0045     /* Interface index */
0046     int ifindex;
0047 
0048     /* Protocol head room added for CAIF link layer */
0049     int head_room;
0050 
0051     /* Use Start of frame checksum */
0052     bool use_fcs;
0053 };
0054 
0055 struct cfcnfg {
0056     struct cflayer layer;
0057     struct cflayer *ctrl;
0058     struct cflayer *mux;
0059     struct list_head phys;
0060     struct mutex lock;
0061 };
0062 
0063 static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id,
0064                   enum cfctrl_srv serv, u8 phyid,
0065                   struct cflayer *adapt_layer);
0066 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id);
0067 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
0068                   struct cflayer *adapt_layer);
0069 static void cfctrl_resp_func(void);
0070 static void cfctrl_enum_resp(void);
0071 
0072 struct cfcnfg *cfcnfg_create(void)
0073 {
0074     struct cfcnfg *this;
0075     struct cfctrl_rsp *resp;
0076 
0077     might_sleep();
0078 
0079     /* Initiate this layer */
0080     this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC);
0081     if (!this)
0082         return NULL;
0083     this->mux = cfmuxl_create();
0084     if (!this->mux)
0085         goto out_of_mem;
0086     this->ctrl = cfctrl_create();
0087     if (!this->ctrl)
0088         goto out_of_mem;
0089     /* Initiate response functions */
0090     resp = cfctrl_get_respfuncs(this->ctrl);
0091     resp->enum_rsp = cfctrl_enum_resp;
0092     resp->linkerror_ind = cfctrl_resp_func;
0093     resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
0094     resp->sleep_rsp = cfctrl_resp_func;
0095     resp->wake_rsp = cfctrl_resp_func;
0096     resp->restart_rsp = cfctrl_resp_func;
0097     resp->radioset_rsp = cfctrl_resp_func;
0098     resp->linksetup_rsp = cfcnfg_linkup_rsp;
0099     resp->reject_rsp = cfcnfg_reject_rsp;
0100     INIT_LIST_HEAD(&this->phys);
0101 
0102     cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
0103     layer_set_dn(this->ctrl, this->mux);
0104     layer_set_up(this->ctrl, this);
0105     mutex_init(&this->lock);
0106 
0107     return this;
0108 out_of_mem:
0109     synchronize_rcu();
0110 
0111     kfree(this->mux);
0112     kfree(this->ctrl);
0113     kfree(this);
0114     return NULL;
0115 }
0116 
0117 void cfcnfg_remove(struct cfcnfg *cfg)
0118 {
0119     might_sleep();
0120     if (cfg) {
0121         synchronize_rcu();
0122 
0123         kfree(cfg->mux);
0124         cfctrl_remove(cfg->ctrl);
0125         kfree(cfg);
0126     }
0127 }
0128 
0129 static void cfctrl_resp_func(void)
0130 {
0131 }
0132 
0133 static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo_rcu(struct cfcnfg *cnfg,
0134                              u8 phyid)
0135 {
0136     struct cfcnfg_phyinfo *phy;
0137 
0138     list_for_each_entry_rcu(phy, &cnfg->phys, node)
0139         if (phy->id == phyid)
0140             return phy;
0141     return NULL;
0142 }
0143 
0144 static void cfctrl_enum_resp(void)
0145 {
0146 }
0147 
0148 static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
0149                   enum cfcnfg_phy_preference phy_pref)
0150 {
0151     /* Try to match with specified preference */
0152     struct cfcnfg_phyinfo *phy;
0153 
0154     list_for_each_entry_rcu(phy, &cnfg->phys, node) {
0155         if (phy->up && phy->pref == phy_pref &&
0156                 phy->frm_layer != NULL)
0157 
0158             return &phy->dev_info;
0159     }
0160 
0161     /* Otherwise just return something */
0162     list_for_each_entry_rcu(phy, &cnfg->phys, node)
0163         if (phy->up)
0164             return &phy->dev_info;
0165 
0166     return NULL;
0167 }
0168 
0169 static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
0170 {
0171     struct cfcnfg_phyinfo *phy;
0172 
0173     list_for_each_entry_rcu(phy, &cnfg->phys, node)
0174         if (phy->ifindex == ifi && phy->up)
0175             return phy->id;
0176     return -ENODEV;
0177 }
0178 
0179 int caif_disconnect_client(struct net *net, struct cflayer *adap_layer)
0180 {
0181     u8 channel_id;
0182     struct cfcnfg *cfg = get_cfcnfg(net);
0183 
0184     caif_assert(adap_layer != NULL);
0185     cfctrl_cancel_req(cfg->ctrl, adap_layer);
0186     channel_id = adap_layer->id;
0187     if (channel_id != 0) {
0188         struct cflayer *servl;
0189         servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
0190         cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
0191         if (servl != NULL)
0192             layer_set_up(servl, NULL);
0193     } else
0194         pr_debug("nothing to disconnect\n");
0195 
0196     /* Do RCU sync before initiating cleanup */
0197     synchronize_rcu();
0198     if (adap_layer->ctrlcmd != NULL)
0199         adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
0200     return 0;
0201 
0202 }
0203 EXPORT_SYMBOL(caif_disconnect_client);
0204 
0205 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
0206 {
0207 }
0208 
0209 static const int protohead[CFCTRL_SRV_MASK] = {
0210     [CFCTRL_SRV_VEI] = 4,
0211     [CFCTRL_SRV_DATAGRAM] = 7,
0212     [CFCTRL_SRV_UTIL] = 4,
0213     [CFCTRL_SRV_RFM] = 3,
0214     [CFCTRL_SRV_DBG] = 3,
0215 };
0216 
0217 
0218 static int caif_connect_req_to_link_param(struct cfcnfg *cnfg,
0219                       struct caif_connect_request *s,
0220                       struct cfctrl_link_param *l)
0221 {
0222     struct dev_info *dev_info;
0223     enum cfcnfg_phy_preference pref;
0224     int res;
0225 
0226     memset(l, 0, sizeof(*l));
0227     /* In caif protocol low value is high priority */
0228     l->priority = CAIF_PRIO_MAX - s->priority + 1;
0229 
0230     if (s->ifindex != 0) {
0231         res = cfcnfg_get_id_from_ifi(cnfg, s->ifindex);
0232         if (res < 0)
0233             return res;
0234         l->phyid = res;
0235     } else {
0236         switch (s->link_selector) {
0237         case CAIF_LINK_HIGH_BANDW:
0238             pref = CFPHYPREF_HIGH_BW;
0239             break;
0240         case CAIF_LINK_LOW_LATENCY:
0241             pref = CFPHYPREF_LOW_LAT;
0242             break;
0243         default:
0244             return -EINVAL;
0245         }
0246         dev_info = cfcnfg_get_phyid(cnfg, pref);
0247         if (dev_info == NULL)
0248             return -ENODEV;
0249         l->phyid = dev_info->id;
0250     }
0251     switch (s->protocol) {
0252     case CAIFPROTO_AT:
0253         l->linktype = CFCTRL_SRV_VEI;
0254         l->endpoint = (s->sockaddr.u.at.type >> 2) & 0x3;
0255         l->chtype = s->sockaddr.u.at.type & 0x3;
0256         break;
0257     case CAIFPROTO_DATAGRAM:
0258         l->linktype = CFCTRL_SRV_DATAGRAM;
0259         l->chtype = 0x00;
0260         l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
0261         break;
0262     case CAIFPROTO_DATAGRAM_LOOP:
0263         l->linktype = CFCTRL_SRV_DATAGRAM;
0264         l->chtype = 0x03;
0265         l->endpoint = 0x00;
0266         l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
0267         break;
0268     case CAIFPROTO_RFM:
0269         l->linktype = CFCTRL_SRV_RFM;
0270         l->u.datagram.connid = s->sockaddr.u.rfm.connection_id;
0271         strlcpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume,
0272             sizeof(l->u.rfm.volume));
0273         break;
0274     case CAIFPROTO_UTIL:
0275         l->linktype = CFCTRL_SRV_UTIL;
0276         l->endpoint = 0x00;
0277         l->chtype = 0x00;
0278         strlcpy(l->u.utility.name, s->sockaddr.u.util.service,
0279             sizeof(l->u.utility.name));
0280         caif_assert(sizeof(l->u.utility.name) > 10);
0281         l->u.utility.paramlen = s->param.size;
0282         if (l->u.utility.paramlen > sizeof(l->u.utility.params))
0283             l->u.utility.paramlen = sizeof(l->u.utility.params);
0284 
0285         memcpy(l->u.utility.params, s->param.data,
0286                l->u.utility.paramlen);
0287 
0288         break;
0289     case CAIFPROTO_DEBUG:
0290         l->linktype = CFCTRL_SRV_DBG;
0291         l->endpoint = s->sockaddr.u.dbg.service;
0292         l->chtype = s->sockaddr.u.dbg.type;
0293         break;
0294     default:
0295         return -EINVAL;
0296     }
0297     return 0;
0298 }
0299 
0300 int caif_connect_client(struct net *net, struct caif_connect_request *conn_req,
0301             struct cflayer *adap_layer, int *ifindex,
0302             int *proto_head, int *proto_tail)
0303 {
0304     struct cflayer *frml;
0305     struct cfcnfg_phyinfo *phy;
0306     int err;
0307     struct cfctrl_link_param param;
0308     struct cfcnfg *cfg = get_cfcnfg(net);
0309 
0310     rcu_read_lock();
0311     err = caif_connect_req_to_link_param(cfg, conn_req, &param);
0312     if (err)
0313         goto unlock;
0314 
0315     phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
0316     if (!phy) {
0317         err = -ENODEV;
0318         goto unlock;
0319     }
0320     err = -EINVAL;
0321 
0322     if (adap_layer == NULL) {
0323         pr_err("adap_layer is zero\n");
0324         goto unlock;
0325     }
0326     if (adap_layer->receive == NULL) {
0327         pr_err("adap_layer->receive is NULL\n");
0328         goto unlock;
0329     }
0330     if (adap_layer->ctrlcmd == NULL) {
0331         pr_err("adap_layer->ctrlcmd == NULL\n");
0332         goto unlock;
0333     }
0334 
0335     err = -ENODEV;
0336     frml = phy->frm_layer;
0337     if (frml == NULL) {
0338         pr_err("Specified PHY type does not exist!\n");
0339         goto unlock;
0340     }
0341     caif_assert(param.phyid == phy->id);
0342     caif_assert(phy->frm_layer->id ==
0343              param.phyid);
0344     caif_assert(phy->phy_layer->id ==
0345              param.phyid);
0346 
0347     *ifindex = phy->ifindex;
0348     *proto_tail = 2;
0349     *proto_head = protohead[param.linktype] + phy->head_room;
0350 
0351     rcu_read_unlock();
0352 
0353     /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
0354     cfctrl_enum_req(cfg->ctrl, param.phyid);
0355     return cfctrl_linkup_request(cfg->ctrl, &param, adap_layer);
0356 
0357 unlock:
0358     rcu_read_unlock();
0359     return err;
0360 }
0361 EXPORT_SYMBOL(caif_connect_client);
0362 
0363 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
0364                   struct cflayer *adapt_layer)
0365 {
0366     if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
0367         adapt_layer->ctrlcmd(adapt_layer,
0368                      CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
0369 }
0370 
0371 static void
0372 cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
0373           u8 phyid, struct cflayer *adapt_layer)
0374 {
0375     struct cfcnfg *cnfg = container_obj(layer);
0376     struct cflayer *servicel = NULL;
0377     struct cfcnfg_phyinfo *phyinfo;
0378     struct net_device *netdev;
0379 
0380     if (channel_id == 0) {
0381         pr_warn("received channel_id zero\n");
0382         if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
0383             adapt_layer->ctrlcmd(adapt_layer,
0384                         CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
0385         return;
0386     }
0387 
0388     rcu_read_lock();
0389 
0390     if (adapt_layer == NULL) {
0391         pr_debug("link setup response but no client exist, send linkdown back\n");
0392         cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
0393         goto unlock;
0394     }
0395 
0396     caif_assert(cnfg != NULL);
0397     caif_assert(phyid != 0);
0398 
0399     phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
0400     if (phyinfo == NULL) {
0401         pr_err("ERROR: Link Layer Device disappeared while connecting\n");
0402         goto unlock;
0403     }
0404 
0405     caif_assert(phyinfo != NULL);
0406     caif_assert(phyinfo->id == phyid);
0407     caif_assert(phyinfo->phy_layer != NULL);
0408     caif_assert(phyinfo->phy_layer->id == phyid);
0409 
0410     adapt_layer->id = channel_id;
0411 
0412     switch (serv) {
0413     case CFCTRL_SRV_VEI:
0414         servicel = cfvei_create(channel_id, &phyinfo->dev_info);
0415         break;
0416     case CFCTRL_SRV_DATAGRAM:
0417         servicel = cfdgml_create(channel_id,
0418                     &phyinfo->dev_info);
0419         break;
0420     case CFCTRL_SRV_RFM:
0421         netdev = phyinfo->dev_info.dev;
0422         servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
0423                         netdev->mtu);
0424         break;
0425     case CFCTRL_SRV_UTIL:
0426         servicel = cfutill_create(channel_id, &phyinfo->dev_info);
0427         break;
0428     case CFCTRL_SRV_VIDEO:
0429         servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
0430         break;
0431     case CFCTRL_SRV_DBG:
0432         servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
0433         break;
0434     default:
0435         pr_err("Protocol error. Link setup response - unknown channel type\n");
0436         goto unlock;
0437     }
0438     if (!servicel)
0439         goto unlock;
0440     layer_set_dn(servicel, cnfg->mux);
0441     cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
0442     layer_set_up(servicel, adapt_layer);
0443     layer_set_dn(adapt_layer, servicel);
0444 
0445     rcu_read_unlock();
0446 
0447     servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
0448     return;
0449 unlock:
0450     rcu_read_unlock();
0451 }
0452 
0453 int
0454 cfcnfg_add_phy_layer(struct cfcnfg *cnfg,
0455              struct net_device *dev, struct cflayer *phy_layer,
0456              enum cfcnfg_phy_preference pref,
0457              struct cflayer *link_support,
0458              bool fcs, int head_room)
0459 {
0460     struct cflayer *frml;
0461     struct cfcnfg_phyinfo *phyinfo = NULL;
0462     int i, res = 0;
0463     u8 phyid;
0464 
0465     mutex_lock(&cnfg->lock);
0466 
0467     /* CAIF protocol allow maximum 6 link-layers */
0468     for (i = 0; i < 7; i++) {
0469         phyid = (dev->ifindex + i) & 0x7;
0470         if (phyid == 0)
0471             continue;
0472         if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
0473             goto got_phyid;
0474     }
0475     pr_warn("Too many CAIF Link Layers (max 6)\n");
0476     res = -EEXIST;
0477     goto out;
0478 
0479 got_phyid:
0480     phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
0481     if (!phyinfo) {
0482         res = -ENOMEM;
0483         goto out;
0484     }
0485 
0486     phy_layer->id = phyid;
0487     phyinfo->pref = pref;
0488     phyinfo->id = phyid;
0489     phyinfo->dev_info.id = phyid;
0490     phyinfo->dev_info.dev = dev;
0491     phyinfo->phy_layer = phy_layer;
0492     phyinfo->ifindex = dev->ifindex;
0493     phyinfo->head_room = head_room;
0494     phyinfo->use_fcs = fcs;
0495 
0496     frml = cffrml_create(phyid, fcs);
0497 
0498     if (!frml) {
0499         res = -ENOMEM;
0500         goto out_err;
0501     }
0502     phyinfo->frm_layer = frml;
0503     layer_set_up(frml, cnfg->mux);
0504 
0505     if (link_support != NULL) {
0506         link_support->id = phyid;
0507         layer_set_dn(frml, link_support);
0508         layer_set_up(link_support, frml);
0509         layer_set_dn(link_support, phy_layer);
0510         layer_set_up(phy_layer, link_support);
0511     } else {
0512         layer_set_dn(frml, phy_layer);
0513         layer_set_up(phy_layer, frml);
0514     }
0515 
0516     list_add_rcu(&phyinfo->node, &cnfg->phys);
0517 out:
0518     mutex_unlock(&cnfg->lock);
0519     return res;
0520 
0521 out_err:
0522     kfree(phyinfo);
0523     mutex_unlock(&cnfg->lock);
0524     return res;
0525 }
0526 EXPORT_SYMBOL(cfcnfg_add_phy_layer);
0527 
0528 int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
0529              bool up)
0530 {
0531     struct cfcnfg_phyinfo *phyinfo;
0532 
0533     rcu_read_lock();
0534     phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
0535     if (phyinfo == NULL) {
0536         rcu_read_unlock();
0537         return -ENODEV;
0538     }
0539 
0540     if (phyinfo->up == up) {
0541         rcu_read_unlock();
0542         return 0;
0543     }
0544     phyinfo->up = up;
0545 
0546     if (up) {
0547         cffrml_hold(phyinfo->frm_layer);
0548         cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
0549                     phy_layer->id);
0550     } else {
0551         cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
0552         cffrml_put(phyinfo->frm_layer);
0553     }
0554 
0555     rcu_read_unlock();
0556     return 0;
0557 }
0558 EXPORT_SYMBOL(cfcnfg_set_phy_state);
0559 
0560 int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
0561 {
0562     struct cflayer *frml, *frml_dn;
0563     u16 phyid;
0564     struct cfcnfg_phyinfo *phyinfo;
0565 
0566     might_sleep();
0567 
0568     mutex_lock(&cnfg->lock);
0569 
0570     phyid = phy_layer->id;
0571     phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
0572 
0573     if (phyinfo == NULL) {
0574         mutex_unlock(&cnfg->lock);
0575         return 0;
0576     }
0577     caif_assert(phyid == phyinfo->id);
0578     caif_assert(phy_layer == phyinfo->phy_layer);
0579     caif_assert(phy_layer->id == phyid);
0580     caif_assert(phyinfo->frm_layer->id == phyid);
0581 
0582     list_del_rcu(&phyinfo->node);
0583     synchronize_rcu();
0584 
0585     /* Fail if reference count is not zero */
0586     if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
0587         pr_info("Wait for device inuse\n");
0588         list_add_rcu(&phyinfo->node, &cnfg->phys);
0589         mutex_unlock(&cnfg->lock);
0590         return -EAGAIN;
0591     }
0592 
0593     frml = phyinfo->frm_layer;
0594     frml_dn = frml->dn;
0595     cffrml_set_uplayer(frml, NULL);
0596     cffrml_set_dnlayer(frml, NULL);
0597     if (phy_layer != frml_dn) {
0598         layer_set_up(frml_dn, NULL);
0599         layer_set_dn(frml_dn, NULL);
0600     }
0601     layer_set_up(phy_layer, NULL);
0602 
0603     if (phyinfo->phy_layer != frml_dn)
0604         kfree(frml_dn);
0605 
0606     cffrml_free(frml);
0607     kfree(phyinfo);
0608     mutex_unlock(&cnfg->lock);
0609 
0610     return 0;
0611 }
0612 EXPORT_SYMBOL(cfcnfg_del_phy_layer);
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!