[mirror_ubuntu-artful-kernel.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
 */
#include <linux/kernel.h>
#include <linux/stddef.h>
#include <linux/slab.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)

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

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 = kmalloc(sizeof(struct cfcnfg), GFP_ATOMIC);
	if (!this) {
		pr_warning("CAIF: %s(): Out of memory\n", __func__);
		return NULL;
	}
	memset(this, 0, sizeof(struct cfcnfg));
	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_warning("CAIF: %s(): Out of memory\n", __func__);
	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("CAIF: %s():adap_layer->id is 0\n", __func__);
		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("CAIF: %s(): PROTOCOL ERROR "
		       "- Error removing service_layer Channel_Id(%d)",
			__func__, 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_warning("CAIF: %s(): "
				"No interface to send disconnect to\n",
				__func__);
			ret = -ENODEV;
			goto end;
		}
		if (phyinfo->id != phyid ||
			phyinfo->phy_layer->id != phyid ||
			phyinfo->frm_layer->id != phyid) {
			pr_err("CAIF: %s(): "
				"Inconsistency in phy registration\n",
				__func__);
			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 cfcnfg_add_adaptation_layer(struct cfcnfg *cnfg,
				struct cfctrl_link_param *param,
				struct cflayer *adap_layer)
{
	struct cflayer *frml;
	if (adap_layer == NULL) {
		pr_err("CAIF: %s(): adap_layer is zero", __func__);
		return -EINVAL;
	}
	if (adap_layer->receive == NULL) {
		pr_err("CAIF: %s(): adap_layer->receive is NULL", __func__);
		return -EINVAL;
	}
	if (adap_layer->ctrlcmd == NULL) {
		pr_err("CAIF: %s(): adap_layer->ctrlcmd == NULL", __func__);
		return -EINVAL;
	}
	frml = cnfg->phy_layers[param->phyid].frm_layer;
	if (frml == NULL) {
		pr_err("CAIF: %s(): Specified PHY type does not exist!",
			__func__);
		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);
	/* 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;
	if (adapt_layer == NULL) {
		pr_debug("CAIF: %s(): link setup response "
				"but no client exist, send linkdown back\n",
				__func__);
		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 != NULL);
	caif_assert(phyinfo->id == phyid);
	caif_assert(phyinfo->phy_layer != NULL);
	caif_assert(phyinfo->phy_layer->id == phyid);

	if (phyinfo != NULL &&
	    phyinfo->phy_ref_count++ == 0 &&
	    phyinfo->phy_layer != NULL &&
	    phyinfo->phy_layer->modemcmd != NULL) {
		caif_assert(phyinfo->phy_layer->id == phyid);
		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:
		servicel = cfrfml_create(channel_id, &phyinfo->dev_info);
		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("CAIF: %s(): Protocol error. "
			"Link setup response - unknown channel type\n",
			__func__);
		return;
	}
	if (!servicel) {
		pr_warning("CAIF: %s(): Out of memory\n", __func__);
		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,
		     void *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("CAIF: %s(): No Available PHY ID\n", __func__);
		return;
	}

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

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