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
7 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
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>
24 #define container_obj(layr) container_of(layr, struct cfcnfg, layer)
26 /* Information about CAIF physical interfaces held by Config Module in order
27 * to manage physical interfaces
29 struct cfcnfg_phyinfo {
30 struct list_head node;
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 */
39 /* Preference of the physical in interface */
40 enum cfcnfg_phy_preference pref;
42 /* Information about the physical device */
43 struct dev_info dev_info;
48 /* Use Start of frame extension */
51 /* Use Start of frame checksum */
59 struct list_head phys;
63 static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id,
64 enum cfctrl_srv serv, u8 phyid,
65 struct cflayer *adapt_layer);
66 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id);
67 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
68 struct cflayer *adapt_layer);
69 static void cfctrl_resp_func(void);
70 static void cfctrl_enum_resp(void);
72 struct cfcnfg *cfcnfg_create(void)
75 struct cfctrl_rsp *resp;
79 /* Initiate this layer */
80 this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC);
82 pr_warn("Out of memory\n");
85 this->mux = cfmuxl_create();
88 this->ctrl = cfctrl_create();
91 /* Initiate response functions */
92 resp = cfctrl_get_respfuncs(this->ctrl);
93 resp->enum_rsp = cfctrl_enum_resp;
94 resp->linkerror_ind = cfctrl_resp_func;
95 resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
96 resp->sleep_rsp = cfctrl_resp_func;
97 resp->wake_rsp = cfctrl_resp_func;
98 resp->restart_rsp = cfctrl_resp_func;
99 resp->radioset_rsp = cfctrl_resp_func;
100 resp->linksetup_rsp = cfcnfg_linkup_rsp;
101 resp->reject_rsp = cfcnfg_reject_rsp;
102 INIT_LIST_HEAD(&this->phys);
104 cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
105 layer_set_dn(this->ctrl, this->mux);
106 layer_set_up(this->ctrl, this);
107 mutex_init(&this->lock);
111 pr_warn("Out of memory\n");
121 void cfcnfg_remove(struct cfcnfg *cfg)
128 cfctrl_remove(cfg->ctrl);
133 static void cfctrl_resp_func(void)
137 static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo_rcu(struct cfcnfg *cnfg,
140 struct cfcnfg_phyinfo *phy;
142 list_for_each_entry_rcu(phy, &cnfg->phys, node)
143 if (phy->id == phyid)
148 static void cfctrl_enum_resp(void)
152 static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
153 enum cfcnfg_phy_preference phy_pref)
155 /* Try to match with specified preference */
156 struct cfcnfg_phyinfo *phy;
158 list_for_each_entry_rcu(phy, &cnfg->phys, node) {
159 if (phy->up && phy->pref == phy_pref &&
160 phy->frm_layer != NULL)
162 return &phy->dev_info;
165 /* Otherwise just return something */
166 list_for_each_entry_rcu(phy, &cnfg->phys, node)
168 return &phy->dev_info;
173 static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
175 struct cfcnfg_phyinfo *phy;
177 list_for_each_entry_rcu(phy, &cnfg->phys, node)
178 if (phy->ifindex == ifi && phy->up)
183 int caif_disconnect_client(struct net *net, struct cflayer *adap_layer)
186 struct cfcnfg *cfg = get_cfcnfg(net);
188 caif_assert(adap_layer != NULL);
189 cfctrl_cancel_req(cfg->ctrl, adap_layer);
190 channel_id = adap_layer->id;
191 if (channel_id != 0) {
192 struct cflayer *servl;
193 servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
195 layer_set_up(servl, NULL);
197 pr_debug("nothing to disconnect\n");
198 cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
200 /* Do RCU sync before initiating cleanup */
202 if (adap_layer->ctrlcmd != NULL)
203 adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
207 EXPORT_SYMBOL(caif_disconnect_client);
209 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
213 static const int protohead[CFCTRL_SRV_MASK] = {
214 [CFCTRL_SRV_VEI] = 4,
215 [CFCTRL_SRV_DATAGRAM] = 7,
216 [CFCTRL_SRV_UTIL] = 4,
217 [CFCTRL_SRV_RFM] = 3,
218 [CFCTRL_SRV_DBG] = 3,
222 static int caif_connect_req_to_link_param(struct cfcnfg *cnfg,
223 struct caif_connect_request *s,
224 struct cfctrl_link_param *l)
226 struct dev_info *dev_info;
227 enum cfcnfg_phy_preference pref;
230 memset(l, 0, sizeof(*l));
231 /* In caif protocol low value is high priority */
232 l->priority = CAIF_PRIO_MAX - s->priority + 1;
234 if (s->ifindex != 0) {
235 res = cfcnfg_get_id_from_ifi(cnfg, s->ifindex);
240 switch (s->link_selector) {
241 case CAIF_LINK_HIGH_BANDW:
242 pref = CFPHYPREF_HIGH_BW;
244 case CAIF_LINK_LOW_LATENCY:
245 pref = CFPHYPREF_LOW_LAT;
250 dev_info = cfcnfg_get_phyid(cnfg, pref);
251 if (dev_info == NULL)
253 l->phyid = dev_info->id;
255 switch (s->protocol) {
257 l->linktype = CFCTRL_SRV_VEI;
258 l->endpoint = (s->sockaddr.u.at.type >> 2) & 0x3;
259 l->chtype = s->sockaddr.u.at.type & 0x3;
261 case CAIFPROTO_DATAGRAM:
262 l->linktype = CFCTRL_SRV_DATAGRAM;
264 l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
266 case CAIFPROTO_DATAGRAM_LOOP:
267 l->linktype = CFCTRL_SRV_DATAGRAM;
270 l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
273 l->linktype = CFCTRL_SRV_RFM;
274 l->u.datagram.connid = s->sockaddr.u.rfm.connection_id;
275 strncpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume,
276 sizeof(l->u.rfm.volume)-1);
277 l->u.rfm.volume[sizeof(l->u.rfm.volume)-1] = 0;
280 l->linktype = CFCTRL_SRV_UTIL;
283 strncpy(l->u.utility.name, s->sockaddr.u.util.service,
284 sizeof(l->u.utility.name)-1);
285 l->u.utility.name[sizeof(l->u.utility.name)-1] = 0;
286 caif_assert(sizeof(l->u.utility.name) > 10);
287 l->u.utility.paramlen = s->param.size;
288 if (l->u.utility.paramlen > sizeof(l->u.utility.params))
289 l->u.utility.paramlen = sizeof(l->u.utility.params);
291 memcpy(l->u.utility.params, s->param.data,
292 l->u.utility.paramlen);
295 case CAIFPROTO_DEBUG:
296 l->linktype = CFCTRL_SRV_DBG;
297 l->endpoint = s->sockaddr.u.dbg.service;
298 l->chtype = s->sockaddr.u.dbg.type;
306 int caif_connect_client(struct net *net, struct caif_connect_request *conn_req,
307 struct cflayer *adap_layer, int *ifindex,
311 struct cflayer *frml;
312 struct cfcnfg_phyinfo *phy;
314 struct cfctrl_link_param param;
315 struct cfcnfg *cfg = get_cfcnfg(net);
318 err = caif_connect_req_to_link_param(cfg, conn_req, ¶m);
322 phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
329 if (adap_layer == NULL) {
330 pr_err("adap_layer is zero\n");
333 if (adap_layer->receive == NULL) {
334 pr_err("adap_layer->receive is NULL\n");
337 if (adap_layer->ctrlcmd == NULL) {
338 pr_err("adap_layer->ctrlcmd == NULL\n");
343 frml = phy->frm_layer;
345 pr_err("Specified PHY type does not exist!\n");
348 caif_assert(param.phyid == phy->id);
349 caif_assert(phy->frm_layer->id ==
351 caif_assert(phy->phy_layer->id ==
354 *ifindex = phy->ifindex;
358 protohead[param.linktype] + (phy->use_stx ? 1 : 0);
362 /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
363 cfctrl_enum_req(cfg->ctrl, param.phyid);
364 return cfctrl_linkup_request(cfg->ctrl, ¶m, adap_layer);
370 EXPORT_SYMBOL(caif_connect_client);
372 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
373 struct cflayer *adapt_layer)
375 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
376 adapt_layer->ctrlcmd(adapt_layer,
377 CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
381 cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
382 u8 phyid, struct cflayer *adapt_layer)
384 struct cfcnfg *cnfg = container_obj(layer);
385 struct cflayer *servicel = NULL;
386 struct cfcnfg_phyinfo *phyinfo;
387 struct net_device *netdev;
389 if (channel_id == 0) {
390 pr_warn("received channel_id zero\n");
391 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
392 adapt_layer->ctrlcmd(adapt_layer,
393 CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
399 if (adapt_layer == NULL) {
400 pr_debug("link setup response but no client exist,"
401 "send linkdown back\n");
402 cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
406 caif_assert(cnfg != NULL);
407 caif_assert(phyid != 0);
409 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
410 if (phyinfo == NULL) {
411 pr_err("ERROR: Link Layer Device dissapeared"
412 "while connecting\n");
416 caif_assert(phyinfo != NULL);
417 caif_assert(phyinfo->id == phyid);
418 caif_assert(phyinfo->phy_layer != NULL);
419 caif_assert(phyinfo->phy_layer->id == phyid);
421 adapt_layer->id = channel_id;
425 servicel = cfvei_create(channel_id, &phyinfo->dev_info);
427 case CFCTRL_SRV_DATAGRAM:
428 servicel = cfdgml_create(channel_id,
432 netdev = phyinfo->dev_info.dev;
433 servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
436 case CFCTRL_SRV_UTIL:
437 servicel = cfutill_create(channel_id, &phyinfo->dev_info);
439 case CFCTRL_SRV_VIDEO:
440 servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
443 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
446 pr_err("Protocol error. Link setup response "
447 "- unknown channel type\n");
451 pr_warn("Out of memory\n");
454 layer_set_dn(servicel, cnfg->mux);
455 cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
456 layer_set_up(servicel, adapt_layer);
457 layer_set_dn(adapt_layer, servicel);
461 servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
468 cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type,
469 struct net_device *dev, struct cflayer *phy_layer,
470 enum cfcnfg_phy_preference pref,
473 struct cflayer *frml;
474 struct cflayer *phy_driver = NULL;
475 struct cfcnfg_phyinfo *phyinfo;
479 mutex_lock(&cnfg->lock);
481 /* CAIF protocol allow maximum 6 link-layers */
482 for (i = 0; i < 7; i++) {
483 phyid = (dev->ifindex + i) & 0x7;
486 if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
489 pr_warn("Too many CAIF Link Layers (max 6)\n");
493 phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
498 cfserl_create(CFPHYTYPE_FRAG, phyid, stx);
500 pr_warn("Out of memory\n");
510 phy_layer->id = phyid;
511 phyinfo->pref = pref;
513 phyinfo->dev_info.id = phyid;
514 phyinfo->dev_info.dev = dev;
515 phyinfo->phy_layer = phy_layer;
516 phyinfo->ifindex = dev->ifindex;
517 phyinfo->use_stx = stx;
518 phyinfo->use_fcs = fcs;
520 frml = cffrml_create(phyid, fcs);
523 pr_warn("Out of memory\n");
527 phyinfo->frm_layer = frml;
528 layer_set_up(frml, cnfg->mux);
530 if (phy_driver != NULL) {
531 phy_driver->id = phyid;
532 layer_set_dn(frml, phy_driver);
533 layer_set_up(phy_driver, frml);
534 layer_set_dn(phy_driver, phy_layer);
535 layer_set_up(phy_layer, phy_driver);
537 layer_set_dn(frml, phy_layer);
538 layer_set_up(phy_layer, frml);
541 list_add_rcu(&phyinfo->node, &cnfg->phys);
543 mutex_unlock(&cnfg->lock);
545 EXPORT_SYMBOL(cfcnfg_add_phy_layer);
547 int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
550 struct cfcnfg_phyinfo *phyinfo;
553 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
554 if (phyinfo == NULL) {
559 if (phyinfo->up == up) {
566 cffrml_hold(phyinfo->frm_layer);
567 cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
570 cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
571 cffrml_put(phyinfo->frm_layer);
577 EXPORT_SYMBOL(cfcnfg_set_phy_state);
579 int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
581 struct cflayer *frml, *frml_dn;
583 struct cfcnfg_phyinfo *phyinfo;
587 mutex_lock(&cnfg->lock);
589 phyid = phy_layer->id;
590 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
592 if (phyinfo == NULL) {
593 mutex_unlock(&cnfg->lock);
596 caif_assert(phyid == phyinfo->id);
597 caif_assert(phy_layer == phyinfo->phy_layer);
598 caif_assert(phy_layer->id == phyid);
599 caif_assert(phyinfo->frm_layer->id == phyid);
601 list_del_rcu(&phyinfo->node);
604 /* Fail if reference count is not zero */
605 if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
606 pr_info("Wait for device inuse\n");
607 list_add_rcu(&phyinfo->node, &cnfg->phys);
608 mutex_unlock(&cnfg->lock);
612 frml = phyinfo->frm_layer;
614 cffrml_set_uplayer(frml, NULL);
615 cffrml_set_dnlayer(frml, NULL);
616 if (phy_layer != frml_dn) {
617 layer_set_up(frml_dn, NULL);
618 layer_set_dn(frml_dn, NULL);
620 layer_set_up(phy_layer, NULL);
622 if (phyinfo->phy_layer != frml_dn)
627 mutex_unlock(&cnfg->lock);
631 EXPORT_SYMBOL(cfcnfg_del_phy_layer);