1 files changed, 612 insertions, 0 deletions

diff --git a/net/caif/cfcnfg.c b/net/caif/cfcnfg.c
new file mode 100644
index 00000000..ba9cfd47
--- /dev/null
+++ b/net/caif/cfcnfg.c
@@ -0,0 +1,612 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ * Author:	Sjur Brendeland/sjur.brandeland@stericsson.com
+ * 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);