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