Loading...
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
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 /* Use Start of frame extension */
49 bool use_stx;
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 pr_warn("Out of memory\n");
83 return NULL;
84 }
85 this->mux = cfmuxl_create();
86 if (!this->mux)
87 goto out_of_mem;
88 this->ctrl = cfctrl_create();
89 if (!this->ctrl)
90 goto out_of_mem;
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);
103
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);
108
109 return this;
110out_of_mem:
111 pr_warn("Out of memory\n");
112
113 synchronize_rcu();
114
115 kfree(this->mux);
116 kfree(this->ctrl);
117 kfree(this);
118 return NULL;
119}
120
121void cfcnfg_remove(struct cfcnfg *cfg)
122{
123 might_sleep();
124 if (cfg) {
125 synchronize_rcu();
126
127 kfree(cfg->mux);
128 cfctrl_remove(cfg->ctrl);
129 kfree(cfg);
130 }
131}
132
133static void cfctrl_resp_func(void)
134{
135}
136
137static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo_rcu(struct cfcnfg *cnfg,
138 u8 phyid)
139{
140 struct cfcnfg_phyinfo *phy;
141
142 list_for_each_entry_rcu(phy, &cnfg->phys, node)
143 if (phy->id == phyid)
144 return phy;
145 return NULL;
146}
147
148static void cfctrl_enum_resp(void)
149{
150}
151
152static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
153 enum cfcnfg_phy_preference phy_pref)
154{
155 /* Try to match with specified preference */
156 struct cfcnfg_phyinfo *phy;
157
158 list_for_each_entry_rcu(phy, &cnfg->phys, node) {
159 if (phy->up && phy->pref == phy_pref &&
160 phy->frm_layer != NULL)
161
162 return &phy->dev_info;
163 }
164
165 /* Otherwise just return something */
166 list_for_each_entry_rcu(phy, &cnfg->phys, node)
167 if (phy->up)
168 return &phy->dev_info;
169
170 return NULL;
171}
172
173static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
174{
175 struct cfcnfg_phyinfo *phy;
176
177 list_for_each_entry_rcu(phy, &cnfg->phys, node)
178 if (phy->ifindex == ifi && phy->up)
179 return phy->id;
180 return -ENODEV;
181}
182
183int caif_disconnect_client(struct net *net, struct cflayer *adap_layer)
184{
185 u8 channel_id;
186 struct cfcnfg *cfg = get_cfcnfg(net);
187
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);
194 if (servl != NULL)
195 layer_set_up(servl, NULL);
196 } else
197 pr_debug("nothing to disconnect\n");
198 cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
199
200 /* Do RCU sync before initiating cleanup */
201 synchronize_rcu();
202 if (adap_layer->ctrlcmd != NULL)
203 adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
204 return 0;
205
206}
207EXPORT_SYMBOL(caif_disconnect_client);
208
209static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
210{
211}
212
213static 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,
219};
220
221
222static int caif_connect_req_to_link_param(struct cfcnfg *cnfg,
223 struct caif_connect_request *s,
224 struct cfctrl_link_param *l)
225{
226 struct dev_info *dev_info;
227 enum cfcnfg_phy_preference pref;
228 int res;
229
230 memset(l, 0, sizeof(*l));
231 /* In caif protocol low value is high priority */
232 l->priority = CAIF_PRIO_MAX - s->priority + 1;
233
234 if (s->ifindex != 0) {
235 res = cfcnfg_get_id_from_ifi(cnfg, s->ifindex);
236 if (res < 0)
237 return res;
238 l->phyid = res;
239 } else {
240 switch (s->link_selector) {
241 case CAIF_LINK_HIGH_BANDW:
242 pref = CFPHYPREF_HIGH_BW;
243 break;
244 case CAIF_LINK_LOW_LATENCY:
245 pref = CFPHYPREF_LOW_LAT;
246 break;
247 default:
248 return -EINVAL;
249 }
250 dev_info = cfcnfg_get_phyid(cnfg, pref);
251 if (dev_info == NULL)
252 return -ENODEV;
253 l->phyid = dev_info->id;
254 }
255 switch (s->protocol) {
256 case CAIFPROTO_AT:
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;
260 break;
261 case CAIFPROTO_DATAGRAM:
262 l->linktype = CFCTRL_SRV_DATAGRAM;
263 l->chtype = 0x00;
264 l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
265 break;
266 case CAIFPROTO_DATAGRAM_LOOP:
267 l->linktype = CFCTRL_SRV_DATAGRAM;
268 l->chtype = 0x03;
269 l->endpoint = 0x00;
270 l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
271 break;
272 case CAIFPROTO_RFM:
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;
278 break;
279 case CAIFPROTO_UTIL:
280 l->linktype = CFCTRL_SRV_UTIL;
281 l->endpoint = 0x00;
282 l->chtype = 0x00;
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);
290
291 memcpy(l->u.utility.params, s->param.data,
292 l->u.utility.paramlen);
293
294 break;
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;
299 break;
300 default:
301 return -EINVAL;
302 }
303 return 0;
304}
305
306int caif_connect_client(struct net *net, struct caif_connect_request *conn_req,
307 struct cflayer *adap_layer, int *ifindex,
308 int *proto_head,
309 int *proto_tail)
310{
311 struct cflayer *frml;
312 struct cfcnfg_phyinfo *phy;
313 int err;
314 struct cfctrl_link_param param;
315 struct cfcnfg *cfg = get_cfcnfg(net);
316 caif_assert(cfg != NULL);
317
318 rcu_read_lock();
319 err = caif_connect_req_to_link_param(cfg, conn_req, ¶m);
320 if (err)
321 goto unlock;
322
323 phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
324 if (!phy) {
325 err = -ENODEV;
326 goto unlock;
327 }
328 err = -EINVAL;
329
330 if (adap_layer == NULL) {
331 pr_err("adap_layer is zero\n");
332 goto unlock;
333 }
334 if (adap_layer->receive == NULL) {
335 pr_err("adap_layer->receive is NULL\n");
336 goto unlock;
337 }
338 if (adap_layer->ctrlcmd == NULL) {
339 pr_err("adap_layer->ctrlcmd == NULL\n");
340 goto unlock;
341 }
342
343 err = -ENODEV;
344 frml = phy->frm_layer;
345 if (frml == NULL) {
346 pr_err("Specified PHY type does not exist!\n");
347 goto unlock;
348 }
349 caif_assert(param.phyid == phy->id);
350 caif_assert(phy->frm_layer->id ==
351 param.phyid);
352 caif_assert(phy->phy_layer->id ==
353 param.phyid);
354
355 *ifindex = phy->ifindex;
356 *proto_tail = 2;
357 *proto_head =
358
359 protohead[param.linktype] + (phy->use_stx ? 1 : 0);
360
361 rcu_read_unlock();
362
363 /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
364 cfctrl_enum_req(cfg->ctrl, param.phyid);
365 return cfctrl_linkup_request(cfg->ctrl, ¶m, adap_layer);
366
367unlock:
368 rcu_read_unlock();
369 return err;
370}
371EXPORT_SYMBOL(caif_connect_client);
372
373static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
374 struct cflayer *adapt_layer)
375{
376 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
377 adapt_layer->ctrlcmd(adapt_layer,
378 CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
379}
380
381static void
382cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
383 u8 phyid, struct cflayer *adapt_layer)
384{
385 struct cfcnfg *cnfg = container_obj(layer);
386 struct cflayer *servicel = NULL;
387 struct cfcnfg_phyinfo *phyinfo;
388 struct net_device *netdev;
389
390 if (channel_id == 0) {
391 pr_warn("received channel_id zero\n");
392 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
393 adapt_layer->ctrlcmd(adapt_layer,
394 CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
395 return;
396 }
397
398 rcu_read_lock();
399
400 if (adapt_layer == NULL) {
401 pr_debug("link setup response but no client exist,"
402 "send linkdown back\n");
403 cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
404 goto unlock;
405 }
406
407 caif_assert(cnfg != NULL);
408 caif_assert(phyid != 0);
409
410 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
411 if (phyinfo == NULL) {
412 pr_err("ERROR: Link Layer Device dissapeared"
413 "while connecting\n");
414 goto unlock;
415 }
416
417 caif_assert(phyinfo != NULL);
418 caif_assert(phyinfo->id == phyid);
419 caif_assert(phyinfo->phy_layer != NULL);
420 caif_assert(phyinfo->phy_layer->id == phyid);
421
422 adapt_layer->id = channel_id;
423
424 switch (serv) {
425 case CFCTRL_SRV_VEI:
426 servicel = cfvei_create(channel_id, &phyinfo->dev_info);
427 break;
428 case CFCTRL_SRV_DATAGRAM:
429 servicel = cfdgml_create(channel_id,
430 &phyinfo->dev_info);
431 break;
432 case CFCTRL_SRV_RFM:
433 netdev = phyinfo->dev_info.dev;
434 servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
435 netdev->mtu);
436 break;
437 case CFCTRL_SRV_UTIL:
438 servicel = cfutill_create(channel_id, &phyinfo->dev_info);
439 break;
440 case CFCTRL_SRV_VIDEO:
441 servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
442 break;
443 case CFCTRL_SRV_DBG:
444 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
445 break;
446 default:
447 pr_err("Protocol error. Link setup response "
448 "- unknown channel type\n");
449 goto unlock;
450 }
451 if (!servicel) {
452 pr_warn("Out of memory\n");
453 goto unlock;
454 }
455 layer_set_dn(servicel, cnfg->mux);
456 cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
457 layer_set_up(servicel, adapt_layer);
458 layer_set_dn(adapt_layer, servicel);
459
460 rcu_read_unlock();
461
462 servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
463 return;
464unlock:
465 rcu_read_unlock();
466}
467
468void
469cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type,
470 struct net_device *dev, struct cflayer *phy_layer,
471 enum cfcnfg_phy_preference pref,
472 bool fcs, bool stx)
473{
474 struct cflayer *frml;
475 struct cflayer *phy_driver = NULL;
476 struct cfcnfg_phyinfo *phyinfo;
477 int i;
478 u8 phyid;
479
480 mutex_lock(&cnfg->lock);
481
482 /* CAIF protocol allow maximum 6 link-layers */
483 for (i = 0; i < 7; i++) {
484 phyid = (dev->ifindex + i) & 0x7;
485 if (phyid == 0)
486 continue;
487 if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
488 goto got_phyid;
489 }
490 pr_warn("Too many CAIF Link Layers (max 6)\n");
491 goto out;
492
493got_phyid:
494 phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
495
496 switch (phy_type) {
497 case CFPHYTYPE_FRAG:
498 phy_driver =
499 cfserl_create(CFPHYTYPE_FRAG, phyid, stx);
500 if (!phy_driver) {
501 pr_warn("Out of memory\n");
502 goto out;
503 }
504 break;
505 case CFPHYTYPE_CAIF:
506 phy_driver = NULL;
507 break;
508 default:
509 goto out;
510 }
511 phy_layer->id = phyid;
512 phyinfo->pref = pref;
513 phyinfo->id = phyid;
514 phyinfo->dev_info.id = phyid;
515 phyinfo->dev_info.dev = dev;
516 phyinfo->phy_layer = phy_layer;
517 phyinfo->ifindex = dev->ifindex;
518 phyinfo->use_stx = stx;
519 phyinfo->use_fcs = fcs;
520
521 frml = cffrml_create(phyid, fcs);
522
523 if (!frml) {
524 pr_warn("Out of memory\n");
525 kfree(phyinfo);
526 goto out;
527 }
528 phyinfo->frm_layer = frml;
529 layer_set_up(frml, cnfg->mux);
530
531 if (phy_driver != NULL) {
532 phy_driver->id = phyid;
533 layer_set_dn(frml, phy_driver);
534 layer_set_up(phy_driver, frml);
535 layer_set_dn(phy_driver, phy_layer);
536 layer_set_up(phy_layer, phy_driver);
537 } else {
538 layer_set_dn(frml, phy_layer);
539 layer_set_up(phy_layer, frml);
540 }
541
542 list_add_rcu(&phyinfo->node, &cnfg->phys);
543out:
544 mutex_unlock(&cnfg->lock);
545}
546EXPORT_SYMBOL(cfcnfg_add_phy_layer);
547
548int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
549 bool up)
550{
551 struct cfcnfg_phyinfo *phyinfo;
552
553 rcu_read_lock();
554 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
555 if (phyinfo == NULL) {
556 rcu_read_unlock();
557 return -ENODEV;
558 }
559
560 if (phyinfo->up == up) {
561 rcu_read_unlock();
562 return 0;
563 }
564 phyinfo->up = up;
565
566 if (up) {
567 cffrml_hold(phyinfo->frm_layer);
568 cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
569 phy_layer->id);
570 } else {
571 cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
572 cffrml_put(phyinfo->frm_layer);
573 }
574
575 rcu_read_unlock();
576 return 0;
577}
578EXPORT_SYMBOL(cfcnfg_set_phy_state);
579
580int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
581{
582 struct cflayer *frml, *frml_dn;
583 u16 phyid;
584 struct cfcnfg_phyinfo *phyinfo;
585
586 might_sleep();
587
588 mutex_lock(&cnfg->lock);
589
590 phyid = phy_layer->id;
591 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
592
593 if (phyinfo == NULL) {
594 mutex_unlock(&cnfg->lock);
595 return 0;
596 }
597 caif_assert(phyid == phyinfo->id);
598 caif_assert(phy_layer == phyinfo->phy_layer);
599 caif_assert(phy_layer->id == phyid);
600 caif_assert(phyinfo->frm_layer->id == phyid);
601
602 list_del_rcu(&phyinfo->node);
603 synchronize_rcu();
604
605 /* Fail if reference count is not zero */
606 if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
607 pr_info("Wait for device inuse\n");
608 list_add_rcu(&phyinfo->node, &cnfg->phys);
609 mutex_unlock(&cnfg->lock);
610 return -EAGAIN;
611 }
612
613 frml = phyinfo->frm_layer;
614 frml_dn = frml->dn;
615 cffrml_set_uplayer(frml, NULL);
616 cffrml_set_dnlayer(frml, NULL);
617 if (phy_layer != frml_dn) {
618 layer_set_up(frml_dn, NULL);
619 layer_set_dn(frml_dn, NULL);
620 }
621 layer_set_up(phy_layer, NULL);
622
623 if (phyinfo->phy_layer != frml_dn)
624 kfree(frml_dn);
625
626 cffrml_free(frml);
627 kfree(phyinfo);
628 mutex_unlock(&cnfg->lock);
629
630 return 0;
631}
632EXPORT_SYMBOL(cfcnfg_del_phy_layer);
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 strscpy(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 strscpy(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, ¶m);
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, ¶m, 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
453int
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, res = 0;
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 res = -EEXIST;
477 goto out;
478
479got_phyid:
480 phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
481 if (!phyinfo) {
482 res = -ENOMEM;
483 goto out;
484 }
485
486 phy_layer->id = phyid;
487 phyinfo->pref = pref;
488 phyinfo->id = phyid;
489 phyinfo->dev_info.id = phyid;
490 phyinfo->dev_info.dev = dev;
491 phyinfo->phy_layer = phy_layer;
492 phyinfo->ifindex = dev->ifindex;
493 phyinfo->head_room = head_room;
494 phyinfo->use_fcs = fcs;
495
496 frml = cffrml_create(phyid, fcs);
497
498 if (!frml) {
499 res = -ENOMEM;
500 goto out_err;
501 }
502 phyinfo->frm_layer = frml;
503 layer_set_up(frml, cnfg->mux);
504
505 if (link_support != NULL) {
506 link_support->id = phyid;
507 layer_set_dn(frml, link_support);
508 layer_set_up(link_support, frml);
509 layer_set_dn(link_support, phy_layer);
510 layer_set_up(phy_layer, link_support);
511 } else {
512 layer_set_dn(frml, phy_layer);
513 layer_set_up(phy_layer, frml);
514 }
515
516 list_add_rcu(&phyinfo->node, &cnfg->phys);
517out:
518 mutex_unlock(&cnfg->lock);
519 return res;
520
521out_err:
522 kfree(phyinfo);
523 mutex_unlock(&cnfg->lock);
524 return res;
525}
526EXPORT_SYMBOL(cfcnfg_add_phy_layer);
527
528int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
529 bool up)
530{
531 struct cfcnfg_phyinfo *phyinfo;
532
533 rcu_read_lock();
534 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
535 if (phyinfo == NULL) {
536 rcu_read_unlock();
537 return -ENODEV;
538 }
539
540 if (phyinfo->up == up) {
541 rcu_read_unlock();
542 return 0;
543 }
544 phyinfo->up = up;
545
546 if (up) {
547 cffrml_hold(phyinfo->frm_layer);
548 cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
549 phy_layer->id);
550 } else {
551 cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
552 cffrml_put(phyinfo->frm_layer);
553 }
554
555 rcu_read_unlock();
556 return 0;
557}
558EXPORT_SYMBOL(cfcnfg_set_phy_state);
559
560int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
561{
562 struct cflayer *frml, *frml_dn;
563 u16 phyid;
564 struct cfcnfg_phyinfo *phyinfo;
565
566 might_sleep();
567
568 mutex_lock(&cnfg->lock);
569
570 phyid = phy_layer->id;
571 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
572
573 if (phyinfo == NULL) {
574 mutex_unlock(&cnfg->lock);
575 return 0;
576 }
577 caif_assert(phyid == phyinfo->id);
578 caif_assert(phy_layer == phyinfo->phy_layer);
579 caif_assert(phy_layer->id == phyid);
580 caif_assert(phyinfo->frm_layer->id == phyid);
581
582 list_del_rcu(&phyinfo->node);
583 synchronize_rcu();
584
585 /* Fail if reference count is not zero */
586 if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
587 pr_info("Wait for device inuse\n");
588 list_add_rcu(&phyinfo->node, &cnfg->phys);
589 mutex_unlock(&cnfg->lock);
590 return -EAGAIN;
591 }
592
593 frml = phyinfo->frm_layer;
594 frml_dn = frml->dn;
595 cffrml_set_uplayer(frml, NULL);
596 cffrml_set_dnlayer(frml, NULL);
597 if (phy_layer != frml_dn) {
598 layer_set_up(frml_dn, NULL);
599 layer_set_dn(frml_dn, NULL);
600 }
601 layer_set_up(phy_layer, NULL);
602
603 if (phyinfo->phy_layer != frml_dn)
604 kfree(frml_dn);
605
606 cffrml_free(frml);
607 kfree(phyinfo);
608 mutex_unlock(&cnfg->lock);
609
610 return 0;
611}
612EXPORT_SYMBOL(cfcnfg_del_phy_layer);