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