GNU Linux-libre 4.19.211-gnu1
[releases.git] / net / caif / cfcnfg.c
1 /*
2  * Copyright (C) ST-Ericsson AB 2010
3  * Author:      Sjur Brendeland
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         /* 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
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                 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;
108 out_of_mem:
109         synchronize_rcu();
110
111         kfree(this->mux);
112         kfree(this->ctrl);
113         kfree(this);
114         return NULL;
115 }
116
117 void 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
129 static void cfctrl_resp_func(void)
130 {
131 }
132
133 static 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
144 static void cfctrl_enum_resp(void)
145 {
146 }
147
148 static 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
169 static 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
179 int 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 }
203 EXPORT_SYMBOL(caif_disconnect_client);
204
205 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
206 {
207 }
208
209 static 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
218 static 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
300 int 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
357 unlock:
358         rcu_read_unlock();
359         return err;
360 }
361 EXPORT_SYMBOL(caif_connect_client);
362
363 static 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
371 static void
372 cfcnfg_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;
449 unlock:
450         rcu_read_unlock();
451 }
452
453 int
454 cfcnfg_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
479 got_phyid:
480         phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
481         if (!phyinfo) {
482                 res = -ENOMEM;
483                 goto out_err;
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);
517 out:
518         mutex_unlock(&cnfg->lock);
519         return res;
520
521 out_err:
522         kfree(phyinfo);
523         mutex_unlock(&cnfg->lock);
524         return res;
525 }
526 EXPORT_SYMBOL(cfcnfg_add_phy_layer);
527
528 int 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 }
558 EXPORT_SYMBOL(cfcnfg_set_phy_state);
559
560 int 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 }
612 EXPORT_SYMBOL(cfcnfg_del_phy_layer);