PageRenderTime 43ms CodeModel.GetById 11ms app.highlight 27ms RepoModel.GetById 1ms app.codeStats 0ms

/net/caif/cfcnfg.c

http://github.com/mirrors/linux
C | 606 lines | 492 code | 89 blank | 25 comment | 91 complexity | 08958405d8ce4931f661b4b2fa10945f MD5 | raw file
  1// SPDX-License-Identifier: GPL-2.0-only
  2/*
  3 * Copyright (C) ST-Ericsson AB 2010
  4 * Author:	Sjur Brendeland
  5 */
  6
  7#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
  8
  9#include <linux/kernel.h>
 10#include <linux/stddef.h>
 11#include <linux/slab.h>
 12#include <linux/netdevice.h>
 13#include <linux/module.h>
 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>
 22#include <net/caif/caif_dev.h>
 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 */
 29struct cfcnfg_phyinfo {
 30	struct list_head node;
 31	bool up;
 32
 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
 42	/* Information about the physical device */
 43	struct dev_info dev_info;
 44
 45	/* Interface index */
 46	int ifindex;
 47
 48	/* Protocol head room added for CAIF link layer */
 49	int head_room;
 50
 51	/* Use Start of frame checksum */
 52	bool use_fcs;
 53};
 54
 55struct cfcnfg {
 56	struct cflayer layer;
 57	struct cflayer *ctrl;
 58	struct cflayer *mux;
 59	struct list_head phys;
 60	struct mutex lock;
 61};
 62
 63static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id,
 64			      enum cfctrl_srv serv, u8 phyid,
 65			      struct cflayer *adapt_layer);
 66static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id);
 67static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
 68			      struct cflayer *adapt_layer);
 69static void cfctrl_resp_func(void);
 70static void cfctrl_enum_resp(void);
 71
 72struct cfcnfg *cfcnfg_create(void)
 73{
 74	struct cfcnfg *this;
 75	struct cfctrl_rsp *resp;
 76
 77	might_sleep();
 78
 79	/* Initiate this layer */
 80	this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC);
 81	if (!this)
 82		return NULL;
 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;
 93	resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
 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;
 98	resp->linksetup_rsp = cfcnfg_linkup_rsp;
 99	resp->reject_rsp = cfcnfg_reject_rsp;
100	INIT_LIST_HEAD(&this->phys);
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);
105	mutex_init(&this->lock);
106
107	return this;
108out_of_mem:
109	synchronize_rcu();
110
111	kfree(this->mux);
112	kfree(this->ctrl);
113	kfree(this);
114	return NULL;
115}
116
117void cfcnfg_remove(struct cfcnfg *cfg)
118{
119	might_sleep();
120	if (cfg) {
121		synchronize_rcu();
122
123		kfree(cfg->mux);
124		cfctrl_remove(cfg->ctrl);
125		kfree(cfg);
126	}
127}
128
129static void cfctrl_resp_func(void)
130{
131}
132
133static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo_rcu(struct cfcnfg *cnfg,
134						     u8 phyid)
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
144static void cfctrl_enum_resp(void)
145{
146}
147
148static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
149				  enum cfcnfg_phy_preference phy_pref)
150{
151	/* Try to match with specified preference */
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;
159	}
160
161	/* Otherwise just return something */
162	list_for_each_entry_rcu(phy, &cnfg->phys, node)
163		if (phy->up)
164			return &phy->dev_info;
165
166	return NULL;
167}
168
169static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
170{
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;
176	return -ENODEV;
177}
178
179int caif_disconnect_client(struct net *net, struct cflayer *adap_layer)
180{
181	u8 channel_id;
182	struct cfcnfg *cfg = get_cfcnfg(net);
183
184	caif_assert(adap_layer != NULL);
185	cfctrl_cancel_req(cfg->ctrl, adap_layer);
186	channel_id = adap_layer->id;
187	if (channel_id != 0) {
188		struct cflayer *servl;
189		servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
190		cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
191		if (servl != NULL)
192			layer_set_up(servl, NULL);
193	} else
194		pr_debug("nothing to disconnect\n");
195
196	/* Do RCU sync before initiating cleanup */
197	synchronize_rcu();
198	if (adap_layer->ctrlcmd != NULL)
199		adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
200	return 0;
201
202}
203EXPORT_SYMBOL(caif_disconnect_client);
204
205static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
206{
207}
208
209static const int protohead[CFCTRL_SRV_MASK] = {
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
217
218static int caif_connect_req_to_link_param(struct cfcnfg *cnfg,
219					  struct caif_connect_request *s,
220					  struct cfctrl_link_param *l)
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		strlcpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume,
272			sizeof(l->u.rfm.volume));
273		break;
274	case CAIFPROTO_UTIL:
275		l->linktype = CFCTRL_SRV_UTIL;
276		l->endpoint = 0x00;
277		l->chtype = 0x00;
278		strlcpy(l->u.utility.name, s->sockaddr.u.util.service,
279			sizeof(l->u.utility.name));
280		caif_assert(sizeof(l->u.utility.name) > 10);
281		l->u.utility.paramlen = s->param.size;
282		if (l->u.utility.paramlen > sizeof(l->u.utility.params))
283			l->u.utility.paramlen = sizeof(l->u.utility.params);
284
285		memcpy(l->u.utility.params, s->param.data,
286		       l->u.utility.paramlen);
287
288		break;
289	case CAIFPROTO_DEBUG:
290		l->linktype = CFCTRL_SRV_DBG;
291		l->endpoint = s->sockaddr.u.dbg.service;
292		l->chtype = s->sockaddr.u.dbg.type;
293		break;
294	default:
295		return -EINVAL;
296	}
297	return 0;
298}
299
300int caif_connect_client(struct net *net, struct caif_connect_request *conn_req,
301			struct cflayer *adap_layer, int *ifindex,
302			int *proto_head, int *proto_tail)
303{
304	struct cflayer *frml;
305	struct cfcnfg_phyinfo *phy;
306	int err;
307	struct cfctrl_link_param param;
308	struct cfcnfg *cfg = get_cfcnfg(net);
309
310	rcu_read_lock();
311	err = caif_connect_req_to_link_param(cfg, conn_req, &param);
312	if (err)
313		goto unlock;
314
315	phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
316	if (!phy) {
317		err = -ENODEV;
318		goto unlock;
319	}
320	err = -EINVAL;
321
322	if (adap_layer == NULL) {
323		pr_err("adap_layer is zero\n");
324		goto unlock;
325	}
326	if (adap_layer->receive == NULL) {
327		pr_err("adap_layer->receive is NULL\n");
328		goto unlock;
329	}
330	if (adap_layer->ctrlcmd == NULL) {
331		pr_err("adap_layer->ctrlcmd == NULL\n");
332		goto unlock;
333	}
334
335	err = -ENODEV;
336	frml = phy->frm_layer;
337	if (frml == NULL) {
338		pr_err("Specified PHY type does not exist!\n");
339		goto unlock;
340	}
341	caif_assert(param.phyid == phy->id);
342	caif_assert(phy->frm_layer->id ==
343		     param.phyid);
344	caif_assert(phy->phy_layer->id ==
345		     param.phyid);
346
347	*ifindex = phy->ifindex;
348	*proto_tail = 2;
349	*proto_head = protohead[param.linktype] + phy->head_room;
350
351	rcu_read_unlock();
352
353	/* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
354	cfctrl_enum_req(cfg->ctrl, param.phyid);
355	return cfctrl_linkup_request(cfg->ctrl, &param, adap_layer);
356
357unlock:
358	rcu_read_unlock();
359	return err;
360}
361EXPORT_SYMBOL(caif_connect_client);
362
363static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
364			      struct cflayer *adapt_layer)
365{
366	if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
367		adapt_layer->ctrlcmd(adapt_layer,
368				     CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
369}
370
371static void
372cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
373		  u8 phyid, struct cflayer *adapt_layer)
374{
375	struct cfcnfg *cnfg = container_obj(layer);
376	struct cflayer *servicel = NULL;
377	struct cfcnfg_phyinfo *phyinfo;
378	struct net_device *netdev;
379
380	if (channel_id == 0) {
381		pr_warn("received channel_id zero\n");
382		if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
383			adapt_layer->ctrlcmd(adapt_layer,
384						CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
385		return;
386	}
387
388	rcu_read_lock();
389
390	if (adapt_layer == NULL) {
391		pr_debug("link setup response but no client exist, send linkdown back\n");
392		cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
393		goto unlock;
394	}
395
396	caif_assert(cnfg != NULL);
397	caif_assert(phyid != 0);
398
399	phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
400	if (phyinfo == NULL) {
401		pr_err("ERROR: Link Layer Device disappeared while connecting\n");
402		goto unlock;
403	}
404
405	caif_assert(phyinfo != NULL);
406	caif_assert(phyinfo->id == phyid);
407	caif_assert(phyinfo->phy_layer != NULL);
408	caif_assert(phyinfo->phy_layer->id == phyid);
409
410	adapt_layer->id = channel_id;
411
412	switch (serv) {
413	case CFCTRL_SRV_VEI:
414		servicel = cfvei_create(channel_id, &phyinfo->dev_info);
415		break;
416	case CFCTRL_SRV_DATAGRAM:
417		servicel = cfdgml_create(channel_id,
418					&phyinfo->dev_info);
419		break;
420	case CFCTRL_SRV_RFM:
421		netdev = phyinfo->dev_info.dev;
422		servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
423						netdev->mtu);
424		break;
425	case CFCTRL_SRV_UTIL:
426		servicel = cfutill_create(channel_id, &phyinfo->dev_info);
427		break;
428	case CFCTRL_SRV_VIDEO:
429		servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
430		break;
431	case CFCTRL_SRV_DBG:
432		servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
433		break;
434	default:
435		pr_err("Protocol error. Link setup response - unknown channel type\n");
436		goto unlock;
437	}
438	if (!servicel)
439		goto unlock;
440	layer_set_dn(servicel, cnfg->mux);
441	cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
442	layer_set_up(servicel, adapt_layer);
443	layer_set_dn(adapt_layer, servicel);
444
445	rcu_read_unlock();
446
447	servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
448	return;
449unlock:
450	rcu_read_unlock();
451}
452
453void
454cfcnfg_add_phy_layer(struct cfcnfg *cnfg,
455		     struct net_device *dev, struct cflayer *phy_layer,
456		     enum cfcnfg_phy_preference pref,
457		     struct cflayer *link_support,
458		     bool fcs, int head_room)
459{
460	struct cflayer *frml;
461	struct cfcnfg_phyinfo *phyinfo = NULL;
462	int i;
463	u8 phyid;
464
465	mutex_lock(&cnfg->lock);
466
467	/* CAIF protocol allow maximum 6 link-layers */
468	for (i = 0; i < 7; i++) {
469		phyid = (dev->ifindex + i) & 0x7;
470		if (phyid == 0)
471			continue;
472		if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
473			goto got_phyid;
474	}
475	pr_warn("Too many CAIF Link Layers (max 6)\n");
476	goto out;
477
478got_phyid:
479	phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
480	if (!phyinfo)
481		goto out_err;
482
483	phy_layer->id = phyid;
484	phyinfo->pref = pref;
485	phyinfo->id = phyid;
486	phyinfo->dev_info.id = phyid;
487	phyinfo->dev_info.dev = dev;
488	phyinfo->phy_layer = phy_layer;
489	phyinfo->ifindex = dev->ifindex;
490	phyinfo->head_room = head_room;
491	phyinfo->use_fcs = fcs;
492
493	frml = cffrml_create(phyid, fcs);
494
495	if (!frml)
496		goto out_err;
497	phyinfo->frm_layer = frml;
498	layer_set_up(frml, cnfg->mux);
499
500	if (link_support != NULL) {
501		link_support->id = phyid;
502		layer_set_dn(frml, link_support);
503		layer_set_up(link_support, frml);
504		layer_set_dn(link_support, phy_layer);
505		layer_set_up(phy_layer, link_support);
506	} else {
507		layer_set_dn(frml, phy_layer);
508		layer_set_up(phy_layer, frml);
509	}
510
511	list_add_rcu(&phyinfo->node, &cnfg->phys);
512out:
513	mutex_unlock(&cnfg->lock);
514	return;
515
516out_err:
517	kfree(phyinfo);
518	mutex_unlock(&cnfg->lock);
519}
520EXPORT_SYMBOL(cfcnfg_add_phy_layer);
521
522int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
523			 bool up)
524{
525	struct cfcnfg_phyinfo *phyinfo;
526
527	rcu_read_lock();
528	phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
529	if (phyinfo == NULL) {
530		rcu_read_unlock();
531		return -ENODEV;
532	}
533
534	if (phyinfo->up == up) {
535		rcu_read_unlock();
536		return 0;
537	}
538	phyinfo->up = up;
539
540	if (up) {
541		cffrml_hold(phyinfo->frm_layer);
542		cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
543					phy_layer->id);
544	} else {
545		cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
546		cffrml_put(phyinfo->frm_layer);
547	}
548
549	rcu_read_unlock();
550	return 0;
551}
552EXPORT_SYMBOL(cfcnfg_set_phy_state);
553
554int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
555{
556	struct cflayer *frml, *frml_dn;
557	u16 phyid;
558	struct cfcnfg_phyinfo *phyinfo;
559
560	might_sleep();
561
562	mutex_lock(&cnfg->lock);
563
564	phyid = phy_layer->id;
565	phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
566
567	if (phyinfo == NULL) {
568		mutex_unlock(&cnfg->lock);
569		return 0;
570	}
571	caif_assert(phyid == phyinfo->id);
572	caif_assert(phy_layer == phyinfo->phy_layer);
573	caif_assert(phy_layer->id == phyid);
574	caif_assert(phyinfo->frm_layer->id == phyid);
575
576	list_del_rcu(&phyinfo->node);
577	synchronize_rcu();
578
579	/* Fail if reference count is not zero */
580	if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
581		pr_info("Wait for device inuse\n");
582		list_add_rcu(&phyinfo->node, &cnfg->phys);
583		mutex_unlock(&cnfg->lock);
584		return -EAGAIN;
585	}
586
587	frml = phyinfo->frm_layer;
588	frml_dn = frml->dn;
589	cffrml_set_uplayer(frml, NULL);
590	cffrml_set_dnlayer(frml, NULL);
591	if (phy_layer != frml_dn) {
592		layer_set_up(frml_dn, NULL);
593		layer_set_dn(frml_dn, NULL);
594	}
595	layer_set_up(phy_layer, NULL);
596
597	if (phyinfo->phy_layer != frml_dn)
598		kfree(frml_dn);
599
600	cffrml_free(frml);
601	kfree(phyinfo);
602	mutex_unlock(&cnfg->lock);
603
604	return 0;
605}
606EXPORT_SYMBOL(cfcnfg_del_phy_layer);