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/stddef.h>
10 #include <linux/spinlock.h>
11 #include <linux/slab.h>
12 #include <linux/pkt_sched.h>
13 #include <net/caif/caif_layer.h>
14 #include <net/caif/cfpkt.h>
15 #include <net/caif/cfctrl.h>
17 #define container_obj(layr) container_of(layr, struct cfctrl, serv.layer)
18 #define UTILITY_NAME_LENGTH 16
19 #define CFPKT_CTRL_PKT_LEN 20
22 static int handle_loop(struct cfctrl
*ctrl
,
23 int cmd
, struct cfpkt
*pkt
){
27 static int handle_loop(struct cfctrl
*ctrl
,
28 int cmd
, struct cfpkt
*pkt
);
30 static int cfctrl_recv(struct cflayer
*layr
, struct cfpkt
*pkt
);
31 static void cfctrl_ctrlcmd(struct cflayer
*layr
, enum caif_ctrlcmd ctrl
,
35 struct cflayer
*cfctrl_create(void)
37 struct dev_info dev_info
;
39 kzalloc(sizeof(struct cfctrl
), GFP_ATOMIC
);
42 caif_assert(offsetof(struct cfctrl
, serv
.layer
) == 0);
43 memset(&dev_info
, 0, sizeof(dev_info
));
45 cfsrvl_init(&this->serv
, 0, &dev_info
, false);
46 atomic_set(&this->req_seq_no
, 1);
47 atomic_set(&this->rsp_seq_no
, 1);
48 this->serv
.layer
.receive
= cfctrl_recv
;
49 sprintf(this->serv
.layer
.name
, "ctrl");
50 this->serv
.layer
.ctrlcmd
= cfctrl_ctrlcmd
;
52 spin_lock_init(&this->loop_linkid_lock
);
53 this->loop_linkid
= 1;
55 spin_lock_init(&this->info_list_lock
);
56 INIT_LIST_HEAD(&this->list
);
57 return &this->serv
.layer
;
60 void cfctrl_remove(struct cflayer
*layer
)
62 struct cfctrl_request_info
*p
, *tmp
;
63 struct cfctrl
*ctrl
= container_obj(layer
);
65 spin_lock_bh(&ctrl
->info_list_lock
);
66 list_for_each_entry_safe(p
, tmp
, &ctrl
->list
, list
) {
70 spin_unlock_bh(&ctrl
->info_list_lock
);
74 static bool param_eq(const struct cfctrl_link_param
*p1
,
75 const struct cfctrl_link_param
*p2
)
78 p1
->linktype
== p2
->linktype
&&
79 p1
->priority
== p2
->priority
&&
80 p1
->phyid
== p2
->phyid
&&
81 p1
->endpoint
== p2
->endpoint
&& p1
->chtype
== p2
->chtype
;
86 switch (p1
->linktype
) {
89 case CFCTRL_SRV_DATAGRAM
:
90 return p1
->u
.datagram
.connid
== p2
->u
.datagram
.connid
;
93 p1
->u
.rfm
.connid
== p2
->u
.rfm
.connid
&&
94 strcmp(p1
->u
.rfm
.volume
, p2
->u
.rfm
.volume
) == 0;
97 p1
->u
.utility
.fifosize_kb
== p2
->u
.utility
.fifosize_kb
98 && p1
->u
.utility
.fifosize_bufs
==
99 p2
->u
.utility
.fifosize_bufs
100 && strcmp(p1
->u
.utility
.name
, p2
->u
.utility
.name
) == 0
101 && p1
->u
.utility
.paramlen
== p2
->u
.utility
.paramlen
102 && memcmp(p1
->u
.utility
.params
, p2
->u
.utility
.params
,
103 p1
->u
.utility
.paramlen
) == 0;
105 case CFCTRL_SRV_VIDEO
:
106 return p1
->u
.video
.connid
== p2
->u
.video
.connid
;
109 case CFCTRL_SRV_DECM
:
117 static bool cfctrl_req_eq(const struct cfctrl_request_info
*r1
,
118 const struct cfctrl_request_info
*r2
)
120 if (r1
->cmd
!= r2
->cmd
)
122 if (r1
->cmd
== CFCTRL_CMD_LINK_SETUP
)
123 return param_eq(&r1
->param
, &r2
->param
);
125 return r1
->channel_id
== r2
->channel_id
;
128 /* Insert request at the end */
129 static void cfctrl_insert_req(struct cfctrl
*ctrl
,
130 struct cfctrl_request_info
*req
)
132 spin_lock_bh(&ctrl
->info_list_lock
);
133 atomic_inc(&ctrl
->req_seq_no
);
134 req
->sequence_no
= atomic_read(&ctrl
->req_seq_no
);
135 list_add_tail(&req
->list
, &ctrl
->list
);
136 spin_unlock_bh(&ctrl
->info_list_lock
);
139 /* Compare and remove request */
140 static struct cfctrl_request_info
*cfctrl_remove_req(struct cfctrl
*ctrl
,
141 struct cfctrl_request_info
*req
)
143 struct cfctrl_request_info
*p
, *tmp
, *first
;
145 first
= list_first_entry(&ctrl
->list
, struct cfctrl_request_info
, list
);
147 list_for_each_entry_safe(p
, tmp
, &ctrl
->list
, list
) {
148 if (cfctrl_req_eq(req
, p
)) {
150 pr_warn("Requests are not received in order\n");
152 atomic_set(&ctrl
->rsp_seq_no
,
163 struct cfctrl_rsp
*cfctrl_get_respfuncs(struct cflayer
*layer
)
165 struct cfctrl
*this = container_obj(layer
);
169 static void init_info(struct caif_payload_info
*info
, struct cfctrl
*cfctrl
)
172 info
->channel_id
= cfctrl
->serv
.layer
.id
;
173 info
->dev_info
= &cfctrl
->serv
.dev_info
;
176 void cfctrl_enum_req(struct cflayer
*layer
, u8 physlinkid
)
178 struct cfctrl
*cfctrl
= container_obj(layer
);
179 struct cfpkt
*pkt
= cfpkt_create(CFPKT_CTRL_PKT_LEN
);
180 struct cflayer
*dn
= cfctrl
->serv
.layer
.dn
;
184 pr_debug("not able to send enum request\n");
187 caif_assert(offsetof(struct cfctrl
, serv
.layer
) == 0);
188 init_info(cfpkt_info(pkt
), cfctrl
);
189 cfpkt_info(pkt
)->dev_info
->id
= physlinkid
;
190 cfctrl
->serv
.dev_info
.id
= physlinkid
;
191 cfpkt_addbdy(pkt
, CFCTRL_CMD_ENUM
);
192 cfpkt_addbdy(pkt
, physlinkid
);
193 cfpkt_set_prio(pkt
, TC_PRIO_CONTROL
);
194 dn
->transmit(dn
, pkt
);
197 int cfctrl_linkup_request(struct cflayer
*layer
,
198 struct cfctrl_link_param
*param
,
199 struct cflayer
*user_layer
)
201 struct cfctrl
*cfctrl
= container_obj(layer
);
205 struct cfctrl_request_info
*req
;
207 char utility_name
[16];
209 struct cflayer
*dn
= cfctrl
->serv
.layer
.dn
;
212 pr_debug("not able to send linkup request\n");
216 if (cfctrl_cancel_req(layer
, user_layer
) > 0) {
217 /* Slight Paranoia, check if already connecting */
218 pr_err("Duplicate connect request for same client\n");
223 pkt
= cfpkt_create(CFPKT_CTRL_PKT_LEN
);
226 cfpkt_addbdy(pkt
, CFCTRL_CMD_LINK_SETUP
);
227 cfpkt_addbdy(pkt
, (param
->chtype
<< 4) | param
->linktype
);
228 cfpkt_addbdy(pkt
, (param
->priority
<< 3) | param
->phyid
);
229 cfpkt_addbdy(pkt
, param
->endpoint
& 0x03);
231 switch (param
->linktype
) {
234 case CFCTRL_SRV_VIDEO
:
235 cfpkt_addbdy(pkt
, (u8
) param
->u
.video
.connid
);
239 case CFCTRL_SRV_DATAGRAM
:
240 tmp32
= cpu_to_le32(param
->u
.datagram
.connid
);
241 cfpkt_add_body(pkt
, &tmp32
, 4);
244 /* Construct a frame, convert DatagramConnectionID to network
245 * format long and copy it out...
247 tmp32
= cpu_to_le32(param
->u
.rfm
.connid
);
248 cfpkt_add_body(pkt
, &tmp32
, 4);
249 /* Add volume name, including zero termination... */
250 cfpkt_add_body(pkt
, param
->u
.rfm
.volume
,
251 strlen(param
->u
.rfm
.volume
) + 1);
253 case CFCTRL_SRV_UTIL
:
254 tmp16
= cpu_to_le16(param
->u
.utility
.fifosize_kb
);
255 cfpkt_add_body(pkt
, &tmp16
, 2);
256 tmp16
= cpu_to_le16(param
->u
.utility
.fifosize_bufs
);
257 cfpkt_add_body(pkt
, &tmp16
, 2);
258 memset(utility_name
, 0, sizeof(utility_name
));
259 strncpy(utility_name
, param
->u
.utility
.name
,
260 UTILITY_NAME_LENGTH
- 1);
261 cfpkt_add_body(pkt
, utility_name
, UTILITY_NAME_LENGTH
);
262 tmp8
= param
->u
.utility
.paramlen
;
263 cfpkt_add_body(pkt
, &tmp8
, 1);
264 cfpkt_add_body(pkt
, param
->u
.utility
.params
,
265 param
->u
.utility
.paramlen
);
268 pr_warn("Request setup of bad link type = %d\n",
272 req
= kzalloc(sizeof(*req
), GFP_KERNEL
);
275 req
->client_layer
= user_layer
;
276 req
->cmd
= CFCTRL_CMD_LINK_SETUP
;
278 cfctrl_insert_req(cfctrl
, req
);
279 init_info(cfpkt_info(pkt
), cfctrl
);
281 * NOTE:Always send linkup and linkdown request on the same
282 * device as the payload. Otherwise old queued up payload
283 * might arrive with the newly allocated channel ID.
285 cfpkt_info(pkt
)->dev_info
->id
= param
->phyid
;
286 cfpkt_set_prio(pkt
, TC_PRIO_CONTROL
);
288 dn
->transmit(dn
, pkt
);
292 count
= cfctrl_cancel_req(&cfctrl
->serv
.layer
,
295 pr_err("Could not remove request (%d)", count
);
301 int cfctrl_linkdown_req(struct cflayer
*layer
, u8 channelid
,
302 struct cflayer
*client
)
305 struct cfctrl
*cfctrl
= container_obj(layer
);
306 struct cfpkt
*pkt
= cfpkt_create(CFPKT_CTRL_PKT_LEN
);
307 struct cflayer
*dn
= cfctrl
->serv
.layer
.dn
;
313 pr_debug("not able to send link-down request\n");
317 cfpkt_addbdy(pkt
, CFCTRL_CMD_LINK_DESTROY
);
318 cfpkt_addbdy(pkt
, channelid
);
319 init_info(cfpkt_info(pkt
), cfctrl
);
320 cfpkt_set_prio(pkt
, TC_PRIO_CONTROL
);
322 dn
->transmit(dn
, pkt
);
324 cfctrl
->loop_linkused
[channelid
] = 0;
329 int cfctrl_cancel_req(struct cflayer
*layr
, struct cflayer
*adap_layer
)
331 struct cfctrl_request_info
*p
, *tmp
;
332 struct cfctrl
*ctrl
= container_obj(layr
);
334 spin_lock_bh(&ctrl
->info_list_lock
);
336 list_for_each_entry_safe(p
, tmp
, &ctrl
->list
, list
) {
337 if (p
->client_layer
== adap_layer
) {
344 spin_unlock_bh(&ctrl
->info_list_lock
);
348 static int cfctrl_recv(struct cflayer
*layer
, struct cfpkt
*pkt
)
357 struct cfctrl
*cfctrl
= container_obj(layer
);
358 struct cfctrl_request_info rsp
, *req
;
361 cfpkt_extr_head(pkt
, &cmdrsp
, 1);
362 cmd
= cmdrsp
& CFCTRL_CMD_MASK
;
363 if (cmd
!= CFCTRL_CMD_LINK_ERR
364 && CFCTRL_RSP_BIT
!= (CFCTRL_RSP_BIT
& cmdrsp
)
365 && CFCTRL_ERR_BIT
!= (CFCTRL_ERR_BIT
& cmdrsp
)) {
366 if (handle_loop(cfctrl
, cmd
, pkt
) != 0)
367 cmdrsp
|= CFCTRL_ERR_BIT
;
371 case CFCTRL_CMD_LINK_SETUP
:
373 enum cfctrl_srv serv
;
374 enum cfctrl_srv servtype
;
382 struct cfctrl_link_param linkparam
;
383 memset(&linkparam
, 0, sizeof(linkparam
));
385 cfpkt_extr_head(pkt
, &tmp
, 1);
387 serv
= tmp
& CFCTRL_SRV_MASK
;
388 linkparam
.linktype
= serv
;
391 linkparam
.chtype
= servtype
;
393 cfpkt_extr_head(pkt
, &tmp
, 1);
394 physlinkid
= tmp
& 0x07;
397 linkparam
.priority
= prio
;
398 linkparam
.phyid
= physlinkid
;
399 cfpkt_extr_head(pkt
, &endpoint
, 1);
400 linkparam
.endpoint
= endpoint
& 0x03;
405 if (CFCTRL_ERR_BIT
& cmdrsp
)
408 cfpkt_extr_head(pkt
, &linkid
, 1);
410 case CFCTRL_SRV_VIDEO
:
411 cfpkt_extr_head(pkt
, &tmp
, 1);
412 linkparam
.u
.video
.connid
= tmp
;
413 if (CFCTRL_ERR_BIT
& cmdrsp
)
416 cfpkt_extr_head(pkt
, &linkid
, 1);
419 case CFCTRL_SRV_DATAGRAM
:
420 cfpkt_extr_head(pkt
, &tmp32
, 4);
421 linkparam
.u
.datagram
.connid
=
423 if (CFCTRL_ERR_BIT
& cmdrsp
)
426 cfpkt_extr_head(pkt
, &linkid
, 1);
429 /* Construct a frame, convert
430 * DatagramConnectionID
431 * to network format long and copy it out...
433 cfpkt_extr_head(pkt
, &tmp32
, 4);
434 linkparam
.u
.rfm
.connid
=
436 cp
= (u8
*) linkparam
.u
.rfm
.volume
;
437 for (cfpkt_extr_head(pkt
, &tmp
, 1);
438 cfpkt_more(pkt
) && tmp
!= '\0';
439 cfpkt_extr_head(pkt
, &tmp
, 1))
443 if (CFCTRL_ERR_BIT
& cmdrsp
)
446 cfpkt_extr_head(pkt
, &linkid
, 1);
449 case CFCTRL_SRV_UTIL
:
450 /* Construct a frame, convert
451 * DatagramConnectionID
452 * to network format long and copy it out...
455 cfpkt_extr_head(pkt
, &tmp16
, 2);
456 linkparam
.u
.utility
.fifosize_kb
=
459 cfpkt_extr_head(pkt
, &tmp16
, 2);
460 linkparam
.u
.utility
.fifosize_bufs
=
463 cp
= (u8
*) linkparam
.u
.utility
.name
;
464 caif_assert(sizeof(linkparam
.u
.utility
.name
)
465 >= UTILITY_NAME_LENGTH
);
467 i
< UTILITY_NAME_LENGTH
468 && cfpkt_more(pkt
); i
++) {
469 cfpkt_extr_head(pkt
, &tmp
, 1);
473 cfpkt_extr_head(pkt
, &len
, 1);
474 linkparam
.u
.utility
.paramlen
= len
;
476 cp
= linkparam
.u
.utility
.params
;
477 while (cfpkt_more(pkt
) && len
--) {
478 cfpkt_extr_head(pkt
, &tmp
, 1);
481 if (CFCTRL_ERR_BIT
& cmdrsp
)
484 cfpkt_extr_head(pkt
, &linkid
, 1);
486 cfpkt_extr_head(pkt
, &len
, 1);
488 cfpkt_extr_head(pkt
, ¶m
, len
);
491 pr_warn("Request setup, invalid type (%d)\n",
497 rsp
.param
= linkparam
;
498 spin_lock_bh(&cfctrl
->info_list_lock
);
499 req
= cfctrl_remove_req(cfctrl
, &rsp
);
501 if (CFCTRL_ERR_BIT
== (CFCTRL_ERR_BIT
& cmdrsp
) ||
502 cfpkt_erroneous(pkt
)) {
503 pr_err("Invalid O/E bit or parse error "
504 "on CAIF control channel\n");
505 cfctrl
->res
.reject_rsp(cfctrl
->serv
.layer
.up
,
507 req
? req
->client_layer
510 cfctrl
->res
.linksetup_rsp(cfctrl
->serv
.
514 client_layer
: NULL
);
520 spin_unlock_bh(&cfctrl
->info_list_lock
);
523 case CFCTRL_CMD_LINK_DESTROY
:
524 cfpkt_extr_head(pkt
, &linkid
, 1);
525 cfctrl
->res
.linkdestroy_rsp(cfctrl
->serv
.layer
.up
, linkid
);
527 case CFCTRL_CMD_LINK_ERR
:
528 pr_err("Frame Error Indication received\n");
529 cfctrl
->res
.linkerror_ind();
531 case CFCTRL_CMD_ENUM
:
532 cfctrl
->res
.enum_rsp();
534 case CFCTRL_CMD_SLEEP
:
535 cfctrl
->res
.sleep_rsp();
537 case CFCTRL_CMD_WAKE
:
538 cfctrl
->res
.wake_rsp();
540 case CFCTRL_CMD_LINK_RECONF
:
541 cfctrl
->res
.restart_rsp();
543 case CFCTRL_CMD_RADIO_SET
:
544 cfctrl
->res
.radioset_rsp();
547 pr_err("Unrecognized Control Frame\n");
557 static void cfctrl_ctrlcmd(struct cflayer
*layr
, enum caif_ctrlcmd ctrl
,
560 struct cfctrl
*this = container_obj(layr
);
562 case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND
:
563 case CAIF_CTRLCMD_FLOW_OFF_IND
:
564 spin_lock_bh(&this->info_list_lock
);
565 if (!list_empty(&this->list
))
566 pr_debug("Received flow off in control layer\n");
567 spin_unlock_bh(&this->info_list_lock
);
569 case _CAIF_CTRLCMD_PHYIF_DOWN_IND
: {
570 struct cfctrl_request_info
*p
, *tmp
;
572 /* Find all connect request and report failure */
573 spin_lock_bh(&this->info_list_lock
);
574 list_for_each_entry_safe(p
, tmp
, &this->list
, list
) {
575 if (p
->param
.phyid
== phyid
) {
577 p
->client_layer
->ctrlcmd(p
->client_layer
,
578 CAIF_CTRLCMD_INIT_FAIL_RSP
,
583 spin_unlock_bh(&this->info_list_lock
);
592 static int handle_loop(struct cfctrl
*ctrl
, int cmd
, struct cfpkt
*pkt
)
594 static int last_linkid
;
596 u8 linkid
, linktype
, tmp
;
598 case CFCTRL_CMD_LINK_SETUP
:
599 spin_lock_bh(&ctrl
->loop_linkid_lock
);
601 for (linkid
= last_linkid
+ 1; linkid
< 254; linkid
++)
602 if (!ctrl
->loop_linkused
[linkid
])
606 for (linkid
= last_linkid
- 1; linkid
> 1; linkid
--)
607 if (!ctrl
->loop_linkused
[linkid
])
609 spin_unlock_bh(&ctrl
->loop_linkid_lock
);
615 if (!ctrl
->loop_linkused
[linkid
])
616 ctrl
->loop_linkused
[linkid
] = 1;
618 last_linkid
= linkid
;
620 cfpkt_add_trail(pkt
, &linkid
, 1);
621 spin_unlock_bh(&ctrl
->loop_linkid_lock
);
622 cfpkt_peek_head(pkt
, &linktype
, 1);
623 if (linktype
== CFCTRL_SRV_UTIL
) {
625 cfpkt_add_trail(pkt
, &tmp
, 1);
626 cfpkt_add_trail(pkt
, &tmp
, 1);
630 case CFCTRL_CMD_LINK_DESTROY
:
631 spin_lock_bh(&ctrl
->loop_linkid_lock
);
632 cfpkt_peek_head(pkt
, &linkid
, 1);
633 ctrl
->loop_linkused
[linkid
] = 0;
634 spin_unlock_bh(&ctrl
->loop_linkid_lock
);