// SPDX-License-Identifier: GPL-2.0-only

  /*
   * Copyright (C) ST-Ericsson AB 2010

   * Author:	Sjur Brendeland

   */

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

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

  		strlcpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume,
  			sizeof(l->u.rfm.volume));

  		break;
  	case CAIFPROTO_UTIL:
  		l->linktype = CFCTRL_SRV_UTIL;
  		l->endpoint = 0x00;
  		l->chtype = 0x00;

  		strlcpy(l->u.utility.name, s->sockaddr.u.util.service,
  			sizeof(l->u.utility.name));

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

  		goto unlock;

  	}
  	if (adap_layer->receive == NULL) {

  		pr_err("adap_layer->receive is NULL
  ");

  		goto unlock;

  	}
  	if (adap_layer->ctrlcmd == NULL) {

  		pr_err("adap_layer->ctrlcmd == NULL
  ");

  		goto unlock;

  	}

  
  	err = -ENODEV;
  	frml = phy->frm_layer;

  	if (frml == NULL) {

  		pr_err("Specified PHY type does not exist!
  ");

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

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

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

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

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

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