codec: 3028a & 3026 record enable
[firefly-linux-kernel-4.4.55.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
121 void 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
133 static void cfctrl_resp_func(void)
134 {
135 }
136
137 static 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
148 static void cfctrl_enum_resp(void)
149 {
150 }
151
152 static 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
173 static 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
183 int 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 }
207 EXPORT_SYMBOL(caif_disconnect_client);
208
209 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
210 {
211 }
212
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,
219 };
220
221
222 static 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
306 int 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
317         rcu_read_lock();
318         err = caif_connect_req_to_link_param(cfg, conn_req, &param);
319         if (err)
320                 goto unlock;
321
322         phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
323         if (!phy) {
324                 err = -ENODEV;
325                 goto unlock;
326         }
327         err = -EINVAL;
328
329         if (adap_layer == NULL) {
330                 pr_err("adap_layer is zero\n");
331                 goto unlock;
332         }
333         if (adap_layer->receive == NULL) {
334                 pr_err("adap_layer->receive is NULL\n");
335                 goto unlock;
336         }
337         if (adap_layer->ctrlcmd == NULL) {
338                 pr_err("adap_layer->ctrlcmd == NULL\n");
339                 goto unlock;
340         }
341
342         err = -ENODEV;
343         frml = phy->frm_layer;
344         if (frml == NULL) {
345                 pr_err("Specified PHY type does not exist!\n");
346                 goto unlock;
347         }
348         caif_assert(param.phyid == phy->id);
349         caif_assert(phy->frm_layer->id ==
350                      param.phyid);
351         caif_assert(phy->phy_layer->id ==
352                      param.phyid);
353
354         *ifindex = phy->ifindex;
355         *proto_tail = 2;
356         *proto_head =
357
358         protohead[param.linktype] + (phy->use_stx ? 1 : 0);
359
360         rcu_read_unlock();
361
362         /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
363         cfctrl_enum_req(cfg->ctrl, param.phyid);
364         return cfctrl_linkup_request(cfg->ctrl, &param, adap_layer);
365
366 unlock:
367         rcu_read_unlock();
368         return err;
369 }
370 EXPORT_SYMBOL(caif_connect_client);
371
372 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
373                              struct cflayer *adapt_layer)
374 {
375         if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
376                 adapt_layer->ctrlcmd(adapt_layer,
377                                      CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
378 }
379
380 static void
381 cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
382                   u8 phyid, struct cflayer *adapt_layer)
383 {
384         struct cfcnfg *cnfg = container_obj(layer);
385         struct cflayer *servicel = NULL;
386         struct cfcnfg_phyinfo *phyinfo;
387         struct net_device *netdev;
388
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);
394                 return;
395         }
396
397         rcu_read_lock();
398
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);
403                 goto unlock;
404         }
405
406         caif_assert(cnfg != NULL);
407         caif_assert(phyid != 0);
408
409         phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
410         if (phyinfo == NULL) {
411                 pr_err("ERROR: Link Layer Device dissapeared"
412                                 "while connecting\n");
413                 goto unlock;
414         }
415
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);
420
421         adapt_layer->id = channel_id;
422
423         switch (serv) {
424         case CFCTRL_SRV_VEI:
425                 servicel = cfvei_create(channel_id, &phyinfo->dev_info);
426                 break;
427         case CFCTRL_SRV_DATAGRAM:
428                 servicel = cfdgml_create(channel_id,
429                                         &phyinfo->dev_info);
430                 break;
431         case CFCTRL_SRV_RFM:
432                 netdev = phyinfo->dev_info.dev;
433                 servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
434                                                 netdev->mtu);
435                 break;
436         case CFCTRL_SRV_UTIL:
437                 servicel = cfutill_create(channel_id, &phyinfo->dev_info);
438                 break;
439         case CFCTRL_SRV_VIDEO:
440                 servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
441                 break;
442         case CFCTRL_SRV_DBG:
443                 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
444                 break;
445         default:
446                 pr_err("Protocol error. Link setup response "
447                                 "- unknown channel type\n");
448                 goto unlock;
449         }
450         if (!servicel) {
451                 pr_warn("Out of memory\n");
452                 goto unlock;
453         }
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);
458
459         rcu_read_unlock();
460
461         servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
462         return;
463 unlock:
464         rcu_read_unlock();
465 }
466
467 void
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,
471                      bool fcs, bool stx)
472 {
473         struct cflayer *frml;
474         struct cflayer *phy_driver = NULL;
475         struct cfcnfg_phyinfo *phyinfo;
476         int i;
477         u8 phyid;
478
479         mutex_lock(&cnfg->lock);
480
481         /* CAIF protocol allow maximum 6 link-layers */
482         for (i = 0; i < 7; i++) {
483                 phyid = (dev->ifindex + i) & 0x7;
484                 if (phyid == 0)
485                         continue;
486                 if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
487                         goto got_phyid;
488         }
489         pr_warn("Too many CAIF Link Layers (max 6)\n");
490         goto out;
491
492 got_phyid:
493         phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
494
495         switch (phy_type) {
496         case CFPHYTYPE_FRAG:
497                 phy_driver =
498                     cfserl_create(CFPHYTYPE_FRAG, phyid, stx);
499                 if (!phy_driver) {
500                         pr_warn("Out of memory\n");
501                         goto out;
502                 }
503                 break;
504         case CFPHYTYPE_CAIF:
505                 phy_driver = NULL;
506                 break;
507         default:
508                 goto out;
509         }
510         phy_layer->id = phyid;
511         phyinfo->pref = pref;
512         phyinfo->id = phyid;
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;
519
520         frml = cffrml_create(phyid, fcs);
521
522         if (!frml) {
523                 pr_warn("Out of memory\n");
524                 kfree(phyinfo);
525                 goto out;
526         }
527         phyinfo->frm_layer = frml;
528         layer_set_up(frml, cnfg->mux);
529
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);
536         } else {
537                 layer_set_dn(frml, phy_layer);
538                 layer_set_up(phy_layer, frml);
539         }
540
541         list_add_rcu(&phyinfo->node, &cnfg->phys);
542 out:
543         mutex_unlock(&cnfg->lock);
544 }
545 EXPORT_SYMBOL(cfcnfg_add_phy_layer);
546
547 int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
548                 bool up)
549 {
550         struct cfcnfg_phyinfo *phyinfo;
551
552         rcu_read_lock();
553         phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
554         if (phyinfo == NULL) {
555                 rcu_read_unlock();
556                 return -ENODEV;
557         }
558
559         if (phyinfo->up == up) {
560                 rcu_read_unlock();
561                 return 0;
562         }
563         phyinfo->up = up;
564
565         if (up) {
566                 cffrml_hold(phyinfo->frm_layer);
567                 cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
568                                         phy_layer->id);
569         } else {
570                 cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
571                 cffrml_put(phyinfo->frm_layer);
572         }
573
574         rcu_read_unlock();
575         return 0;
576 }
577 EXPORT_SYMBOL(cfcnfg_set_phy_state);
578
579 int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
580 {
581         struct cflayer *frml, *frml_dn;
582         u16 phyid;
583         struct cfcnfg_phyinfo *phyinfo;
584
585         might_sleep();
586
587         mutex_lock(&cnfg->lock);
588
589         phyid = phy_layer->id;
590         phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
591
592         if (phyinfo == NULL) {
593                 mutex_unlock(&cnfg->lock);
594                 return 0;
595         }
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);
600
601         list_del_rcu(&phyinfo->node);
602         synchronize_rcu();
603
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);
609                 return -EAGAIN;
610         }
611
612         frml = phyinfo->frm_layer;
613         frml_dn = frml->dn;
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);
619         }
620         layer_set_up(phy_layer, NULL);
621
622         if (phyinfo->phy_layer != frml_dn)
623                 kfree(frml_dn);
624
625         cffrml_free(frml);
626         kfree(phyinfo);
627         mutex_unlock(&cnfg->lock);
628
629         return 0;
630 }
631 EXPORT_SYMBOL(cfcnfg_del_phy_layer);