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
7 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
9 #include <linux/kernel.h>
10 #include <linux/stddef.h>
11 #include <linux/slab.h>
12 #include <linux/netdevice.h>
13 #include <net/caif/caif_layer.h>
14 #include <net/caif/cfpkt.h>
15 #include <net/caif/cfcnfg.h>
16 #include <net/caif/cfctrl.h>
17 #include <net/caif/cfmuxl.h>
18 #include <net/caif/cffrml.h>
19 #include <net/caif/cfserl.h>
20 #include <net/caif/cfsrvl.h>
22 #include <linux/module.h>
23 #include <asm/atomic.h>
25 #define MAX_PHY_LAYERS 7
26 #define PHY_NAME_LEN 20
28 #define container_obj(layr) container_of(layr, struct cfcnfg, layer)
29 #define RFM_FRAGMENT_SIZE 4030
31 /* Information about CAIF physical interfaces held by Config Module in order
32 * to manage physical interfaces
34 struct cfcnfg_phyinfo
{
35 /* Pointer to the layer below the MUX (framing layer) */
36 struct cflayer
*frm_layer
;
37 /* Pointer to the lowest actual physical layer */
38 struct cflayer
*phy_layer
;
39 /* Unique identifier of the physical interface */
41 /* Preference of the physical in interface */
42 enum cfcnfg_phy_preference pref
;
44 /* Reference count, number of channels using the device */
47 /* Information about the physical device */
48 struct dev_info dev_info
;
53 /* Use Start of frame extension */
56 /* Use Start of frame checksum */
65 struct cfcnfg_phyinfo phy_layers
[MAX_PHY_LAYERS
];
68 static void cfcnfg_linkup_rsp(struct cflayer
*layer
, u8 channel_id
,
69 enum cfctrl_srv serv
, u8 phyid
,
70 struct cflayer
*adapt_layer
);
71 static void cfcnfg_linkdestroy_rsp(struct cflayer
*layer
, u8 channel_id
);
72 static void cfcnfg_reject_rsp(struct cflayer
*layer
, u8 channel_id
,
73 struct cflayer
*adapt_layer
);
74 static void cfctrl_resp_func(void);
75 static void cfctrl_enum_resp(void);
77 struct cfcnfg
*cfcnfg_create(void)
80 struct cfctrl_rsp
*resp
;
81 /* Initiate this layer */
82 this = kzalloc(sizeof(struct cfcnfg
), GFP_ATOMIC
);
84 pr_warn("Out of memory\n");
87 this->mux
= cfmuxl_create();
90 this->ctrl
= cfctrl_create();
93 /* Initiate response functions */
94 resp
= cfctrl_get_respfuncs(this->ctrl
);
95 resp
->enum_rsp
= cfctrl_enum_resp
;
96 resp
->linkerror_ind
= cfctrl_resp_func
;
97 resp
->linkdestroy_rsp
= cfcnfg_linkdestroy_rsp
;
98 resp
->sleep_rsp
= cfctrl_resp_func
;
99 resp
->wake_rsp
= cfctrl_resp_func
;
100 resp
->restart_rsp
= cfctrl_resp_func
;
101 resp
->radioset_rsp
= cfctrl_resp_func
;
102 resp
->linksetup_rsp
= cfcnfg_linkup_rsp
;
103 resp
->reject_rsp
= cfcnfg_reject_rsp
;
105 this->last_phyid
= 1;
107 cfmuxl_set_uplayer(this->mux
, this->ctrl
, 0);
108 layer_set_dn(this->ctrl
, this->mux
);
109 layer_set_up(this->ctrl
, this);
112 pr_warn("Out of memory\n");
118 EXPORT_SYMBOL(cfcnfg_create
);
120 void cfcnfg_remove(struct cfcnfg
*cfg
)
129 static void cfctrl_resp_func(void)
133 static void cfctrl_enum_resp(void)
137 struct dev_info
*cfcnfg_get_phyid(struct cfcnfg
*cnfg
,
138 enum cfcnfg_phy_preference phy_pref
)
142 /* Try to match with specified preference */
143 for (i
= 1; i
< MAX_PHY_LAYERS
; i
++) {
144 if (cnfg
->phy_layers
[i
].id
== i
&&
145 cnfg
->phy_layers
[i
].pref
== phy_pref
&&
146 cnfg
->phy_layers
[i
].frm_layer
!= NULL
) {
147 caif_assert(cnfg
->phy_layers
!= NULL
);
148 caif_assert(cnfg
->phy_layers
[i
].id
== i
);
149 return &cnfg
->phy_layers
[i
].dev_info
;
152 /* Otherwise just return something */
153 for (i
= 1; i
< MAX_PHY_LAYERS
; i
++) {
154 if (cnfg
->phy_layers
[i
].id
== i
) {
155 caif_assert(cnfg
->phy_layers
!= NULL
);
156 caif_assert(cnfg
->phy_layers
[i
].id
== i
);
157 return &cnfg
->phy_layers
[i
].dev_info
;
164 static struct cfcnfg_phyinfo
*cfcnfg_get_phyinfo(struct cfcnfg
*cnfg
,
168 /* Try to match with specified preference */
169 for (i
= 0; i
< MAX_PHY_LAYERS
; i
++)
170 if (cnfg
->phy_layers
[i
].frm_layer
!= NULL
&&
171 cnfg
->phy_layers
[i
].id
== phyid
)
172 return &cnfg
->phy_layers
[i
];
176 int cfcnfg_get_named(struct cfcnfg
*cnfg
, char *name
)
180 /* Try to match with specified name */
181 for (i
= 0; i
< MAX_PHY_LAYERS
; i
++) {
182 if (cnfg
->phy_layers
[i
].frm_layer
!= NULL
183 && strcmp(cnfg
->phy_layers
[i
].phy_layer
->name
,
185 return cnfg
->phy_layers
[i
].frm_layer
->id
;
190 int cfcnfg_disconn_adapt_layer(struct cfcnfg
*cnfg
, struct cflayer
*adap_layer
)
194 struct cflayer
*servl
= NULL
;
195 struct cfcnfg_phyinfo
*phyinfo
= NULL
;
197 caif_assert(adap_layer
!= NULL
);
198 channel_id
= adap_layer
->id
;
199 if (adap_layer
->dn
== NULL
|| channel_id
== 0) {
200 pr_err("adap_layer->dn == NULL or adap_layer->id is 0\n");
204 servl
= cfmuxl_remove_uplayer(cnfg
->mux
, channel_id
);
207 layer_set_up(servl
, NULL
);
208 ret
= cfctrl_linkdown_req(cnfg
->ctrl
, channel_id
, adap_layer
);
210 pr_err("PROTOCOL ERROR - Error removing service_layer Channel_Id(%d)",
215 caif_assert(channel_id
== servl
->id
);
216 if (adap_layer
->dn
!= NULL
) {
217 phyid
= cfsrvl_getphyid(adap_layer
->dn
);
219 phyinfo
= cfcnfg_get_phyinfo(cnfg
, phyid
);
220 if (phyinfo
== NULL
) {
221 pr_warn("No interface to send disconnect to\n");
225 if (phyinfo
->id
!= phyid
||
226 phyinfo
->phy_layer
->id
!= phyid
||
227 phyinfo
->frm_layer
->id
!= phyid
) {
228 pr_err("Inconsistency in phy registration\n");
233 if (phyinfo
!= NULL
&& --phyinfo
->phy_ref_count
== 0 &&
234 phyinfo
->phy_layer
!= NULL
&&
235 phyinfo
->phy_layer
->modemcmd
!= NULL
) {
236 phyinfo
->phy_layer
->modemcmd(phyinfo
->phy_layer
,
237 _CAIF_MODEMCMD_PHYIF_USELESS
);
241 cfctrl_cancel_req(cnfg
->ctrl
, adap_layer
);
242 if (adap_layer
->ctrlcmd
!= NULL
)
243 adap_layer
->ctrlcmd(adap_layer
, CAIF_CTRLCMD_DEINIT_RSP
, 0);
247 EXPORT_SYMBOL(cfcnfg_disconn_adapt_layer
);
249 void cfcnfg_release_adap_layer(struct cflayer
*adap_layer
)
252 cfsrvl_put(adap_layer
->dn
);
254 EXPORT_SYMBOL(cfcnfg_release_adap_layer
);
256 static void cfcnfg_linkdestroy_rsp(struct cflayer
*layer
, u8 channel_id
)
260 int protohead
[CFCTRL_SRV_MASK
] = {
261 [CFCTRL_SRV_VEI
] = 4,
262 [CFCTRL_SRV_DATAGRAM
] = 7,
263 [CFCTRL_SRV_UTIL
] = 4,
264 [CFCTRL_SRV_RFM
] = 3,
265 [CFCTRL_SRV_DBG
] = 3,
268 int cfcnfg_add_adaptation_layer(struct cfcnfg
*cnfg
,
269 struct cfctrl_link_param
*param
,
270 struct cflayer
*adap_layer
,
275 struct cflayer
*frml
;
276 if (adap_layer
== NULL
) {
277 pr_err("adap_layer is zero\n");
280 if (adap_layer
->receive
== NULL
) {
281 pr_err("adap_layer->receive is NULL\n");
284 if (adap_layer
->ctrlcmd
== NULL
) {
285 pr_err("adap_layer->ctrlcmd == NULL\n");
288 frml
= cnfg
->phy_layers
[param
->phyid
].frm_layer
;
290 pr_err("Specified PHY type does not exist!\n");
293 caif_assert(param
->phyid
== cnfg
->phy_layers
[param
->phyid
].id
);
294 caif_assert(cnfg
->phy_layers
[param
->phyid
].frm_layer
->id
==
296 caif_assert(cnfg
->phy_layers
[param
->phyid
].phy_layer
->id
==
299 *ifindex
= cnfg
->phy_layers
[param
->phyid
].ifindex
;
301 protohead
[param
->linktype
]+
302 (cnfg
->phy_layers
[param
->phyid
].use_stx
? 1 : 0);
306 /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
307 cfctrl_enum_req(cnfg
->ctrl
, param
->phyid
);
308 return cfctrl_linkup_request(cnfg
->ctrl
, param
, adap_layer
);
310 EXPORT_SYMBOL(cfcnfg_add_adaptation_layer
);
312 static void cfcnfg_reject_rsp(struct cflayer
*layer
, u8 channel_id
,
313 struct cflayer
*adapt_layer
)
315 if (adapt_layer
!= NULL
&& adapt_layer
->ctrlcmd
!= NULL
)
316 adapt_layer
->ctrlcmd(adapt_layer
,
317 CAIF_CTRLCMD_INIT_FAIL_RSP
, 0);
321 cfcnfg_linkup_rsp(struct cflayer
*layer
, u8 channel_id
, enum cfctrl_srv serv
,
322 u8 phyid
, struct cflayer
*adapt_layer
)
324 struct cfcnfg
*cnfg
= container_obj(layer
);
325 struct cflayer
*servicel
= NULL
;
326 struct cfcnfg_phyinfo
*phyinfo
;
327 struct net_device
*netdev
;
329 if (adapt_layer
== NULL
) {
330 pr_debug("link setup response but no client exist, send linkdown back\n");
331 cfctrl_linkdown_req(cnfg
->ctrl
, channel_id
, NULL
);
335 caif_assert(cnfg
!= NULL
);
336 caif_assert(phyid
!= 0);
337 phyinfo
= &cnfg
->phy_layers
[phyid
];
338 caif_assert(phyinfo
->id
== phyid
);
339 caif_assert(phyinfo
->phy_layer
!= NULL
);
340 caif_assert(phyinfo
->phy_layer
->id
== phyid
);
342 phyinfo
->phy_ref_count
++;
343 if (phyinfo
->phy_ref_count
== 1 &&
344 phyinfo
->phy_layer
->modemcmd
!= NULL
) {
345 phyinfo
->phy_layer
->modemcmd(phyinfo
->phy_layer
,
346 _CAIF_MODEMCMD_PHYIF_USEFULL
);
348 adapt_layer
->id
= channel_id
;
352 servicel
= cfvei_create(channel_id
, &phyinfo
->dev_info
);
354 case CFCTRL_SRV_DATAGRAM
:
355 servicel
= cfdgml_create(channel_id
, &phyinfo
->dev_info
);
358 netdev
= phyinfo
->dev_info
.dev
;
359 servicel
= cfrfml_create(channel_id
, &phyinfo
->dev_info
,
362 case CFCTRL_SRV_UTIL
:
363 servicel
= cfutill_create(channel_id
, &phyinfo
->dev_info
);
365 case CFCTRL_SRV_VIDEO
:
366 servicel
= cfvidl_create(channel_id
, &phyinfo
->dev_info
);
369 servicel
= cfdbgl_create(channel_id
, &phyinfo
->dev_info
);
372 pr_err("Protocol error. Link setup response - unknown channel type\n");
376 pr_warn("Out of memory\n");
379 layer_set_dn(servicel
, cnfg
->mux
);
380 cfmuxl_set_uplayer(cnfg
->mux
, servicel
, channel_id
);
381 layer_set_up(servicel
, adapt_layer
);
382 layer_set_dn(adapt_layer
, servicel
);
383 cfsrvl_get(servicel
);
384 servicel
->ctrlcmd(servicel
, CAIF_CTRLCMD_INIT_RSP
, 0);
388 cfcnfg_add_phy_layer(struct cfcnfg
*cnfg
, enum cfcnfg_phy_type phy_type
,
389 struct net_device
*dev
, struct cflayer
*phy_layer
,
390 u16
*phyid
, enum cfcnfg_phy_preference pref
,
393 struct cflayer
*frml
;
394 struct cflayer
*phy_driver
= NULL
;
398 if (cnfg
->phy_layers
[cnfg
->last_phyid
].frm_layer
== NULL
) {
399 *phyid
= cnfg
->last_phyid
;
401 /* range: * 1..(MAX_PHY_LAYERS-1) */
403 (cnfg
->last_phyid
% (MAX_PHY_LAYERS
- 1)) + 1;
406 for (i
= 1; i
< MAX_PHY_LAYERS
; i
++) {
407 if (cnfg
->phy_layers
[i
].frm_layer
== NULL
) {
414 pr_err("No Available PHY ID\n");
421 cfserl_create(CFPHYTYPE_FRAG
, *phyid
, stx
);
423 pr_warn("Out of memory\n");
432 pr_err("%d\n", phy_type
);
437 phy_layer
->id
= *phyid
;
438 cnfg
->phy_layers
[*phyid
].pref
= pref
;
439 cnfg
->phy_layers
[*phyid
].id
= *phyid
;
440 cnfg
->phy_layers
[*phyid
].dev_info
.id
= *phyid
;
441 cnfg
->phy_layers
[*phyid
].dev_info
.dev
= dev
;
442 cnfg
->phy_layers
[*phyid
].phy_layer
= phy_layer
;
443 cnfg
->phy_layers
[*phyid
].phy_ref_count
= 0;
444 cnfg
->phy_layers
[*phyid
].ifindex
= dev
->ifindex
;
445 cnfg
->phy_layers
[*phyid
].use_stx
= stx
;
446 cnfg
->phy_layers
[*phyid
].use_fcs
= fcs
;
448 phy_layer
->type
= phy_type
;
449 frml
= cffrml_create(*phyid
, fcs
);
451 pr_warn("Out of memory\n");
454 cnfg
->phy_layers
[*phyid
].frm_layer
= frml
;
455 cfmuxl_set_dnlayer(cnfg
->mux
, frml
, *phyid
);
456 layer_set_up(frml
, cnfg
->mux
);
458 if (phy_driver
!= NULL
) {
459 phy_driver
->id
= *phyid
;
460 layer_set_dn(frml
, phy_driver
);
461 layer_set_up(phy_driver
, frml
);
462 layer_set_dn(phy_driver
, phy_layer
);
463 layer_set_up(phy_layer
, phy_driver
);
465 layer_set_dn(frml
, phy_layer
);
466 layer_set_up(phy_layer
, frml
);
469 EXPORT_SYMBOL(cfcnfg_add_phy_layer
);
471 int cfcnfg_del_phy_layer(struct cfcnfg
*cnfg
, struct cflayer
*phy_layer
)
473 struct cflayer
*frml
, *frml_dn
;
475 phyid
= phy_layer
->id
;
476 caif_assert(phyid
== cnfg
->phy_layers
[phyid
].id
);
477 caif_assert(phy_layer
== cnfg
->phy_layers
[phyid
].phy_layer
);
478 caif_assert(phy_layer
->id
== phyid
);
479 caif_assert(cnfg
->phy_layers
[phyid
].frm_layer
->id
== phyid
);
481 memset(&cnfg
->phy_layers
[phy_layer
->id
], 0,
482 sizeof(struct cfcnfg_phyinfo
));
483 frml
= cfmuxl_remove_dnlayer(cnfg
->mux
, phy_layer
->id
);
485 cffrml_set_uplayer(frml
, NULL
);
486 cffrml_set_dnlayer(frml
, NULL
);
489 if (phy_layer
!= frml_dn
) {
490 layer_set_up(frml_dn
, NULL
);
491 layer_set_dn(frml_dn
, NULL
);
494 layer_set_up(phy_layer
, NULL
);
497 EXPORT_SYMBOL(cfcnfg_del_phy_layer
);