1 files changed, 326 insertions, 181 deletions

diff --git a/net/caif/cfcnfg.c b/net/caif/cfcnfg.c
index f1f98d967d8a..351c2ca7e7b9 100644
--- a/net/caif/cfcnfg.c
+++ b/net/caif/cfcnfg.c
@@ -10,6 +10,7 @@
 #include <linux/stddef.h>
 #include <linux/slab.h>
 #include <linux/netdevice.h>
+#include <linux/module.h>
 #include <net/caif/caif_layer.h>
 #include <net/caif/cfpkt.h>
 #include <net/caif/cfcnfg.h>
@@ -18,11 +19,7 @@
 #include <net/caif/cffrml.h>
 #include <net/caif/cfserl.h>
 #include <net/caif/cfsrvl.h>
-
-#include <linux/module.h>
-#include <asm/atomic.h>
-
-#define MAX_PHY_LAYERS 7
+#include <net/caif/caif_dev.h>
 
 #define container_obj(layr) container_of(layr, struct cfcnfg, layer)
 
@@ -30,6 +27,9 @@
  * to manage physical interfaces
  */
 struct cfcnfg_phyinfo {
+        struct list_head node;
+        bool up;
+
         /* Pointer to the layer below the MUX (framing layer) */
         struct cflayer *frm_layer;
         /* Pointer to the lowest actual physical layer */
@@ -39,9 +39,6 @@ struct cfcnfg_phyinfo {
         /* Preference of the physical in interface */
         enum cfcnfg_phy_preference pref;
 
-        /* Reference count, number of channels using the device */
-        int phy_ref_count;
-
         /* Information about the physical device */
         struct dev_info dev_info;
 
@@ -59,8 +56,8 @@ struct cfcnfg {
         struct cflayer layer;
         struct cflayer *ctrl;
         struct cflayer *mux;
-        u8 last_phyid;
-        struct cfcnfg_phyinfo phy_layers[MAX_PHY_LAYERS];
+        struct list_head phys;
+        struct mutex lock;
 };
 
 static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id,
@@ -76,6 +73,9 @@ struct cfcnfg *cfcnfg_create(void)
 {
         struct cfcnfg *this;
         struct cfctrl_rsp *resp;
+
+        might_sleep();
+
         /* Initiate this layer */
         this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC);
         if (!this) {
@@ -99,27 +99,33 @@ struct cfcnfg *cfcnfg_create(void)
         resp->radioset_rsp = cfctrl_resp_func;
         resp->linksetup_rsp = cfcnfg_linkup_rsp;
         resp->reject_rsp = cfcnfg_reject_rsp;
-
-        this->last_phyid = 1;
+        INIT_LIST_HEAD(&this->phys);
 
         cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
         layer_set_dn(this->ctrl, this->mux);
         layer_set_up(this->ctrl, this);
+        mutex_init(&this->lock);
+
         return this;
 out_of_mem:
         pr_warn("Out of memory\n");
+
+        synchronize_rcu();
+
         kfree(this->mux);
         kfree(this->ctrl);
         kfree(this);
         return NULL;
 }
-EXPORT_SYMBOL(cfcnfg_create);
 
 void cfcnfg_remove(struct cfcnfg *cfg)
 {
+        might_sleep();
         if (cfg) {
+                synchronize_rcu();
+
                 kfree(cfg->mux);
-                kfree(cfg->ctrl);
+                cfctrl_remove(cfg->ctrl);
                 kfree(cfg);
         }
 }
@@ -128,132 +134,96 @@ static void cfctrl_resp_func(void)
 {
 }
 
+static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo_rcu(struct cfcnfg *cnfg,
+                                                        u8 phyid)
+{
+        struct cfcnfg_phyinfo *phy;
+
+        list_for_each_entry_rcu(phy, &cnfg->phys, node)
+                if (phy->id == phyid)
+                        return phy;
+        return NULL;
+}
+
 static void cfctrl_enum_resp(void)
 {
 }
 
-struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
+static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
                                   enum cfcnfg_phy_preference phy_pref)
 {
-        u16 i;
-
         /* Try to match with specified preference */
-        for (i = 1; i < MAX_PHY_LAYERS; i++) {
-                if (cnfg->phy_layers[i].id == i &&
-                     cnfg->phy_layers[i].pref == phy_pref &&
-                     cnfg->phy_layers[i].frm_layer != NULL) {
-                        caif_assert(cnfg->phy_layers != NULL);
-                        caif_assert(cnfg->phy_layers[i].id == i);
-                        return &cnfg->phy_layers[i].dev_info;
-                }
+        struct cfcnfg_phyinfo *phy;
+
+        list_for_each_entry_rcu(phy, &cnfg->phys, node) {
+                if (phy->up && phy->pref == phy_pref &&
+                                phy->frm_layer != NULL)
+
+                        return &phy->dev_info;
         }
+
         /* Otherwise just return something */
-        for (i = 1; i < MAX_PHY_LAYERS; i++) {
-                if (cnfg->phy_layers[i].id == i) {
-                        caif_assert(cnfg->phy_layers != NULL);
-                        caif_assert(cnfg->phy_layers[i].id == i);
-                        return &cnfg->phy_layers[i].dev_info;
-                }
-        }
+        list_for_each_entry_rcu(phy, &cnfg->phys, node)
+                if (phy->up)
+                        return &phy->dev_info;
 
         return NULL;
 }
 
-static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo(struct cfcnfg *cnfg,
-                                                        u8 phyid)
+static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
 {
-        int i;
-        /* Try to match with specified preference */
-        for (i = 0; i < MAX_PHY_LAYERS; i++)
-                if (cnfg->phy_layers[i].frm_layer != NULL &&
-                    cnfg->phy_layers[i].id == phyid)
-                        return &cnfg->phy_layers[i];
-        return NULL;
-}
-
+        struct cfcnfg_phyinfo *phy;
 
-int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
-{
-        int i;
-        for (i = 0; i < MAX_PHY_LAYERS; i++)
-                if (cnfg->phy_layers[i].frm_layer != NULL &&
-                                cnfg->phy_layers[i].ifindex == ifi)
-                        return i;
+        list_for_each_entry_rcu(phy, &cnfg->phys, node)
+                if (phy->ifindex == ifi && phy->up)
+                        return phy->id;
         return -ENODEV;
 }
 
-int cfcnfg_disconn_adapt_layer(struct cfcnfg *cnfg, struct cflayer *adap_layer)
+int caif_disconnect_client(struct net *net, struct cflayer *adap_layer)
 {
         u8 channel_id = 0;
         int ret = 0;
         struct cflayer *servl = NULL;
-        struct cfcnfg_phyinfo *phyinfo = NULL;
-        u8 phyid = 0;
+        struct cfcnfg *cfg = get_cfcnfg(net);
 
         caif_assert(adap_layer != NULL);
+
         channel_id = adap_layer->id;
         if (adap_layer->dn == NULL || channel_id == 0) {
                 pr_err("adap_layer->dn == NULL or adap_layer->id is 0\n");
                 ret = -ENOTCONN;
                 goto end;
         }
-        servl = cfmuxl_remove_uplayer(cnfg->mux, channel_id);
+
+        servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
         if (servl == NULL) {
-                pr_err("PROTOCOL ERROR - Error removing service_layer Channel_Id(%d)",
-                       channel_id);
+                pr_err("PROTOCOL ERROR - "
+                                "Error removing service_layer Channel_Id(%d)",
+                                channel_id);
                 ret = -EINVAL;
                 goto end;
         }
-        layer_set_up(servl, NULL);
-        ret = cfctrl_linkdown_req(cnfg->ctrl, channel_id, adap_layer);
-        if (ret)
-                goto end;
-        caif_assert(channel_id == servl->id);
-        if (adap_layer->dn != NULL) {
-                phyid = cfsrvl_getphyid(adap_layer->dn);
-
-                phyinfo = cfcnfg_get_phyinfo(cnfg, phyid);
-                if (phyinfo == NULL) {
-                        pr_warn("No interface to send disconnect to\n");
-                        ret = -ENODEV;
-                        goto end;
-                }
-                if (phyinfo->id != phyid ||
-                        phyinfo->phy_layer->id != phyid ||
-                        phyinfo->frm_layer->id != phyid) {
-                        pr_err("Inconsistency in phy registration\n");
-                        ret = -EINVAL;
-                        goto end;
-                }
-        }
-        if (phyinfo != NULL && --phyinfo->phy_ref_count == 0 &&
-                phyinfo->phy_layer != NULL &&
-                phyinfo->phy_layer->modemcmd != NULL) {
-                phyinfo->phy_layer->modemcmd(phyinfo->phy_layer,
-                                             _CAIF_MODEMCMD_PHYIF_USELESS);
-        }
+
+        ret = cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
+
 end:
-        cfsrvl_put(servl);
-        cfctrl_cancel_req(cnfg->ctrl, adap_layer);
+        cfctrl_cancel_req(cfg->ctrl, adap_layer);
+
+        /* Do RCU sync before initiating cleanup */
+        synchronize_rcu();
         if (adap_layer->ctrlcmd != NULL)
                 adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
         return ret;
 
 }
-EXPORT_SYMBOL(cfcnfg_disconn_adapt_layer);
-
-void cfcnfg_release_adap_layer(struct cflayer *adap_layer)
-{
-        if (adap_layer->dn)
-                cfsrvl_put(adap_layer->dn);
-}
-EXPORT_SYMBOL(cfcnfg_release_adap_layer);
+EXPORT_SYMBOL(caif_disconnect_client);
 
 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
 {
 }
 
-int protohead[CFCTRL_SRV_MASK] = {
+static const int protohead[CFCTRL_SRV_MASK] = {
         [CFCTRL_SRV_VEI] = 4,
         [CFCTRL_SRV_DATAGRAM] = 7,
         [CFCTRL_SRV_UTIL] = 4,
@@ -261,49 +231,157 @@ int protohead[CFCTRL_SRV_MASK] = {
         [CFCTRL_SRV_DBG] = 3,
 };
 
-int cfcnfg_add_adaptation_layer(struct cfcnfg *cnfg,
-                                struct cfctrl_link_param *param,
-                                struct cflayer *adap_layer,
-                                int *ifindex,
+
+static int caif_connect_req_to_link_param(struct cfcnfg *cnfg,
+                                   struct caif_connect_request *s,
+                                   struct cfctrl_link_param *l)
+{
+        struct dev_info *dev_info;
+        enum cfcnfg_phy_preference pref;
+        int res;
+
+        memset(l, 0, sizeof(*l));
+        /* In caif protocol low value is high priority */
+        l->priority = CAIF_PRIO_MAX - s->priority + 1;
+
+        if (s->ifindex != 0) {
+                res = cfcnfg_get_id_from_ifi(cnfg, s->ifindex);
+                if (res < 0)
+                        return res;
+                l->phyid = res;
+        } else {
+                switch (s->link_selector) {
+                case CAIF_LINK_HIGH_BANDW:
+                        pref = CFPHYPREF_HIGH_BW;
+                        break;
+                case CAIF_LINK_LOW_LATENCY:
+                        pref = CFPHYPREF_LOW_LAT;
+                        break;
+                default:
+                        return -EINVAL;
+                }
+                dev_info = cfcnfg_get_phyid(cnfg, pref);
+                if (dev_info == NULL)
+                        return -ENODEV;
+                l->phyid = dev_info->id;
+        }
+        switch (s->protocol) {
+        case CAIFPROTO_AT:
+                l->linktype = CFCTRL_SRV_VEI;
+                l->endpoint = (s->sockaddr.u.at.type >> 2) & 0x3;
+                l->chtype = s->sockaddr.u.at.type & 0x3;
+                break;
+        case CAIFPROTO_DATAGRAM:
+                l->linktype = CFCTRL_SRV_DATAGRAM;
+                l->chtype = 0x00;
+                l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
+                break;
+        case CAIFPROTO_DATAGRAM_LOOP:
+                l->linktype = CFCTRL_SRV_DATAGRAM;
+                l->chtype = 0x03;
+                l->endpoint = 0x00;
+                l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
+                break;
+        case CAIFPROTO_RFM:
+                l->linktype = CFCTRL_SRV_RFM;
+                l->u.datagram.connid = s->sockaddr.u.rfm.connection_id;
+                strncpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume,
+                        sizeof(l->u.rfm.volume)-1);
+                l->u.rfm.volume[sizeof(l->u.rfm.volume)-1] = 0;
+                break;
+        case CAIFPROTO_UTIL:
+                l->linktype = CFCTRL_SRV_UTIL;
+                l->endpoint = 0x00;
+                l->chtype = 0x00;
+                strncpy(l->u.utility.name, s->sockaddr.u.util.service,
+                        sizeof(l->u.utility.name)-1);
+                l->u.utility.name[sizeof(l->u.utility.name)-1] = 0;
+                caif_assert(sizeof(l->u.utility.name) > 10);
+                l->u.utility.paramlen = s->param.size;
+                if (l->u.utility.paramlen > sizeof(l->u.utility.params))
+                        l->u.utility.paramlen = sizeof(l->u.utility.params);
+
+                memcpy(l->u.utility.params, s->param.data,
+                       l->u.utility.paramlen);
+
+                break;
+        case CAIFPROTO_DEBUG:
+                l->linktype = CFCTRL_SRV_DBG;
+                l->endpoint = s->sockaddr.u.dbg.service;
+                l->chtype = s->sockaddr.u.dbg.type;
+                break;
+        default:
+                return -EINVAL;
+        }
+        return 0;
+}
+
+int caif_connect_client(struct net *net, struct caif_connect_request *conn_req,
+                        struct cflayer *adap_layer, int *ifindex,
                                 int *proto_head,
                                 int *proto_tail)
 {
         struct cflayer *frml;
+        struct cfcnfg_phyinfo *phy;
+        int err;
+        struct cfctrl_link_param param;
+        struct cfcnfg *cfg = get_cfcnfg(net);
+        caif_assert(cfg != NULL);
+
+        rcu_read_lock();
+        err = caif_connect_req_to_link_param(cfg, conn_req, &param);
+        if (err)
+                goto unlock;
+
+        phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
+        if (!phy) {
+                err = -ENODEV;
+                goto unlock;
+        }
+        err = -EINVAL;
+
         if (adap_layer == NULL) {
                 pr_err("adap_layer is zero\n");
-                return -EINVAL;
+                goto unlock;
         }
         if (adap_layer->receive == NULL) {
                 pr_err("adap_layer->receive is NULL\n");
-                return -EINVAL;
+                goto unlock;
         }
         if (adap_layer->ctrlcmd == NULL) {
                 pr_err("adap_layer->ctrlcmd == NULL\n");
-                return -EINVAL;
+                goto unlock;
         }
-        frml = cnfg->phy_layers[param->phyid].frm_layer;
+
+        err = -ENODEV;
+        frml = phy->frm_layer;
         if (frml == NULL) {
                 pr_err("Specified PHY type does not exist!\n");
-                return -ENODEV;
+                goto unlock;
         }
-        caif_assert(param->phyid == cnfg->phy_layers[param->phyid].id);
-        caif_assert(cnfg->phy_layers[param->phyid].frm_layer->id ==
-                     param->phyid);
-        caif_assert(cnfg->phy_layers[param->phyid].phy_layer->id ==
-                     param->phyid);
+        caif_assert(param.phyid == phy->id);
+        caif_assert(phy->frm_layer->id ==
+                     param.phyid);
+        caif_assert(phy->phy_layer->id ==
+                     param.phyid);
 
-        *ifindex = cnfg->phy_layers[param->phyid].ifindex;
+        *ifindex = phy->ifindex;
+        *proto_tail = 2;
         *proto_head =
-                protohead[param->linktype]+
-                (cnfg->phy_layers[param->phyid].use_stx ? 1 : 0);
 
-        *proto_tail = 2;
+        protohead[param.linktype] + (phy->use_stx ? 1 : 0);
+
+        rcu_read_unlock();
 
         /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
-        cfctrl_enum_req(cnfg->ctrl, param->phyid);
-        return cfctrl_linkup_request(cnfg->ctrl, param, adap_layer);
+        cfctrl_enum_req(cfg->ctrl, param.phyid);
+        return cfctrl_linkup_request(cfg->ctrl, &param, adap_layer);
+
+unlock:
+        rcu_read_unlock();
+        return err;
 }
-EXPORT_SYMBOL(cfcnfg_add_adaptation_layer);
+EXPORT_SYMBOL(caif_connect_client);
 
 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
                              struct cflayer *adapt_layer)
@@ -315,32 +393,37 @@ static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
 
 static void
 cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
-                 u8 phyid, struct cflayer *adapt_layer)
+                  u8 phyid, struct cflayer *adapt_layer)
 {
         struct cfcnfg *cnfg = container_obj(layer);
         struct cflayer *servicel = NULL;
         struct cfcnfg_phyinfo *phyinfo;
         struct net_device *netdev;
 
+        rcu_read_lock();
+
         if (adapt_layer == NULL) {
-                pr_debug("link setup response but no client exist, send linkdown back\n");
+                pr_debug("link setup response but no client exist,"
+                                "send linkdown back\n");
                 cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
-                return;
+                goto unlock;
         }
 
         caif_assert(cnfg != NULL);
         caif_assert(phyid != 0);
-        phyinfo = &cnfg->phy_layers[phyid];
+
+        phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
+        if (phyinfo == NULL) {
+                pr_err("ERROR: Link Layer Device dissapeared"
+                                "while connecting\n");
+                goto unlock;
+        }
+
+        caif_assert(phyinfo != NULL);
         caif_assert(phyinfo->id == phyid);
         caif_assert(phyinfo->phy_layer != NULL);
         caif_assert(phyinfo->phy_layer->id == phyid);
 
-        phyinfo->phy_ref_count++;
-        if (phyinfo->phy_ref_count == 1 &&
-            phyinfo->phy_layer->modemcmd != NULL) {
-                phyinfo->phy_layer->modemcmd(phyinfo->phy_layer,
-                                             _CAIF_MODEMCMD_PHYIF_USEFULL);
-        }
         adapt_layer->id = channel_id;
 
         switch (serv) {
@@ -348,7 +431,8 @@ cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
                 servicel = cfvei_create(channel_id, &phyinfo->dev_info);
                 break;
         case CFCTRL_SRV_DATAGRAM:
-                servicel = cfdgml_create(channel_id, &phyinfo->dev_info);
+                servicel = cfdgml_create(channel_id,
+                                        &phyinfo->dev_info);
                 break;
         case CFCTRL_SRV_RFM:
                 netdev = phyinfo->dev_info.dev;
@@ -365,94 +449,93 @@ cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
                 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
                 break;
         default:
-                pr_err("Protocol error. Link setup response - unknown channel type\n");
-                return;
+                pr_err("Protocol error. Link setup response "
+                                "- unknown channel type\n");
+                goto unlock;
         }
         if (!servicel) {
                 pr_warn("Out of memory\n");
-                return;
+                goto unlock;
         }
         layer_set_dn(servicel, cnfg->mux);
         cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
         layer_set_up(servicel, adapt_layer);
         layer_set_dn(adapt_layer, servicel);
-        cfsrvl_get(servicel);
+
+        rcu_read_unlock();
+
         servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
+        return;
+unlock:
+        rcu_read_unlock();
 }
 
 void
 cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type,
                      struct net_device *dev, struct cflayer *phy_layer,
-                     u16 *phyid, enum cfcnfg_phy_preference pref,
+                     enum cfcnfg_phy_preference pref,
                      bool fcs, bool stx)
 {
         struct cflayer *frml;
         struct cflayer *phy_driver = NULL;
+        struct cfcnfg_phyinfo *phyinfo;
         int i;
+        u8 phyid;
 
+        mutex_lock(&cnfg->lock);
 
-        if (cnfg->phy_layers[cnfg->last_phyid].frm_layer == NULL) {
-                *phyid = cnfg->last_phyid;
-
-                /* range: * 1..(MAX_PHY_LAYERS-1) */
-                cnfg->last_phyid =
-                    (cnfg->last_phyid % (MAX_PHY_LAYERS - 1)) + 1;
-        } else {
-                *phyid = 0;
-                for (i = 1; i < MAX_PHY_LAYERS; i++) {
-                        if (cnfg->phy_layers[i].frm_layer == NULL) {
-                                *phyid = i;
-                                break;
-                        }
-                }
-        }
-        if (*phyid == 0) {
-                pr_err("No Available PHY ID\n");
-                return;
+        /* CAIF protocol allow maximum 6 link-layers */
+        for (i = 0; i < 7; i++) {
+                phyid = (dev->ifindex + i) & 0x7;
+                if (phyid == 0)
+                        continue;
+                if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
+                        goto got_phyid;
         }
+        pr_warn("Too many CAIF Link Layers (max 6)\n");
+        goto out;
+
+got_phyid:
+        phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
 
         switch (phy_type) {
         case CFPHYTYPE_FRAG:
                 phy_driver =
-                    cfserl_create(CFPHYTYPE_FRAG, *phyid, stx);
+                    cfserl_create(CFPHYTYPE_FRAG, phyid, stx);
                 if (!phy_driver) {
                         pr_warn("Out of memory\n");
-                        return;
+                        goto out;
                 }
-
                 break;
         case CFPHYTYPE_CAIF:
                 phy_driver = NULL;
                 break;
         default:
-                pr_err("%d\n", phy_type);
-                return;
-                break;
+                goto out;
         }
-
-        phy_layer->id = *phyid;
-        cnfg->phy_layers[*phyid].pref = pref;
-        cnfg->phy_layers[*phyid].id = *phyid;
-        cnfg->phy_layers[*phyid].dev_info.id = *phyid;
-        cnfg->phy_layers[*phyid].dev_info.dev = dev;
-        cnfg->phy_layers[*phyid].phy_layer = phy_layer;
-        cnfg->phy_layers[*phyid].phy_ref_count = 0;
-        cnfg->phy_layers[*phyid].ifindex = dev->ifindex;
-        cnfg->phy_layers[*phyid].use_stx = stx;
-        cnfg->phy_layers[*phyid].use_fcs = fcs;
+        phy_layer->id = phyid;
+        phyinfo->pref = pref;
+        phyinfo->id = phyid;
+        phyinfo->dev_info.id = phyid;
+        phyinfo->dev_info.dev = dev;
+        phyinfo->phy_layer = phy_layer;
+        phyinfo->ifindex = dev->ifindex;
+        phyinfo->use_stx = stx;
+        phyinfo->use_fcs = fcs;
 
         phy_layer->type = phy_type;
-        frml = cffrml_create(*phyid, fcs);
+        frml = cffrml_create(phyid, fcs);
+
         if (!frml) {
                 pr_warn("Out of memory\n");
-                return;
+                kfree(phyinfo);
+                goto out;
         }
-        cnfg->phy_layers[*phyid].frm_layer = frml;
-        cfmuxl_set_dnlayer(cnfg->mux, frml, *phyid);
+        phyinfo->frm_layer = frml;
         layer_set_up(frml, cnfg->mux);
 
         if (phy_driver != NULL) {
-                phy_driver->id = *phyid;
+                phy_driver->id = phyid;
                 layer_set_dn(frml, phy_driver);
                 layer_set_up(phy_driver, frml);
                 layer_set_dn(phy_driver, phy_layer);
@@ -461,33 +544,95 @@ cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type,
                 layer_set_dn(frml, phy_layer);
                 layer_set_up(phy_layer, frml);
         }
+
+        list_add_rcu(&phyinfo->node, &cnfg->phys);
+out:
+        mutex_unlock(&cnfg->lock);
 }
 EXPORT_SYMBOL(cfcnfg_add_phy_layer);
 
+int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
+                bool up)
+{
+        struct cfcnfg_phyinfo *phyinfo;
+
+        rcu_read_lock();
+        phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
+        if (phyinfo == NULL) {
+                rcu_read_unlock();
+                return -ENODEV;
+        }
+
+        if (phyinfo->up == up) {
+                rcu_read_unlock();
+                return 0;
+        }
+        phyinfo->up = up;
+
+        if (up) {
+                cffrml_hold(phyinfo->frm_layer);
+                cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
+                                        phy_layer->id);
+        } else {
+                cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
+                cffrml_put(phyinfo->frm_layer);
+        }
+
+        rcu_read_unlock();
+        return 0;
+}
+EXPORT_SYMBOL(cfcnfg_set_phy_state);
+
 int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
 {
         struct cflayer *frml, *frml_dn;
         u16 phyid;
+        struct cfcnfg_phyinfo *phyinfo;
+
+        might_sleep();
+
+        mutex_lock(&cnfg->lock);
+
         phyid = phy_layer->id;
-        caif_assert(phyid == cnfg->phy_layers[phyid].id);
-        caif_assert(phy_layer == cnfg->phy_layers[phyid].phy_layer);
+        phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
+
+        if (phyinfo == NULL) {
+                mutex_unlock(&cnfg->lock);
+                return 0;
+        }
+        caif_assert(phyid == phyinfo->id);
+        caif_assert(phy_layer == phyinfo->phy_layer);
         caif_assert(phy_layer->id == phyid);
-        caif_assert(cnfg->phy_layers[phyid].frm_layer->id == phyid);
+        caif_assert(phyinfo->frm_layer->id == phyid);
 
-        memset(&cnfg->phy_layers[phy_layer->id], 0,
-               sizeof(struct cfcnfg_phyinfo));
-        frml = cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
+        list_del_rcu(&phyinfo->node);
+        synchronize_rcu();
+
+        /* Fail if reference count is not zero */
+        if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
+                pr_info("Wait for device inuse\n");
+                list_add_rcu(&phyinfo->node, &cnfg->phys);
+                mutex_unlock(&cnfg->lock);
+                return -EAGAIN;
+        }
+
+        frml = phyinfo->frm_layer;
         frml_dn = frml->dn;
         cffrml_set_uplayer(frml, NULL);
         cffrml_set_dnlayer(frml, NULL);
-        kfree(frml);
-
         if (phy_layer != frml_dn) {
                 layer_set_up(frml_dn, NULL);
                 layer_set_dn(frml_dn, NULL);
-                kfree(frml_dn);
         }
         layer_set_up(phy_layer, NULL);
+
+        if (phyinfo->phy_layer != frml_dn)
+                kfree(frml_dn);
+
+        cffrml_free(frml);
+        kfree(phyinfo);
+        mutex_unlock(&cnfg->lock);
+
         return 0;
 }
 EXPORT_SYMBOL(cfcnfg_del_phy_layer);