diff options
Diffstat (limited to 'net/caif/cfcnfg.c')
-rw-r--r-- | net/caif/cfcnfg.c | 470 |
1 files changed, 470 insertions, 0 deletions
diff --git a/net/caif/cfcnfg.c b/net/caif/cfcnfg.c new file mode 100644 index 000000000000..df43f264d9fb --- /dev/null +++ b/net/caif/cfcnfg.c | |||
@@ -0,0 +1,470 @@ | |||
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 | #include <linux/kernel.h> | ||
7 | #include <linux/stddef.h> | ||
8 | #include <linux/slab.h> | ||
9 | #include <net/caif/caif_layer.h> | ||
10 | #include <net/caif/cfpkt.h> | ||
11 | #include <net/caif/cfcnfg.h> | ||
12 | #include <net/caif/cfctrl.h> | ||
13 | #include <net/caif/cfmuxl.h> | ||
14 | #include <net/caif/cffrml.h> | ||
15 | #include <net/caif/cfserl.h> | ||
16 | #include <net/caif/cfsrvl.h> | ||
17 | |||
18 | #include <linux/module.h> | ||
19 | #include <asm/atomic.h> | ||
20 | |||
21 | #define MAX_PHY_LAYERS 7 | ||
22 | #define PHY_NAME_LEN 20 | ||
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 | /* Pointer to the layer below the MUX (framing layer) */ | ||
31 | struct cflayer *frm_layer; | ||
32 | /* Pointer to the lowest actual physical layer */ | ||
33 | struct cflayer *phy_layer; | ||
34 | /* Unique identifier of the physical interface */ | ||
35 | unsigned int id; | ||
36 | /* Preference of the physical in interface */ | ||
37 | enum cfcnfg_phy_preference pref; | ||
38 | |||
39 | /* Reference count, number of channels using the device */ | ||
40 | int phy_ref_count; | ||
41 | |||
42 | /* Information about the physical device */ | ||
43 | struct dev_info dev_info; | ||
44 | }; | ||
45 | |||
46 | struct cfcnfg { | ||
47 | struct cflayer layer; | ||
48 | struct cflayer *ctrl; | ||
49 | struct cflayer *mux; | ||
50 | u8 last_phyid; | ||
51 | struct cfcnfg_phyinfo phy_layers[MAX_PHY_LAYERS]; | ||
52 | }; | ||
53 | |||
54 | static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, | ||
55 | enum cfctrl_srv serv, u8 phyid, | ||
56 | struct cflayer *adapt_layer); | ||
57 | static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id); | ||
58 | static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id, | ||
59 | struct cflayer *adapt_layer); | ||
60 | static void cfctrl_resp_func(void); | ||
61 | static void cfctrl_enum_resp(void); | ||
62 | |||
63 | struct cfcnfg *cfcnfg_create(void) | ||
64 | { | ||
65 | struct cfcnfg *this; | ||
66 | struct cfctrl_rsp *resp; | ||
67 | /* Initiate this layer */ | ||
68 | this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC); | ||
69 | if (!this) { | ||
70 | pr_warning("CAIF: %s(): Out of memory\n", __func__); | ||
71 | return NULL; | ||
72 | } | ||
73 | this->mux = cfmuxl_create(); | ||
74 | if (!this->mux) | ||
75 | goto out_of_mem; | ||
76 | this->ctrl = cfctrl_create(); | ||
77 | if (!this->ctrl) | ||
78 | goto out_of_mem; | ||
79 | /* Initiate response functions */ | ||
80 | resp = cfctrl_get_respfuncs(this->ctrl); | ||
81 | resp->enum_rsp = cfctrl_enum_resp; | ||
82 | resp->linkerror_ind = cfctrl_resp_func; | ||
83 | resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp; | ||
84 | resp->sleep_rsp = cfctrl_resp_func; | ||
85 | resp->wake_rsp = cfctrl_resp_func; | ||
86 | resp->restart_rsp = cfctrl_resp_func; | ||
87 | resp->radioset_rsp = cfctrl_resp_func; | ||
88 | resp->linksetup_rsp = cfcnfg_linkup_rsp; | ||
89 | resp->reject_rsp = cfcnfg_reject_rsp; | ||
90 | |||
91 | this->last_phyid = 1; | ||
92 | |||
93 | cfmuxl_set_uplayer(this->mux, this->ctrl, 0); | ||
94 | layer_set_dn(this->ctrl, this->mux); | ||
95 | layer_set_up(this->ctrl, this); | ||
96 | return this; | ||
97 | out_of_mem: | ||
98 | pr_warning("CAIF: %s(): Out of memory\n", __func__); | ||
99 | kfree(this->mux); | ||
100 | kfree(this->ctrl); | ||
101 | kfree(this); | ||
102 | return NULL; | ||
103 | } | ||
104 | EXPORT_SYMBOL(cfcnfg_create); | ||
105 | |||
106 | void cfcnfg_remove(struct cfcnfg *cfg) | ||
107 | { | ||
108 | if (cfg) { | ||
109 | kfree(cfg->mux); | ||
110 | kfree(cfg->ctrl); | ||
111 | kfree(cfg); | ||
112 | } | ||
113 | } | ||
114 | |||
115 | static void cfctrl_resp_func(void) | ||
116 | { | ||
117 | } | ||
118 | |||
119 | static void cfctrl_enum_resp(void) | ||
120 | { | ||
121 | } | ||
122 | |||
123 | struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg, | ||
124 | enum cfcnfg_phy_preference phy_pref) | ||
125 | { | ||
126 | u16 i; | ||
127 | |||
128 | /* Try to match with specified preference */ | ||
129 | for (i = 1; i < MAX_PHY_LAYERS; i++) { | ||
130 | if (cnfg->phy_layers[i].id == i && | ||
131 | cnfg->phy_layers[i].pref == phy_pref && | ||
132 | cnfg->phy_layers[i].frm_layer != NULL) { | ||
133 | caif_assert(cnfg->phy_layers != NULL); | ||
134 | caif_assert(cnfg->phy_layers[i].id == i); | ||
135 | return &cnfg->phy_layers[i].dev_info; | ||
136 | } | ||
137 | } | ||
138 | /* Otherwise just return something */ | ||
139 | for (i = 1; i < MAX_PHY_LAYERS; i++) { | ||
140 | if (cnfg->phy_layers[i].id == i) { | ||
141 | caif_assert(cnfg->phy_layers != NULL); | ||
142 | caif_assert(cnfg->phy_layers[i].id == i); | ||
143 | return &cnfg->phy_layers[i].dev_info; | ||
144 | } | ||
145 | } | ||
146 | |||
147 | return NULL; | ||
148 | } | ||
149 | |||
150 | static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo(struct cfcnfg *cnfg, | ||
151 | u8 phyid) | ||
152 | { | ||
153 | int i; | ||
154 | /* Try to match with specified preference */ | ||
155 | for (i = 0; i < MAX_PHY_LAYERS; i++) | ||
156 | if (cnfg->phy_layers[i].frm_layer != NULL && | ||
157 | cnfg->phy_layers[i].id == phyid) | ||
158 | return &cnfg->phy_layers[i]; | ||
159 | return NULL; | ||
160 | } | ||
161 | |||
162 | int cfcnfg_get_named(struct cfcnfg *cnfg, char *name) | ||
163 | { | ||
164 | int i; | ||
165 | |||
166 | /* Try to match with specified name */ | ||
167 | for (i = 0; i < MAX_PHY_LAYERS; i++) { | ||
168 | if (cnfg->phy_layers[i].frm_layer != NULL | ||
169 | && strcmp(cnfg->phy_layers[i].phy_layer->name, | ||
170 | name) == 0) | ||
171 | return cnfg->phy_layers[i].frm_layer->id; | ||
172 | } | ||
173 | return 0; | ||
174 | } | ||
175 | |||
176 | int cfcnfg_disconn_adapt_layer(struct cfcnfg *cnfg, struct cflayer *adap_layer) | ||
177 | { | ||
178 | u8 channel_id = 0; | ||
179 | int ret = 0; | ||
180 | struct cflayer *servl = NULL; | ||
181 | struct cfcnfg_phyinfo *phyinfo = NULL; | ||
182 | u8 phyid = 0; | ||
183 | caif_assert(adap_layer != NULL); | ||
184 | channel_id = adap_layer->id; | ||
185 | if (adap_layer->dn == NULL || channel_id == 0) { | ||
186 | pr_err("CAIF: %s():adap_layer->id is 0\n", __func__); | ||
187 | ret = -ENOTCONN; | ||
188 | goto end; | ||
189 | } | ||
190 | servl = cfmuxl_remove_uplayer(cnfg->mux, channel_id); | ||
191 | if (servl == NULL) | ||
192 | goto end; | ||
193 | layer_set_up(servl, NULL); | ||
194 | ret = cfctrl_linkdown_req(cnfg->ctrl, channel_id, adap_layer); | ||
195 | if (servl == NULL) { | ||
196 | pr_err("CAIF: %s(): PROTOCOL ERROR " | ||
197 | "- Error removing service_layer Channel_Id(%d)", | ||
198 | __func__, channel_id); | ||
199 | ret = -EINVAL; | ||
200 | goto end; | ||
201 | } | ||
202 | caif_assert(channel_id == servl->id); | ||
203 | if (adap_layer->dn != NULL) { | ||
204 | phyid = cfsrvl_getphyid(adap_layer->dn); | ||
205 | |||
206 | phyinfo = cfcnfg_get_phyinfo(cnfg, phyid); | ||
207 | if (phyinfo == NULL) { | ||
208 | pr_warning("CAIF: %s(): " | ||
209 | "No interface to send disconnect to\n", | ||
210 | __func__); | ||
211 | ret = -ENODEV; | ||
212 | goto end; | ||
213 | } | ||
214 | if (phyinfo->id != phyid || | ||
215 | phyinfo->phy_layer->id != phyid || | ||
216 | phyinfo->frm_layer->id != phyid) { | ||
217 | pr_err("CAIF: %s(): " | ||
218 | "Inconsistency in phy registration\n", | ||
219 | __func__); | ||
220 | ret = -EINVAL; | ||
221 | goto end; | ||
222 | } | ||
223 | } | ||
224 | if (phyinfo != NULL && --phyinfo->phy_ref_count == 0 && | ||
225 | phyinfo->phy_layer != NULL && | ||
226 | phyinfo->phy_layer->modemcmd != NULL) { | ||
227 | phyinfo->phy_layer->modemcmd(phyinfo->phy_layer, | ||
228 | _CAIF_MODEMCMD_PHYIF_USELESS); | ||
229 | } | ||
230 | end: | ||
231 | cfsrvl_put(servl); | ||
232 | cfctrl_cancel_req(cnfg->ctrl, adap_layer); | ||
233 | if (adap_layer->ctrlcmd != NULL) | ||
234 | adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0); | ||
235 | return ret; | ||
236 | |||
237 | } | ||
238 | EXPORT_SYMBOL(cfcnfg_disconn_adapt_layer); | ||
239 | |||
240 | void cfcnfg_release_adap_layer(struct cflayer *adap_layer) | ||
241 | { | ||
242 | if (adap_layer->dn) | ||
243 | cfsrvl_put(adap_layer->dn); | ||
244 | } | ||
245 | EXPORT_SYMBOL(cfcnfg_release_adap_layer); | ||
246 | |||
247 | static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id) | ||
248 | { | ||
249 | } | ||
250 | |||
251 | int cfcnfg_add_adaptation_layer(struct cfcnfg *cnfg, | ||
252 | struct cfctrl_link_param *param, | ||
253 | struct cflayer *adap_layer) | ||
254 | { | ||
255 | struct cflayer *frml; | ||
256 | if (adap_layer == NULL) { | ||
257 | pr_err("CAIF: %s(): adap_layer is zero", __func__); | ||
258 | return -EINVAL; | ||
259 | } | ||
260 | if (adap_layer->receive == NULL) { | ||
261 | pr_err("CAIF: %s(): adap_layer->receive is NULL", __func__); | ||
262 | return -EINVAL; | ||
263 | } | ||
264 | if (adap_layer->ctrlcmd == NULL) { | ||
265 | pr_err("CAIF: %s(): adap_layer->ctrlcmd == NULL", __func__); | ||
266 | return -EINVAL; | ||
267 | } | ||
268 | frml = cnfg->phy_layers[param->phyid].frm_layer; | ||
269 | if (frml == NULL) { | ||
270 | pr_err("CAIF: %s(): Specified PHY type does not exist!", | ||
271 | __func__); | ||
272 | return -ENODEV; | ||
273 | } | ||
274 | caif_assert(param->phyid == cnfg->phy_layers[param->phyid].id); | ||
275 | caif_assert(cnfg->phy_layers[param->phyid].frm_layer->id == | ||
276 | param->phyid); | ||
277 | caif_assert(cnfg->phy_layers[param->phyid].phy_layer->id == | ||
278 | param->phyid); | ||
279 | /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */ | ||
280 | cfctrl_enum_req(cnfg->ctrl, param->phyid); | ||
281 | return cfctrl_linkup_request(cnfg->ctrl, param, adap_layer); | ||
282 | } | ||
283 | EXPORT_SYMBOL(cfcnfg_add_adaptation_layer); | ||
284 | |||
285 | static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id, | ||
286 | struct cflayer *adapt_layer) | ||
287 | { | ||
288 | if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL) | ||
289 | adapt_layer->ctrlcmd(adapt_layer, | ||
290 | CAIF_CTRLCMD_INIT_FAIL_RSP, 0); | ||
291 | } | ||
292 | |||
293 | static void | ||
294 | cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv, | ||
295 | u8 phyid, struct cflayer *adapt_layer) | ||
296 | { | ||
297 | struct cfcnfg *cnfg = container_obj(layer); | ||
298 | struct cflayer *servicel = NULL; | ||
299 | struct cfcnfg_phyinfo *phyinfo; | ||
300 | if (adapt_layer == NULL) { | ||
301 | pr_debug("CAIF: %s(): link setup response " | ||
302 | "but no client exist, send linkdown back\n", | ||
303 | __func__); | ||
304 | cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL); | ||
305 | return; | ||
306 | } | ||
307 | |||
308 | caif_assert(cnfg != NULL); | ||
309 | caif_assert(phyid != 0); | ||
310 | phyinfo = &cnfg->phy_layers[phyid]; | ||
311 | caif_assert(phyinfo != NULL); | ||
312 | caif_assert(phyinfo->id == phyid); | ||
313 | caif_assert(phyinfo->phy_layer != NULL); | ||
314 | caif_assert(phyinfo->phy_layer->id == phyid); | ||
315 | |||
316 | if (phyinfo != NULL && | ||
317 | phyinfo->phy_ref_count++ == 0 && | ||
318 | phyinfo->phy_layer != NULL && | ||
319 | phyinfo->phy_layer->modemcmd != NULL) { | ||
320 | caif_assert(phyinfo->phy_layer->id == phyid); | ||
321 | phyinfo->phy_layer->modemcmd(phyinfo->phy_layer, | ||
322 | _CAIF_MODEMCMD_PHYIF_USEFULL); | ||
323 | |||
324 | } | ||
325 | adapt_layer->id = channel_id; | ||
326 | |||
327 | switch (serv) { | ||
328 | case CFCTRL_SRV_VEI: | ||
329 | servicel = cfvei_create(channel_id, &phyinfo->dev_info); | ||
330 | break; | ||
331 | case CFCTRL_SRV_DATAGRAM: | ||
332 | servicel = cfdgml_create(channel_id, &phyinfo->dev_info); | ||
333 | break; | ||
334 | case CFCTRL_SRV_RFM: | ||
335 | servicel = cfrfml_create(channel_id, &phyinfo->dev_info); | ||
336 | break; | ||
337 | case CFCTRL_SRV_UTIL: | ||
338 | servicel = cfutill_create(channel_id, &phyinfo->dev_info); | ||
339 | break; | ||
340 | case CFCTRL_SRV_VIDEO: | ||
341 | servicel = cfvidl_create(channel_id, &phyinfo->dev_info); | ||
342 | break; | ||
343 | case CFCTRL_SRV_DBG: | ||
344 | servicel = cfdbgl_create(channel_id, &phyinfo->dev_info); | ||
345 | break; | ||
346 | default: | ||
347 | pr_err("CAIF: %s(): Protocol error. " | ||
348 | "Link setup response - unknown channel type\n", | ||
349 | __func__); | ||
350 | return; | ||
351 | } | ||
352 | if (!servicel) { | ||
353 | pr_warning("CAIF: %s(): Out of memory\n", __func__); | ||
354 | return; | ||
355 | } | ||
356 | layer_set_dn(servicel, cnfg->mux); | ||
357 | cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id); | ||
358 | layer_set_up(servicel, adapt_layer); | ||
359 | layer_set_dn(adapt_layer, servicel); | ||
360 | cfsrvl_get(servicel); | ||
361 | servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0); | ||
362 | } | ||
363 | |||
364 | void | ||
365 | cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type, | ||
366 | void *dev, struct cflayer *phy_layer, u16 *phyid, | ||
367 | enum cfcnfg_phy_preference pref, | ||
368 | bool fcs, bool stx) | ||
369 | { | ||
370 | struct cflayer *frml; | ||
371 | struct cflayer *phy_driver = NULL; | ||
372 | int i; | ||
373 | |||
374 | |||
375 | if (cnfg->phy_layers[cnfg->last_phyid].frm_layer == NULL) { | ||
376 | *phyid = cnfg->last_phyid; | ||
377 | |||
378 | /* range: * 1..(MAX_PHY_LAYERS-1) */ | ||
379 | cnfg->last_phyid = | ||
380 | (cnfg->last_phyid % (MAX_PHY_LAYERS - 1)) + 1; | ||
381 | } else { | ||
382 | *phyid = 0; | ||
383 | for (i = 1; i < MAX_PHY_LAYERS; i++) { | ||
384 | if (cnfg->phy_layers[i].frm_layer == NULL) { | ||
385 | *phyid = i; | ||
386 | break; | ||
387 | } | ||
388 | } | ||
389 | } | ||
390 | if (*phyid == 0) { | ||
391 | pr_err("CAIF: %s(): No Available PHY ID\n", __func__); | ||
392 | return; | ||
393 | } | ||
394 | |||
395 | switch (phy_type) { | ||
396 | case CFPHYTYPE_FRAG: | ||
397 | phy_driver = | ||
398 | cfserl_create(CFPHYTYPE_FRAG, *phyid, stx); | ||
399 | if (!phy_driver) { | ||
400 | pr_warning("CAIF: %s(): Out of memory\n", __func__); | ||
401 | return; | ||
402 | } | ||
403 | |||
404 | break; | ||
405 | case CFPHYTYPE_CAIF: | ||
406 | phy_driver = NULL; | ||
407 | break; | ||
408 | default: | ||
409 | pr_err("CAIF: %s(): %d", __func__, phy_type); | ||
410 | return; | ||
411 | break; | ||
412 | } | ||
413 | |||
414 | phy_layer->id = *phyid; | ||
415 | cnfg->phy_layers[*phyid].pref = pref; | ||
416 | cnfg->phy_layers[*phyid].id = *phyid; | ||
417 | cnfg->phy_layers[*phyid].dev_info.id = *phyid; | ||
418 | cnfg->phy_layers[*phyid].dev_info.dev = dev; | ||
419 | cnfg->phy_layers[*phyid].phy_layer = phy_layer; | ||
420 | cnfg->phy_layers[*phyid].phy_ref_count = 0; | ||
421 | phy_layer->type = phy_type; | ||
422 | frml = cffrml_create(*phyid, fcs); | ||
423 | if (!frml) { | ||
424 | pr_warning("CAIF: %s(): Out of memory\n", __func__); | ||
425 | return; | ||
426 | } | ||
427 | cnfg->phy_layers[*phyid].frm_layer = frml; | ||
428 | cfmuxl_set_dnlayer(cnfg->mux, frml, *phyid); | ||
429 | layer_set_up(frml, cnfg->mux); | ||
430 | |||
431 | if (phy_driver != NULL) { | ||
432 | phy_driver->id = *phyid; | ||
433 | layer_set_dn(frml, phy_driver); | ||
434 | layer_set_up(phy_driver, frml); | ||
435 | layer_set_dn(phy_driver, phy_layer); | ||
436 | layer_set_up(phy_layer, phy_driver); | ||
437 | } else { | ||
438 | layer_set_dn(frml, phy_layer); | ||
439 | layer_set_up(phy_layer, frml); | ||
440 | } | ||
441 | } | ||
442 | EXPORT_SYMBOL(cfcnfg_add_phy_layer); | ||
443 | |||
444 | int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer) | ||
445 | { | ||
446 | struct cflayer *frml, *frml_dn; | ||
447 | u16 phyid; | ||
448 | phyid = phy_layer->id; | ||
449 | caif_assert(phyid == cnfg->phy_layers[phyid].id); | ||
450 | caif_assert(phy_layer == cnfg->phy_layers[phyid].phy_layer); | ||
451 | caif_assert(phy_layer->id == phyid); | ||
452 | caif_assert(cnfg->phy_layers[phyid].frm_layer->id == phyid); | ||
453 | |||
454 | memset(&cnfg->phy_layers[phy_layer->id], 0, | ||
455 | sizeof(struct cfcnfg_phyinfo)); | ||
456 | frml = cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id); | ||
457 | frml_dn = frml->dn; | ||
458 | cffrml_set_uplayer(frml, NULL); | ||
459 | cffrml_set_dnlayer(frml, NULL); | ||
460 | kfree(frml); | ||
461 | |||
462 | if (phy_layer != frml_dn) { | ||
463 | layer_set_up(frml_dn, NULL); | ||
464 | layer_set_dn(frml_dn, NULL); | ||
465 | kfree(frml_dn); | ||
466 | } | ||
467 | layer_set_up(phy_layer, NULL); | ||
468 | return 0; | ||
469 | } | ||
470 | EXPORT_SYMBOL(cfcnfg_del_phy_layer); | ||