cfcnfg.c

Go to the documentation of this file.

/*
 * Copyright (C) ST-Ericsson AB 2010
 * Author:  Sjur Brendeland/[email protected]
 * License terms: GNU General Public License (GPL) version 2
 */

#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__

#include <linux/kernel.h>
#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>
#include <net/caif/cfctrl.h>
#include <net/caif/cfmuxl.h>
#include <net/caif/cffrml.h>
#include <net/caif/cfserl.h>
#include <net/caif/cfsrvl.h>
#include <net/caif/caif_dev.h>

#define container_obj(layr) container_of(layr, struct cfcnfg, layer)

/* Information about CAIF physical interfaces held by Config Module in order
 * 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 */
    struct cflayer *phy_layer;
    /* Unique identifier of the physical interface */
    unsigned int id;
    /* Preference of the physical in interface */
    enum cfcnfg_phy_preference pref;

    /* Information about the physical device */
    struct dev_info dev_info;

    /* Interface index */
    int ifindex;

    /* Protocol head room added for CAIF link layer */
    int head_room;

    /* Use Start of frame checksum */
    bool use_fcs;
};

struct cfcnfg {
    struct cflayer layer;
    struct cflayer *ctrl;
    struct cflayer *mux;
    struct list_head phys;
    struct mutex lock;
};

static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id,
                 enum cfctrl_srv serv, u8 phyid,
                 struct cflayer *adapt_layer);
static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id);
static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
                 struct cflayer *adapt_layer);
static void cfctrl_resp_func(void);
static void cfctrl_enum_resp(void);

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)
        return NULL;
    this->mux = cfmuxl_create();
    if (!this->mux)
        goto out_of_mem;
    this->ctrl = cfctrl_create();
    if (!this->ctrl)
        goto out_of_mem;
    /* Initiate response functions */
    resp = cfctrl_get_respfuncs(this->ctrl);
    resp->enum_rsp = cfctrl_enum_resp;
    resp->linkerror_ind = cfctrl_resp_func;
    resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
    resp->sleep_rsp = cfctrl_resp_func;
    resp->wake_rsp = cfctrl_resp_func;
    resp->restart_rsp = cfctrl_resp_func;
    resp->radioset_rsp = cfctrl_resp_func;
    resp->linksetup_rsp = cfcnfg_linkup_rsp;
    resp->reject_rsp = cfcnfg_reject_rsp;
    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:
    synchronize_rcu();

    kfree(this->mux);
    kfree(this->ctrl);
    kfree(this);
    return NULL;
}

void cfcnfg_remove(struct cfcnfg *cfg)
{
    might_sleep();
    if (cfg) {
        synchronize_rcu();

        kfree(cfg->mux);
        cfctrl_remove(cfg->ctrl);
        kfree(cfg);
    }
}

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)
{
}

static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
                  enum cfcnfg_phy_preference phy_pref)
{
    /* Try to match with specified preference */
    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 */
    list_for_each_entry_rcu(phy, &cnfg->phys, node)
        if (phy->up)
            return &phy->dev_info;

    return NULL;
}

static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
{
    struct cfcnfg_phyinfo *phy;

    list_for_each_entry_rcu(phy, &cnfg->phys, node)
        if (phy->ifindex == ifi && phy->up)
            return phy->id;
    return -ENODEV;
}

int caif_disconnect_client(struct net *net, struct cflayer *adap_layer)
{
    u8 channel_id;
    struct cfcnfg *cfg = get_cfcnfg(net);

    caif_assert(adap_layer != NULL);
    cfctrl_cancel_req(cfg->ctrl, adap_layer);
    channel_id = adap_layer->id;
    if (channel_id != 0) {
        struct cflayer *servl;
        servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
        cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
        if (servl != NULL)
            layer_set_up(servl, NULL);
    } else
        pr_debug("nothing to disconnect\n");

    /* Do RCU sync before initiating cleanup */
    synchronize_rcu();
    if (adap_layer->ctrlcmd != NULL)
        adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
    return 0;

}
EXPORT_SYMBOL(caif_disconnect_client);

static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
{
}

static const int protohead[CFCTRL_SRV_MASK] = {
    [CFCTRL_SRV_VEI] = 4,
    [CFCTRL_SRV_DATAGRAM] = 7,
    [CFCTRL_SRV_UTIL] = 4,
    [CFCTRL_SRV_RFM] = 3,
    [CFCTRL_SRV_DBG] = 3,
};


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);

    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");
        goto unlock;
    }
    if (adap_layer->receive == NULL) {
        pr_err("adap_layer->receive is NULL\n");
        goto unlock;
    }
    if (adap_layer->ctrlcmd == NULL) {
        pr_err("adap_layer->ctrlcmd == NULL\n");
        goto unlock;
    }

    err = -ENODEV;
    frml = phy->frm_layer;
    if (frml == NULL) {
        pr_err("Specified PHY type does not exist!\n");
        goto unlock;
    }
    caif_assert(param.phyid == phy->id);
    caif_assert(phy->frm_layer->id ==
             param.phyid);
    caif_assert(phy->phy_layer->id ==
             param.phyid);

    *ifindex = phy->ifindex;
    *proto_tail = 2;
    *proto_head = protohead[param.linktype] + phy->head_room;

    rcu_read_unlock();

    /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
    cfctrl_enum_req(cfg->ctrl, param.phyid);
    return cfctrl_linkup_request(cfg->ctrl, &param, adap_layer);

unlock:
    rcu_read_unlock();
    return err;
}
EXPORT_SYMBOL(caif_connect_client);

static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
                 struct cflayer *adapt_layer)
{
    if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
        adapt_layer->ctrlcmd(adapt_layer,
                     CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
}

static void
cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
          u8 phyid, struct cflayer *adapt_layer)
{
    struct cfcnfg *cnfg = container_obj(layer);
    struct cflayer *servicel = NULL;
    struct cfcnfg_phyinfo *phyinfo;
    struct net_device *netdev;

    if (channel_id == 0) {
        pr_warn("received channel_id zero\n");
        if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
            adapt_layer->ctrlcmd(adapt_layer,
                        CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
        return;
    }

    rcu_read_lock();

    if (adapt_layer == NULL) {
        pr_debug("link setup response but no client exist,"
                "send linkdown back\n");
        cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
        goto unlock;
    }

    caif_assert(cnfg != NULL);
    caif_assert(phyid != 0);

    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);

    adapt_layer->id = channel_id;

    switch (serv) {
    case CFCTRL_SRV_VEI:
        servicel = cfvei_create(channel_id, &phyinfo->dev_info);
        break;
    case CFCTRL_SRV_DATAGRAM:
        servicel = cfdgml_create(channel_id,
                    &phyinfo->dev_info);
        break;
    case CFCTRL_SRV_RFM:
        netdev = phyinfo->dev_info.dev;
        servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
                        netdev->mtu);
        break;
    case CFCTRL_SRV_UTIL:
        servicel = cfutill_create(channel_id, &phyinfo->dev_info);
        break;
    case CFCTRL_SRV_VIDEO:
        servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
        break;
    case CFCTRL_SRV_DBG:
        servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
        break;
    default:
        pr_err("Protocol error. Link setup response "
                "- unknown channel type\n");
        goto unlock;
    }
    if (!servicel)
        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);

    rcu_read_unlock();

    servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
    return;
unlock:
    rcu_read_unlock();
}

void
cfcnfg_add_phy_layer(struct cfcnfg *cnfg,
             struct net_device *dev, struct cflayer *phy_layer,
             enum cfcnfg_phy_preference pref,
             struct cflayer *link_support,
             bool fcs, int head_room)
{
    struct cflayer *frml;
    struct cfcnfg_phyinfo *phyinfo = NULL;
    int i;
    u8 phyid;

    mutex_lock(&cnfg->lock);

    /* 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);
    if (!phyinfo)
        goto out_err;

    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->head_room = head_room;
    phyinfo->use_fcs = fcs;

    frml = cffrml_create(phyid, fcs);

    if (!frml)
        goto out_err;
    phyinfo->frm_layer = frml;
    layer_set_up(frml, cnfg->mux);

    if (link_support != NULL) {
        link_support->id = phyid;
        layer_set_dn(frml, link_support);
        layer_set_up(link_support, frml);
        layer_set_dn(link_support, phy_layer);
        layer_set_up(phy_layer, link_support);
    } else {
        layer_set_dn(frml, phy_layer);
        layer_set_up(phy_layer, frml);
    }

    list_add_rcu(&phyinfo->node, &cnfg->phys);
out:
    mutex_unlock(&cnfg->lock);
    return;

out_err:
    kfree(phyinfo);
    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;
    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(phyinfo->frm_layer->id == phyid);

    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);
    if (phy_layer != frml_dn) {
        layer_set_up(frml_dn, NULL);
        layer_set_dn(frml_dn, NULL);
    }
    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);