[deliverable/linux.git] / net / caif / cfcnfg.c

/*
 * 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;

	/* Use Start of frame extension */
	bool use_stx;

	/* 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) {
		pr_warn("Out of memory\n");
		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:
	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);
		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)
{
}

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

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 cfcnfg_disconn_adapt_layer(struct cfcnfg *cfg, struct cflayer *adap_layer)
{
	u8 channel_id = 0;
	int ret = 0;
	struct cflayer *servl = NULL;

	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(cfg->mux, channel_id);
	if (servl == NULL) {
		pr_err("PROTOCOL ERROR - "
				"Error removing service_layer Channel_Id(%d)",
				channel_id);
		ret = -EINVAL;
		goto end;
	}

	ret = cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);

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

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,
};

int cfcnfg_add_adaptation_layer(struct cfcnfg *cnfg,
				struct cfctrl_link_param *param,
				struct cflayer *adap_layer,
				int *ifindex,
				int *proto_head,
				int *proto_tail)
{
	struct cflayer *frml;
	struct cfcnfg_phyinfo *phy;
	int err;

	rcu_read_lock();
	phy = cfcnfg_get_phyinfo_rcu(cnfg, 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->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);

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

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;

	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) {
		pr_warn("Out of memory\n");
		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, enum cfcnfg_phy_type phy_type,
		     struct net_device *dev, struct cflayer *phy_layer,
		     u16 *phy_id, 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);

	/* 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);
		if (!phy_driver) {
			pr_warn("Out of memory\n");
			goto out;
		}
		break;
	case CFPHYTYPE_CAIF:
		phy_driver = NULL;
		break;
	default:
		goto out;
	}
	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);

	if (!frml) {
		pr_warn("Out of memory\n");
		kfree(phyinfo);
		goto out;
	}
	phyinfo->frm_layer = frml;
	layer_set_up(frml, cnfg->mux);

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

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

	kfree(frml);
	kfree(phyinfo);
	mutex_unlock(&cnfg->lock);

	return 0;
}
EXPORT_SYMBOL(cfcnfg_del_phy_layer);
Commit	Line	Data
15c9ac0c SB	1	/*
	2	* Copyright (C) ST-Ericsson AB 2010
	3	* Author: Sjur Brendeland/sjur.brandeland@stericsson.com
	4	* License terms: GNU General Public License (GPL) version 2
	5	*/
b31fa5ba JP	6
	7	#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
	8
15c9ac0c SB	9	#include <linux/kernel.h>
15c9ac0c SB	10	#include <linux/stddef.h>
6c579906	11	#include <linux/slab.h>
2aa40aef	12	#include <linux/netdevice.h>
f3621440	13	#include <linux/module.h>
15c9ac0c SB	14	#include <net/caif/caif_layer.h>
	15	#include <net/caif/cfpkt.h>
	16	#include <net/caif/cfcnfg.h>
	17	#include <net/caif/cfctrl.h>
	18	#include <net/caif/cfmuxl.h>
	19	#include <net/caif/cffrml.h>
	20	#include <net/caif/cfserl.h>
	21	#include <net/caif/cfsrvl.h>
f3621440	22	#include <net/caif/caif_dev.h>
15c9ac0c SB	23
	24	#define container_obj(layr) container_of(layr, struct cfcnfg, layer)
	25
	26	/* Information about CAIF physical interfaces held by Config Module in order
	27	* to manage physical interfaces
	28	*/
	29	struct cfcnfg_phyinfo {
f3621440	30	struct list_head node;
	31	bool up;
	32
15c9ac0c SB	33	/* Pointer to the layer below the MUX (framing layer) */
	34	struct cflayer *frm_layer;
	35	/* Pointer to the lowest actual physical layer */
	36	struct cflayer *phy_layer;
	37	/* Unique identifier of the physical interface */
	38	unsigned int id;
	39	/* Preference of the physical in interface */
	40	enum cfcnfg_phy_preference pref;
	41
15c9ac0c SB	42	/* Information about the physical device */
15c9ac0c SB	43	struct dev_info dev_info;
2aa40aef SB	44
	45	/* Interface index */
	46	int ifindex;
	47
	48	/* Use Start of frame extension */
	49	bool use_stx;
	50
	51	/* Use Start of frame checksum */
	52	bool use_fcs;
15c9ac0c SB	53	};
	54
	55	struct cfcnfg {
	56	struct cflayer layer;
	57	struct cflayer *ctrl;
	58	struct cflayer *mux;
f3621440	59	struct list_head phys;
f3621440	60	struct mutex lock;
15c9ac0c SB	61	};
15c9ac0c SB	62
e539d83c	63	static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id,
15c9ac0c SB	64	enum cfctrl_srv serv, u8 phyid,
15c9ac0c SB	65	struct cflayer *adapt_layer);
8d545c8f	66	static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id);
e539d83c	67	static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
15c9ac0c SB	68	struct cflayer *adapt_layer);
	69	static void cfctrl_resp_func(void);
	70	static void cfctrl_enum_resp(void);
	71
	72	struct cfcnfg *cfcnfg_create(void)
	73	{
	74	struct cfcnfg *this;
	75	struct cfctrl_rsp *resp;
f3621440	76
	77	might_sleep();
	78
15c9ac0c	79	/* Initiate this layer */
49afa55b	80	this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC);
15c9ac0c	81	if (!this) {
b31fa5ba	82	pr_warn("Out of memory\n");
15c9ac0c SB	83	return NULL;
15c9ac0c SB	84	}
15c9ac0c SB	85	this->mux = cfmuxl_create();
	86	if (!this->mux)
	87	goto out_of_mem;
	88	this->ctrl = cfctrl_create();
	89	if (!this->ctrl)
	90	goto out_of_mem;
	91	/* Initiate response functions */
	92	resp = cfctrl_get_respfuncs(this->ctrl);
	93	resp->enum_rsp = cfctrl_enum_resp;
	94	resp->linkerror_ind = cfctrl_resp_func;
e539d83c	95	resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
15c9ac0c SB	96	resp->sleep_rsp = cfctrl_resp_func;
	97	resp->wake_rsp = cfctrl_resp_func;
	98	resp->restart_rsp = cfctrl_resp_func;
	99	resp->radioset_rsp = cfctrl_resp_func;
e539d83c SB	100	resp->linksetup_rsp = cfcnfg_linkup_rsp;
e539d83c SB	101	resp->reject_rsp = cfcnfg_reject_rsp;
f3621440	102	INIT_LIST_HEAD(&this->phys);
15c9ac0c SB	103
	104	cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
	105	layer_set_dn(this->ctrl, this->mux);
	106	layer_set_up(this->ctrl, this);
f3621440	107	mutex_init(&this->lock);
f3621440	108
15c9ac0c SB	109	return this;
15c9ac0c SB	110	out_of_mem:
b31fa5ba	111	pr_warn("Out of memory\n");
f3621440	112
	113	synchronize_rcu();
	114
15c9ac0c SB	115	kfree(this->mux);
	116	kfree(this->ctrl);
	117	kfree(this);
	118	return NULL;
	119	}
	120	EXPORT_SYMBOL(cfcnfg_create);
	121
	122	void cfcnfg_remove(struct cfcnfg *cfg)
	123	{
f3621440	124	might_sleep();
15c9ac0c	125	if (cfg) {
f3621440	126	synchronize_rcu();
f3621440	127
15c9ac0c SB	128	kfree(cfg->mux);
	129	kfree(cfg->ctrl);
	130	kfree(cfg);
	131	}
	132	}
	133
	134	static void cfctrl_resp_func(void)
	135	{
	136	}
	137
f3621440	138	static struct cfcnfg_phyinfo cfcnfg_get_phyinfo_rcu(struct cfcnfg cnfg,
	139	u8 phyid)
	140	{
	141	struct cfcnfg_phyinfo *phy;
	142
	143	list_for_each_entry_rcu(phy, &cnfg->phys, node)
	144	if (phy->id == phyid)
	145	return phy;
	146	return NULL;
	147	}
	148
15c9ac0c SB	149	static void cfctrl_enum_resp(void)
	150	{
	151	}
	152
	153	struct dev_info cfcnfg_get_phyid(struct cfcnfg cnfg,
	154	enum cfcnfg_phy_preference phy_pref)
	155	{
15c9ac0c	156	/* Try to match with specified preference */
f3621440	157	struct cfcnfg_phyinfo *phy;
	158
	159	list_for_each_entry_rcu(phy, &cnfg->phys, node) {
	160	if (phy->up && phy->pref == phy_pref &&
	161	phy->frm_layer != NULL)
	162
	163	return &phy->dev_info;
15c9ac0c SB	164	}
15c9ac0c SB	165
f3621440	166	/* Otherwise just return something */
	167	list_for_each_entry_rcu(phy, &cnfg->phys, node)
	168	if (phy->up)
	169	return &phy->dev_info;
15c9ac0c	170
15c9ac0c SB	171	return NULL;
	172	}
	173
f2527ec4	174	int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
15c9ac0c	175	{
f3621440	176	struct cfcnfg_phyinfo *phy;
	177
	178	list_for_each_entry_rcu(phy, &cnfg->phys, node)
	179	if (phy->ifindex == ifi && phy->up)
	180	return phy->id;
f2527ec4	181	return -ENODEV;
15c9ac0c SB	182	}
15c9ac0c SB	183
f3621440	184	int cfcnfg_disconn_adapt_layer(struct cfcnfg cfg, struct cflayer adap_layer)
15c9ac0c SB	185	{
	186	u8 channel_id = 0;
	187	int ret = 0;
8d545c8f	188	struct cflayer *servl = NULL;
01a85901	189
15c9ac0c	190	caif_assert(adap_layer != NULL);
f3621440	191
15c9ac0c	192	channel_id = adap_layer->id;
8d545c8f	193	if (adap_layer->dn == NULL \|\| channel_id == 0) {
b04367df	194	pr_err("adap_layer->dn == NULL or adap_layer->id is 0\n");
15c9ac0c SB	195	ret = -ENOTCONN;
	196	goto end;
	197	}
f3621440	198
f3621440	199	servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
8d545c8f	200	if (servl == NULL) {
f3621440	201	pr_err("PROTOCOL ERROR - "
	202	"Error removing service_layer Channel_Id(%d)",
	203	channel_id);
15c9ac0c SB	204	ret = -EINVAL;
	205	goto end;
	206	}
f3621440	207
	208	ret = cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
	209
8d545c8f	210	end:
f3621440	211	cfctrl_cancel_req(cfg->ctrl, adap_layer);
	212
	213	/* Do RCU sync before initiating cleanup */
	214	synchronize_rcu();
8d545c8f SB	215	if (adap_layer->ctrlcmd != NULL)
8d545c8f SB	216	adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
15c9ac0c SB	217	return ret;
	218
	219	}
e539d83c	220	EXPORT_SYMBOL(cfcnfg_disconn_adapt_layer);
15c9ac0c	221
5b208656 SB	222	void cfcnfg_release_adap_layer(struct cflayer *adap_layer)
	223	{
	224	if (adap_layer->dn)
	225	cfsrvl_put(adap_layer->dn);
	226	}
	227	EXPORT_SYMBOL(cfcnfg_release_adap_layer);
	228
8d545c8f	229	static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
15c9ac0c	230	{
15c9ac0c SB	231	}
15c9ac0c SB	232
73d6ac63	233	static const int protohead[CFCTRL_SRV_MASK] = {
2aa40aef SB	234	[CFCTRL_SRV_VEI] = 4,
	235	[CFCTRL_SRV_DATAGRAM] = 7,
	236	[CFCTRL_SRV_UTIL] = 4,
	237	[CFCTRL_SRV_RFM] = 3,
	238	[CFCTRL_SRV_DBG] = 3,
	239	};
	240
8d545c8f	241	int cfcnfg_add_adaptation_layer(struct cfcnfg *cnfg,
15c9ac0c	242	struct cfctrl_link_param *param,
2aa40aef SB	243	struct cflayer *adap_layer,
	244	int *ifindex,
	245	int *proto_head,
	246	int *proto_tail)
15c9ac0c SB	247	{
15c9ac0c SB	248	struct cflayer *frml;
f3621440	249	struct cfcnfg_phyinfo *phy;
	250	int err;
	251
	252	rcu_read_lock();
	253	phy = cfcnfg_get_phyinfo_rcu(cnfg, param->phyid);
	254	if (!phy) {
	255	err = -ENODEV;
	256	goto unlock;
	257	}
	258	err = -EINVAL;
	259
15c9ac0c	260	if (adap_layer == NULL) {
b31fa5ba	261	pr_err("adap_layer is zero\n");
f3621440	262	goto unlock;
15c9ac0c SB	263	}
15c9ac0c SB	264	if (adap_layer->receive == NULL) {
b31fa5ba	265	pr_err("adap_layer->receive is NULL\n");
f3621440	266	goto unlock;
15c9ac0c SB	267	}
15c9ac0c SB	268	if (adap_layer->ctrlcmd == NULL) {
b31fa5ba	269	pr_err("adap_layer->ctrlcmd == NULL\n");
f3621440	270	goto unlock;
15c9ac0c	271	}
f3621440	272
	273	err = -ENODEV;
	274	frml = phy->frm_layer;
15c9ac0c	275	if (frml == NULL) {
b31fa5ba	276	pr_err("Specified PHY type does not exist!\n");
f3621440	277	goto unlock;
15c9ac0c	278	}
f3621440	279	caif_assert(param->phyid == phy->id);
f3621440	280	caif_assert(phy->frm_layer->id ==
15c9ac0c	281	param->phyid);
f3621440	282	caif_assert(phy->phy_layer->id ==
15c9ac0c	283	param->phyid);
2aa40aef	284
f3621440	285	*ifindex = phy->ifindex;
f3621440	286	*proto_tail = 2;
2aa40aef	287	*proto_head =
f3621440	288	protohead[param->linktype] + (phy->use_stx ? 1 : 0);
2aa40aef	289
f3621440	290	rcu_read_unlock();
2aa40aef	291
15c9ac0c SB	292	/* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
15c9ac0c SB	293	cfctrl_enum_req(cnfg->ctrl, param->phyid);
8d545c8f	294	return cfctrl_linkup_request(cnfg->ctrl, param, adap_layer);
f3621440	295
	296	unlock:
	297	rcu_read_unlock();
	298	return err;
15c9ac0c SB	299	}
	300	EXPORT_SYMBOL(cfcnfg_add_adaptation_layer);
	301
e539d83c	302	static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
15c9ac0c SB	303	struct cflayer *adapt_layer)
	304	{
	305	if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
	306	adapt_layer->ctrlcmd(adapt_layer,
	307	CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
	308	}
	309
	310	static void
e539d83c	311	cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
f3621440	312	u8 phyid, struct cflayer *adapt_layer)
15c9ac0c SB	313	{
	314	struct cfcnfg *cnfg = container_obj(layer);
	315	struct cflayer *servicel = NULL;
	316	struct cfcnfg_phyinfo *phyinfo;
2aa40aef SB	317	struct net_device *netdev;
2aa40aef SB	318
f3621440	319	rcu_read_lock();
f3621440	320
15c9ac0c	321	if (adapt_layer == NULL) {
f3621440	322	pr_debug("link setup response but no client exist,"
f3621440	323	"send linkdown back\n");
8d545c8f	324	cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
f3621440	325	goto unlock;
15c9ac0c SB	326	}
	327
	328	caif_assert(cnfg != NULL);
	329	caif_assert(phyid != 0);
f3621440	330
	331	phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
	332	if (phyinfo == NULL) {
	333	pr_err("ERROR: Link Layer Device dissapeared"
	334	"while connecting\n");
	335	goto unlock;
	336	}
	337
	338	caif_assert(phyinfo != NULL);
15c9ac0c SB	339	caif_assert(phyinfo->id == phyid);
	340	caif_assert(phyinfo->phy_layer != NULL);
	341	caif_assert(phyinfo->phy_layer->id == phyid);
	342
e539d83c	343	adapt_layer->id = channel_id;
15c9ac0c SB	344
	345	switch (serv) {
	346	case CFCTRL_SRV_VEI:
e539d83c	347	servicel = cfvei_create(channel_id, &phyinfo->dev_info);
15c9ac0c SB	348	break;
15c9ac0c SB	349	case CFCTRL_SRV_DATAGRAM:
f3621440	350	servicel = cfdgml_create(channel_id,
f3621440	351	&phyinfo->dev_info);
15c9ac0c SB	352	break;
15c9ac0c SB	353	case CFCTRL_SRV_RFM:
2aa40aef	354	netdev = phyinfo->dev_info.dev;
a7da1f55	355	servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
2aa40aef	356	netdev->mtu);
15c9ac0c SB	357	break;
15c9ac0c SB	358	case CFCTRL_SRV_UTIL:
e539d83c	359	servicel = cfutill_create(channel_id, &phyinfo->dev_info);
15c9ac0c SB	360	break;
15c9ac0c SB	361	case CFCTRL_SRV_VIDEO:
e539d83c	362	servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
15c9ac0c SB	363	break;
15c9ac0c SB	364	case CFCTRL_SRV_DBG:
e539d83c	365	servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
15c9ac0c SB	366	break;
15c9ac0c SB	367	default:
f3621440	368	pr_err("Protocol error. Link setup response "
	369	"- unknown channel type\n");
	370	goto unlock;
15c9ac0c SB	371	}
15c9ac0c SB	372	if (!servicel) {
b31fa5ba	373	pr_warn("Out of memory\n");
f3621440	374	goto unlock;
15c9ac0c SB	375	}
15c9ac0c SB	376	layer_set_dn(servicel, cnfg->mux);
e539d83c	377	cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
15c9ac0c SB	378	layer_set_up(servicel, adapt_layer);
15c9ac0c SB	379	layer_set_dn(adapt_layer, servicel);
f3621440	380
	381	rcu_read_unlock();
	382
15c9ac0c	383	servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
f3621440	384	return;
	385	unlock:
	386	rcu_read_unlock();
15c9ac0c SB	387	}
	388
	389	void
	390	cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type,
2aa40aef	391	struct net_device dev, struct cflayer phy_layer,
f3621440	392	u16 *phy_id, enum cfcnfg_phy_preference pref,
15c9ac0c SB	393	bool fcs, bool stx)
	394	{
	395	struct cflayer *frml;
	396	struct cflayer *phy_driver = NULL;
f3621440	397	struct cfcnfg_phyinfo *phyinfo;
15c9ac0c	398	int i;
f3621440	399	u8 phyid;
15c9ac0c	400
f3621440	401	mutex_lock(&cnfg->lock);
15c9ac0c	402
f3621440	403	/* CAIF protocol allow maximum 6 link-layers */
	404	for (i = 0; i < 7; i++) {
	405	phyid = (dev->ifindex + i) & 0x7;
	406	if (phyid == 0)
	407	continue;
	408	if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
	409	goto got_phyid;
15c9ac0c	410	}
f3621440	411	pr_warn("Too many CAIF Link Layers (max 6)\n");
	412	goto out;
	413
	414	got_phyid:
	415	phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
15c9ac0c SB	416
	417	switch (phy_type) {
	418	case CFPHYTYPE_FRAG:
	419	phy_driver =
f3621440	420	cfserl_create(CFPHYTYPE_FRAG, phyid, stx);
15c9ac0c	421	if (!phy_driver) {
b31fa5ba	422	pr_warn("Out of memory\n");
f3621440	423	goto out;
15c9ac0c	424	}
15c9ac0c SB	425	break;
	426	case CFPHYTYPE_CAIF:
	427	phy_driver = NULL;
	428	break;
	429	default:
f3621440	430	goto out;
15c9ac0c	431	}
f3621440	432	phy_layer->id = phyid;
	433	phyinfo->pref = pref;
	434	phyinfo->id = phyid;
	435	phyinfo->dev_info.id = phyid;
	436	phyinfo->dev_info.dev = dev;
	437	phyinfo->phy_layer = phy_layer;
	438	phyinfo->ifindex = dev->ifindex;
	439	phyinfo->use_stx = stx;
	440	phyinfo->use_fcs = fcs;
2aa40aef	441
15c9ac0c	442	phy_layer->type = phy_type;
f3621440	443	frml = cffrml_create(phyid, fcs);
f3621440	444
15c9ac0c	445	if (!frml) {
b31fa5ba	446	pr_warn("Out of memory\n");
f3621440	447	kfree(phyinfo);
f3621440	448	goto out;
15c9ac0c	449	}
f3621440	450	phyinfo->frm_layer = frml;
15c9ac0c SB	451	layer_set_up(frml, cnfg->mux);
	452
	453	if (phy_driver != NULL) {
f3621440	454	phy_driver->id = phyid;
15c9ac0c SB	455	layer_set_dn(frml, phy_driver);
	456	layer_set_up(phy_driver, frml);
	457	layer_set_dn(phy_driver, phy_layer);
	458	layer_set_up(phy_layer, phy_driver);
	459	} else {
	460	layer_set_dn(frml, phy_layer);
	461	layer_set_up(phy_layer, frml);
	462	}
f3621440	463
	464	list_add_rcu(&phyinfo->node, &cnfg->phys);
	465	out:
	466	mutex_unlock(&cnfg->lock);
15c9ac0c SB	467	}
	468	EXPORT_SYMBOL(cfcnfg_add_phy_layer);
	469
f3621440	470	int cfcnfg_set_phy_state(struct cfcnfg cnfg, struct cflayer phy_layer,
	471	bool up)
	472	{
	473	struct cfcnfg_phyinfo *phyinfo;
	474
	475	rcu_read_lock();
	476	phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
	477	if (phyinfo == NULL) {
	478	rcu_read_unlock();
	479	return -ENODEV;
	480	}
	481
	482	if (phyinfo->up == up) {
	483	rcu_read_unlock();
	484	return 0;
	485	}
	486	phyinfo->up = up;
	487
	488	if (up) {
	489	cffrml_hold(phyinfo->frm_layer);
	490	cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
	491	phy_layer->id);
	492	} else {
	493	cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
	494	cffrml_put(phyinfo->frm_layer);
	495	}
	496
	497	rcu_read_unlock();
	498	return 0;
	499	}
	500	EXPORT_SYMBOL(cfcnfg_set_phy_state);
	501
15c9ac0c SB	502	int cfcnfg_del_phy_layer(struct cfcnfg cnfg, struct cflayer phy_layer)
	503	{
	504	struct cflayer frml, frml_dn;
	505	u16 phyid;
f3621440	506	struct cfcnfg_phyinfo *phyinfo;
	507
	508	might_sleep();
	509
	510	mutex_lock(&cnfg->lock);
	511
15c9ac0c	512	phyid = phy_layer->id;
f3621440	513	phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
	514
	515	if (phyinfo == NULL)
	516	return 0;
	517	caif_assert(phyid == phyinfo->id);
	518	caif_assert(phy_layer == phyinfo->phy_layer);
15c9ac0c	519	caif_assert(phy_layer->id == phyid);
f3621440	520	caif_assert(phyinfo->frm_layer->id == phyid);
	521
	522	list_del_rcu(&phyinfo->node);
	523	synchronize_rcu();
15c9ac0c	524
f3621440	525	frml = phyinfo->frm_layer;
15c9ac0c SB	526	frml_dn = frml->dn;
	527	cffrml_set_uplayer(frml, NULL);
	528	cffrml_set_dnlayer(frml, NULL);
15c9ac0c SB	529	if (phy_layer != frml_dn) {
	530	layer_set_up(frml_dn, NULL);
	531	layer_set_dn(frml_dn, NULL);
15c9ac0c SB	532	}
15c9ac0c SB	533	layer_set_up(phy_layer, NULL);
f3621440	534
	535
	536
	537	if (phyinfo->phy_layer != frml_dn)
	538	kfree(frml_dn);
	539
	540	kfree(frml);
	541	kfree(phyinfo);
	542	mutex_unlock(&cnfg->lock);
	543
15c9ac0c SB	544	return 0;
	545	}
	546	EXPORT_SYMBOL(cfcnfg_del_phy_layer);