1
2
3
4
5
6
7#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
8
9#include <linux/stddef.h>
10#include <linux/spinlock.h>
11#include <linux/slab.h>
12#include <net/caif/caif_layer.h>
13#include <net/caif/cfpkt.h>
14#include <net/caif/cfctrl.h>
15
16#define container_obj(layr) container_of(layr, struct cfctrl, serv.layer)
17#define UTILITY_NAME_LENGTH 16
18#define CFPKT_CTRL_PKT_LEN 20
19
20
21#ifdef CAIF_NO_LOOP
22static int handle_loop(struct cfctrl *ctrl,
23 int cmd, struct cfpkt *pkt){
24 return -1;
25}
26#else
27static int handle_loop(struct cfctrl *ctrl,
28 int cmd, struct cfpkt *pkt);
29#endif
30static int cfctrl_recv(struct cflayer *layr, struct cfpkt *pkt);
31static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
32 int phyid);
33
34
35struct cflayer *cfctrl_create(void)
36{
37 struct dev_info dev_info;
38 struct cfctrl *this =
39 kmalloc(sizeof(struct cfctrl), GFP_ATOMIC);
40 if (!this) {
41 pr_warn("Out of memory\n");
42 return NULL;
43 }
44 caif_assert(offsetof(struct cfctrl, serv.layer) == 0);
45 memset(&dev_info, 0, sizeof(dev_info));
46 dev_info.id = 0xff;
47 memset(this, 0, sizeof(*this));
48 cfsrvl_init(&this->serv, 0, &dev_info, false);
49 atomic_set(&this->req_seq_no, 1);
50 atomic_set(&this->rsp_seq_no, 1);
51 this->serv.layer.receive = cfctrl_recv;
52 sprintf(this->serv.layer.name, "ctrl");
53 this->serv.layer.ctrlcmd = cfctrl_ctrlcmd;
54 spin_lock_init(&this->loop_linkid_lock);
55 spin_lock_init(&this->info_list_lock);
56 INIT_LIST_HEAD(&this->list);
57 this->loop_linkid = 1;
58 return &this->serv.layer;
59}
60
61static bool param_eq(struct cfctrl_link_param *p1, struct cfctrl_link_param *p2)
62{
63 bool eq =
64 p1->linktype == p2->linktype &&
65 p1->priority == p2->priority &&
66 p1->phyid == p2->phyid &&
67 p1->endpoint == p2->endpoint && p1->chtype == p2->chtype;
68
69 if (!eq)
70 return false;
71
72 switch (p1->linktype) {
73 case CFCTRL_SRV_VEI:
74 return true;
75 case CFCTRL_SRV_DATAGRAM:
76 return p1->u.datagram.connid == p2->u.datagram.connid;
77 case CFCTRL_SRV_RFM:
78 return
79 p1->u.rfm.connid == p2->u.rfm.connid &&
80 strcmp(p1->u.rfm.volume, p2->u.rfm.volume) == 0;
81 case CFCTRL_SRV_UTIL:
82 return
83 p1->u.utility.fifosize_kb == p2->u.utility.fifosize_kb
84 && p1->u.utility.fifosize_bufs ==
85 p2->u.utility.fifosize_bufs
86 && strcmp(p1->u.utility.name, p2->u.utility.name) == 0
87 && p1->u.utility.paramlen == p2->u.utility.paramlen
88 && memcmp(p1->u.utility.params, p2->u.utility.params,
89 p1->u.utility.paramlen) == 0;
90
91 case CFCTRL_SRV_VIDEO:
92 return p1->u.video.connid == p2->u.video.connid;
93 case CFCTRL_SRV_DBG:
94 return true;
95 case CFCTRL_SRV_DECM:
96 return false;
97 default:
98 return false;
99 }
100 return false;
101}
102
103bool cfctrl_req_eq(struct cfctrl_request_info *r1,
104 struct cfctrl_request_info *r2)
105{
106 if (r1->cmd != r2->cmd)
107 return false;
108 if (r1->cmd == CFCTRL_CMD_LINK_SETUP)
109 return param_eq(&r1->param, &r2->param);
110 else
111 return r1->channel_id == r2->channel_id;
112}
113
114
115void cfctrl_insert_req(struct cfctrl *ctrl,
116 struct cfctrl_request_info *req)
117{
118 spin_lock(&ctrl->info_list_lock);
119 atomic_inc(&ctrl->req_seq_no);
120 req->sequence_no = atomic_read(&ctrl->req_seq_no);
121 list_add_tail(&req->list, &ctrl->list);
122 spin_unlock(&ctrl->info_list_lock);
123}
124
125
126struct cfctrl_request_info *cfctrl_remove_req(struct cfctrl *ctrl,
127 struct cfctrl_request_info *req)
128{
129 struct cfctrl_request_info *p, *tmp, *first;
130
131 spin_lock(&ctrl->info_list_lock);
132 first = list_first_entry(&ctrl->list, struct cfctrl_request_info, list);
133
134 list_for_each_entry_safe(p, tmp, &ctrl->list, list) {
135 if (cfctrl_req_eq(req, p)) {
136 if (p != first)
137 pr_warn("Requests are not received in order\n");
138
139 atomic_set(&ctrl->rsp_seq_no,
140 p->sequence_no);
141 list_del(&p->list);
142 goto out;
143 }
144 }
145 p = NULL;
146out:
147 spin_unlock(&ctrl->info_list_lock);
148 return p;
149}
150
151struct cfctrl_rsp *cfctrl_get_respfuncs(struct cflayer *layer)
152{
153 struct cfctrl *this = container_obj(layer);
154 return &this->res;
155}
156
157void cfctrl_set_dnlayer(struct cflayer *this, struct cflayer *dn)
158{
159 this->dn = dn;
160}
161
162void cfctrl_set_uplayer(struct cflayer *this, struct cflayer *up)
163{
164 this->up = up;
165}
166
167static void init_info(struct caif_payload_info *info, struct cfctrl *cfctrl)
168{
169 info->hdr_len = 0;
170 info->channel_id = cfctrl->serv.layer.id;
171 info->dev_info = &cfctrl->serv.dev_info;
172}
173
174void cfctrl_enum_req(struct cflayer *layer, u8 physlinkid)
175{
176 struct cfctrl *cfctrl = container_obj(layer);
177 int ret;
178 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
179 if (!pkt) {
180 pr_warn("Out of memory\n");
181 return;
182 }
183 caif_assert(offsetof(struct cfctrl, serv.layer) == 0);
184 init_info(cfpkt_info(pkt), cfctrl);
185 cfpkt_info(pkt)->dev_info->id = physlinkid;
186 cfctrl->serv.dev_info.id = physlinkid;
187 cfpkt_addbdy(pkt, CFCTRL_CMD_ENUM);
188 cfpkt_addbdy(pkt, physlinkid);
189 ret =
190 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
191 if (ret < 0) {
192 pr_err("Could not transmit enum message\n");
193 cfpkt_destroy(pkt);
194 }
195}
196
197int cfctrl_linkup_request(struct cflayer *layer,
198 struct cfctrl_link_param *param,
199 struct cflayer *user_layer)
200{
201 struct cfctrl *cfctrl = container_obj(layer);
202 u32 tmp32;
203 u16 tmp16;
204 u8 tmp8;
205 struct cfctrl_request_info *req;
206 int ret;
207 char utility_name[16];
208 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
209 if (!pkt) {
210 pr_warn("Out of memory\n");
211 return -ENOMEM;
212 }
213 cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_SETUP);
214 cfpkt_addbdy(pkt, (param->chtype << 4) + param->linktype);
215 cfpkt_addbdy(pkt, (param->priority << 3) + param->phyid);
216 cfpkt_addbdy(pkt, param->endpoint & 0x03);
217
218 switch (param->linktype) {
219 case CFCTRL_SRV_VEI:
220 break;
221 case CFCTRL_SRV_VIDEO:
222 cfpkt_addbdy(pkt, (u8) param->u.video.connid);
223 break;
224 case CFCTRL_SRV_DBG:
225 break;
226 case CFCTRL_SRV_DATAGRAM:
227 tmp32 = cpu_to_le32(param->u.datagram.connid);
228 cfpkt_add_body(pkt, &tmp32, 4);
229 break;
230 case CFCTRL_SRV_RFM:
231
232
233
234 tmp32 = cpu_to_le32(param->u.rfm.connid);
235 cfpkt_add_body(pkt, &tmp32, 4);
236
237 cfpkt_add_body(pkt, param->u.rfm.volume,
238 strlen(param->u.rfm.volume) + 1);
239 break;
240 case CFCTRL_SRV_UTIL:
241 tmp16 = cpu_to_le16(param->u.utility.fifosize_kb);
242 cfpkt_add_body(pkt, &tmp16, 2);
243 tmp16 = cpu_to_le16(param->u.utility.fifosize_bufs);
244 cfpkt_add_body(pkt, &tmp16, 2);
245 memset(utility_name, 0, sizeof(utility_name));
246 strncpy(utility_name, param->u.utility.name,
247 UTILITY_NAME_LENGTH - 1);
248 cfpkt_add_body(pkt, utility_name, UTILITY_NAME_LENGTH);
249 tmp8 = param->u.utility.paramlen;
250 cfpkt_add_body(pkt, &tmp8, 1);
251 cfpkt_add_body(pkt, param->u.utility.params,
252 param->u.utility.paramlen);
253 break;
254 default:
255 pr_warn("Request setup of bad link type = %d\n",
256 param->linktype);
257 return -EINVAL;
258 }
259 req = kzalloc(sizeof(*req), GFP_KERNEL);
260 if (!req) {
261 pr_warn("Out of memory\n");
262 return -ENOMEM;
263 }
264 req->client_layer = user_layer;
265 req->cmd = CFCTRL_CMD_LINK_SETUP;
266 req->param = *param;
267 cfctrl_insert_req(cfctrl, req);
268 init_info(cfpkt_info(pkt), cfctrl);
269
270
271
272
273
274 cfpkt_info(pkt)->dev_info->id = param->phyid;
275 ret =
276 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
277 if (ret < 0) {
278 pr_err("Could not transmit linksetup request\n");
279 cfpkt_destroy(pkt);
280 return -ENODEV;
281 }
282 return 0;
283}
284
285int cfctrl_linkdown_req(struct cflayer *layer, u8 channelid,
286 struct cflayer *client)
287{
288 int ret;
289 struct cfctrl *cfctrl = container_obj(layer);
290 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
291 if (!pkt) {
292 pr_warn("Out of memory\n");
293 return -ENOMEM;
294 }
295 cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_DESTROY);
296 cfpkt_addbdy(pkt, channelid);
297 init_info(cfpkt_info(pkt), cfctrl);
298 ret =
299 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
300 if (ret < 0) {
301 pr_err("Could not transmit link-down request\n");
302 cfpkt_destroy(pkt);
303 }
304 return ret;
305}
306
307void cfctrl_sleep_req(struct cflayer *layer)
308{
309 int ret;
310 struct cfctrl *cfctrl = container_obj(layer);
311 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
312 if (!pkt) {
313 pr_warn("Out of memory\n");
314 return;
315 }
316 cfpkt_addbdy(pkt, CFCTRL_CMD_SLEEP);
317 init_info(cfpkt_info(pkt), cfctrl);
318 ret =
319 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
320 if (ret < 0)
321 cfpkt_destroy(pkt);
322}
323
324void cfctrl_wake_req(struct cflayer *layer)
325{
326 int ret;
327 struct cfctrl *cfctrl = container_obj(layer);
328 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
329 if (!pkt) {
330 pr_warn("Out of memory\n");
331 return;
332 }
333 cfpkt_addbdy(pkt, CFCTRL_CMD_WAKE);
334 init_info(cfpkt_info(pkt), cfctrl);
335 ret =
336 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
337 if (ret < 0)
338 cfpkt_destroy(pkt);
339}
340
341void cfctrl_getstartreason_req(struct cflayer *layer)
342{
343 int ret;
344 struct cfctrl *cfctrl = container_obj(layer);
345 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
346 if (!pkt) {
347 pr_warn("Out of memory\n");
348 return;
349 }
350 cfpkt_addbdy(pkt, CFCTRL_CMD_START_REASON);
351 init_info(cfpkt_info(pkt), cfctrl);
352 ret =
353 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
354 if (ret < 0)
355 cfpkt_destroy(pkt);
356}
357
358
359void cfctrl_cancel_req(struct cflayer *layr, struct cflayer *adap_layer)
360{
361 struct cfctrl_request_info *p, *tmp;
362 struct cfctrl *ctrl = container_obj(layr);
363 spin_lock(&ctrl->info_list_lock);
364
365 list_for_each_entry_safe(p, tmp, &ctrl->list, list) {
366 if (p->client_layer == adap_layer) {
367 pr_debug("cancel req :%d\n", p->sequence_no);
368 list_del(&p->list);
369 kfree(p);
370 }
371 }
372
373 spin_unlock(&ctrl->info_list_lock);
374}
375
376static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt)
377{
378 u8 cmdrsp;
379 u8 cmd;
380 int ret = -1;
381 u16 tmp16;
382 u8 len;
383 u8 param[255];
384 u8 linkid;
385 struct cfctrl *cfctrl = container_obj(layer);
386 struct cfctrl_request_info rsp, *req;
387
388
389 cfpkt_extr_head(pkt, &cmdrsp, 1);
390 cmd = cmdrsp & CFCTRL_CMD_MASK;
391 if (cmd != CFCTRL_CMD_LINK_ERR
392 && CFCTRL_RSP_BIT != (CFCTRL_RSP_BIT & cmdrsp)) {
393 if (handle_loop(cfctrl, cmd, pkt) != 0)
394 cmdrsp |= CFCTRL_ERR_BIT;
395 }
396
397 switch (cmd) {
398 case CFCTRL_CMD_LINK_SETUP:
399 {
400 enum cfctrl_srv serv;
401 enum cfctrl_srv servtype;
402 u8 endpoint;
403 u8 physlinkid;
404 u8 prio;
405 u8 tmp;
406 u32 tmp32;
407 u8 *cp;
408 int i;
409 struct cfctrl_link_param linkparam;
410 memset(&linkparam, 0, sizeof(linkparam));
411
412 cfpkt_extr_head(pkt, &tmp, 1);
413
414 serv = tmp & CFCTRL_SRV_MASK;
415 linkparam.linktype = serv;
416
417 servtype = tmp >> 4;
418 linkparam.chtype = servtype;
419
420 cfpkt_extr_head(pkt, &tmp, 1);
421 physlinkid = tmp & 0x07;
422 prio = tmp >> 3;
423
424 linkparam.priority = prio;
425 linkparam.phyid = physlinkid;
426 cfpkt_extr_head(pkt, &endpoint, 1);
427 linkparam.endpoint = endpoint & 0x03;
428
429 switch (serv) {
430 case CFCTRL_SRV_VEI:
431 case CFCTRL_SRV_DBG:
432 if (CFCTRL_ERR_BIT & cmdrsp)
433 break;
434
435 cfpkt_extr_head(pkt, &linkid, 1);
436 break;
437 case CFCTRL_SRV_VIDEO:
438 cfpkt_extr_head(pkt, &tmp, 1);
439 linkparam.u.video.connid = tmp;
440 if (CFCTRL_ERR_BIT & cmdrsp)
441 break;
442
443 cfpkt_extr_head(pkt, &linkid, 1);
444 break;
445
446 case CFCTRL_SRV_DATAGRAM:
447 cfpkt_extr_head(pkt, &tmp32, 4);
448 linkparam.u.datagram.connid =
449 le32_to_cpu(tmp32);
450 if (CFCTRL_ERR_BIT & cmdrsp)
451 break;
452
453 cfpkt_extr_head(pkt, &linkid, 1);
454 break;
455 case CFCTRL_SRV_RFM:
456
457
458
459
460 cfpkt_extr_head(pkt, &tmp32, 4);
461 linkparam.u.rfm.connid =
462 le32_to_cpu(tmp32);
463 cp = (u8 *) linkparam.u.rfm.volume;
464 for (cfpkt_extr_head(pkt, &tmp, 1);
465 cfpkt_more(pkt) && tmp != '\0';
466 cfpkt_extr_head(pkt, &tmp, 1))
467 *cp++ = tmp;
468 *cp = '\0';
469
470 if (CFCTRL_ERR_BIT & cmdrsp)
471 break;
472
473 cfpkt_extr_head(pkt, &linkid, 1);
474
475 break;
476 case CFCTRL_SRV_UTIL:
477
478
479
480
481
482 cfpkt_extr_head(pkt, &tmp16, 2);
483 linkparam.u.utility.fifosize_kb =
484 le16_to_cpu(tmp16);
485
486 cfpkt_extr_head(pkt, &tmp16, 2);
487 linkparam.u.utility.fifosize_bufs =
488 le16_to_cpu(tmp16);
489
490 cp = (u8 *) linkparam.u.utility.name;
491 caif_assert(sizeof(linkparam.u.utility.name)
492 >= UTILITY_NAME_LENGTH);
493 for (i = 0;
494 i < UTILITY_NAME_LENGTH
495 && cfpkt_more(pkt); i++) {
496 cfpkt_extr_head(pkt, &tmp, 1);
497 *cp++ = tmp;
498 }
499
500 cfpkt_extr_head(pkt, &len, 1);
501 linkparam.u.utility.paramlen = len;
502
503 cp = linkparam.u.utility.params;
504 while (cfpkt_more(pkt) && len--) {
505 cfpkt_extr_head(pkt, &tmp, 1);
506 *cp++ = tmp;
507 }
508 if (CFCTRL_ERR_BIT & cmdrsp)
509 break;
510
511 cfpkt_extr_head(pkt, &linkid, 1);
512
513 cfpkt_extr_head(pkt, &len, 1);
514
515 cfpkt_extr_head(pkt, ¶m, len);
516 break;
517 default:
518 pr_warn("Request setup - invalid link type (%d)\n",
519 serv);
520 goto error;
521 }
522
523 rsp.cmd = cmd;
524 rsp.param = linkparam;
525 req = cfctrl_remove_req(cfctrl, &rsp);
526
527 if (CFCTRL_ERR_BIT == (CFCTRL_ERR_BIT & cmdrsp) ||
528 cfpkt_erroneous(pkt)) {
529 pr_err("Invalid O/E bit or parse error on CAIF control channel\n");
530 cfctrl->res.reject_rsp(cfctrl->serv.layer.up,
531 0,
532 req ? req->client_layer
533 : NULL);
534 } else {
535 cfctrl->res.linksetup_rsp(cfctrl->serv.
536 layer.up, linkid,
537 serv, physlinkid,
538 req ? req->
539 client_layer : NULL);
540 }
541
542 if (req != NULL)
543 kfree(req);
544 }
545 break;
546 case CFCTRL_CMD_LINK_DESTROY:
547 cfpkt_extr_head(pkt, &linkid, 1);
548 cfctrl->res.linkdestroy_rsp(cfctrl->serv.layer.up, linkid);
549 break;
550 case CFCTRL_CMD_LINK_ERR:
551 pr_err("Frame Error Indication received\n");
552 cfctrl->res.linkerror_ind();
553 break;
554 case CFCTRL_CMD_ENUM:
555 cfctrl->res.enum_rsp();
556 break;
557 case CFCTRL_CMD_SLEEP:
558 cfctrl->res.sleep_rsp();
559 break;
560 case CFCTRL_CMD_WAKE:
561 cfctrl->res.wake_rsp();
562 break;
563 case CFCTRL_CMD_LINK_RECONF:
564 cfctrl->res.restart_rsp();
565 break;
566 case CFCTRL_CMD_RADIO_SET:
567 cfctrl->res.radioset_rsp();
568 break;
569 default:
570 pr_err("Unrecognized Control Frame\n");
571 goto error;
572 break;
573 }
574 ret = 0;
575error:
576 cfpkt_destroy(pkt);
577 return ret;
578}
579
580static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
581 int phyid)
582{
583 struct cfctrl *this = container_obj(layr);
584 switch (ctrl) {
585 case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND:
586 case CAIF_CTRLCMD_FLOW_OFF_IND:
587 spin_lock(&this->info_list_lock);
588 if (!list_empty(&this->list)) {
589 pr_debug("Received flow off in control layer\n");
590 }
591 spin_unlock(&this->info_list_lock);
592 break;
593 default:
594 break;
595 }
596}
597
598#ifndef CAIF_NO_LOOP
599static int handle_loop(struct cfctrl *ctrl, int cmd, struct cfpkt *pkt)
600{
601 static int last_linkid;
602 u8 linkid, linktype, tmp;
603 switch (cmd) {
604 case CFCTRL_CMD_LINK_SETUP:
605 spin_lock(&ctrl->loop_linkid_lock);
606 for (linkid = last_linkid + 1; linkid < 255; linkid++)
607 if (!ctrl->loop_linkused[linkid])
608 goto found;
609 for (linkid = last_linkid - 1; linkid > 0; linkid--)
610 if (!ctrl->loop_linkused[linkid])
611 goto found;
612 spin_unlock(&ctrl->loop_linkid_lock);
613 pr_err("Out of link-ids\n");
614 return -EINVAL;
615found:
616 if (!ctrl->loop_linkused[linkid])
617 ctrl->loop_linkused[linkid] = 1;
618
619 last_linkid = linkid;
620
621 cfpkt_add_trail(pkt, &linkid, 1);
622 spin_unlock(&ctrl->loop_linkid_lock);
623 cfpkt_peek_head(pkt, &linktype, 1);
624 if (linktype == CFCTRL_SRV_UTIL) {
625 tmp = 0x01;
626 cfpkt_add_trail(pkt, &tmp, 1);
627 cfpkt_add_trail(pkt, &tmp, 1);
628 }
629 break;
630
631 case CFCTRL_CMD_LINK_DESTROY:
632 spin_lock(&ctrl->loop_linkid_lock);
633 cfpkt_peek_head(pkt, &linkid, 1);
634 ctrl->loop_linkused[linkid] = 0;
635 spin_unlock(&ctrl->loop_linkid_lock);
636 break;
637 default:
638 break;
639 }
640 return 0;
641}
642#endif
643
