1
2
3
4
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
27
28
29struct cfcnfg_phyinfo {
30 struct list_head node;
31 bool up;
32
33
34 struct cflayer *frm_layer;
35
36 struct cflayer *phy_layer;
37
38 unsigned int id;
39
40 enum cfcnfg_phy_preference pref;
41
42
43 struct dev_info dev_info;
44
45
46 int ifindex;
47
48
49 int head_room;
50
51
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
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
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
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
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
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
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 strncpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume,
272 sizeof(l->u.rfm.volume)-1);
273 l->u.rfm.volume[sizeof(l->u.rfm.volume)-1] = 0;
274 break;
275 case CAIFPROTO_UTIL:
276 l->linktype = CFCTRL_SRV_UTIL;
277 l->endpoint = 0x00;
278 l->chtype = 0x00;
279 strncpy(l->u.utility.name, s->sockaddr.u.util.service,
280 sizeof(l->u.utility.name)-1);
281 l->u.utility.name[sizeof(l->u.utility.name)-1] = 0;
282 caif_assert(sizeof(l->u.utility.name) > 10);
283 l->u.utility.paramlen = s->param.size;
284 if (l->u.utility.paramlen > sizeof(l->u.utility.params))
285 l->u.utility.paramlen = sizeof(l->u.utility.params);
286
287 memcpy(l->u.utility.params, s->param.data,
288 l->u.utility.paramlen);
289
290 break;
291 case CAIFPROTO_DEBUG:
292 l->linktype = CFCTRL_SRV_DBG;
293 l->endpoint = s->sockaddr.u.dbg.service;
294 l->chtype = s->sockaddr.u.dbg.type;
295 break;
296 default:
297 return -EINVAL;
298 }
299 return 0;
300}
301
302int caif_connect_client(struct net *net, struct caif_connect_request *conn_req,
303 struct cflayer *adap_layer, int *ifindex,
304 int *proto_head, int *proto_tail)
305{
306 struct cflayer *frml;
307 struct cfcnfg_phyinfo *phy;
308 int err;
309 struct cfctrl_link_param param;
310 struct cfcnfg *cfg = get_cfcnfg(net);
311
312 rcu_read_lock();
313 err = caif_connect_req_to_link_param(cfg, conn_req, ¶m);
314 if (err)
315 goto unlock;
316
317 phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
318 if (!phy) {
319 err = -ENODEV;
320 goto unlock;
321 }
322 err = -EINVAL;
323
324 if (adap_layer == NULL) {
325 pr_err("adap_layer is zero\n");
326 goto unlock;
327 }
328 if (adap_layer->receive == NULL) {
329 pr_err("adap_layer->receive is NULL\n");
330 goto unlock;
331 }
332 if (adap_layer->ctrlcmd == NULL) {
333 pr_err("adap_layer->ctrlcmd == NULL\n");
334 goto unlock;
335 }
336
337 err = -ENODEV;
338 frml = phy->frm_layer;
339 if (frml == NULL) {
340 pr_err("Specified PHY type does not exist!\n");
341 goto unlock;
342 }
343 caif_assert(param.phyid == phy->id);
344 caif_assert(phy->frm_layer->id ==
345 param.phyid);
346 caif_assert(phy->phy_layer->id ==
347 param.phyid);
348
349 *ifindex = phy->ifindex;
350 *proto_tail = 2;
351 *proto_head = protohead[param.linktype] + phy->head_room;
352
353 rcu_read_unlock();
354
355
356 cfctrl_enum_req(cfg->ctrl, param.phyid);
357 return cfctrl_linkup_request(cfg->ctrl, ¶m, adap_layer);
358
359unlock:
360 rcu_read_unlock();
361 return err;
362}
363EXPORT_SYMBOL(caif_connect_client);
364
365static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
366 struct cflayer *adapt_layer)
367{
368 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
369 adapt_layer->ctrlcmd(adapt_layer,
370 CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
371}
372
373static void
374cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
375 u8 phyid, struct cflayer *adapt_layer)
376{
377 struct cfcnfg *cnfg = container_obj(layer);
378 struct cflayer *servicel = NULL;
379 struct cfcnfg_phyinfo *phyinfo;
380 struct net_device *netdev;
381
382 if (channel_id == 0) {
383 pr_warn("received channel_id zero\n");
384 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
385 adapt_layer->ctrlcmd(adapt_layer,
386 CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
387 return;
388 }
389
390 rcu_read_lock();
391
392 if (adapt_layer == NULL) {
393 pr_debug("link setup response but no client exist,"
394 "send linkdown back\n");
395 cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
396 goto unlock;
397 }
398
399 caif_assert(cnfg != NULL);
400 caif_assert(phyid != 0);
401
402 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
403 if (phyinfo == NULL) {
404 pr_err("ERROR: Link Layer Device disappeared"
405 "while connecting\n");
406 goto unlock;
407 }
408
409 caif_assert(phyinfo != NULL);
410 caif_assert(phyinfo->id == phyid);
411 caif_assert(phyinfo->phy_layer != NULL);
412 caif_assert(phyinfo->phy_layer->id == phyid);
413
414 adapt_layer->id = channel_id;
415
416 switch (serv) {
417 case CFCTRL_SRV_VEI:
418 servicel = cfvei_create(channel_id, &phyinfo->dev_info);
419 break;
420 case CFCTRL_SRV_DATAGRAM:
421 servicel = cfdgml_create(channel_id,
422 &phyinfo->dev_info);
423 break;
424 case CFCTRL_SRV_RFM:
425 netdev = phyinfo->dev_info.dev;
426 servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
427 netdev->mtu);
428 break;
429 case CFCTRL_SRV_UTIL:
430 servicel = cfutill_create(channel_id, &phyinfo->dev_info);
431 break;
432 case CFCTRL_SRV_VIDEO:
433 servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
434 break;
435 case CFCTRL_SRV_DBG:
436 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
437 break;
438 default:
439 pr_err("Protocol error. Link setup response "
440 "- unknown channel type\n");
441 goto unlock;
442 }
443 if (!servicel)
444 goto unlock;
445 layer_set_dn(servicel, cnfg->mux);
446 cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
447 layer_set_up(servicel, adapt_layer);
448 layer_set_dn(adapt_layer, servicel);
449
450 rcu_read_unlock();
451
452 servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
453 return;
454unlock:
455 rcu_read_unlock();
456}
457
458void
459cfcnfg_add_phy_layer(struct cfcnfg *cnfg,
460 struct net_device *dev, struct cflayer *phy_layer,
461 enum cfcnfg_phy_preference pref,
462 struct cflayer *link_support,
463 bool fcs, int head_room)
464{
465 struct cflayer *frml;
466 struct cfcnfg_phyinfo *phyinfo = NULL;
467 int i;
468 u8 phyid;
469
470 mutex_lock(&cnfg->lock);
471
472
473 for (i = 0; i < 7; i++) {
474 phyid = (dev->ifindex + i) & 0x7;
475 if (phyid == 0)
476 continue;
477 if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
478 goto got_phyid;
479 }
480 pr_warn("Too many CAIF Link Layers (max 6)\n");
481 goto out;
482
483got_phyid:
484 phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
485 if (!phyinfo)
486 goto out_err;
487
488 phy_layer->id = phyid;
489 phyinfo->pref = pref;
490 phyinfo->id = phyid;
491 phyinfo->dev_info.id = phyid;
492 phyinfo->dev_info.dev = dev;
493 phyinfo->phy_layer = phy_layer;
494 phyinfo->ifindex = dev->ifindex;
495 phyinfo->head_room = head_room;
496 phyinfo->use_fcs = fcs;
497
498 frml = cffrml_create(phyid, fcs);
499
500 if (!frml)
501 goto out_err;
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;
520
521out_err:
522 kfree(phyinfo);
523 mutex_unlock(&cnfg->lock);
524}
525EXPORT_SYMBOL(cfcnfg_add_phy_layer);
526
527int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
528 bool up)
529{
530 struct cfcnfg_phyinfo *phyinfo;
531
532 rcu_read_lock();
533 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
534 if (phyinfo == NULL) {
535 rcu_read_unlock();
536 return -ENODEV;
537 }
538
539 if (phyinfo->up == up) {
540 rcu_read_unlock();
541 return 0;
542 }
543 phyinfo->up = up;
544
545 if (up) {
546 cffrml_hold(phyinfo->frm_layer);
547 cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
548 phy_layer->id);
549 } else {
550 cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
551 cffrml_put(phyinfo->frm_layer);
552 }
553
554 rcu_read_unlock();
555 return 0;
556}
557EXPORT_SYMBOL(cfcnfg_set_phy_state);
558
559int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
560{
561 struct cflayer *frml, *frml_dn;
562 u16 phyid;
563 struct cfcnfg_phyinfo *phyinfo;
564
565 might_sleep();
566
567 mutex_lock(&cnfg->lock);
568
569 phyid = phy_layer->id;
570 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
571
572 if (phyinfo == NULL) {
573 mutex_unlock(&cnfg->lock);
574 return 0;
575 }
576 caif_assert(phyid == phyinfo->id);
577 caif_assert(phy_layer == phyinfo->phy_layer);
578 caif_assert(phy_layer->id == phyid);
579 caif_assert(phyinfo->frm_layer->id == phyid);
580
581 list_del_rcu(&phyinfo->node);
582 synchronize_rcu();
583
584
585 if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
586 pr_info("Wait for device inuse\n");
587 list_add_rcu(&phyinfo->node, &cnfg->phys);
588 mutex_unlock(&cnfg->lock);
589 return -EAGAIN;
590 }
591
592 frml = phyinfo->frm_layer;
593 frml_dn = frml->dn;
594 cffrml_set_uplayer(frml, NULL);
595 cffrml_set_dnlayer(frml, NULL);
596 if (phy_layer != frml_dn) {
597 layer_set_up(frml_dn, NULL);
598 layer_set_dn(frml_dn, NULL);
599 }
600 layer_set_up(phy_layer, NULL);
601
602 if (phyinfo->phy_layer != frml_dn)
603 kfree(frml_dn);
604
605 cffrml_free(frml);
606 kfree(phyinfo);
607 mutex_unlock(&cnfg->lock);
608
609 return 0;
610}
611EXPORT_SYMBOL(cfcnfg_del_phy_layer);
612
