aboutsummaryrefslogtreecommitdiffstats
path: root/net
diff options
context:
space:
mode:
authorSjur Braendeland <sjur.brandeland@stericsson.com>2010-03-30 09:56:23 -0400
committerDavid S. Miller <davem@davemloft.net>2010-03-30 22:08:46 -0400
commitb482cd2053e3b90a7b33a78c63cdb6badf2ec383 (patch)
tree1c05e28f19a194b52bf43730fa498dcdb0e92b59 /net
parent2721c5b9dd2a56a9710021c00146bb26ba8dd7b3 (diff)
net-caif: add CAIF core protocol stack
CAIF generic protocol implementation. This layer is somewhat generic in order to be able to use and test it outside the Linux Kernel. cfctrl.c - CAIF control protocol layer cfdbgl.c - CAIF debug protocol layer cfdgml.c - CAIF datagram protocol layer cffrml.c - CAIF framing protocol layer cfmuxl.c - CAIF mux protocol layer cfrfml.c - CAIF remote file manager protocol layer cfserl.c - CAIF serial (fragmentation) protocol layer cfsrvl.c - CAIF generic service layer functions cfutill.c - CAIF utility protocol layer cfveil.c - CAIF AT protocol layer cfvidl.c - CAIF video protocol layer Signed-off-by: Sjur Braendeland <sjur.brandeland@stericsson.com> Signed-off-by: David S. Miller <davem@davemloft.net>
Diffstat (limited to 'net')
-rw-r--r--net/caif/cfctrl.c664
-rw-r--r--net/caif/cfdbgl.c40
-rw-r--r--net/caif/cfdgml.c108
-rw-r--r--net/caif/cffrml.c151
-rw-r--r--net/caif/cfmuxl.c246
-rw-r--r--net/caif/cfrfml.c108
-rw-r--r--net/caif/cfserl.c192
-rw-r--r--net/caif/cfsrvl.c185
-rw-r--r--net/caif/cfutill.c115
-rw-r--r--net/caif/cfveil.c107
-rw-r--r--net/caif/cfvidl.c65
11 files changed, 1981 insertions, 0 deletions
diff --git a/net/caif/cfctrl.c b/net/caif/cfctrl.c
new file mode 100644
index 000000000000..11f80140f3cb
--- /dev/null
+++ b/net/caif/cfctrl.c
@@ -0,0 +1,664 @@
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#include <linux/stddef.h>
8#include <linux/spinlock.h>
9#include <linux/slab.h>
10#include <net/caif/caif_layer.h>
11#include <net/caif/cfpkt.h>
12#include <net/caif/cfctrl.h>
13
14#define container_obj(layr) container_of(layr, struct cfctrl, serv.layer)
15#define UTILITY_NAME_LENGTH 16
16#define CFPKT_CTRL_PKT_LEN 20
17
18
19#ifdef CAIF_NO_LOOP
20static int handle_loop(struct cfctrl *ctrl,
21 int cmd, struct cfpkt *pkt){
22 return CAIF_FAILURE;
23}
24#else
25static int handle_loop(struct cfctrl *ctrl,
26 int cmd, struct cfpkt *pkt);
27#endif
28static int cfctrl_recv(struct cflayer *layr, struct cfpkt *pkt);
29static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
30 int phyid);
31
32
33struct cflayer *cfctrl_create(void)
34{
35 struct cfctrl *this =
36 kmalloc(sizeof(struct cfctrl), GFP_ATOMIC);
37 if (!this) {
38 pr_warning("CAIF: %s(): Out of memory\n", __func__);
39 return NULL;
40 }
41 caif_assert(offsetof(struct cfctrl, serv.layer) == 0);
42 memset(this, 0, sizeof(*this));
43 spin_lock_init(&this->info_list_lock);
44 atomic_set(&this->req_seq_no, 1);
45 atomic_set(&this->rsp_seq_no, 1);
46 this->serv.dev_info.id = 0xff;
47 this->serv.layer.id = 0;
48 this->serv.layer.receive = cfctrl_recv;
49 sprintf(this->serv.layer.name, "ctrl");
50 this->serv.layer.ctrlcmd = cfctrl_ctrlcmd;
51 spin_lock_init(&this->loop_linkid_lock);
52 this->loop_linkid = 1;
53 return &this->serv.layer;
54}
55
56static bool param_eq(struct cfctrl_link_param *p1, struct cfctrl_link_param *p2)
57{
58 bool eq =
59 p1->linktype == p2->linktype &&
60 p1->priority == p2->priority &&
61 p1->phyid == p2->phyid &&
62 p1->endpoint == p2->endpoint && p1->chtype == p2->chtype;
63
64 if (!eq)
65 return false;
66
67 switch (p1->linktype) {
68 case CFCTRL_SRV_VEI:
69 return true;
70 case CFCTRL_SRV_DATAGRAM:
71 return p1->u.datagram.connid == p2->u.datagram.connid;
72 case CFCTRL_SRV_RFM:
73 return
74 p1->u.rfm.connid == p2->u.rfm.connid &&
75 strcmp(p1->u.rfm.volume, p2->u.rfm.volume) == 0;
76 case CFCTRL_SRV_UTIL:
77 return
78 p1->u.utility.fifosize_kb == p2->u.utility.fifosize_kb
79 && p1->u.utility.fifosize_bufs ==
80 p2->u.utility.fifosize_bufs
81 && strcmp(p1->u.utility.name, p2->u.utility.name) == 0
82 && p1->u.utility.paramlen == p2->u.utility.paramlen
83 && memcmp(p1->u.utility.params, p2->u.utility.params,
84 p1->u.utility.paramlen) == 0;
85
86 case CFCTRL_SRV_VIDEO:
87 return p1->u.video.connid == p2->u.video.connid;
88 case CFCTRL_SRV_DBG:
89 return true;
90 case CFCTRL_SRV_DECM:
91 return false;
92 default:
93 return false;
94 }
95 return false;
96}
97
98bool cfctrl_req_eq(struct cfctrl_request_info *r1,
99 struct cfctrl_request_info *r2)
100{
101 if (r1->cmd != r2->cmd)
102 return false;
103 if (r1->cmd == CFCTRL_CMD_LINK_SETUP)
104 return param_eq(&r1->param, &r2->param);
105 else
106 return r1->channel_id == r2->channel_id;
107}
108
109/* Insert request at the end */
110void cfctrl_insert_req(struct cfctrl *ctrl,
111 struct cfctrl_request_info *req)
112{
113 struct cfctrl_request_info *p;
114 spin_lock(&ctrl->info_list_lock);
115 req->next = NULL;
116 atomic_inc(&ctrl->req_seq_no);
117 req->sequence_no = atomic_read(&ctrl->req_seq_no);
118 if (ctrl->first_req == NULL) {
119 ctrl->first_req = req;
120 spin_unlock(&ctrl->info_list_lock);
121 return;
122 }
123 p = ctrl->first_req;
124 while (p->next != NULL)
125 p = p->next;
126 p->next = req;
127 spin_unlock(&ctrl->info_list_lock);
128}
129
130static void cfctrl_insert_req2(struct cfctrl *ctrl, enum cfctrl_cmd cmd,
131 u8 linkid, struct cflayer *user_layer)
132{
133 struct cfctrl_request_info *req = kmalloc(sizeof(*req), GFP_KERNEL);
134 if (!req) {
135 pr_warning("CAIF: %s(): Out of memory\n", __func__);
136 return;
137 }
138 req->client_layer = user_layer;
139 req->cmd = cmd;
140 req->channel_id = linkid;
141 cfctrl_insert_req(ctrl, req);
142}
143
144/* Compare and remove request */
145struct cfctrl_request_info *cfctrl_remove_req(struct cfctrl *ctrl,
146 struct cfctrl_request_info *req)
147{
148 struct cfctrl_request_info *p;
149 struct cfctrl_request_info *ret;
150
151 spin_lock(&ctrl->info_list_lock);
152 if (ctrl->first_req == NULL) {
153 spin_unlock(&ctrl->info_list_lock);
154 return NULL;
155 }
156
157 if (cfctrl_req_eq(req, ctrl->first_req)) {
158 ret = ctrl->first_req;
159 caif_assert(ctrl->first_req);
160 atomic_set(&ctrl->rsp_seq_no,
161 ctrl->first_req->sequence_no);
162 ctrl->first_req = ctrl->first_req->next;
163 spin_unlock(&ctrl->info_list_lock);
164 return ret;
165 }
166
167 p = ctrl->first_req;
168
169 while (p->next != NULL) {
170 if (cfctrl_req_eq(req, p->next)) {
171 pr_warning("CAIF: %s(): Requests are not "
172 "received in order\n",
173 __func__);
174 ret = p->next;
175 atomic_set(&ctrl->rsp_seq_no,
176 p->next->sequence_no);
177 p->next = p->next->next;
178 spin_unlock(&ctrl->info_list_lock);
179 return ret;
180 }
181 p = p->next;
182 }
183 spin_unlock(&ctrl->info_list_lock);
184
185 pr_warning("CAIF: %s(): Request does not match\n",
186 __func__);
187 return NULL;
188}
189
190struct cfctrl_rsp *cfctrl_get_respfuncs(struct cflayer *layer)
191{
192 struct cfctrl *this = container_obj(layer);
193 return &this->res;
194}
195
196void cfctrl_set_dnlayer(struct cflayer *this, struct cflayer *dn)
197{
198 this->dn = dn;
199}
200
201void cfctrl_set_uplayer(struct cflayer *this, struct cflayer *up)
202{
203 this->up = up;
204}
205
206static void init_info(struct caif_payload_info *info, struct cfctrl *cfctrl)
207{
208 info->hdr_len = 0;
209 info->channel_id = cfctrl->serv.layer.id;
210 info->dev_info = &cfctrl->serv.dev_info;
211}
212
213void cfctrl_enum_req(struct cflayer *layer, u8 physlinkid)
214{
215 struct cfctrl *cfctrl = container_obj(layer);
216 int ret;
217 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
218 if (!pkt) {
219 pr_warning("CAIF: %s(): Out of memory\n", __func__);
220 return;
221 }
222 caif_assert(offsetof(struct cfctrl, serv.layer) == 0);
223 init_info(cfpkt_info(pkt), cfctrl);
224 cfpkt_info(pkt)->dev_info->id = physlinkid;
225 cfctrl->serv.dev_info.id = physlinkid;
226 cfpkt_addbdy(pkt, CFCTRL_CMD_ENUM);
227 cfpkt_addbdy(pkt, physlinkid);
228 ret =
229 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
230 if (ret < 0) {
231 pr_err("CAIF: %s(): Could not transmit enum message\n",
232 __func__);
233 cfpkt_destroy(pkt);
234 }
235}
236
237void cfctrl_linkup_request(struct cflayer *layer,
238 struct cfctrl_link_param *param,
239 struct cflayer *user_layer)
240{
241 struct cfctrl *cfctrl = container_obj(layer);
242 u32 tmp32;
243 u16 tmp16;
244 u8 tmp8;
245 struct cfctrl_request_info *req;
246 int ret;
247 char utility_name[16];
248 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
249 if (!pkt) {
250 pr_warning("CAIF: %s(): Out of memory\n", __func__);
251 return;
252 }
253 cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_SETUP);
254 cfpkt_addbdy(pkt, (param->chtype << 4) + param->linktype);
255 cfpkt_addbdy(pkt, (param->priority << 3) + param->phyid);
256 cfpkt_addbdy(pkt, param->endpoint & 0x03);
257
258 switch (param->linktype) {
259 case CFCTRL_SRV_VEI:
260 break;
261 case CFCTRL_SRV_VIDEO:
262 cfpkt_addbdy(pkt, (u8) param->u.video.connid);
263 break;
264 case CFCTRL_SRV_DBG:
265 break;
266 case CFCTRL_SRV_DATAGRAM:
267 tmp32 = cpu_to_le32(param->u.datagram.connid);
268 cfpkt_add_body(pkt, &tmp32, 4);
269 break;
270 case CFCTRL_SRV_RFM:
271 /* Construct a frame, convert DatagramConnectionID to network
272 * format long and copy it out...
273 */
274 tmp32 = cpu_to_le32(param->u.rfm.connid);
275 cfpkt_add_body(pkt, &tmp32, 4);
276 /* Add volume name, including zero termination... */
277 cfpkt_add_body(pkt, param->u.rfm.volume,
278 strlen(param->u.rfm.volume) + 1);
279 break;
280 case CFCTRL_SRV_UTIL:
281 tmp16 = cpu_to_le16(param->u.utility.fifosize_kb);
282 cfpkt_add_body(pkt, &tmp16, 2);
283 tmp16 = cpu_to_le16(param->u.utility.fifosize_bufs);
284 cfpkt_add_body(pkt, &tmp16, 2);
285 memset(utility_name, 0, sizeof(utility_name));
286 strncpy(utility_name, param->u.utility.name,
287 UTILITY_NAME_LENGTH - 1);
288 cfpkt_add_body(pkt, utility_name, UTILITY_NAME_LENGTH);
289 tmp8 = param->u.utility.paramlen;
290 cfpkt_add_body(pkt, &tmp8, 1);
291 cfpkt_add_body(pkt, param->u.utility.params,
292 param->u.utility.paramlen);
293 break;
294 default:
295 pr_warning("CAIF: %s():Request setup of bad link type = %d\n",
296 __func__, param->linktype);
297 }
298 req = kmalloc(sizeof(*req), GFP_KERNEL);
299 if (!req) {
300 pr_warning("CAIF: %s(): Out of memory\n", __func__);
301 return;
302 }
303 memset(req, 0, sizeof(*req));
304 req->client_layer = user_layer;
305 req->cmd = CFCTRL_CMD_LINK_SETUP;
306 req->param = *param;
307 cfctrl_insert_req(cfctrl, req);
308 init_info(cfpkt_info(pkt), cfctrl);
309 cfpkt_info(pkt)->dev_info->id = param->phyid;
310 ret =
311 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
312 if (ret < 0) {
313 pr_err("CAIF: %s(): Could not transmit linksetup request\n",
314 __func__);
315 cfpkt_destroy(pkt);
316 }
317}
318
319int cfctrl_linkdown_req(struct cflayer *layer, u8 channelid,
320 struct cflayer *client)
321{
322 int ret;
323 struct cfctrl *cfctrl = container_obj(layer);
324 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
325 if (!pkt) {
326 pr_warning("CAIF: %s(): Out of memory\n", __func__);
327 return -ENOMEM;
328 }
329 cfctrl_insert_req2(cfctrl, CFCTRL_CMD_LINK_DESTROY, channelid, client);
330 cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_DESTROY);
331 cfpkt_addbdy(pkt, channelid);
332 init_info(cfpkt_info(pkt), cfctrl);
333 ret =
334 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
335 if (ret < 0) {
336 pr_err("CAIF: %s(): Could not transmit link-down request\n",
337 __func__);
338 cfpkt_destroy(pkt);
339 }
340 return ret;
341}
342
343void cfctrl_sleep_req(struct cflayer *layer)
344{
345 int ret;
346 struct cfctrl *cfctrl = container_obj(layer);
347 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
348 if (!pkt) {
349 pr_warning("CAIF: %s(): Out of memory\n", __func__);
350 return;
351 }
352 cfpkt_addbdy(pkt, CFCTRL_CMD_SLEEP);
353 init_info(cfpkt_info(pkt), cfctrl);
354 ret =
355 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
356 if (ret < 0)
357 cfpkt_destroy(pkt);
358}
359
360void cfctrl_wake_req(struct cflayer *layer)
361{
362 int ret;
363 struct cfctrl *cfctrl = container_obj(layer);
364 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
365 if (!pkt) {
366 pr_warning("CAIF: %s(): Out of memory\n", __func__);
367 return;
368 }
369 cfpkt_addbdy(pkt, CFCTRL_CMD_WAKE);
370 init_info(cfpkt_info(pkt), cfctrl);
371 ret =
372 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
373 if (ret < 0)
374 cfpkt_destroy(pkt);
375}
376
377void cfctrl_getstartreason_req(struct cflayer *layer)
378{
379 int ret;
380 struct cfctrl *cfctrl = container_obj(layer);
381 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
382 if (!pkt) {
383 pr_warning("CAIF: %s(): Out of memory\n", __func__);
384 return;
385 }
386 cfpkt_addbdy(pkt, CFCTRL_CMD_START_REASON);
387 init_info(cfpkt_info(pkt), cfctrl);
388 ret =
389 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
390 if (ret < 0)
391 cfpkt_destroy(pkt);
392}
393
394
395static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt)
396{
397 u8 cmdrsp;
398 u8 cmd;
399 int ret = -1;
400 u16 tmp16;
401 u8 len;
402 u8 param[255];
403 u8 linkid;
404 struct cfctrl *cfctrl = container_obj(layer);
405 struct cfctrl_request_info rsp, *req;
406
407
408 cfpkt_extr_head(pkt, &cmdrsp, 1);
409 cmd = cmdrsp & CFCTRL_CMD_MASK;
410 if (cmd != CFCTRL_CMD_LINK_ERR
411 && CFCTRL_RSP_BIT != (CFCTRL_RSP_BIT & cmdrsp)) {
412 if (handle_loop(cfctrl, cmd, pkt) == CAIF_FAILURE) {
413 pr_info("CAIF: %s() CAIF Protocol error:"
414 "Response bit not set\n", __func__);
415 goto error;
416 }
417 }
418
419 switch (cmd) {
420 case CFCTRL_CMD_LINK_SETUP:
421 {
422 enum cfctrl_srv serv;
423 enum cfctrl_srv servtype;
424 u8 endpoint;
425 u8 physlinkid;
426 u8 prio;
427 u8 tmp;
428 u32 tmp32;
429 u8 *cp;
430 int i;
431 struct cfctrl_link_param linkparam;
432 memset(&linkparam, 0, sizeof(linkparam));
433
434 cfpkt_extr_head(pkt, &tmp, 1);
435
436 serv = tmp & CFCTRL_SRV_MASK;
437 linkparam.linktype = serv;
438
439 servtype = tmp >> 4;
440 linkparam.chtype = servtype;
441
442 cfpkt_extr_head(pkt, &tmp, 1);
443 physlinkid = tmp & 0x07;
444 prio = tmp >> 3;
445
446 linkparam.priority = prio;
447 linkparam.phyid = physlinkid;
448 cfpkt_extr_head(pkt, &endpoint, 1);
449 linkparam.endpoint = endpoint & 0x03;
450
451 switch (serv) {
452 case CFCTRL_SRV_VEI:
453 case CFCTRL_SRV_DBG:
454 /* Link ID */
455 cfpkt_extr_head(pkt, &linkid, 1);
456 break;
457 case CFCTRL_SRV_VIDEO:
458 cfpkt_extr_head(pkt, &tmp, 1);
459 linkparam.u.video.connid = tmp;
460 /* Link ID */
461 cfpkt_extr_head(pkt, &linkid, 1);
462 break;
463
464 case CFCTRL_SRV_DATAGRAM:
465 cfpkt_extr_head(pkt, &tmp32, 4);
466 linkparam.u.datagram.connid =
467 le32_to_cpu(tmp32);
468 /* Link ID */
469 cfpkt_extr_head(pkt, &linkid, 1);
470 break;
471 case CFCTRL_SRV_RFM:
472 /* Construct a frame, convert
473 * DatagramConnectionID
474 * to network format long and copy it out...
475 */
476 cfpkt_extr_head(pkt, &tmp32, 4);
477 linkparam.u.rfm.connid =
478 le32_to_cpu(tmp32);
479 cp = (u8 *) linkparam.u.rfm.volume;
480 for (cfpkt_extr_head(pkt, &tmp, 1);
481 cfpkt_more(pkt) && tmp != '\0';
482 cfpkt_extr_head(pkt, &tmp, 1))
483 *cp++ = tmp;
484 *cp = '\0';
485
486 /* Link ID */
487 cfpkt_extr_head(pkt, &linkid, 1);
488
489 break;
490 case CFCTRL_SRV_UTIL:
491 /* Construct a frame, convert
492 * DatagramConnectionID
493 * to network format long and copy it out...
494 */
495 /* Fifosize KB */
496 cfpkt_extr_head(pkt, &tmp16, 2);
497 linkparam.u.utility.fifosize_kb =
498 le16_to_cpu(tmp16);
499 /* Fifosize bufs */
500 cfpkt_extr_head(pkt, &tmp16, 2);
501 linkparam.u.utility.fifosize_bufs =
502 le16_to_cpu(tmp16);
503 /* name */
504 cp = (u8 *) linkparam.u.utility.name;
505 caif_assert(sizeof(linkparam.u.utility.name)
506 >= UTILITY_NAME_LENGTH);
507 for (i = 0;
508 i < UTILITY_NAME_LENGTH
509 && cfpkt_more(pkt); i++) {
510 cfpkt_extr_head(pkt, &tmp, 1);
511 *cp++ = tmp;
512 }
513 /* Length */
514 cfpkt_extr_head(pkt, &len, 1);
515 linkparam.u.utility.paramlen = len;
516 /* Param Data */
517 cp = linkparam.u.utility.params;
518 while (cfpkt_more(pkt) && len--) {
519 cfpkt_extr_head(pkt, &tmp, 1);
520 *cp++ = tmp;
521 }
522 /* Link ID */
523 cfpkt_extr_head(pkt, &linkid, 1);
524 /* Length */
525 cfpkt_extr_head(pkt, &len, 1);
526 /* Param Data */
527 cfpkt_extr_head(pkt, &param, len);
528 break;
529 default:
530 pr_warning("CAIF: %s(): Request setup "
531 "- invalid link type (%d)",
532 __func__, serv);
533 goto error;
534 }
535
536 rsp.cmd = cmd;
537 rsp.param = linkparam;
538 req = cfctrl_remove_req(cfctrl, &rsp);
539
540 if (CFCTRL_ERR_BIT == (CFCTRL_ERR_BIT & cmdrsp) ||
541 cfpkt_erroneous(pkt)) {
542 pr_err("CAIF: %s(): Invalid O/E bit or parse "
543 "error on CAIF control channel",
544 __func__);
545 cfctrl->res.reject_rsp(cfctrl->serv.layer.up,
546 0,
547 req ? req->client_layer
548 : NULL);
549 } else {
550 cfctrl->res.linksetup_rsp(cfctrl->serv.
551 layer.up, linkid,
552 serv, physlinkid,
553 req ? req->
554 client_layer : NULL);
555 }
556
557 if (req != NULL)
558 kfree(req);
559 }
560 break;
561 case CFCTRL_CMD_LINK_DESTROY:
562 cfpkt_extr_head(pkt, &linkid, 1);
563 rsp.cmd = cmd;
564 rsp.channel_id = linkid;
565 req = cfctrl_remove_req(cfctrl, &rsp);
566 cfctrl->res.linkdestroy_rsp(cfctrl->serv.layer.up, linkid,
567 req ? req->client_layer : NULL);
568 if (req != NULL)
569 kfree(req);
570 break;
571 case CFCTRL_CMD_LINK_ERR:
572 pr_err("CAIF: %s(): Frame Error Indication received\n",
573 __func__);
574 cfctrl->res.linkerror_ind();
575 break;
576 case CFCTRL_CMD_ENUM:
577 cfctrl->res.enum_rsp();
578 break;
579 case CFCTRL_CMD_SLEEP:
580 cfctrl->res.sleep_rsp();
581 break;
582 case CFCTRL_CMD_WAKE:
583 cfctrl->res.wake_rsp();
584 break;
585 case CFCTRL_CMD_LINK_RECONF:
586 cfctrl->res.restart_rsp();
587 break;
588 case CFCTRL_CMD_RADIO_SET:
589 cfctrl->res.radioset_rsp();
590 break;
591 default:
592 pr_err("CAIF: %s(): Unrecognized Control Frame\n", __func__);
593 goto error;
594 break;
595 }
596 ret = 0;
597error:
598 cfpkt_destroy(pkt);
599 return ret;
600}
601
602static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
603 int phyid)
604{
605 struct cfctrl *this = container_obj(layr);
606 switch (ctrl) {
607 case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND:
608 case CAIF_CTRLCMD_FLOW_OFF_IND:
609 spin_lock(&this->info_list_lock);
610 if (this->first_req != NULL) {
611 pr_warning("CAIF: %s(): Received flow off in "
612 "control layer", __func__);
613 }
614 spin_unlock(&this->info_list_lock);
615 break;
616 default:
617 break;
618 }
619}
620
621#ifndef CAIF_NO_LOOP
622static int handle_loop(struct cfctrl *ctrl, int cmd, struct cfpkt *pkt)
623{
624 static int last_linkid;
625 u8 linkid, linktype, tmp;
626 switch (cmd) {
627 case CFCTRL_CMD_LINK_SETUP:
628 spin_lock(&ctrl->loop_linkid_lock);
629 for (linkid = last_linkid + 1; linkid < 255; linkid++)
630 if (!ctrl->loop_linkused[linkid])
631 goto found;
632 for (linkid = last_linkid - 1; linkid > 0; linkid--)
633 if (!ctrl->loop_linkused[linkid])
634 goto found;
635 spin_unlock(&ctrl->loop_linkid_lock);
636 return -EINVAL;
637found:
638 if (!ctrl->loop_linkused[linkid])
639 ctrl->loop_linkused[linkid] = 1;
640
641 last_linkid = linkid;
642
643 cfpkt_add_trail(pkt, &linkid, 1);
644 spin_unlock(&ctrl->loop_linkid_lock);
645 cfpkt_peek_head(pkt, &linktype, 1);
646 if (linktype == CFCTRL_SRV_UTIL) {
647 tmp = 0x01;
648 cfpkt_add_trail(pkt, &tmp, 1);
649 cfpkt_add_trail(pkt, &tmp, 1);
650 }
651 break;
652
653 case CFCTRL_CMD_LINK_DESTROY:
654 spin_lock(&ctrl->loop_linkid_lock);
655 cfpkt_peek_head(pkt, &linkid, 1);
656 ctrl->loop_linkused[linkid] = 0;
657 spin_unlock(&ctrl->loop_linkid_lock);
658 break;
659 default:
660 break;
661 }
662 return CAIF_SUCCESS;
663}
664#endif
diff --git a/net/caif/cfdbgl.c b/net/caif/cfdbgl.c
new file mode 100644
index 000000000000..ab6b6dc34cf8
--- /dev/null
+++ b/net/caif/cfdbgl.c
@@ -0,0 +1,40 @@
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#include <linux/stddef.h>
8#include <linux/slab.h>
9#include <net/caif/caif_layer.h>
10#include <net/caif/cfsrvl.h>
11#include <net/caif/cfpkt.h>
12
13static int cfdbgl_receive(struct cflayer *layr, struct cfpkt *pkt);
14static int cfdbgl_transmit(struct cflayer *layr, struct cfpkt *pkt);
15
16struct cflayer *cfdbgl_create(u8 channel_id, struct dev_info *dev_info)
17{
18 struct cfsrvl *dbg = kmalloc(sizeof(struct cfsrvl), GFP_ATOMIC);
19 if (!dbg) {
20 pr_warning("CAIF: %s(): Out of memory\n", __func__);
21 return NULL;
22 }
23 caif_assert(offsetof(struct cfsrvl, layer) == 0);
24 memset(dbg, 0, sizeof(struct cfsrvl));
25 cfsrvl_init(dbg, channel_id, dev_info);
26 dbg->layer.receive = cfdbgl_receive;
27 dbg->layer.transmit = cfdbgl_transmit;
28 snprintf(dbg->layer.name, CAIF_LAYER_NAME_SZ - 1, "dbg%d", channel_id);
29 return &dbg->layer;
30}
31
32static int cfdbgl_receive(struct cflayer *layr, struct cfpkt *pkt)
33{
34 return layr->up->receive(layr->up, pkt);
35}
36
37static int cfdbgl_transmit(struct cflayer *layr, struct cfpkt *pkt)
38{
39 return layr->dn->transmit(layr->dn, pkt);
40}
diff --git a/net/caif/cfdgml.c b/net/caif/cfdgml.c
new file mode 100644
index 000000000000..53194840ecb6
--- /dev/null
+++ b/net/caif/cfdgml.c
@@ -0,0 +1,108 @@
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#include <linux/stddef.h>
8#include <linux/spinlock.h>
9#include <linux/slab.h>
10#include <net/caif/caif_layer.h>
11#include <net/caif/cfsrvl.h>
12#include <net/caif/cfpkt.h>
13
14#define container_obj(layr) ((struct cfsrvl *) layr)
15
16#define DGM_CMD_BIT 0x80
17#define DGM_FLOW_OFF 0x81
18#define DGM_FLOW_ON 0x80
19#define DGM_CTRL_PKT_SIZE 1
20
21static int cfdgml_receive(struct cflayer *layr, struct cfpkt *pkt);
22static int cfdgml_transmit(struct cflayer *layr, struct cfpkt *pkt);
23
24struct cflayer *cfdgml_create(u8 channel_id, struct dev_info *dev_info)
25{
26 struct cfsrvl *dgm = kmalloc(sizeof(struct cfsrvl), GFP_ATOMIC);
27 if (!dgm) {
28 pr_warning("CAIF: %s(): Out of memory\n", __func__);
29 return NULL;
30 }
31 caif_assert(offsetof(struct cfsrvl, layer) == 0);
32 memset(dgm, 0, sizeof(struct cfsrvl));
33 cfsrvl_init(dgm, channel_id, dev_info);
34 dgm->layer.receive = cfdgml_receive;
35 dgm->layer.transmit = cfdgml_transmit;
36 snprintf(dgm->layer.name, CAIF_LAYER_NAME_SZ - 1, "dgm%d", channel_id);
37 dgm->layer.name[CAIF_LAYER_NAME_SZ - 1] = '\0';
38 return &dgm->layer;
39}
40
41static int cfdgml_receive(struct cflayer *layr, struct cfpkt *pkt)
42{
43 u8 cmd = -1;
44 u8 dgmhdr[3];
45 int ret;
46 caif_assert(layr->up != NULL);
47 caif_assert(layr->receive != NULL);
48 caif_assert(layr->ctrlcmd != NULL);
49
50 if (cfpkt_extr_head(pkt, &cmd, 1) < 0) {
51 pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
52 cfpkt_destroy(pkt);
53 return -EPROTO;
54 }
55
56 if ((cmd & DGM_CMD_BIT) == 0) {
57 if (cfpkt_extr_head(pkt, &dgmhdr, 3) < 0) {
58 pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
59 cfpkt_destroy(pkt);
60 return -EPROTO;
61 }
62 ret = layr->up->receive(layr->up, pkt);
63 return ret;
64 }
65
66 switch (cmd) {
67 case DGM_FLOW_OFF: /* FLOW OFF */
68 layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0);
69 cfpkt_destroy(pkt);
70 return 0;
71 case DGM_FLOW_ON: /* FLOW ON */
72 layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0);
73 cfpkt_destroy(pkt);
74 return 0;
75 default:
76 cfpkt_destroy(pkt);
77 pr_info("CAIF: %s(): Unknown datagram control %d (0x%x)\n",
78 __func__, cmd, cmd);
79 return -EPROTO;
80 }
81}
82
83static int cfdgml_transmit(struct cflayer *layr, struct cfpkt *pkt)
84{
85 u32 zero = 0;
86 struct caif_payload_info *info;
87 struct cfsrvl *service = container_obj(layr);
88 int ret;
89 if (!cfsrvl_ready(service, &ret))
90 return ret;
91
92 cfpkt_add_head(pkt, &zero, 4);
93
94 /* Add info for MUX-layer to route the packet out. */
95 info = cfpkt_info(pkt);
96 info->channel_id = service->layer.id;
97 /* To optimize alignment, we add up the size of CAIF header
98 * before payload.
99 */
100 info->hdr_len = 4;
101 info->dev_info = &service->dev_info;
102 ret = layr->dn->transmit(layr->dn, pkt);
103 if (ret < 0) {
104 u32 tmp32;
105 cfpkt_extr_head(pkt, &tmp32, 4);
106 }
107 return ret;
108}
diff --git a/net/caif/cffrml.c b/net/caif/cffrml.c
new file mode 100644
index 000000000000..e86a4ca3b217
--- /dev/null
+++ b/net/caif/cffrml.c
@@ -0,0 +1,151 @@
1/*
2 * CAIF Framing Layer.
3 *
4 * Copyright (C) ST-Ericsson AB 2010
5 * Author: Sjur Brendeland/sjur.brandeland@stericsson.com
6 * License terms: GNU General Public License (GPL) version 2
7 */
8
9#include <linux/stddef.h>
10#include <linux/spinlock.h>
11#include <linux/slab.h>
12#include <linux/crc-ccitt.h>
13#include <net/caif/caif_layer.h>
14#include <net/caif/cfpkt.h>
15#include <net/caif/cffrml.h>
16
17#define container_obj(layr) container_of(layr, struct cffrml, layer)
18
19struct cffrml {
20 struct cflayer layer;
21 bool dofcs; /* !< FCS active */
22};
23
24static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt);
25static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt);
26static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
27 int phyid);
28
29static u32 cffrml_rcv_error;
30static u32 cffrml_rcv_checsum_error;
31struct cflayer *cffrml_create(u16 phyid, bool use_fcs)
32{
33 struct cffrml *this = kmalloc(sizeof(struct cffrml), GFP_ATOMIC);
34 if (!this) {
35 pr_warning("CAIF: %s(): Out of memory\n", __func__);
36 return NULL;
37 }
38 caif_assert(offsetof(struct cffrml, layer) == 0);
39
40 memset(this, 0, sizeof(struct cflayer));
41 this->layer.receive = cffrml_receive;
42 this->layer.transmit = cffrml_transmit;
43 this->layer.ctrlcmd = cffrml_ctrlcmd;
44 snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "frm%d", phyid);
45 this->dofcs = use_fcs;
46 this->layer.id = phyid;
47 return (struct cflayer *) this;
48}
49
50void cffrml_set_uplayer(struct cflayer *this, struct cflayer *up)
51{
52 this->up = up;
53}
54
55void cffrml_set_dnlayer(struct cflayer *this, struct cflayer *dn)
56{
57 this->dn = dn;
58}
59
60static u16 cffrml_checksum(u16 chks, void *buf, u16 len)
61{
62 /* FIXME: FCS should be moved to glue in order to use OS-Specific
63 * solutions
64 */
65 return crc_ccitt(chks, buf, len);
66}
67
68static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt)
69{
70 u16 tmp;
71 u16 len;
72 u16 hdrchks;
73 u16 pktchks;
74 struct cffrml *this;
75 this = container_obj(layr);
76
77 cfpkt_extr_head(pkt, &tmp, 2);
78 len = le16_to_cpu(tmp);
79
80 /* Subtract for FCS on length if FCS is not used. */
81 if (!this->dofcs)
82 len -= 2;
83
84 if (cfpkt_setlen(pkt, len) < 0) {
85 ++cffrml_rcv_error;
86 pr_err("CAIF: %s():Framing length error (%d)\n", __func__, len);
87 cfpkt_destroy(pkt);
88 return -EPROTO;
89 }
90 /*
91 * Don't do extract if FCS is false, rather do setlen - then we don't
92 * get a cache-miss.
93 */
94 if (this->dofcs) {
95 cfpkt_extr_trail(pkt, &tmp, 2);
96 hdrchks = le16_to_cpu(tmp);
97 pktchks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff);
98 if (pktchks != hdrchks) {
99 cfpkt_add_trail(pkt, &tmp, 2);
100 ++cffrml_rcv_error;
101 ++cffrml_rcv_checsum_error;
102 pr_info("CAIF: %s(): Frame checksum error "
103 "(0x%x != 0x%x)\n", __func__, hdrchks, pktchks);
104 return -EILSEQ;
105 }
106 }
107 if (cfpkt_erroneous(pkt)) {
108 ++cffrml_rcv_error;
109 pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
110 cfpkt_destroy(pkt);
111 return -EPROTO;
112 }
113 return layr->up->receive(layr->up, pkt);
114}
115
116static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt)
117{
118 int tmp;
119 u16 chks;
120 u16 len;
121 int ret;
122 struct cffrml *this = container_obj(layr);
123 if (this->dofcs) {
124 chks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff);
125 tmp = cpu_to_le16(chks);
126 cfpkt_add_trail(pkt, &tmp, 2);
127 } else {
128 cfpkt_pad_trail(pkt, 2);
129 }
130 len = cfpkt_getlen(pkt);
131 tmp = cpu_to_le16(len);
132 cfpkt_add_head(pkt, &tmp, 2);
133 cfpkt_info(pkt)->hdr_len += 2;
134 if (cfpkt_erroneous(pkt)) {
135 pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
136 return -EPROTO;
137 }
138 ret = layr->dn->transmit(layr->dn, pkt);
139 if (ret < 0) {
140 /* Remove header on faulty packet. */
141 cfpkt_extr_head(pkt, &tmp, 2);
142 }
143 return ret;
144}
145
146static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
147 int phyid)
148{
149 if (layr->up->ctrlcmd)
150 layr->up->ctrlcmd(layr->up, ctrl, layr->id);
151}
diff --git a/net/caif/cfmuxl.c b/net/caif/cfmuxl.c
new file mode 100644
index 000000000000..6fb9f9e96cf8
--- /dev/null
+++ b/net/caif/cfmuxl.c
@@ -0,0 +1,246 @@
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/stddef.h>
7#include <linux/spinlock.h>
8#include <linux/slab.h>
9#include <net/caif/cfpkt.h>
10#include <net/caif/cfmuxl.h>
11#include <net/caif/cfsrvl.h>
12#include <net/caif/cffrml.h>
13
14#define container_obj(layr) container_of(layr, struct cfmuxl, layer)
15
16#define CAIF_CTRL_CHANNEL 0
17#define UP_CACHE_SIZE 8
18#define DN_CACHE_SIZE 8
19
20struct cfmuxl {
21 struct cflayer layer;
22 struct list_head srvl_list;
23 struct list_head frml_list;
24 struct cflayer *up_cache[UP_CACHE_SIZE];
25 struct cflayer *dn_cache[DN_CACHE_SIZE];
26 /*
27 * Set when inserting or removing downwards layers.
28 */
29 spinlock_t transmit_lock;
30
31 /*
32 * Set when inserting or removing upwards layers.
33 */
34 spinlock_t receive_lock;
35
36};
37
38static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt);
39static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt);
40static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
41 int phyid);
42static struct cflayer *get_up(struct cfmuxl *muxl, u16 id);
43
44struct cflayer *cfmuxl_create(void)
45{
46 struct cfmuxl *this = kmalloc(sizeof(struct cfmuxl), GFP_ATOMIC);
47 if (!this)
48 return NULL;
49 memset(this, 0, sizeof(*this));
50 this->layer.receive = cfmuxl_receive;
51 this->layer.transmit = cfmuxl_transmit;
52 this->layer.ctrlcmd = cfmuxl_ctrlcmd;
53 INIT_LIST_HEAD(&this->srvl_list);
54 INIT_LIST_HEAD(&this->frml_list);
55 spin_lock_init(&this->transmit_lock);
56 spin_lock_init(&this->receive_lock);
57 snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "mux");
58 return &this->layer;
59}
60
61int cfmuxl_set_uplayer(struct cflayer *layr, struct cflayer *up, u8 linkid)
62{
63 struct cfmuxl *muxl = container_obj(layr);
64 spin_lock(&muxl->receive_lock);
65 list_add(&up->node, &muxl->srvl_list);
66 spin_unlock(&muxl->receive_lock);
67 return 0;
68}
69
70bool cfmuxl_is_phy_inuse(struct cflayer *layr, u8 phyid)
71{
72 struct list_head *node;
73 struct cflayer *layer;
74 struct cfmuxl *muxl = container_obj(layr);
75 bool match = false;
76 spin_lock(&muxl->receive_lock);
77
78 list_for_each(node, &muxl->srvl_list) {
79 layer = list_entry(node, struct cflayer, node);
80 if (cfsrvl_phyid_match(layer, phyid)) {
81 match = true;
82 break;
83 }
84
85 }
86 spin_unlock(&muxl->receive_lock);
87 return match;
88}
89
90u8 cfmuxl_get_phyid(struct cflayer *layr, u8 channel_id)
91{
92 struct cflayer *up;
93 int phyid;
94 struct cfmuxl *muxl = container_obj(layr);
95 spin_lock(&muxl->receive_lock);
96 up = get_up(muxl, channel_id);
97 if (up != NULL)
98 phyid = cfsrvl_getphyid(up);
99 else
100 phyid = 0;
101 spin_unlock(&muxl->receive_lock);
102 return phyid;
103}
104
105int cfmuxl_set_dnlayer(struct cflayer *layr, struct cflayer *dn, u8 phyid)
106{
107 struct cfmuxl *muxl = (struct cfmuxl *) layr;
108 spin_lock(&muxl->transmit_lock);
109 list_add(&dn->node, &muxl->frml_list);
110 spin_unlock(&muxl->transmit_lock);
111 return 0;
112}
113
114static struct cflayer *get_from_id(struct list_head *list, u16 id)
115{
116 struct list_head *node;
117 struct cflayer *layer;
118 list_for_each(node, list) {
119 layer = list_entry(node, struct cflayer, node);
120 if (layer->id == id)
121 return layer;
122 }
123 return NULL;
124}
125
126struct cflayer *cfmuxl_remove_dnlayer(struct cflayer *layr, u8 phyid)
127{
128 struct cfmuxl *muxl = container_obj(layr);
129 struct cflayer *dn;
130 spin_lock(&muxl->transmit_lock);
131 memset(muxl->dn_cache, 0, sizeof(muxl->dn_cache));
132 dn = get_from_id(&muxl->frml_list, phyid);
133 if (dn == NULL) {
134 spin_unlock(&muxl->transmit_lock);
135 return NULL;
136 }
137 list_del(&dn->node);
138 caif_assert(dn != NULL);
139 spin_unlock(&muxl->transmit_lock);
140 return dn;
141}
142
143/* Invariant: lock is taken */
144static struct cflayer *get_up(struct cfmuxl *muxl, u16 id)
145{
146 struct cflayer *up;
147 int idx = id % UP_CACHE_SIZE;
148 up = muxl->up_cache[idx];
149 if (up == NULL || up->id != id) {
150 up = get_from_id(&muxl->srvl_list, id);
151 muxl->up_cache[idx] = up;
152 }
153 return up;
154}
155
156/* Invariant: lock is taken */
157static struct cflayer *get_dn(struct cfmuxl *muxl, struct dev_info *dev_info)
158{
159 struct cflayer *dn;
160 int idx = dev_info->id % DN_CACHE_SIZE;
161 dn = muxl->dn_cache[idx];
162 if (dn == NULL || dn->id != dev_info->id) {
163 dn = get_from_id(&muxl->frml_list, dev_info->id);
164 muxl->dn_cache[idx] = dn;
165 }
166 return dn;
167}
168
169struct cflayer *cfmuxl_remove_uplayer(struct cflayer *layr, u8 id)
170{
171 struct cflayer *up;
172 struct cfmuxl *muxl = container_obj(layr);
173 spin_lock(&muxl->receive_lock);
174 up = get_up(muxl, id);
175 memset(muxl->up_cache, 0, sizeof(muxl->up_cache));
176 list_del(&up->node);
177 spin_unlock(&muxl->receive_lock);
178 return up;
179}
180
181static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt)
182{
183 int ret;
184 struct cfmuxl *muxl = container_obj(layr);
185 u8 id;
186 struct cflayer *up;
187 if (cfpkt_extr_head(pkt, &id, 1) < 0) {
188 pr_err("CAIF: %s(): erroneous Caif Packet\n", __func__);
189 cfpkt_destroy(pkt);
190 return -EPROTO;
191 }
192
193 spin_lock(&muxl->receive_lock);
194 up = get_up(muxl, id);
195 spin_unlock(&muxl->receive_lock);
196 if (up == NULL) {
197 pr_info("CAIF: %s():Received data on unknown link ID = %d "
198 "(0x%x) up == NULL", __func__, id, id);
199 cfpkt_destroy(pkt);
200 /*
201 * Don't return ERROR, since modem misbehaves and sends out
202 * flow on before linksetup response.
203 */
204 return /* CFGLU_EPROT; */ 0;
205 }
206
207 ret = up->receive(up, pkt);
208 return ret;
209}
210
211static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt)
212{
213 int ret;
214 struct cfmuxl *muxl = container_obj(layr);
215 u8 linkid;
216 struct cflayer *dn;
217 struct caif_payload_info *info = cfpkt_info(pkt);
218 dn = get_dn(muxl, cfpkt_info(pkt)->dev_info);
219 if (dn == NULL) {
220 pr_warning("CAIF: %s(): Send data on unknown phy "
221 "ID = %d (0x%x)\n",
222 __func__, info->dev_info->id, info->dev_info->id);
223 return -ENOTCONN;
224 }
225 info->hdr_len += 1;
226 linkid = info->channel_id;
227 cfpkt_add_head(pkt, &linkid, 1);
228 ret = dn->transmit(dn, pkt);
229 /* Remove MUX protocol header upon error. */
230 if (ret < 0)
231 cfpkt_extr_head(pkt, &linkid, 1);
232 return ret;
233}
234
235static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
236 int phyid)
237{
238 struct cfmuxl *muxl = container_obj(layr);
239 struct list_head *node;
240 struct cflayer *layer;
241 list_for_each(node, &muxl->srvl_list) {
242 layer = list_entry(node, struct cflayer, node);
243 if (cfsrvl_phyid_match(layer, phyid))
244 layer->ctrlcmd(layer, ctrl, phyid);
245 }
246}
diff --git a/net/caif/cfrfml.c b/net/caif/cfrfml.c
new file mode 100644
index 000000000000..cd2830fec935
--- /dev/null
+++ b/net/caif/cfrfml.c
@@ -0,0 +1,108 @@
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#include <linux/stddef.h>
8#include <linux/spinlock.h>
9#include <linux/slab.h>
10#include <net/caif/caif_layer.h>
11#include <net/caif/cfsrvl.h>
12#include <net/caif/cfpkt.h>
13
14#define container_obj(layr) container_of(layr, struct cfsrvl, layer)
15
16#define RFM_SEGMENTATION_BIT 0x01
17#define RFM_PAYLOAD 0x00
18#define RFM_CMD_BIT 0x80
19#define RFM_FLOW_OFF 0x81
20#define RFM_FLOW_ON 0x80
21#define RFM_SET_PIN 0x82
22#define RFM_CTRL_PKT_SIZE 1
23
24static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt);
25static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt);
26static int cfservl_modemcmd(struct cflayer *layr, enum caif_modemcmd ctrl);
27
28struct cflayer *cfrfml_create(u8 channel_id, struct dev_info *dev_info)
29{
30 struct cfsrvl *rfm = kmalloc(sizeof(struct cfsrvl), GFP_ATOMIC);
31 if (!rfm) {
32 pr_warning("CAIF: %s(): Out of memory\n", __func__);
33 return NULL;
34 }
35 caif_assert(offsetof(struct cfsrvl, layer) == 0);
36 memset(rfm, 0, sizeof(struct cfsrvl));
37 cfsrvl_init(rfm, channel_id, dev_info);
38 rfm->layer.modemcmd = cfservl_modemcmd;
39 rfm->layer.receive = cfrfml_receive;
40 rfm->layer.transmit = cfrfml_transmit;
41 snprintf(rfm->layer.name, CAIF_LAYER_NAME_SZ, "rfm%d", channel_id);
42 return &rfm->layer;
43}
44
45static int cfservl_modemcmd(struct cflayer *layr, enum caif_modemcmd ctrl)
46{
47 return -EPROTO;
48}
49
50static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt)
51{
52 u8 tmp;
53 bool segmented;
54 int ret;
55 caif_assert(layr->up != NULL);
56 caif_assert(layr->receive != NULL);
57
58 /*
59 * RFM is taking care of segmentation and stripping of
60 * segmentation bit.
61 */
62 if (cfpkt_extr_head(pkt, &tmp, 1) < 0) {
63 pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
64 cfpkt_destroy(pkt);
65 return -EPROTO;
66 }
67 segmented = tmp & RFM_SEGMENTATION_BIT;
68 caif_assert(!segmented);
69
70 ret = layr->up->receive(layr->up, pkt);
71 return ret;
72}
73
74static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt)
75{
76 u8 tmp = 0;
77 int ret;
78 struct cfsrvl *service = container_obj(layr);
79
80 caif_assert(layr->dn != NULL);
81 caif_assert(layr->dn->transmit != NULL);
82
83 if (!cfsrvl_ready(service, &ret))
84 return ret;
85
86 if (!cfpkt_getlen(pkt) > CAIF_MAX_PAYLOAD_SIZE) {
87 pr_err("CAIF: %s():Packet too large - size=%d\n",
88 __func__, cfpkt_getlen(pkt));
89 return -EOVERFLOW;
90 }
91 if (cfpkt_add_head(pkt, &tmp, 1) < 0) {
92 pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
93 return -EPROTO;
94 }
95
96 /* Add info for MUX-layer to route the packet out. */
97 cfpkt_info(pkt)->channel_id = service->layer.id;
98 /*
99 * To optimize alignment, we add up the size of CAIF header before
100 * payload.
101 */
102 cfpkt_info(pkt)->hdr_len = 1;
103 cfpkt_info(pkt)->dev_info = &service->dev_info;
104 ret = layr->dn->transmit(layr->dn, pkt);
105 if (ret < 0)
106 cfpkt_extr_head(pkt, &tmp, 1);
107 return ret;
108}
diff --git a/net/caif/cfserl.c b/net/caif/cfserl.c
new file mode 100644
index 000000000000..06029ea2da2f
--- /dev/null
+++ b/net/caif/cfserl.c
@@ -0,0 +1,192 @@
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#include <linux/stddef.h>
8#include <linux/spinlock.h>
9#include <linux/slab.h>
10#include <net/caif/caif_layer.h>
11#include <net/caif/cfpkt.h>
12#include <net/caif/cfserl.h>
13
14#define container_obj(layr) ((struct cfserl *) layr)
15
16#define CFSERL_STX 0x02
17#define CAIF_MINIUM_PACKET_SIZE 4
18struct cfserl {
19 struct cflayer layer;
20 struct cfpkt *incomplete_frm;
21 /* Protects parallel processing of incoming packets */
22 spinlock_t sync;
23 bool usestx;
24};
25#define STXLEN(layr) (layr->usestx ? 1 : 0)
26
27static int cfserl_receive(struct cflayer *layr, struct cfpkt *pkt);
28static int cfserl_transmit(struct cflayer *layr, struct cfpkt *pkt);
29static void cfserl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
30 int phyid);
31
32struct cflayer *cfserl_create(int type, int instance, bool use_stx)
33{
34 struct cfserl *this = kmalloc(sizeof(struct cfserl), GFP_ATOMIC);
35 if (!this) {
36 pr_warning("CAIF: %s(): Out of memory\n", __func__);
37 return NULL;
38 }
39 caif_assert(offsetof(struct cfserl, layer) == 0);
40 memset(this, 0, sizeof(struct cfserl));
41 this->layer.receive = cfserl_receive;
42 this->layer.transmit = cfserl_transmit;
43 this->layer.ctrlcmd = cfserl_ctrlcmd;
44 this->layer.type = type;
45 this->usestx = use_stx;
46 spin_lock_init(&this->sync);
47 snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "ser1");
48 return &this->layer;
49}
50
51static int cfserl_receive(struct cflayer *l, struct cfpkt *newpkt)
52{
53 struct cfserl *layr = container_obj(l);
54 u16 pkt_len;
55 struct cfpkt *pkt = NULL;
56 struct cfpkt *tail_pkt = NULL;
57 u8 tmp8;
58 u16 tmp;
59 u8 stx = CFSERL_STX;
60 int ret;
61 u16 expectlen = 0;
62 caif_assert(newpkt != NULL);
63 spin_lock(&layr->sync);
64
65 if (layr->incomplete_frm != NULL) {
66
67 layr->incomplete_frm =
68 cfpkt_append(layr->incomplete_frm, newpkt, expectlen);
69 pkt = layr->incomplete_frm;
70 } else {
71 pkt = newpkt;
72 }
73 layr->incomplete_frm = NULL;
74
75 do {
76 /* Search for STX at start of pkt if STX is used */
77 if (layr->usestx) {
78 cfpkt_extr_head(pkt, &tmp8, 1);
79 if (tmp8 != CFSERL_STX) {
80 while (cfpkt_more(pkt)
81 && tmp8 != CFSERL_STX) {
82 cfpkt_extr_head(pkt, &tmp8, 1);
83 }
84 if (!cfpkt_more(pkt)) {
85 cfpkt_destroy(pkt);
86 layr->incomplete_frm = NULL;
87 spin_unlock(&layr->sync);
88 return -EPROTO;
89 }
90 }
91 }
92
93 pkt_len = cfpkt_getlen(pkt);
94
95 /*
96 * pkt_len is the accumulated length of the packet data
97 * we have received so far.
98 * Exit if frame doesn't hold length.
99 */
100
101 if (pkt_len < 2) {
102 if (layr->usestx)
103 cfpkt_add_head(pkt, &stx, 1);
104 layr->incomplete_frm = pkt;
105 spin_unlock(&layr->sync);
106 return 0;
107 }
108
109 /*
110 * Find length of frame.
111 * expectlen is the length we need for a full frame.
112 */
113 cfpkt_peek_head(pkt, &tmp, 2);
114 expectlen = le16_to_cpu(tmp) + 2;
115 /*
116 * Frame error handling
117 */
118 if (expectlen < CAIF_MINIUM_PACKET_SIZE
119 || expectlen > CAIF_MAX_FRAMESIZE) {
120 if (!layr->usestx) {
121 if (pkt != NULL)
122 cfpkt_destroy(pkt);
123 layr->incomplete_frm = NULL;
124 expectlen = 0;
125 spin_unlock(&layr->sync);
126 return -EPROTO;
127 }
128 continue;
129 }
130
131 if (pkt_len < expectlen) {
132 /* Too little received data */
133 if (layr->usestx)
134 cfpkt_add_head(pkt, &stx, 1);
135 layr->incomplete_frm = pkt;
136 spin_unlock(&layr->sync);
137 return 0;
138 }
139
140 /*
141 * Enough data for at least one frame.
142 * Split the frame, if too long
143 */
144 if (pkt_len > expectlen)
145 tail_pkt = cfpkt_split(pkt, expectlen);
146 else
147 tail_pkt = NULL;
148
149 /* Send the first part of packet upwards.*/
150 spin_unlock(&layr->sync);
151 ret = layr->layer.up->receive(layr->layer.up, pkt);
152 spin_lock(&layr->sync);
153 if (ret == -EILSEQ) {
154 if (layr->usestx) {
155 if (tail_pkt != NULL)
156 pkt = cfpkt_append(pkt, tail_pkt, 0);
157
158 /* Start search for next STX if frame failed */
159 continue;
160 } else {
161 cfpkt_destroy(pkt);
162 pkt = NULL;
163 }
164 }
165
166 pkt = tail_pkt;
167
168 } while (pkt != NULL);
169
170 spin_unlock(&layr->sync);
171 return 0;
172}
173
174static int cfserl_transmit(struct cflayer *layer, struct cfpkt *newpkt)
175{
176 struct cfserl *layr = container_obj(layer);
177 int ret;
178 u8 tmp8 = CFSERL_STX;
179 if (layr->usestx)
180 cfpkt_add_head(newpkt, &tmp8, 1);
181 ret = layer->dn->transmit(layer->dn, newpkt);
182 if (ret < 0)
183 cfpkt_extr_head(newpkt, &tmp8, 1);
184
185 return ret;
186}
187
188static void cfserl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
189 int phyid)
190{
191 layr->up->ctrlcmd(layr->up, ctrl, phyid);
192}
diff --git a/net/caif/cfsrvl.c b/net/caif/cfsrvl.c
new file mode 100644
index 000000000000..d470c51c6431
--- /dev/null
+++ b/net/caif/cfsrvl.c
@@ -0,0 +1,185 @@
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#include <linux/kernel.h>
8#include <linux/types.h>
9#include <linux/errno.h>
10#include <linux/slab.h>
11#include <net/caif/caif_layer.h>
12#include <net/caif/cfsrvl.h>
13#include <net/caif/cfpkt.h>
14
15#define SRVL_CTRL_PKT_SIZE 1
16#define SRVL_FLOW_OFF 0x81
17#define SRVL_FLOW_ON 0x80
18#define SRVL_SET_PIN 0x82
19#define SRVL_CTRL_PKT_SIZE 1
20
21#define container_obj(layr) container_of(layr, struct cfsrvl, layer)
22
23static void cfservl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
24 int phyid)
25{
26 struct cfsrvl *service = container_obj(layr);
27 caif_assert(layr->up != NULL);
28 caif_assert(layr->up->ctrlcmd != NULL);
29 switch (ctrl) {
30 case CAIF_CTRLCMD_INIT_RSP:
31 service->open = true;
32 layr->up->ctrlcmd(layr->up, ctrl, phyid);
33 break;
34 case CAIF_CTRLCMD_DEINIT_RSP:
35 case CAIF_CTRLCMD_INIT_FAIL_RSP:
36 service->open = false;
37 layr->up->ctrlcmd(layr->up, ctrl, phyid);
38 break;
39 case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND:
40 if (phyid != service->dev_info.id)
41 break;
42 if (service->modem_flow_on)
43 layr->up->ctrlcmd(layr->up,
44 CAIF_CTRLCMD_FLOW_OFF_IND, phyid);
45 service->phy_flow_on = false;
46 break;
47 case _CAIF_CTRLCMD_PHYIF_FLOW_ON_IND:
48 if (phyid != service->dev_info.id)
49 return;
50 if (service->modem_flow_on) {
51 layr->up->ctrlcmd(layr->up,
52 CAIF_CTRLCMD_FLOW_ON_IND,
53 phyid);
54 }
55 service->phy_flow_on = true;
56 break;
57 case CAIF_CTRLCMD_FLOW_OFF_IND:
58 if (service->phy_flow_on) {
59 layr->up->ctrlcmd(layr->up,
60 CAIF_CTRLCMD_FLOW_OFF_IND, phyid);
61 }
62 service->modem_flow_on = false;
63 break;
64 case CAIF_CTRLCMD_FLOW_ON_IND:
65 if (service->phy_flow_on) {
66 layr->up->ctrlcmd(layr->up,
67 CAIF_CTRLCMD_FLOW_ON_IND, phyid);
68 }
69 service->modem_flow_on = true;
70 break;
71 case _CAIF_CTRLCMD_PHYIF_DOWN_IND:
72 /* In case interface is down, let's fake a remove shutdown */
73 layr->up->ctrlcmd(layr->up,
74 CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND, phyid);
75 break;
76 case CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND:
77 layr->up->ctrlcmd(layr->up, ctrl, phyid);
78 break;
79 default:
80 pr_warning("CAIF: %s(): "
81 "Unexpected ctrl in cfsrvl (%d)\n", __func__, ctrl);
82 /* We have both modem and phy flow on, send flow on */
83 layr->up->ctrlcmd(layr->up, ctrl, phyid);
84 service->phy_flow_on = true;
85 break;
86 }
87}
88
89static int cfservl_modemcmd(struct cflayer *layr, enum caif_modemcmd ctrl)
90{
91 struct cfsrvl *service = container_obj(layr);
92 caif_assert(layr != NULL);
93 caif_assert(layr->dn != NULL);
94 caif_assert(layr->dn->transmit != NULL);
95 switch (ctrl) {
96 case CAIF_MODEMCMD_FLOW_ON_REQ:
97 {
98 struct cfpkt *pkt;
99 struct caif_payload_info *info;
100 u8 flow_on = SRVL_FLOW_ON;
101 pkt = cfpkt_create(SRVL_CTRL_PKT_SIZE);
102 if (!pkt) {
103 pr_warning("CAIF: %s(): Out of memory\n",
104 __func__);
105 return -ENOMEM;
106 }
107
108 if (cfpkt_add_head(pkt, &flow_on, 1) < 0) {
109 pr_err("CAIF: %s(): Packet is erroneous!\n",
110 __func__);
111 cfpkt_destroy(pkt);
112 return -EPROTO;
113 }
114 info = cfpkt_info(pkt);
115 info->channel_id = service->layer.id;
116 info->hdr_len = 1;
117 info->dev_info = &service->dev_info;
118 return layr->dn->transmit(layr->dn, pkt);
119 }
120 case CAIF_MODEMCMD_FLOW_OFF_REQ:
121 {
122 struct cfpkt *pkt;
123 struct caif_payload_info *info;
124 u8 flow_off = SRVL_FLOW_OFF;
125 pkt = cfpkt_create(SRVL_CTRL_PKT_SIZE);
126 if (cfpkt_add_head(pkt, &flow_off, 1) < 0) {
127 pr_err("CAIF: %s(): Packet is erroneous!\n",
128 __func__);
129 cfpkt_destroy(pkt);
130 return -EPROTO;
131 }
132 info = cfpkt_info(pkt);
133 info->channel_id = service->layer.id;
134 info->hdr_len = 1;
135 info->dev_info = &service->dev_info;
136 return layr->dn->transmit(layr->dn, pkt);
137 }
138 default:
139 break;
140 }
141 return -EINVAL;
142}
143
144void cfservl_destroy(struct cflayer *layer)
145{
146 kfree(layer);
147}
148
149void cfsrvl_init(struct cfsrvl *service,
150 u8 channel_id,
151 struct dev_info *dev_info)
152{
153 caif_assert(offsetof(struct cfsrvl, layer) == 0);
154 service->open = false;
155 service->modem_flow_on = true;
156 service->phy_flow_on = true;
157 service->layer.id = channel_id;
158 service->layer.ctrlcmd = cfservl_ctrlcmd;
159 service->layer.modemcmd = cfservl_modemcmd;
160 service->dev_info = *dev_info;
161}
162
163bool cfsrvl_ready(struct cfsrvl *service, int *err)
164{
165 if (service->open && service->modem_flow_on && service->phy_flow_on)
166 return true;
167 if (!service->open) {
168 *err = -ENOTCONN;
169 return false;
170 }
171 caif_assert(!(service->modem_flow_on && service->phy_flow_on));
172 *err = -EAGAIN;
173 return false;
174}
175u8 cfsrvl_getphyid(struct cflayer *layer)
176{
177 struct cfsrvl *servl = container_obj(layer);
178 return servl->dev_info.id;
179}
180
181bool cfsrvl_phyid_match(struct cflayer *layer, int phyid)
182{
183 struct cfsrvl *servl = container_obj(layer);
184 return servl->dev_info.id == phyid;
185}
diff --git a/net/caif/cfutill.c b/net/caif/cfutill.c
new file mode 100644
index 000000000000..5fd2c9ea8b42
--- /dev/null
+++ b/net/caif/cfutill.c
@@ -0,0 +1,115 @@
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#include <linux/kernel.h>
8#include <linux/types.h>
9#include <linux/slab.h>
10#include <linux/errno.h>
11#include <net/caif/caif_layer.h>
12#include <net/caif/cfsrvl.h>
13#include <net/caif/cfpkt.h>
14
15#define container_obj(layr) ((struct cfsrvl *) layr)
16#define UTIL_PAYLOAD 0x00
17#define UTIL_CMD_BIT 0x80
18#define UTIL_REMOTE_SHUTDOWN 0x82
19#define UTIL_FLOW_OFF 0x81
20#define UTIL_FLOW_ON 0x80
21#define UTIL_CTRL_PKT_SIZE 1
22static int cfutill_receive(struct cflayer *layr, struct cfpkt *pkt);
23static int cfutill_transmit(struct cflayer *layr, struct cfpkt *pkt);
24
25struct cflayer *cfutill_create(u8 channel_id, struct dev_info *dev_info)
26{
27 struct cfsrvl *util = kmalloc(sizeof(struct cfsrvl), GFP_ATOMIC);
28 if (!util) {
29 pr_warning("CAIF: %s(): Out of memory\n", __func__);
30 return NULL;
31 }
32 caif_assert(offsetof(struct cfsrvl, layer) == 0);
33 memset(util, 0, sizeof(struct cfsrvl));
34 cfsrvl_init(util, channel_id, dev_info);
35 util->layer.receive = cfutill_receive;
36 util->layer.transmit = cfutill_transmit;
37 snprintf(util->layer.name, CAIF_LAYER_NAME_SZ - 1, "util1");
38 return &util->layer;
39}
40
41static int cfutill_receive(struct cflayer *layr, struct cfpkt *pkt)
42{
43 u8 cmd = -1;
44 struct cfsrvl *service = container_obj(layr);
45 caif_assert(layr != NULL);
46 caif_assert(layr->up != NULL);
47 caif_assert(layr->up->receive != NULL);
48 caif_assert(layr->up->ctrlcmd != NULL);
49 if (cfpkt_extr_head(pkt, &cmd, 1) < 0) {
50 pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
51 cfpkt_destroy(pkt);
52 return -EPROTO;
53 }
54
55 switch (cmd) {
56 case UTIL_PAYLOAD:
57 return layr->up->receive(layr->up, pkt);
58 case UTIL_FLOW_OFF:
59 layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0);
60 cfpkt_destroy(pkt);
61 return 0;
62 case UTIL_FLOW_ON:
63 layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0);
64 cfpkt_destroy(pkt);
65 return 0;
66 case UTIL_REMOTE_SHUTDOWN: /* Remote Shutdown Request */
67 pr_err("CAIF: %s(): REMOTE SHUTDOWN REQUEST RECEIVED\n",
68 __func__);
69 layr->ctrlcmd(layr, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND, 0);
70 service->open = false;
71 cfpkt_destroy(pkt);
72 return 0;
73 default:
74 cfpkt_destroy(pkt);
75 pr_warning("CAIF: %s(): Unknown service control %d (0x%x)\n",
76 __func__, cmd, cmd);
77 return -EPROTO;
78 }
79}
80
81static int cfutill_transmit(struct cflayer *layr, struct cfpkt *pkt)
82{
83 u8 zero = 0;
84 struct caif_payload_info *info;
85 int ret;
86 struct cfsrvl *service = container_obj(layr);
87 caif_assert(layr != NULL);
88 caif_assert(layr->dn != NULL);
89 caif_assert(layr->dn->transmit != NULL);
90 if (!cfsrvl_ready(service, &ret))
91 return ret;
92
93 if (cfpkt_getlen(pkt) > CAIF_MAX_PAYLOAD_SIZE) {
94 pr_err("CAIF: %s(): packet too large size=%d\n",
95 __func__, cfpkt_getlen(pkt));
96 return -EOVERFLOW;
97 }
98
99 cfpkt_add_head(pkt, &zero, 1);
100 /* Add info for MUX-layer to route the packet out. */
101 info = cfpkt_info(pkt);
102 info->channel_id = service->layer.id;
103 /*
104 * To optimize alignment, we add up the size of CAIF header before
105 * payload.
106 */
107 info->hdr_len = 1;
108 info->dev_info = &service->dev_info;
109 ret = layr->dn->transmit(layr->dn, pkt);
110 if (ret < 0) {
111 u32 tmp32;
112 cfpkt_extr_head(pkt, &tmp32, 4);
113 }
114 return ret;
115}
diff --git a/net/caif/cfveil.c b/net/caif/cfveil.c
new file mode 100644
index 000000000000..0fd827f49491
--- /dev/null
+++ b/net/caif/cfveil.c
@@ -0,0 +1,107 @@
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#include <linux/stddef.h>
8#include <linux/slab.h>
9#include <net/caif/caif_layer.h>
10#include <net/caif/cfsrvl.h>
11#include <net/caif/cfpkt.h>
12
13#define VEI_PAYLOAD 0x00
14#define VEI_CMD_BIT 0x80
15#define VEI_FLOW_OFF 0x81
16#define VEI_FLOW_ON 0x80
17#define VEI_SET_PIN 0x82
18#define VEI_CTRL_PKT_SIZE 1
19#define container_obj(layr) container_of(layr, struct cfsrvl, layer)
20
21static int cfvei_receive(struct cflayer *layr, struct cfpkt *pkt);
22static int cfvei_transmit(struct cflayer *layr, struct cfpkt *pkt);
23
24struct cflayer *cfvei_create(u8 channel_id, struct dev_info *dev_info)
25{
26 struct cfsrvl *vei = kmalloc(sizeof(struct cfsrvl), GFP_ATOMIC);
27 if (!vei) {
28 pr_warning("CAIF: %s(): Out of memory\n", __func__);
29 return NULL;
30 }
31 caif_assert(offsetof(struct cfsrvl, layer) == 0);
32 memset(vei, 0, sizeof(struct cfsrvl));
33 cfsrvl_init(vei, channel_id, dev_info);
34 vei->layer.receive = cfvei_receive;
35 vei->layer.transmit = cfvei_transmit;
36 snprintf(vei->layer.name, CAIF_LAYER_NAME_SZ - 1, "vei%d", channel_id);
37 return &vei->layer;
38}
39
40static int cfvei_receive(struct cflayer *layr, struct cfpkt *pkt)
41{
42 u8 cmd;
43 int ret;
44 caif_assert(layr->up != NULL);
45 caif_assert(layr->receive != NULL);
46 caif_assert(layr->ctrlcmd != NULL);
47
48
49 if (cfpkt_extr_head(pkt, &cmd, 1) < 0) {
50 pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
51 cfpkt_destroy(pkt);
52 return -EPROTO;
53 }
54 switch (cmd) {
55 case VEI_PAYLOAD:
56 ret = layr->up->receive(layr->up, pkt);
57 return ret;
58 case VEI_FLOW_OFF:
59 layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0);
60 cfpkt_destroy(pkt);
61 return 0;
62 case VEI_FLOW_ON:
63 layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0);
64 cfpkt_destroy(pkt);
65 return 0;
66 case VEI_SET_PIN: /* SET RS232 PIN */
67 cfpkt_destroy(pkt);
68 return 0;
69 default: /* SET RS232 PIN */
70 pr_warning("CAIF: %s():Unknown VEI control packet %d (0x%x)!\n",
71 __func__, cmd, cmd);
72 cfpkt_destroy(pkt);
73 return -EPROTO;
74 }
75}
76
77static int cfvei_transmit(struct cflayer *layr, struct cfpkt *pkt)
78{
79 u8 tmp = 0;
80 struct caif_payload_info *info;
81 int ret;
82 struct cfsrvl *service = container_obj(layr);
83 if (!cfsrvl_ready(service, &ret))
84 return ret;
85 caif_assert(layr->dn != NULL);
86 caif_assert(layr->dn->transmit != NULL);
87 if (!cfpkt_getlen(pkt) > CAIF_MAX_PAYLOAD_SIZE) {
88 pr_warning("CAIF: %s(): Packet too large - size=%d\n",
89 __func__, cfpkt_getlen(pkt));
90 return -EOVERFLOW;
91 }
92
93 if (cfpkt_add_head(pkt, &tmp, 1) < 0) {
94 pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
95 return -EPROTO;
96 }
97
98 /* Add info-> for MUX-layer to route the packet out. */
99 info = cfpkt_info(pkt);
100 info->channel_id = service->layer.id;
101 info->hdr_len = 1;
102 info->dev_info = &service->dev_info;
103 ret = layr->dn->transmit(layr->dn, pkt);
104 if (ret < 0)
105 cfpkt_extr_head(pkt, &tmp, 1);
106 return ret;
107}
diff --git a/net/caif/cfvidl.c b/net/caif/cfvidl.c
new file mode 100644
index 000000000000..89ad4ea239f1
--- /dev/null
+++ b/net/caif/cfvidl.c
@@ -0,0 +1,65 @@
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#include <linux/kernel.h>
8#include <linux/types.h>
9#include <linux/slab.h>
10#include <linux/errno.h>
11#include <net/caif/caif_layer.h>
12#include <net/caif/cfsrvl.h>
13#include <net/caif/cfpkt.h>
14
15#define container_obj(layr) ((struct cfsrvl *) layr)
16
17static int cfvidl_receive(struct cflayer *layr, struct cfpkt *pkt);
18static int cfvidl_transmit(struct cflayer *layr, struct cfpkt *pkt);
19
20struct cflayer *cfvidl_create(u8 channel_id, struct dev_info *dev_info)
21{
22 struct cfsrvl *vid = kmalloc(sizeof(struct cfsrvl), GFP_ATOMIC);
23 if (!vid) {
24 pr_warning("CAIF: %s(): Out of memory\n", __func__);
25 return NULL;
26 }
27 caif_assert(offsetof(struct cfsrvl, layer) == 0);
28
29 memset(vid, 0, sizeof(struct cfsrvl));
30 cfsrvl_init(vid, channel_id, dev_info);
31 vid->layer.receive = cfvidl_receive;
32 vid->layer.transmit = cfvidl_transmit;
33 snprintf(vid->layer.name, CAIF_LAYER_NAME_SZ - 1, "vid1");
34 return &vid->layer;
35}
36
37static int cfvidl_receive(struct cflayer *layr, struct cfpkt *pkt)
38{
39 u32 videoheader;
40 if (cfpkt_extr_head(pkt, &videoheader, 4) < 0) {
41 pr_err("CAIF: %s(): Packet is erroneous!\n", __func__);
42 cfpkt_destroy(pkt);
43 return -EPROTO;
44 }
45 return layr->up->receive(layr->up, pkt);
46}
47
48static int cfvidl_transmit(struct cflayer *layr, struct cfpkt *pkt)
49{
50 struct cfsrvl *service = container_obj(layr);
51 struct caif_payload_info *info;
52 u32 videoheader = 0;
53 int ret;
54 if (!cfsrvl_ready(service, &ret))
55 return ret;
56 cfpkt_add_head(pkt, &videoheader, 4);
57 /* Add info for MUX-layer to route the packet out */
58 info = cfpkt_info(pkt);
59 info->channel_id = service->layer.id;
60 info->dev_info = &service->dev_info;
61 ret = layr->dn->transmit(layr->dn, pkt);
62 if (ret < 0)
63 cfpkt_extr_head(pkt, &videoheader, 4);
64 return ret;
65}
generated by cgit v1.2.2 (git 2.25.0) at 2024-10-07 14:21:34 -0400