[GitHub/mt8127/android_kernel_alcatel_ttab.git] / net / caif / cfcnfg.c

/*
 * 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, &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 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);
Commit	Line	Data
15c9ac0c SB	1	/*
15c9ac0c SB	2	* Copyright (C) ST-Ericsson AB 2010
26ee65e6	3	* Author: Sjur Brendeland
15c9ac0c SB	4	* License terms: GNU General Public License (GPL) version 2
15c9ac0c SB	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
7c18d220	48	/* Protocol head room added for CAIF link layer */
7c18d220	49	int head_room;
2aa40aef SB	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,
3bffc475 SMP	64	enum cfctrl_srv serv, u8 phyid,
3bffc475 SMP	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,
3bffc475	68	struct cflayer *adapt_layer);
15c9ac0c SB	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);
7ac2ed0c	81	if (!this)
15c9ac0c	82	return NULL;
15c9ac0c SB	83	this->mux = cfmuxl_create();
	84	if (!this->mux)
	85	goto out_of_mem;
	86	this->ctrl = cfctrl_create();
	87	if (!this->ctrl)
	88	goto out_of_mem;
	89	/* Initiate response functions */
	90	resp = cfctrl_get_respfuncs(this->ctrl);
	91	resp->enum_rsp = cfctrl_enum_resp;
	92	resp->linkerror_ind = cfctrl_resp_func;
e539d83c	93	resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
15c9ac0c SB	94	resp->sleep_rsp = cfctrl_resp_func;
	95	resp->wake_rsp = cfctrl_resp_func;
	96	resp->restart_rsp = cfctrl_resp_func;
	97	resp->radioset_rsp = cfctrl_resp_func;
e539d83c SB	98	resp->linksetup_rsp = cfcnfg_linkup_rsp;
e539d83c SB	99	resp->reject_rsp = cfcnfg_reject_rsp;
f3621440	100	INIT_LIST_HEAD(&this->phys);
15c9ac0c SB	101
	102	cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
	103	layer_set_dn(this->ctrl, this->mux);
	104	layer_set_up(this->ctrl, this);
f3621440	105	mutex_init(&this->lock);
f3621440	106
15c9ac0c SB	107	return this;
15c9ac0c SB	108	out_of_mem:
f3621440	109	synchronize_rcu();
f3621440	110
15c9ac0c SB	111	kfree(this->mux);
	112	kfree(this->ctrl);
	113	kfree(this);
	114	return NULL;
	115	}
15c9ac0c SB	116
	117	void cfcnfg_remove(struct cfcnfg *cfg)
	118	{
f3621440	119	might_sleep();
15c9ac0c	120	if (cfg) {
f3621440	121	synchronize_rcu();
f3621440	122
15c9ac0c	123	kfree(cfg->mux);
c85c2951	124	cfctrl_remove(cfg->ctrl);
15c9ac0c SB	125	kfree(cfg);
	126	}
	127	}
	128
	129	static void cfctrl_resp_func(void)
	130	{
	131	}
	132
f3621440	133	static struct cfcnfg_phyinfo cfcnfg_get_phyinfo_rcu(struct cfcnfg cnfg,
3bffc475	134	u8 phyid)
f3621440	135	{
	136	struct cfcnfg_phyinfo *phy;
	137
	138	list_for_each_entry_rcu(phy, &cnfg->phys, node)
	139	if (phy->id == phyid)
	140	return phy;
	141	return NULL;
	142	}
	143
15c9ac0c SB	144	static void cfctrl_enum_resp(void)
	145	{
	146	}
	147
bee925db	148	static struct dev_info cfcnfg_get_phyid(struct cfcnfg cnfg,
15c9ac0c SB	149	enum cfcnfg_phy_preference phy_pref)
15c9ac0c SB	150	{
15c9ac0c	151	/* Try to match with specified preference */
f3621440	152	struct cfcnfg_phyinfo *phy;
	153
	154	list_for_each_entry_rcu(phy, &cnfg->phys, node) {
	155	if (phy->up && phy->pref == phy_pref &&
	156	phy->frm_layer != NULL)
	157
	158	return &phy->dev_info;
15c9ac0c SB	159	}
15c9ac0c SB	160
f3621440	161	/* Otherwise just return something */
	162	list_for_each_entry_rcu(phy, &cnfg->phys, node)
	163	if (phy->up)
	164	return &phy->dev_info;
15c9ac0c	165
15c9ac0c SB	166	return NULL;
	167	}
	168
bee925db	169	static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
15c9ac0c	170	{
f3621440	171	struct cfcnfg_phyinfo *phy;
	172
	173	list_for_each_entry_rcu(phy, &cnfg->phys, node)
	174	if (phy->ifindex == ifi && phy->up)
	175	return phy->id;
f2527ec4	176	return -ENODEV;
15c9ac0c SB	177	}
15c9ac0c SB	178
bee925db	179	int caif_disconnect_client(struct net net, struct cflayer adap_layer)
15c9ac0c	180	{
54e90fb5	181	u8 channel_id;
bee925db	182	struct cfcnfg *cfg = get_cfcnfg(net);
01a85901	183
15c9ac0c	184	caif_assert(adap_layer != NULL);
f3621440	185	cfctrl_cancel_req(cfg->ctrl, adap_layer);
54e90fb5	186	channel_id = adap_layer->id;
	187	if (channel_id != 0) {
	188	struct cflayer *servl;
	189	servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
7c18d220	190	cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
54e90fb5	191	if (servl != NULL)
	192	layer_set_up(servl, NULL);
	193	} else
	194	pr_debug("nothing to disconnect\n");
f3621440	195
	196	/* Do RCU sync before initiating cleanup */
	197	synchronize_rcu();
8d545c8f SB	198	if (adap_layer->ctrlcmd != NULL)
8d545c8f SB	199	adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
54e90fb5	200	return 0;
15c9ac0c SB	201
15c9ac0c SB	202	}
bee925db	203	EXPORT_SYMBOL(caif_disconnect_client);
5b208656	204
8d545c8f	205	static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
15c9ac0c	206	{
15c9ac0c SB	207	}
15c9ac0c SB	208
73d6ac63	209	static const int protohead[CFCTRL_SRV_MASK] = {
2aa40aef SB	210	[CFCTRL_SRV_VEI] = 4,
	211	[CFCTRL_SRV_DATAGRAM] = 7,
	212	[CFCTRL_SRV_UTIL] = 4,
	213	[CFCTRL_SRV_RFM] = 3,
	214	[CFCTRL_SRV_DBG] = 3,
	215	};
	216
bee925db	217
bee925db	218	static int caif_connect_req_to_link_param(struct cfcnfg *cnfg,
3bffc475 SMP	219	struct caif_connect_request *s,
3bffc475 SMP	220	struct cfctrl_link_param *l)
bee925db	221	{
	222	struct dev_info *dev_info;
	223	enum cfcnfg_phy_preference pref;
	224	int res;
	225
	226	memset(l, 0, sizeof(*l));
	227	/* In caif protocol low value is high priority */
	228	l->priority = CAIF_PRIO_MAX - s->priority + 1;
	229
	230	if (s->ifindex != 0) {
	231	res = cfcnfg_get_id_from_ifi(cnfg, s->ifindex);
	232	if (res < 0)
	233	return res;
	234	l->phyid = res;
	235	} else {
	236	switch (s->link_selector) {
	237	case CAIF_LINK_HIGH_BANDW:
	238	pref = CFPHYPREF_HIGH_BW;
	239	break;
	240	case CAIF_LINK_LOW_LATENCY:
	241	pref = CFPHYPREF_LOW_LAT;
	242	break;
	243	default:
	244	return -EINVAL;
	245	}
	246	dev_info = cfcnfg_get_phyid(cnfg, pref);
	247	if (dev_info == NULL)
	248	return -ENODEV;
	249	l->phyid = dev_info->id;
	250	}
	251	switch (s->protocol) {
	252	case CAIFPROTO_AT:
	253	l->linktype = CFCTRL_SRV_VEI;
	254	l->endpoint = (s->sockaddr.u.at.type >> 2) & 0x3;
	255	l->chtype = s->sockaddr.u.at.type & 0x3;
	256	break;
	257	case CAIFPROTO_DATAGRAM:
	258	l->linktype = CFCTRL_SRV_DATAGRAM;
	259	l->chtype = 0x00;
	260	l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
	261	break;
	262	case CAIFPROTO_DATAGRAM_LOOP:
	263	l->linktype = CFCTRL_SRV_DATAGRAM;
	264	l->chtype = 0x03;
	265	l->endpoint = 0x00;
	266	l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
	267	break;
	268	case CAIFPROTO_RFM:
	269	l->linktype = CFCTRL_SRV_RFM;
	270	l->u.datagram.connid = s->sockaddr.u.rfm.connection_id;
	271	strncpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume,
	272	sizeof(l->u.rfm.volume)-1);
	273	l->u.rfm.volume[sizeof(l->u.rfm.volume)-1] = 0;
	274	break;
	275	case CAIFPROTO_UTIL:
	276	l->linktype = CFCTRL_SRV_UTIL;
	277	l->endpoint = 0x00;
	278	l->chtype = 0x00;
	279	strncpy(l->u.utility.name, s->sockaddr.u.util.service,
	280	sizeof(l->u.utility.name)-1);
	281	l->u.utility.name[sizeof(l->u.utility.name)-1] = 0;
	282	caif_assert(sizeof(l->u.utility.name) > 10);
	283	l->u.utility.paramlen = s->param.size;
	284	if (l->u.utility.paramlen > sizeof(l->u.utility.params))
285	l->u.utility.paramlen = sizeof(l->u.utility.params);
286
287	memcpy(l->u.utility.params, s->param.data,
288	l->u.utility.paramlen);
289
290	break;
291	case CAIFPROTO_DEBUG:
292	l->linktype = CFCTRL_SRV_DBG;
293	l->endpoint = s->sockaddr.u.dbg.service;
294	l->chtype = s->sockaddr.u.dbg.type;
295	break;
296	default:
297	return -EINVAL;
298	}
299	return 0;
300	}
301
302	int caif_connect_client(struct net net, struct caif_connect_request conn_req,
303	struct cflayer adap_layer, int ifindex,
3bffc475	304	int proto_head, int proto_tail)
15c9ac0c SB	305	{
15c9ac0c SB	306	struct cflayer *frml;
f3621440	307	struct cfcnfg_phyinfo *phy;
f3621440	308	int err;
bee925db	309	struct cfctrl_link_param param;
bee925db	310	struct cfcnfg *cfg = get_cfcnfg(net);
f3621440	311
f3621440	312	rcu_read_lock();
bee925db	313	err = caif_connect_req_to_link_param(cfg, conn_req, &param);
	314	if (err)
	315	goto unlock;
	316
	317	phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
f3621440	318	if (!phy) {
	319	err = -ENODEV;
	320	goto unlock;
	321	}
	322	err = -EINVAL;
	323
15c9ac0c	324	if (adap_layer == NULL) {
b31fa5ba	325	pr_err("adap_layer is zero\n");
f3621440	326	goto unlock;
15c9ac0c SB	327	}
15c9ac0c SB	328	if (adap_layer->receive == NULL) {
b31fa5ba	329	pr_err("adap_layer->receive is NULL\n");
f3621440	330	goto unlock;
15c9ac0c SB	331	}
15c9ac0c SB	332	if (adap_layer->ctrlcmd == NULL) {
b31fa5ba	333	pr_err("adap_layer->ctrlcmd == NULL\n");
f3621440	334	goto unlock;
15c9ac0c	335	}
f3621440	336
	337	err = -ENODEV;
	338	frml = phy->frm_layer;
15c9ac0c	339	if (frml == NULL) {
b31fa5ba	340	pr_err("Specified PHY type does not exist!\n");
f3621440	341	goto unlock;
15c9ac0c	342	}
bee925db	343	caif_assert(param.phyid == phy->id);
f3621440	344	caif_assert(phy->frm_layer->id ==
bee925db	345	param.phyid);
f3621440	346	caif_assert(phy->phy_layer->id ==
bee925db	347	param.phyid);
2aa40aef	348
f3621440	349	*ifindex = phy->ifindex;
f3621440	350	*proto_tail = 2;
7c18d220	351	*proto_head = protohead[param.linktype] + phy->head_room;
2aa40aef	352
f3621440	353	rcu_read_unlock();
2aa40aef	354
15c9ac0c	355	/* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
bee925db	356	cfctrl_enum_req(cfg->ctrl, param.phyid);
bee925db	357	return cfctrl_linkup_request(cfg->ctrl, &param, adap_layer);
f3621440	358
	359	unlock:
	360	rcu_read_unlock();
	361	return err;
15c9ac0c	362	}
bee925db	363	EXPORT_SYMBOL(caif_connect_client);
15c9ac0c	364
e539d83c	365	static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
3bffc475	366	struct cflayer *adapt_layer)
15c9ac0c SB	367	{
	368	if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
	369	adapt_layer->ctrlcmd(adapt_layer,
	370	CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
	371	}
	372
	373	static void
e539d83c	374	cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
f3621440	375	u8 phyid, struct cflayer *adapt_layer)
15c9ac0c SB	376	{
	377	struct cfcnfg *cnfg = container_obj(layer);
	378	struct cflayer *servicel = NULL;
	379	struct cfcnfg_phyinfo *phyinfo;
2aa40aef SB	380	struct net_device *netdev;
2aa40aef SB	381
54e90fb5	382	if (channel_id == 0) {
	383	pr_warn("received channel_id zero\n");
	384	if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
	385	adapt_layer->ctrlcmd(adapt_layer,
	386	CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
	387	return;
	388	}
	389
f3621440	390	rcu_read_lock();
f3621440	391
15c9ac0c	392	if (adapt_layer == NULL) {
f3621440	393	pr_debug("link setup response but no client exist,"
f3621440	394	"send linkdown back\n");
8d545c8f	395	cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
f3621440	396	goto unlock;
15c9ac0c SB	397	}
	398
	399	caif_assert(cnfg != NULL);
	400	caif_assert(phyid != 0);
f3621440	401
	402	phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
	403	if (phyinfo == NULL) {
8a168ca7	404	pr_err("ERROR: Link Layer Device disappeared"
f3621440	405	"while connecting\n");
	406	goto unlock;
	407	}
	408
	409	caif_assert(phyinfo != NULL);
15c9ac0c SB	410	caif_assert(phyinfo->id == phyid);
	411	caif_assert(phyinfo->phy_layer != NULL);
	412	caif_assert(phyinfo->phy_layer->id == phyid);
	413
e539d83c	414	adapt_layer->id = channel_id;
15c9ac0c SB	415
	416	switch (serv) {
	417	case CFCTRL_SRV_VEI:
e539d83c	418	servicel = cfvei_create(channel_id, &phyinfo->dev_info);
15c9ac0c SB	419	break;
15c9ac0c SB	420	case CFCTRL_SRV_DATAGRAM:
f3621440	421	servicel = cfdgml_create(channel_id,
f3621440	422	&phyinfo->dev_info);
15c9ac0c SB	423	break;
15c9ac0c SB	424	case CFCTRL_SRV_RFM:
2aa40aef	425	netdev = phyinfo->dev_info.dev;
a7da1f55	426	servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
2aa40aef	427	netdev->mtu);
15c9ac0c SB	428	break;
15c9ac0c SB	429	case CFCTRL_SRV_UTIL:
e539d83c	430	servicel = cfutill_create(channel_id, &phyinfo->dev_info);
15c9ac0c SB	431	break;
15c9ac0c SB	432	case CFCTRL_SRV_VIDEO:
e539d83c	433	servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
15c9ac0c SB	434	break;
15c9ac0c SB	435	case CFCTRL_SRV_DBG:
e539d83c	436	servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
15c9ac0c SB	437	break;
15c9ac0c SB	438	default:
f3621440	439	pr_err("Protocol error. Link setup response "
	440	"- unknown channel type\n");
	441	goto unlock;
15c9ac0c	442	}
7ac2ed0c	443	if (!servicel)
f3621440	444	goto unlock;
15c9ac0c	445	layer_set_dn(servicel, cnfg->mux);
e539d83c	446	cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
15c9ac0c SB	447	layer_set_up(servicel, adapt_layer);
15c9ac0c SB	448	layer_set_dn(adapt_layer, servicel);
f3621440	449
	450	rcu_read_unlock();
	451
15c9ac0c	452	servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
f3621440	453	return;
	454	unlock:
	455	rcu_read_unlock();
15c9ac0c SB	456	}
	457
	458	void
7c18d220	459	cfcnfg_add_phy_layer(struct cfcnfg *cnfg,
2aa40aef	460	struct net_device dev, struct cflayer phy_layer,
bee925db	461	enum cfcnfg_phy_preference pref,
7c18d220	462	struct cflayer *link_support,
7c18d220	463	bool fcs, int head_room)
15c9ac0c SB	464	{
15c9ac0c SB	465	struct cflayer *frml;
5bb20ed8	466	struct cfcnfg_phyinfo *phyinfo = NULL;
15c9ac0c	467	int i;
f3621440	468	u8 phyid;
15c9ac0c	469
f3621440	470	mutex_lock(&cnfg->lock);
15c9ac0c	471
f3621440	472	/* CAIF protocol allow maximum 6 link-layers */
	473	for (i = 0; i < 7; i++) {
	474	phyid = (dev->ifindex + i) & 0x7;
	475	if (phyid == 0)
	476	continue;
	477	if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
	478	goto got_phyid;
15c9ac0c	479	}
f3621440	480	pr_warn("Too many CAIF Link Layers (max 6)\n");
7c18d220	481	goto out;
f3621440	482
	483	got_phyid:
	484	phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
5bb20ed8 DC	485	if (!phyinfo)
5bb20ed8 DC	486	goto out_err;
15c9ac0c	487
f3621440	488	phy_layer->id = phyid;
	489	phyinfo->pref = pref;
	490	phyinfo->id = phyid;
	491	phyinfo->dev_info.id = phyid;
	492	phyinfo->dev_info.dev = dev;
	493	phyinfo->phy_layer = phy_layer;
	494	phyinfo->ifindex = dev->ifindex;
7c18d220	495	phyinfo->head_room = head_room;
f3621440	496	phyinfo->use_fcs = fcs;
2aa40aef	497
f3621440	498	frml = cffrml_create(phyid, fcs);
f3621440	499
5bb20ed8 DC	500	if (!frml)
5bb20ed8 DC	501	goto out_err;
f3621440	502	phyinfo->frm_layer = frml;
15c9ac0c SB	503	layer_set_up(frml, cnfg->mux);
15c9ac0c SB	504
7c18d220	505	if (link_support != NULL) {
	506	link_support->id = phyid;
	507	layer_set_dn(frml, link_support);
	508	layer_set_up(link_support, frml);
	509	layer_set_dn(link_support, phy_layer);
	510	layer_set_up(phy_layer, link_support);
15c9ac0c SB	511	} else {
	512	layer_set_dn(frml, phy_layer);
	513	layer_set_up(phy_layer, frml);
	514	}
f3621440	515
f3621440	516	list_add_rcu(&phyinfo->node, &cnfg->phys);
7c18d220	517	out:
5bb20ed8 DC	518	mutex_unlock(&cnfg->lock);
	519	return;
	520
	521	out_err:
5bb20ed8	522	kfree(phyinfo);
f3621440	523	mutex_unlock(&cnfg->lock);
15c9ac0c SB	524	}
	525	EXPORT_SYMBOL(cfcnfg_add_phy_layer);
	526
f3621440	527	int cfcnfg_set_phy_state(struct cfcnfg cnfg, struct cflayer phy_layer,
3bffc475	528	bool up)
f3621440	529	{
	530	struct cfcnfg_phyinfo *phyinfo;
	531
	532	rcu_read_lock();
	533	phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
	534	if (phyinfo == NULL) {
	535	rcu_read_unlock();
	536	return -ENODEV;
	537	}
	538
	539	if (phyinfo->up == up) {
	540	rcu_read_unlock();
	541	return 0;
	542	}
	543	phyinfo->up = up;
	544
	545	if (up) {
	546	cffrml_hold(phyinfo->frm_layer);
	547	cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
	548	phy_layer->id);
	549	} else {
	550	cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
	551	cffrml_put(phyinfo->frm_layer);
	552	}
	553
	554	rcu_read_unlock();
	555	return 0;
	556	}
	557	EXPORT_SYMBOL(cfcnfg_set_phy_state);
	558
15c9ac0c SB	559	int cfcnfg_del_phy_layer(struct cfcnfg cnfg, struct cflayer phy_layer)
	560	{
	561	struct cflayer frml, frml_dn;
	562	u16 phyid;
f3621440	563	struct cfcnfg_phyinfo *phyinfo;
	564
	565	might_sleep();
	566
	567	mutex_lock(&cnfg->lock);
	568
15c9ac0c	569	phyid = phy_layer->id;
f3621440	570	phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
f3621440	571
bee925db	572	if (phyinfo == NULL) {
bee925db	573	mutex_unlock(&cnfg->lock);
f3621440	574	return 0;
bee925db	575	}
f3621440	576	caif_assert(phyid == phyinfo->id);
f3621440	577	caif_assert(phy_layer == phyinfo->phy_layer);
15c9ac0c	578	caif_assert(phy_layer->id == phyid);
f3621440	579	caif_assert(phyinfo->frm_layer->id == phyid);
f3621440	580
bee925db	581	list_del_rcu(&phyinfo->node);
	582	synchronize_rcu();
	583
cb3cb423	584	/* Fail if reference count is not zero */
	585	if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
	586	pr_info("Wait for device inuse\n");
bee925db	587	list_add_rcu(&phyinfo->node, &cnfg->phys);
cb3cb423	588	mutex_unlock(&cnfg->lock);
	589	return -EAGAIN;
	590	}
	591
f3621440	592	frml = phyinfo->frm_layer;
15c9ac0c SB	593	frml_dn = frml->dn;
	594	cffrml_set_uplayer(frml, NULL);
	595	cffrml_set_dnlayer(frml, NULL);
15c9ac0c SB	596	if (phy_layer != frml_dn) {
	597	layer_set_up(frml_dn, NULL);
	598	layer_set_dn(frml_dn, NULL);
15c9ac0c SB	599	}
15c9ac0c SB	600	layer_set_up(phy_layer, NULL);
f3621440	601
f3621440	602	if (phyinfo->phy_layer != frml_dn)
	603	kfree(frml_dn);
	604
cb3cb423	605	cffrml_free(frml);
f3621440	606	kfree(phyinfo);
	607	mutex_unlock(&cnfg->lock);
	608
15c9ac0c SB	609	return 0;
	610	}
	611	EXPORT_SYMBOL(cfcnfg_del_phy_layer);