diff options
Diffstat (limited to 'kernel/net/caif/cfcnfg.c')
| -rw-r--r-- | kernel/net/caif/cfcnfg.c | 611 |
1 files changed, 611 insertions, 0 deletions
diff --git a/kernel/net/caif/cfcnfg.c b/kernel/net/caif/cfcnfg.c new file mode 100644 index 000000000..fa39fc298 --- /dev/null +++ b/kernel/net/caif/cfcnfg.c @@ -0,0 +1,611 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * 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, ¶m); + 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, ¶m, 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 disappeared" + "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); |