net/caif/cfmuxl.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/rculist.h>
0013 #include <net/caif/cfpkt.h>
0014 #include <net/caif/cfmuxl.h>
0015 #include <net/caif/cfsrvl.h>
0016 #include <net/caif/cffrml.h>
0017
0018 #define container_obj(layr) container_of(layr, struct cfmuxl, layer)
0019
0020 #define CAIF_CTRL_CHANNEL 0
0021 #define UP_CACHE_SIZE 8
0022 #define DN_CACHE_SIZE 8
0023
0024 struct cfmuxl {
0025     struct cflayer layer;
0026     struct list_head srvl_list;
0027     struct list_head frml_list;
0028     struct cflayer *up_cache[UP_CACHE_SIZE];
0029     struct cflayer *dn_cache[DN_CACHE_SIZE];
0030     /*
0031      * Set when inserting or removing downwards layers.
0032      */
0033     spinlock_t transmit_lock;
0034
0035     /*
0036      * Set when inserting or removing upwards layers.
0037      */
0038     spinlock_t receive_lock;
0039
0040 };
0041
0042 static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt);
0043 static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt);
0044 static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
0045                int phyid);
0046 static struct cflayer *get_up(struct cfmuxl *muxl, u16 id);
0047
0048 struct cflayer *cfmuxl_create(void)
0049 {
0050     struct cfmuxl *this = kzalloc(sizeof(struct cfmuxl), GFP_ATOMIC);
0051
0052     if (!this)
0053         return NULL;
0054     this->layer.receive = cfmuxl_receive;
0055     this->layer.transmit = cfmuxl_transmit;
0056     this->layer.ctrlcmd = cfmuxl_ctrlcmd;
0057     INIT_LIST_HEAD(&this->srvl_list);
0058     INIT_LIST_HEAD(&this->frml_list);
0059     spin_lock_init(&this->transmit_lock);
0060     spin_lock_init(&this->receive_lock);
0061     snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "mux");
0062     return &this->layer;
0063 }
0064
0065 int cfmuxl_set_dnlayer(struct cflayer *layr, struct cflayer *dn, u8 phyid)
0066 {
0067     struct cfmuxl *muxl = (struct cfmuxl *) layr;
0068
0069     spin_lock_bh(&muxl->transmit_lock);
0070     list_add_rcu(&dn->node, &muxl->frml_list);
0071     spin_unlock_bh(&muxl->transmit_lock);
0072     return 0;
0073 }
0074
0075 static struct cflayer *get_from_id(struct list_head *list, u16 id)
0076 {
0077     struct cflayer *lyr;
0078     list_for_each_entry_rcu(lyr, list, node) {
0079         if (lyr->id == id)
0080             return lyr;
0081     }
0082
0083     return NULL;
0084 }
0085
0086 int cfmuxl_set_uplayer(struct cflayer *layr, struct cflayer *up, u8 linkid)
0087 {
0088     struct cfmuxl *muxl = container_obj(layr);
0089     struct cflayer *old;
0090
0091     spin_lock_bh(&muxl->receive_lock);
0092
0093     /* Two entries with same id is wrong, so remove old layer from mux */
0094     old = get_from_id(&muxl->srvl_list, linkid);
0095     if (old != NULL)
0096         list_del_rcu(&old->node);
0097
0098     list_add_rcu(&up->node, &muxl->srvl_list);
0099     spin_unlock_bh(&muxl->receive_lock);
0100
0101     return 0;
0102 }
0103
0104 struct cflayer *cfmuxl_remove_dnlayer(struct cflayer *layr, u8 phyid)
0105 {
0106     struct cfmuxl *muxl = container_obj(layr);
0107     struct cflayer *dn;
0108     int idx = phyid % DN_CACHE_SIZE;
0109
0110     spin_lock_bh(&muxl->transmit_lock);
0111     RCU_INIT_POINTER(muxl->dn_cache[idx], NULL);
0112     dn = get_from_id(&muxl->frml_list, phyid);
0113     if (dn == NULL)
0114         goto out;
0115
0116     list_del_rcu(&dn->node);
0117     caif_assert(dn != NULL);
0118 out:
0119     spin_unlock_bh(&muxl->transmit_lock);
0120     return dn;
0121 }
0122
0123 static struct cflayer *get_up(struct cfmuxl *muxl, u16 id)
0124 {
0125     struct cflayer *up;
0126     int idx = id % UP_CACHE_SIZE;
0127     up = rcu_dereference(muxl->up_cache[idx]);
0128     if (up == NULL || up->id != id) {
0129         spin_lock_bh(&muxl->receive_lock);
0130         up = get_from_id(&muxl->srvl_list, id);
0131         rcu_assign_pointer(muxl->up_cache[idx], up);
0132         spin_unlock_bh(&muxl->receive_lock);
0133     }
0134     return up;
0135 }
0136
0137 static struct cflayer *get_dn(struct cfmuxl *muxl, struct dev_info *dev_info)
0138 {
0139     struct cflayer *dn;
0140     int idx = dev_info->id % DN_CACHE_SIZE;
0141     dn = rcu_dereference(muxl->dn_cache[idx]);
0142     if (dn == NULL || dn->id != dev_info->id) {
0143         spin_lock_bh(&muxl->transmit_lock);
0144         dn = get_from_id(&muxl->frml_list, dev_info->id);
0145         rcu_assign_pointer(muxl->dn_cache[idx], dn);
0146         spin_unlock_bh(&muxl->transmit_lock);
0147     }
0148     return dn;
0149 }
0150
0151 struct cflayer *cfmuxl_remove_uplayer(struct cflayer *layr, u8 id)
0152 {
0153     struct cflayer *up;
0154     struct cfmuxl *muxl = container_obj(layr);
0155     int idx = id % UP_CACHE_SIZE;
0156
0157     if (id == 0) {
0158         pr_warn("Trying to remove control layer\n");
0159         return NULL;
0160     }
0161
0162     spin_lock_bh(&muxl->receive_lock);
0163     up = get_from_id(&muxl->srvl_list, id);
0164     if (up == NULL)
0165         goto out;
0166
0167     RCU_INIT_POINTER(muxl->up_cache[idx], NULL);
0168     list_del_rcu(&up->node);
0169 out:
0170     spin_unlock_bh(&muxl->receive_lock);
0171     return up;
0172 }
0173
0174 static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt)
0175 {
0176     int ret;
0177     struct cfmuxl *muxl = container_obj(layr);
0178     u8 id;
0179     struct cflayer *up;
0180     if (cfpkt_extr_head(pkt, &id, 1) < 0) {
0181         pr_err("erroneous Caif Packet\n");
0182         cfpkt_destroy(pkt);
0183         return -EPROTO;
0184     }
0185     rcu_read_lock();
0186     up = get_up(muxl, id);
0187
0188     if (up == NULL) {
0189         pr_debug("Received data on unknown link ID = %d (0x%x)"
0190             " up == NULL", id, id);
0191         cfpkt_destroy(pkt);
0192         /*
0193          * Don't return ERROR, since modem misbehaves and sends out
0194          * flow on before linksetup response.
0195          */
0196
0197         rcu_read_unlock();
0198         return /* CFGLU_EPROT; */ 0;
0199     }
0200
0201     /* We can't hold rcu_lock during receive, so take a ref count instead */
0202     cfsrvl_get(up);
0203     rcu_read_unlock();
0204
0205     ret = up->receive(up, pkt);
0206
0207     cfsrvl_put(up);
0208     return ret;
0209 }
0210
0211 static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt)
0212 {
0213     struct cfmuxl *muxl = container_obj(layr);
0214     int err;
0215     u8 linkid;
0216     struct cflayer *dn;
0217     struct caif_payload_info *info = cfpkt_info(pkt);
0218     BUG_ON(!info);
0219
0220     rcu_read_lock();
0221
0222     dn = get_dn(muxl, info->dev_info);
0223     if (dn == NULL) {
0224         pr_debug("Send data on unknown phy ID = %d (0x%x)\n",
0225             info->dev_info->id, info->dev_info->id);
0226         rcu_read_unlock();
0227         cfpkt_destroy(pkt);
0228         return -ENOTCONN;
0229     }
0230
0231     info->hdr_len += 1;
0232     linkid = info->channel_id;
0233     cfpkt_add_head(pkt, &linkid, 1);
0234
0235     /* We can't hold rcu_lock during receive, so take a ref count instead */
0236     cffrml_hold(dn);
0237
0238     rcu_read_unlock();
0239
0240     err = dn->transmit(dn, pkt);
0241
0242     cffrml_put(dn);
0243     return err;
0244 }
0245
0246 static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
0247                int phyid)
0248 {
0249     struct cfmuxl *muxl = container_obj(layr);
0250     struct cflayer *layer;
0251
0252     rcu_read_lock();
0253     list_for_each_entry_rcu(layer, &muxl->srvl_list, node) {
0254
0255         if (cfsrvl_phyid_match(layer, phyid) && layer->ctrlcmd) {
0256
0257             if ((ctrl == _CAIF_CTRLCMD_PHYIF_DOWN_IND ||
0258                 ctrl == CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND) &&
0259                     layer->id != 0)
0260                 cfmuxl_remove_uplayer(layr, layer->id);
0261
0262             /* NOTE: ctrlcmd is not allowed to block */
0263             layer->ctrlcmd(layer, ctrl, phyid);
0264         }
0265     }
0266     rcu_read_unlock();
0267 }