[net-next-2.6.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 <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 <linux/module.h>
#include <asm/atomic.h>

#define MAX_PHY_LAYERS 7
#define PHY_NAME_LEN 20

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

/* Information about CAIF physical interfaces held by Config Module in order
 * to manage physical interfaces
 */
struct cfcnfg_phyinfo {
	/* 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;

	/* Reference count, number of channels using the device */
	int phy_ref_count;

	/* 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;
	u8 last_phyid;
	struct cfcnfg_phyinfo phy_layers[MAX_PHY_LAYERS];
};

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

	this->last_phyid = 1;

	cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
	layer_set_dn(this->ctrl, this->mux);
	layer_set_up(this->ctrl, this);
	return this;
out_of_mem:
	pr_warn("Out of memory\n");
	kfree(this->mux);
	kfree(this->ctrl);
	kfree(this);
	return NULL;
}
EXPORT_SYMBOL(cfcnfg_create);

void cfcnfg_remove(struct cfcnfg *cfg)
{
	if (cfg) {
		kfree(cfg->mux);
		kfree(cfg->ctrl);
		kfree(cfg);
	}
}

static void cfctrl_resp_func(void)
{
}

static void cfctrl_enum_resp(void)
{
}

struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
				  enum cfcnfg_phy_preference phy_pref)
{
	u16 i;

	/* Try to match with specified preference */
	for (i = 1; i < MAX_PHY_LAYERS; i++) {
		if (cnfg->phy_layers[i].id == i &&
		     cnfg->phy_layers[i].pref == phy_pref &&
		     cnfg->phy_layers[i].frm_layer != NULL) {
			caif_assert(cnfg->phy_layers != NULL);
			caif_assert(cnfg->phy_layers[i].id == i);
			return &cnfg->phy_layers[i].dev_info;
		}
	}
	/* Otherwise just return something */
	for (i = 1; i < MAX_PHY_LAYERS; i++) {
		if (cnfg->phy_layers[i].id == i) {
			caif_assert(cnfg->phy_layers != NULL);
			caif_assert(cnfg->phy_layers[i].id == i);
			return &cnfg->phy_layers[i].dev_info;
		}
	}

	return NULL;
}

static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo(struct cfcnfg *cnfg,
							u8 phyid)
{
	int i;
	/* Try to match with specified preference */
	for (i = 0; i < MAX_PHY_LAYERS; i++)
		if (cnfg->phy_layers[i].frm_layer != NULL &&
		    cnfg->phy_layers[i].id == phyid)
			return &cnfg->phy_layers[i];
	return NULL;
}

int cfcnfg_get_named(struct cfcnfg *cnfg, char *name)
{
	int i;

	/* Try to match with specified name */
	for (i = 0; i < MAX_PHY_LAYERS; i++) {
		if (cnfg->phy_layers[i].frm_layer != NULL
		    && strcmp(cnfg->phy_layers[i].phy_layer->name,
			      name) == 0)
			return cnfg->phy_layers[i].frm_layer->id;
	}
	return 0;
}

int cfcnfg_disconn_adapt_layer(struct cfcnfg *cnfg, struct cflayer *adap_layer)
{
	u8 channel_id = 0;
	int ret = 0;
	struct cflayer *servl = NULL;
	struct cfcnfg_phyinfo *phyinfo = NULL;
	u8 phyid = 0;
	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(cnfg->mux, channel_id);
	if (servl == NULL)
		goto end;
	layer_set_up(servl, NULL);
	ret = cfctrl_linkdown_req(cnfg->ctrl, channel_id, adap_layer);
	if (servl == NULL) {
		pr_err("PROTOCOL ERROR - Error removing service_layer Channel_Id(%d)",
		       channel_id);
		ret = -EINVAL;
		goto end;
	}
	caif_assert(channel_id == servl->id);
	if (adap_layer->dn != NULL) {
		phyid = cfsrvl_getphyid(adap_layer->dn);

		phyinfo = cfcnfg_get_phyinfo(cnfg, phyid);
		if (phyinfo == NULL) {
			pr_warn("No interface to send disconnect to\n");
			ret = -ENODEV;
			goto end;
		}
		if (phyinfo->id != phyid ||
			phyinfo->phy_layer->id != phyid ||
			phyinfo->frm_layer->id != phyid) {
			pr_err("Inconsistency in phy registration\n");
			ret = -EINVAL;
			goto end;
		}
	}
	if (phyinfo != NULL && --phyinfo->phy_ref_count == 0 &&
		phyinfo->phy_layer != NULL &&
		phyinfo->phy_layer->modemcmd != NULL) {
		phyinfo->phy_layer->modemcmd(phyinfo->phy_layer,
					     _CAIF_MODEMCMD_PHYIF_USELESS);
	}
end:
	cfsrvl_put(servl);
	cfctrl_cancel_req(cnfg->ctrl, adap_layer);
	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)
{
}

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;
	if (adap_layer == NULL) {
		pr_err("adap_layer is zero\n");
		return -EINVAL;
	}
	if (adap_layer->receive == NULL) {
		pr_err("adap_layer->receive is NULL\n");
		return -EINVAL;
	}
	if (adap_layer->ctrlcmd == NULL) {
		pr_err("adap_layer->ctrlcmd == NULL\n");
		return -EINVAL;
	}
	frml = cnfg->phy_layers[param->phyid].frm_layer;
	if (frml == NULL) {
		pr_err("Specified PHY type does not exist!\n");
		return -ENODEV;
	}
	caif_assert(param->phyid == cnfg->phy_layers[param->phyid].id);
	caif_assert(cnfg->phy_layers[param->phyid].frm_layer->id ==
		     param->phyid);
	caif_assert(cnfg->phy_layers[param->phyid].phy_layer->id ==
		     param->phyid);

	*ifindex = cnfg->phy_layers[param->phyid].ifindex;
	*proto_head =
		protohead[param->linktype]+
		(cnfg->phy_layers[param->phyid].use_stx ? 1 : 0);

	*proto_tail = 2;

	/* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
	cfctrl_enum_req(cnfg->ctrl, param->phyid);
	return cfctrl_linkup_request(cnfg->ctrl, param, adap_layer);
}
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;

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

	caif_assert(cnfg != NULL);
	caif_assert(phyid != 0);
	phyinfo = &cnfg->phy_layers[phyid];
	caif_assert(phyinfo->id == phyid);
	caif_assert(phyinfo->phy_layer != NULL);
	caif_assert(phyinfo->phy_layer->id == phyid);

	phyinfo->phy_ref_count++;
	if (phyinfo->phy_ref_count == 1 &&
	    phyinfo->phy_layer->modemcmd != NULL) {
		phyinfo->phy_layer->modemcmd(phyinfo->phy_layer,
					     _CAIF_MODEMCMD_PHYIF_USEFULL);
	}
	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");
		return;
	}
	if (!servicel) {
		pr_warn("Out of memory\n");
		return;
	}
	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);
	cfsrvl_get(servicel);
	servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
}

void
cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type,
		     struct net_device *dev, struct cflayer *phy_layer,
		     u16 *phyid, enum cfcnfg_phy_preference pref,
		     bool fcs, bool stx)
{
	struct cflayer *frml;
	struct cflayer *phy_driver = NULL;
	int i;


	if (cnfg->phy_layers[cnfg->last_phyid].frm_layer == NULL) {
		*phyid = cnfg->last_phyid;

		/* range: * 1..(MAX_PHY_LAYERS-1) */
		cnfg->last_phyid =
		    (cnfg->last_phyid % (MAX_PHY_LAYERS - 1)) + 1;
	} else {
		*phyid = 0;
		for (i = 1; i < MAX_PHY_LAYERS; i++) {
			if (cnfg->phy_layers[i].frm_layer == NULL) {
				*phyid = i;
				break;
			}
		}
	}
	if (*phyid == 0) {
		pr_err("No Available PHY ID\n");
		return;
	}

	switch (phy_type) {
	case CFPHYTYPE_FRAG:
		phy_driver =
		    cfserl_create(CFPHYTYPE_FRAG, *phyid, stx);
		if (!phy_driver) {
			pr_warn("Out of memory\n");
			return;
		}

		break;
	case CFPHYTYPE_CAIF:
		phy_driver = NULL;
		break;
	default:
		pr_err("%d\n", phy_type);
		return;
		break;
	}

	phy_layer->id = *phyid;
	cnfg->phy_layers[*phyid].pref = pref;
	cnfg->phy_layers[*phyid].id = *phyid;
	cnfg->phy_layers[*phyid].dev_info.id = *phyid;
	cnfg->phy_layers[*phyid].dev_info.dev = dev;
	cnfg->phy_layers[*phyid].phy_layer = phy_layer;
	cnfg->phy_layers[*phyid].phy_ref_count = 0;
	cnfg->phy_layers[*phyid].ifindex = dev->ifindex;
	cnfg->phy_layers[*phyid].use_stx = stx;
	cnfg->phy_layers[*phyid].use_fcs = fcs;

	phy_layer->type = phy_type;
	frml = cffrml_create(*phyid, fcs);
	if (!frml) {
		pr_warn("Out of memory\n");
		return;
	}
	cnfg->phy_layers[*phyid].frm_layer = frml;
	cfmuxl_set_dnlayer(cnfg->mux, frml, *phyid);
	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);
	}
}
EXPORT_SYMBOL(cfcnfg_add_phy_layer);

int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
{
	struct cflayer *frml, *frml_dn;
	u16 phyid;
	phyid = phy_layer->id;
	caif_assert(phyid == cnfg->phy_layers[phyid].id);
	caif_assert(phy_layer == cnfg->phy_layers[phyid].phy_layer);
	caif_assert(phy_layer->id == phyid);
	caif_assert(cnfg->phy_layers[phyid].frm_layer->id == phyid);

	memset(&cnfg->phy_layers[phy_layer->id], 0,
	       sizeof(struct cfcnfg_phyinfo));
	frml = cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
	frml_dn = frml->dn;
	cffrml_set_uplayer(frml, NULL);
	cffrml_set_dnlayer(frml, NULL);
	kfree(frml);

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