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 #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>
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
20 static int handle_loop(struct cfctrl
*ctrl
,
21 int cmd
, struct cfpkt
*pkt
){
25 static int handle_loop(struct cfctrl
*ctrl
,
26 int cmd
, struct cfpkt
*pkt
);
28 static int cfctrl_recv(struct cflayer
*layr
, struct cfpkt
*pkt
);
29 static void cfctrl_ctrlcmd(struct cflayer
*layr
, enum caif_ctrlcmd ctrl
,
33 struct cflayer
*cfctrl_create(void)
35 struct dev_info dev_info
;
37 kmalloc(sizeof(struct cfctrl
), GFP_ATOMIC
);
39 pr_warning("CAIF: %s(): Out of memory\n", __func__
);
42 caif_assert(offsetof(struct cfctrl
, serv
.layer
) == 0);
43 memset(&dev_info
, 0, sizeof(dev_info
));
45 memset(this, 0, sizeof(*this));
46 cfsrvl_init(&this->serv
, 0, &dev_info
);
47 spin_lock_init(&this->info_list_lock
);
48 atomic_set(&this->req_seq_no
, 1);
49 atomic_set(&this->rsp_seq_no
, 1);
50 this->serv
.layer
.receive
= cfctrl_recv
;
51 sprintf(this->serv
.layer
.name
, "ctrl");
52 this->serv
.layer
.ctrlcmd
= cfctrl_ctrlcmd
;
53 spin_lock_init(&this->loop_linkid_lock
);
54 this->loop_linkid
= 1;
55 return &this->serv
.layer
;
58 static bool param_eq(struct cfctrl_link_param
*p1
, struct cfctrl_link_param
*p2
)
61 p1
->linktype
== p2
->linktype
&&
62 p1
->priority
== p2
->priority
&&
63 p1
->phyid
== p2
->phyid
&&
64 p1
->endpoint
== p2
->endpoint
&& p1
->chtype
== p2
->chtype
;
69 switch (p1
->linktype
) {
72 case CFCTRL_SRV_DATAGRAM
:
73 return p1
->u
.datagram
.connid
== p2
->u
.datagram
.connid
;
76 p1
->u
.rfm
.connid
== p2
->u
.rfm
.connid
&&
77 strcmp(p1
->u
.rfm
.volume
, p2
->u
.rfm
.volume
) == 0;
80 p1
->u
.utility
.fifosize_kb
== p2
->u
.utility
.fifosize_kb
81 && p1
->u
.utility
.fifosize_bufs
==
82 p2
->u
.utility
.fifosize_bufs
83 && strcmp(p1
->u
.utility
.name
, p2
->u
.utility
.name
) == 0
84 && p1
->u
.utility
.paramlen
== p2
->u
.utility
.paramlen
85 && memcmp(p1
->u
.utility
.params
, p2
->u
.utility
.params
,
86 p1
->u
.utility
.paramlen
) == 0;
88 case CFCTRL_SRV_VIDEO
:
89 return p1
->u
.video
.connid
== p2
->u
.video
.connid
;
100 bool cfctrl_req_eq(struct cfctrl_request_info
*r1
,
101 struct cfctrl_request_info
*r2
)
103 if (r1
->cmd
!= r2
->cmd
)
105 if (r1
->cmd
== CFCTRL_CMD_LINK_SETUP
)
106 return param_eq(&r1
->param
, &r2
->param
);
108 return r1
->channel_id
== r2
->channel_id
;
111 /* Insert request at the end */
112 void cfctrl_insert_req(struct cfctrl
*ctrl
,
113 struct cfctrl_request_info
*req
)
115 struct cfctrl_request_info
*p
;
116 spin_lock(&ctrl
->info_list_lock
);
118 atomic_inc(&ctrl
->req_seq_no
);
119 req
->sequence_no
= atomic_read(&ctrl
->req_seq_no
);
120 if (ctrl
->first_req
== NULL
) {
121 ctrl
->first_req
= req
;
122 spin_unlock(&ctrl
->info_list_lock
);
126 while (p
->next
!= NULL
)
129 spin_unlock(&ctrl
->info_list_lock
);
132 /* Compare and remove request */
133 struct cfctrl_request_info
*cfctrl_remove_req(struct cfctrl
*ctrl
,
134 struct cfctrl_request_info
*req
)
136 struct cfctrl_request_info
*p
;
137 struct cfctrl_request_info
*ret
;
139 spin_lock(&ctrl
->info_list_lock
);
140 if (ctrl
->first_req
== NULL
) {
141 spin_unlock(&ctrl
->info_list_lock
);
145 if (cfctrl_req_eq(req
, ctrl
->first_req
)) {
146 ret
= ctrl
->first_req
;
147 caif_assert(ctrl
->first_req
);
148 atomic_set(&ctrl
->rsp_seq_no
,
149 ctrl
->first_req
->sequence_no
);
150 ctrl
->first_req
= ctrl
->first_req
->next
;
151 spin_unlock(&ctrl
->info_list_lock
);
157 while (p
->next
!= NULL
) {
158 if (cfctrl_req_eq(req
, p
->next
)) {
159 pr_warning("CAIF: %s(): Requests are not "
160 "received in order\n",
163 atomic_set(&ctrl
->rsp_seq_no
,
164 p
->next
->sequence_no
);
165 p
->next
= p
->next
->next
;
166 spin_unlock(&ctrl
->info_list_lock
);
171 spin_unlock(&ctrl
->info_list_lock
);
173 pr_warning("CAIF: %s(): Request does not match\n",
178 struct cfctrl_rsp
*cfctrl_get_respfuncs(struct cflayer
*layer
)
180 struct cfctrl
*this = container_obj(layer
);
184 void cfctrl_set_dnlayer(struct cflayer
*this, struct cflayer
*dn
)
189 void cfctrl_set_uplayer(struct cflayer
*this, struct cflayer
*up
)
194 static void init_info(struct caif_payload_info
*info
, struct cfctrl
*cfctrl
)
197 info
->channel_id
= cfctrl
->serv
.layer
.id
;
198 info
->dev_info
= &cfctrl
->serv
.dev_info
;
201 void cfctrl_enum_req(struct cflayer
*layer
, u8 physlinkid
)
203 struct cfctrl
*cfctrl
= container_obj(layer
);
205 struct cfpkt
*pkt
= cfpkt_create(CFPKT_CTRL_PKT_LEN
);
207 pr_warning("CAIF: %s(): Out of memory\n", __func__
);
210 caif_assert(offsetof(struct cfctrl
, serv
.layer
) == 0);
211 init_info(cfpkt_info(pkt
), cfctrl
);
212 cfpkt_info(pkt
)->dev_info
->id
= physlinkid
;
213 cfctrl
->serv
.dev_info
.id
= physlinkid
;
214 cfpkt_addbdy(pkt
, CFCTRL_CMD_ENUM
);
215 cfpkt_addbdy(pkt
, physlinkid
);
217 cfctrl
->serv
.layer
.dn
->transmit(cfctrl
->serv
.layer
.dn
, pkt
);
219 pr_err("CAIF: %s(): Could not transmit enum message\n",
225 int cfctrl_linkup_request(struct cflayer
*layer
,
226 struct cfctrl_link_param
*param
,
227 struct cflayer
*user_layer
)
229 struct cfctrl
*cfctrl
= container_obj(layer
);
233 struct cfctrl_request_info
*req
;
235 char utility_name
[16];
236 struct cfpkt
*pkt
= cfpkt_create(CFPKT_CTRL_PKT_LEN
);
238 pr_warning("CAIF: %s(): Out of memory\n", __func__
);
241 cfpkt_addbdy(pkt
, CFCTRL_CMD_LINK_SETUP
);
242 cfpkt_addbdy(pkt
, (param
->chtype
<< 4) + param
->linktype
);
243 cfpkt_addbdy(pkt
, (param
->priority
<< 3) + param
->phyid
);
244 cfpkt_addbdy(pkt
, param
->endpoint
& 0x03);
246 switch (param
->linktype
) {
249 case CFCTRL_SRV_VIDEO
:
250 cfpkt_addbdy(pkt
, (u8
) param
->u
.video
.connid
);
254 case CFCTRL_SRV_DATAGRAM
:
255 tmp32
= cpu_to_le32(param
->u
.datagram
.connid
);
256 cfpkt_add_body(pkt
, &tmp32
, 4);
259 /* Construct a frame, convert DatagramConnectionID to network
260 * format long and copy it out...
262 tmp32
= cpu_to_le32(param
->u
.rfm
.connid
);
263 cfpkt_add_body(pkt
, &tmp32
, 4);
264 /* Add volume name, including zero termination... */
265 cfpkt_add_body(pkt
, param
->u
.rfm
.volume
,
266 strlen(param
->u
.rfm
.volume
) + 1);
268 case CFCTRL_SRV_UTIL
:
269 tmp16
= cpu_to_le16(param
->u
.utility
.fifosize_kb
);
270 cfpkt_add_body(pkt
, &tmp16
, 2);
271 tmp16
= cpu_to_le16(param
->u
.utility
.fifosize_bufs
);
272 cfpkt_add_body(pkt
, &tmp16
, 2);
273 memset(utility_name
, 0, sizeof(utility_name
));
274 strncpy(utility_name
, param
->u
.utility
.name
,
275 UTILITY_NAME_LENGTH
- 1);
276 cfpkt_add_body(pkt
, utility_name
, UTILITY_NAME_LENGTH
);
277 tmp8
= param
->u
.utility
.paramlen
;
278 cfpkt_add_body(pkt
, &tmp8
, 1);
279 cfpkt_add_body(pkt
, param
->u
.utility
.params
,
280 param
->u
.utility
.paramlen
);
283 pr_warning("CAIF: %s():Request setup of bad link type = %d\n",
284 __func__
, param
->linktype
);
287 req
= kzalloc(sizeof(*req
), GFP_KERNEL
);
289 pr_warning("CAIF: %s(): Out of memory\n", __func__
);
292 req
->client_layer
= user_layer
;
293 req
->cmd
= CFCTRL_CMD_LINK_SETUP
;
295 cfctrl_insert_req(cfctrl
, req
);
296 init_info(cfpkt_info(pkt
), cfctrl
);
298 * NOTE:Always send linkup and linkdown request on the same
299 * device as the payload. Otherwise old queued up payload
300 * might arrive with the newly allocated channel ID.
302 cfpkt_info(pkt
)->dev_info
->id
= param
->phyid
;
304 cfctrl
->serv
.layer
.dn
->transmit(cfctrl
->serv
.layer
.dn
, pkt
);
306 pr_err("CAIF: %s(): Could not transmit linksetup request\n",
314 int cfctrl_linkdown_req(struct cflayer
*layer
, u8 channelid
,
315 struct cflayer
*client
)
318 struct cfctrl
*cfctrl
= container_obj(layer
);
319 struct cfpkt
*pkt
= cfpkt_create(CFPKT_CTRL_PKT_LEN
);
321 pr_warning("CAIF: %s(): Out of memory\n", __func__
);
324 cfpkt_addbdy(pkt
, CFCTRL_CMD_LINK_DESTROY
);
325 cfpkt_addbdy(pkt
, channelid
);
326 init_info(cfpkt_info(pkt
), cfctrl
);
328 cfctrl
->serv
.layer
.dn
->transmit(cfctrl
->serv
.layer
.dn
, pkt
);
330 pr_err("CAIF: %s(): Could not transmit link-down request\n",
337 void cfctrl_sleep_req(struct cflayer
*layer
)
340 struct cfctrl
*cfctrl
= container_obj(layer
);
341 struct cfpkt
*pkt
= cfpkt_create(CFPKT_CTRL_PKT_LEN
);
343 pr_warning("CAIF: %s(): Out of memory\n", __func__
);
346 cfpkt_addbdy(pkt
, CFCTRL_CMD_SLEEP
);
347 init_info(cfpkt_info(pkt
), cfctrl
);
349 cfctrl
->serv
.layer
.dn
->transmit(cfctrl
->serv
.layer
.dn
, pkt
);
354 void cfctrl_wake_req(struct cflayer
*layer
)
357 struct cfctrl
*cfctrl
= container_obj(layer
);
358 struct cfpkt
*pkt
= cfpkt_create(CFPKT_CTRL_PKT_LEN
);
360 pr_warning("CAIF: %s(): Out of memory\n", __func__
);
363 cfpkt_addbdy(pkt
, CFCTRL_CMD_WAKE
);
364 init_info(cfpkt_info(pkt
), cfctrl
);
366 cfctrl
->serv
.layer
.dn
->transmit(cfctrl
->serv
.layer
.dn
, pkt
);
371 void cfctrl_getstartreason_req(struct cflayer
*layer
)
374 struct cfctrl
*cfctrl
= container_obj(layer
);
375 struct cfpkt
*pkt
= cfpkt_create(CFPKT_CTRL_PKT_LEN
);
377 pr_warning("CAIF: %s(): Out of memory\n", __func__
);
380 cfpkt_addbdy(pkt
, CFCTRL_CMD_START_REASON
);
381 init_info(cfpkt_info(pkt
), cfctrl
);
383 cfctrl
->serv
.layer
.dn
->transmit(cfctrl
->serv
.layer
.dn
, pkt
);
389 void cfctrl_cancel_req(struct cflayer
*layr
, struct cflayer
*adap_layer
)
391 struct cfctrl_request_info
*p
, *req
;
392 struct cfctrl
*ctrl
= container_obj(layr
);
393 spin_lock(&ctrl
->info_list_lock
);
395 if (ctrl
->first_req
== NULL
) {
396 spin_unlock(&ctrl
->info_list_lock
);
400 if (ctrl
->first_req
->client_layer
== adap_layer
) {
402 req
= ctrl
->first_req
;
403 ctrl
->first_req
= ctrl
->first_req
->next
;
408 while (p
!= NULL
&& p
->next
!= NULL
) {
409 if (p
->next
->client_layer
== adap_layer
) {
412 p
->next
= p
->next
->next
;
418 spin_unlock(&ctrl
->info_list_lock
);
421 static int cfctrl_recv(struct cflayer
*layer
, struct cfpkt
*pkt
)
430 struct cfctrl
*cfctrl
= container_obj(layer
);
431 struct cfctrl_request_info rsp
, *req
;
434 cfpkt_extr_head(pkt
, &cmdrsp
, 1);
435 cmd
= cmdrsp
& CFCTRL_CMD_MASK
;
436 if (cmd
!= CFCTRL_CMD_LINK_ERR
437 && CFCTRL_RSP_BIT
!= (CFCTRL_RSP_BIT
& cmdrsp
)) {
438 if (handle_loop(cfctrl
, cmd
, pkt
) == CAIF_FAILURE
)
439 cmdrsp
|= CFCTRL_ERR_BIT
;
443 case CFCTRL_CMD_LINK_SETUP
:
445 enum cfctrl_srv serv
;
446 enum cfctrl_srv servtype
;
454 struct cfctrl_link_param linkparam
;
455 memset(&linkparam
, 0, sizeof(linkparam
));
457 cfpkt_extr_head(pkt
, &tmp
, 1);
459 serv
= tmp
& CFCTRL_SRV_MASK
;
460 linkparam
.linktype
= serv
;
463 linkparam
.chtype
= servtype
;
465 cfpkt_extr_head(pkt
, &tmp
, 1);
466 physlinkid
= tmp
& 0x07;
469 linkparam
.priority
= prio
;
470 linkparam
.phyid
= physlinkid
;
471 cfpkt_extr_head(pkt
, &endpoint
, 1);
472 linkparam
.endpoint
= endpoint
& 0x03;
477 if (CFCTRL_ERR_BIT
& cmdrsp
)
480 cfpkt_extr_head(pkt
, &linkid
, 1);
482 case CFCTRL_SRV_VIDEO
:
483 cfpkt_extr_head(pkt
, &tmp
, 1);
484 linkparam
.u
.video
.connid
= tmp
;
485 if (CFCTRL_ERR_BIT
& cmdrsp
)
488 cfpkt_extr_head(pkt
, &linkid
, 1);
491 case CFCTRL_SRV_DATAGRAM
:
492 cfpkt_extr_head(pkt
, &tmp32
, 4);
493 linkparam
.u
.datagram
.connid
=
495 if (CFCTRL_ERR_BIT
& cmdrsp
)
498 cfpkt_extr_head(pkt
, &linkid
, 1);
501 /* Construct a frame, convert
502 * DatagramConnectionID
503 * to network format long and copy it out...
505 cfpkt_extr_head(pkt
, &tmp32
, 4);
506 linkparam
.u
.rfm
.connid
=
508 cp
= (u8
*) linkparam
.u
.rfm
.volume
;
509 for (cfpkt_extr_head(pkt
, &tmp
, 1);
510 cfpkt_more(pkt
) && tmp
!= '\0';
511 cfpkt_extr_head(pkt
, &tmp
, 1))
515 if (CFCTRL_ERR_BIT
& cmdrsp
)
518 cfpkt_extr_head(pkt
, &linkid
, 1);
521 case CFCTRL_SRV_UTIL
:
522 /* Construct a frame, convert
523 * DatagramConnectionID
524 * to network format long and copy it out...
527 cfpkt_extr_head(pkt
, &tmp16
, 2);
528 linkparam
.u
.utility
.fifosize_kb
=
531 cfpkt_extr_head(pkt
, &tmp16
, 2);
532 linkparam
.u
.utility
.fifosize_bufs
=
535 cp
= (u8
*) linkparam
.u
.utility
.name
;
536 caif_assert(sizeof(linkparam
.u
.utility
.name
)
537 >= UTILITY_NAME_LENGTH
);
539 i
< UTILITY_NAME_LENGTH
540 && cfpkt_more(pkt
); i
++) {
541 cfpkt_extr_head(pkt
, &tmp
, 1);
545 cfpkt_extr_head(pkt
, &len
, 1);
546 linkparam
.u
.utility
.paramlen
= len
;
548 cp
= linkparam
.u
.utility
.params
;
549 while (cfpkt_more(pkt
) && len
--) {
550 cfpkt_extr_head(pkt
, &tmp
, 1);
553 if (CFCTRL_ERR_BIT
& cmdrsp
)
556 cfpkt_extr_head(pkt
, &linkid
, 1);
558 cfpkt_extr_head(pkt
, &len
, 1);
560 cfpkt_extr_head(pkt
, ¶m
, len
);
563 pr_warning("CAIF: %s(): Request setup "
564 "- invalid link type (%d)",
570 rsp
.param
= linkparam
;
571 req
= cfctrl_remove_req(cfctrl
, &rsp
);
573 if (CFCTRL_ERR_BIT
== (CFCTRL_ERR_BIT
& cmdrsp
) ||
574 cfpkt_erroneous(pkt
)) {
575 pr_err("CAIF: %s(): Invalid O/E bit or parse "
576 "error on CAIF control channel",
578 cfctrl
->res
.reject_rsp(cfctrl
->serv
.layer
.up
,
580 req
? req
->client_layer
583 cfctrl
->res
.linksetup_rsp(cfctrl
->serv
.
587 client_layer
: NULL
);
594 case CFCTRL_CMD_LINK_DESTROY
:
595 cfpkt_extr_head(pkt
, &linkid
, 1);
596 cfctrl
->res
.linkdestroy_rsp(cfctrl
->serv
.layer
.up
, linkid
);
598 case CFCTRL_CMD_LINK_ERR
:
599 pr_err("CAIF: %s(): Frame Error Indication received\n",
601 cfctrl
->res
.linkerror_ind();
603 case CFCTRL_CMD_ENUM
:
604 cfctrl
->res
.enum_rsp();
606 case CFCTRL_CMD_SLEEP
:
607 cfctrl
->res
.sleep_rsp();
609 case CFCTRL_CMD_WAKE
:
610 cfctrl
->res
.wake_rsp();
612 case CFCTRL_CMD_LINK_RECONF
:
613 cfctrl
->res
.restart_rsp();
615 case CFCTRL_CMD_RADIO_SET
:
616 cfctrl
->res
.radioset_rsp();
619 pr_err("CAIF: %s(): Unrecognized Control Frame\n", __func__
);
629 static void cfctrl_ctrlcmd(struct cflayer
*layr
, enum caif_ctrlcmd ctrl
,
632 struct cfctrl
*this = container_obj(layr
);
634 case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND
:
635 case CAIF_CTRLCMD_FLOW_OFF_IND
:
636 spin_lock(&this->info_list_lock
);
637 if (this->first_req
!= NULL
) {
638 pr_debug("CAIF: %s(): Received flow off in "
639 "control layer", __func__
);
641 spin_unlock(&this->info_list_lock
);
649 static int handle_loop(struct cfctrl
*ctrl
, int cmd
, struct cfpkt
*pkt
)
651 static int last_linkid
;
652 u8 linkid
, linktype
, tmp
;
654 case CFCTRL_CMD_LINK_SETUP
:
655 spin_lock(&ctrl
->loop_linkid_lock
);
656 for (linkid
= last_linkid
+ 1; linkid
< 255; linkid
++)
657 if (!ctrl
->loop_linkused
[linkid
])
659 for (linkid
= last_linkid
- 1; linkid
> 0; linkid
--)
660 if (!ctrl
->loop_linkused
[linkid
])
662 spin_unlock(&ctrl
->loop_linkid_lock
);
663 pr_err("CAIF: %s(): Out of link-ids\n", __func__
);
666 if (!ctrl
->loop_linkused
[linkid
])
667 ctrl
->loop_linkused
[linkid
] = 1;
669 last_linkid
= linkid
;
671 cfpkt_add_trail(pkt
, &linkid
, 1);
672 spin_unlock(&ctrl
->loop_linkid_lock
);
673 cfpkt_peek_head(pkt
, &linktype
, 1);
674 if (linktype
== CFCTRL_SRV_UTIL
) {
676 cfpkt_add_trail(pkt
, &tmp
, 1);
677 cfpkt_add_trail(pkt
, &tmp
, 1);
681 case CFCTRL_CMD_LINK_DESTROY
:
682 spin_lock(&ctrl
->loop_linkid_lock
);
683 cfpkt_peek_head(pkt
, &linkid
, 1);
684 ctrl
->loop_linkused
[linkid
] = 0;
685 spin_unlock(&ctrl
->loop_linkid_lock
);