blob: 563145fdc4c368de2c50c8d9d3510742a7d2e658 [file] [log] [blame]
Sjur Braendelandb482cd22010-03-30 13:56:23 +00001/*
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){
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +000022 return -1;
Sjur Braendelandb482cd22010-03-30 13:56:23 +000023}
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{
Sjur Braendeland8d545c82010-04-28 08:54:37 +000035 struct dev_info dev_info;
Sjur Braendelandb482cd22010-03-30 13:56:23 +000036 struct cfctrl *this =
37 kmalloc(sizeof(struct cfctrl), GFP_ATOMIC);
38 if (!this) {
39 pr_warning("CAIF: %s(): Out of memory\n", __func__);
40 return NULL;
41 }
42 caif_assert(offsetof(struct cfctrl, serv.layer) == 0);
Sjur Braendeland8d545c82010-04-28 08:54:37 +000043 memset(&dev_info, 0, sizeof(dev_info));
44 dev_info.id = 0xff;
Sjur Braendelandb482cd22010-03-30 13:56:23 +000045 memset(this, 0, sizeof(*this));
Sjur Braendelandb1c74242010-06-17 06:55:38 +000046 cfsrvl_init(&this->serv, 0, &dev_info, false);
Sjur Braendelandb482cd22010-03-30 13:56:23 +000047 atomic_set(&this->req_seq_no, 1);
48 atomic_set(&this->rsp_seq_no, 1);
Sjur Braendelandb482cd22010-03-30 13:56:23 +000049 this->serv.layer.receive = cfctrl_recv;
50 sprintf(this->serv.layer.name, "ctrl");
51 this->serv.layer.ctrlcmd = cfctrl_ctrlcmd;
52 spin_lock_init(&this->loop_linkid_lock);
Sjur Braendeland7aecf492010-05-21 02:16:08 +000053 spin_lock_init(&this->info_list_lock);
54 INIT_LIST_HEAD(&this->list);
Sjur Braendelandb482cd22010-03-30 13:56:23 +000055 this->loop_linkid = 1;
56 return &this->serv.layer;
57}
58
59static bool param_eq(struct cfctrl_link_param *p1, struct cfctrl_link_param *p2)
60{
61 bool eq =
62 p1->linktype == p2->linktype &&
63 p1->priority == p2->priority &&
64 p1->phyid == p2->phyid &&
65 p1->endpoint == p2->endpoint && p1->chtype == p2->chtype;
66
67 if (!eq)
68 return false;
69
70 switch (p1->linktype) {
71 case CFCTRL_SRV_VEI:
72 return true;
73 case CFCTRL_SRV_DATAGRAM:
74 return p1->u.datagram.connid == p2->u.datagram.connid;
75 case CFCTRL_SRV_RFM:
76 return
77 p1->u.rfm.connid == p2->u.rfm.connid &&
78 strcmp(p1->u.rfm.volume, p2->u.rfm.volume) == 0;
79 case CFCTRL_SRV_UTIL:
80 return
81 p1->u.utility.fifosize_kb == p2->u.utility.fifosize_kb
82 && p1->u.utility.fifosize_bufs ==
83 p2->u.utility.fifosize_bufs
84 && strcmp(p1->u.utility.name, p2->u.utility.name) == 0
85 && p1->u.utility.paramlen == p2->u.utility.paramlen
86 && memcmp(p1->u.utility.params, p2->u.utility.params,
87 p1->u.utility.paramlen) == 0;
88
89 case CFCTRL_SRV_VIDEO:
90 return p1->u.video.connid == p2->u.video.connid;
91 case CFCTRL_SRV_DBG:
92 return true;
93 case CFCTRL_SRV_DECM:
94 return false;
95 default:
96 return false;
97 }
98 return false;
99}
100
101bool cfctrl_req_eq(struct cfctrl_request_info *r1,
102 struct cfctrl_request_info *r2)
103{
104 if (r1->cmd != r2->cmd)
105 return false;
106 if (r1->cmd == CFCTRL_CMD_LINK_SETUP)
107 return param_eq(&r1->param, &r2->param);
108 else
109 return r1->channel_id == r2->channel_id;
110}
111
112/* Insert request at the end */
113void cfctrl_insert_req(struct cfctrl *ctrl,
114 struct cfctrl_request_info *req)
115{
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000116 spin_lock(&ctrl->info_list_lock);
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000117 atomic_inc(&ctrl->req_seq_no);
118 req->sequence_no = atomic_read(&ctrl->req_seq_no);
Sjur Braendeland7aecf492010-05-21 02:16:08 +0000119 list_add_tail(&req->list, &ctrl->list);
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000120 spin_unlock(&ctrl->info_list_lock);
121}
122
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000123/* Compare and remove request */
124struct cfctrl_request_info *cfctrl_remove_req(struct cfctrl *ctrl,
125 struct cfctrl_request_info *req)
126{
Sjur Braendeland7aecf492010-05-21 02:16:08 +0000127 struct cfctrl_request_info *p, *tmp, *first;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000128
129 spin_lock(&ctrl->info_list_lock);
Sjur Braendeland7aecf492010-05-21 02:16:08 +0000130 first = list_first_entry(&ctrl->list, struct cfctrl_request_info, list);
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000131
Sjur Braendeland7aecf492010-05-21 02:16:08 +0000132 list_for_each_entry_safe(p, tmp, &ctrl->list, list) {
133 if (cfctrl_req_eq(req, p)) {
134 if (p != first)
135 pr_warning("CAIF: %s(): Requests are not "
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000136 "received in order\n",
137 __func__);
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000138
Sjur Braendeland7aecf492010-05-21 02:16:08 +0000139 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;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000149}
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_warning("CAIF: %s(): Out of memory\n", __func__);
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("CAIF: %s(): Could not transmit enum message\n",
193 __func__);
194 cfpkt_destroy(pkt);
195 }
196}
197
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000198int cfctrl_linkup_request(struct cflayer *layer,
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000199 struct cfctrl_link_param *param,
200 struct cflayer *user_layer)
201{
202 struct cfctrl *cfctrl = container_obj(layer);
203 u32 tmp32;
204 u16 tmp16;
205 u8 tmp8;
206 struct cfctrl_request_info *req;
207 int ret;
208 char utility_name[16];
209 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
210 if (!pkt) {
211 pr_warning("CAIF: %s(): Out of memory\n", __func__);
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000212 return -ENOMEM;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000213 }
214 cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_SETUP);
215 cfpkt_addbdy(pkt, (param->chtype << 4) + param->linktype);
216 cfpkt_addbdy(pkt, (param->priority << 3) + param->phyid);
217 cfpkt_addbdy(pkt, param->endpoint & 0x03);
218
219 switch (param->linktype) {
220 case CFCTRL_SRV_VEI:
221 break;
222 case CFCTRL_SRV_VIDEO:
223 cfpkt_addbdy(pkt, (u8) param->u.video.connid);
224 break;
225 case CFCTRL_SRV_DBG:
226 break;
227 case CFCTRL_SRV_DATAGRAM:
228 tmp32 = cpu_to_le32(param->u.datagram.connid);
229 cfpkt_add_body(pkt, &tmp32, 4);
230 break;
231 case CFCTRL_SRV_RFM:
232 /* Construct a frame, convert DatagramConnectionID to network
233 * format long and copy it out...
234 */
235 tmp32 = cpu_to_le32(param->u.rfm.connid);
236 cfpkt_add_body(pkt, &tmp32, 4);
237 /* Add volume name, including zero termination... */
238 cfpkt_add_body(pkt, param->u.rfm.volume,
239 strlen(param->u.rfm.volume) + 1);
240 break;
241 case CFCTRL_SRV_UTIL:
242 tmp16 = cpu_to_le16(param->u.utility.fifosize_kb);
243 cfpkt_add_body(pkt, &tmp16, 2);
244 tmp16 = cpu_to_le16(param->u.utility.fifosize_bufs);
245 cfpkt_add_body(pkt, &tmp16, 2);
246 memset(utility_name, 0, sizeof(utility_name));
247 strncpy(utility_name, param->u.utility.name,
248 UTILITY_NAME_LENGTH - 1);
249 cfpkt_add_body(pkt, utility_name, UTILITY_NAME_LENGTH);
250 tmp8 = param->u.utility.paramlen;
251 cfpkt_add_body(pkt, &tmp8, 1);
252 cfpkt_add_body(pkt, param->u.utility.params,
253 param->u.utility.paramlen);
254 break;
255 default:
256 pr_warning("CAIF: %s():Request setup of bad link type = %d\n",
257 __func__, param->linktype);
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000258 return -EINVAL;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000259 }
Julia Lawall49afa552010-05-13 10:03:32 +0000260 req = kzalloc(sizeof(*req), GFP_KERNEL);
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000261 if (!req) {
262 pr_warning("CAIF: %s(): Out of memory\n", __func__);
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000263 return -ENOMEM;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000264 }
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000265 req->client_layer = user_layer;
266 req->cmd = CFCTRL_CMD_LINK_SETUP;
267 req->param = *param;
268 cfctrl_insert_req(cfctrl, req);
269 init_info(cfpkt_info(pkt), cfctrl);
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000270 /*
271 * NOTE:Always send linkup and linkdown request on the same
272 * device as the payload. Otherwise old queued up payload
273 * might arrive with the newly allocated channel ID.
274 */
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000275 cfpkt_info(pkt)->dev_info->id = param->phyid;
276 ret =
277 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
278 if (ret < 0) {
279 pr_err("CAIF: %s(): Could not transmit linksetup request\n",
280 __func__);
281 cfpkt_destroy(pkt);
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000282 return -ENODEV;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000283 }
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000284 return 0;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000285}
286
287int cfctrl_linkdown_req(struct cflayer *layer, u8 channelid,
288 struct cflayer *client)
289{
290 int ret;
291 struct cfctrl *cfctrl = container_obj(layer);
292 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
293 if (!pkt) {
294 pr_warning("CAIF: %s(): Out of memory\n", __func__);
295 return -ENOMEM;
296 }
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000297 cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_DESTROY);
298 cfpkt_addbdy(pkt, channelid);
299 init_info(cfpkt_info(pkt), cfctrl);
300 ret =
301 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
302 if (ret < 0) {
303 pr_err("CAIF: %s(): Could not transmit link-down request\n",
304 __func__);
305 cfpkt_destroy(pkt);
306 }
307 return ret;
308}
309
310void cfctrl_sleep_req(struct cflayer *layer)
311{
312 int ret;
313 struct cfctrl *cfctrl = container_obj(layer);
314 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
315 if (!pkt) {
316 pr_warning("CAIF: %s(): Out of memory\n", __func__);
317 return;
318 }
319 cfpkt_addbdy(pkt, CFCTRL_CMD_SLEEP);
320 init_info(cfpkt_info(pkt), cfctrl);
321 ret =
322 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
323 if (ret < 0)
324 cfpkt_destroy(pkt);
325}
326
327void cfctrl_wake_req(struct cflayer *layer)
328{
329 int ret;
330 struct cfctrl *cfctrl = container_obj(layer);
331 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
332 if (!pkt) {
333 pr_warning("CAIF: %s(): Out of memory\n", __func__);
334 return;
335 }
336 cfpkt_addbdy(pkt, CFCTRL_CMD_WAKE);
337 init_info(cfpkt_info(pkt), cfctrl);
338 ret =
339 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
340 if (ret < 0)
341 cfpkt_destroy(pkt);
342}
343
344void cfctrl_getstartreason_req(struct cflayer *layer)
345{
346 int ret;
347 struct cfctrl *cfctrl = container_obj(layer);
348 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN);
349 if (!pkt) {
350 pr_warning("CAIF: %s(): Out of memory\n", __func__);
351 return;
352 }
353 cfpkt_addbdy(pkt, CFCTRL_CMD_START_REASON);
354 init_info(cfpkt_info(pkt), cfctrl);
355 ret =
356 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt);
357 if (ret < 0)
358 cfpkt_destroy(pkt);
359}
360
361
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000362void cfctrl_cancel_req(struct cflayer *layr, struct cflayer *adap_layer)
363{
Sjur Braendeland7aecf492010-05-21 02:16:08 +0000364 struct cfctrl_request_info *p, *tmp;
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000365 struct cfctrl *ctrl = container_obj(layr);
366 spin_lock(&ctrl->info_list_lock);
Sjur Braendeland7aecf492010-05-21 02:16:08 +0000367 pr_warning("CAIF: %s(): enter\n", __func__);
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000368
Sjur Braendeland7aecf492010-05-21 02:16:08 +0000369 list_for_each_entry_safe(p, tmp, &ctrl->list, list) {
370 if (p->client_layer == adap_layer) {
371 pr_warning("CAIF: %s(): cancel req :%d\n", __func__,
372 p->sequence_no);
373 list_del(&p->list);
374 kfree(p);
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000375 }
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000376 }
377
378 spin_unlock(&ctrl->info_list_lock);
379}
380
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000381static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt)
382{
383 u8 cmdrsp;
384 u8 cmd;
385 int ret = -1;
386 u16 tmp16;
387 u8 len;
388 u8 param[255];
389 u8 linkid;
390 struct cfctrl *cfctrl = container_obj(layer);
391 struct cfctrl_request_info rsp, *req;
392
393
394 cfpkt_extr_head(pkt, &cmdrsp, 1);
395 cmd = cmdrsp & CFCTRL_CMD_MASK;
396 if (cmd != CFCTRL_CMD_LINK_ERR
397 && CFCTRL_RSP_BIT != (CFCTRL_RSP_BIT & cmdrsp)) {
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000398 if (handle_loop(cfctrl, cmd, pkt) != 0)
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000399 cmdrsp |= CFCTRL_ERR_BIT;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000400 }
401
402 switch (cmd) {
403 case CFCTRL_CMD_LINK_SETUP:
404 {
405 enum cfctrl_srv serv;
406 enum cfctrl_srv servtype;
407 u8 endpoint;
408 u8 physlinkid;
409 u8 prio;
410 u8 tmp;
411 u32 tmp32;
412 u8 *cp;
413 int i;
414 struct cfctrl_link_param linkparam;
415 memset(&linkparam, 0, sizeof(linkparam));
416
417 cfpkt_extr_head(pkt, &tmp, 1);
418
419 serv = tmp & CFCTRL_SRV_MASK;
420 linkparam.linktype = serv;
421
422 servtype = tmp >> 4;
423 linkparam.chtype = servtype;
424
425 cfpkt_extr_head(pkt, &tmp, 1);
426 physlinkid = tmp & 0x07;
427 prio = tmp >> 3;
428
429 linkparam.priority = prio;
430 linkparam.phyid = physlinkid;
431 cfpkt_extr_head(pkt, &endpoint, 1);
432 linkparam.endpoint = endpoint & 0x03;
433
434 switch (serv) {
435 case CFCTRL_SRV_VEI:
436 case CFCTRL_SRV_DBG:
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000437 if (CFCTRL_ERR_BIT & cmdrsp)
438 break;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000439 /* Link ID */
440 cfpkt_extr_head(pkt, &linkid, 1);
441 break;
442 case CFCTRL_SRV_VIDEO:
443 cfpkt_extr_head(pkt, &tmp, 1);
444 linkparam.u.video.connid = tmp;
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000445 if (CFCTRL_ERR_BIT & cmdrsp)
446 break;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000447 /* Link ID */
448 cfpkt_extr_head(pkt, &linkid, 1);
449 break;
450
451 case CFCTRL_SRV_DATAGRAM:
452 cfpkt_extr_head(pkt, &tmp32, 4);
453 linkparam.u.datagram.connid =
454 le32_to_cpu(tmp32);
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000455 if (CFCTRL_ERR_BIT & cmdrsp)
456 break;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000457 /* Link ID */
458 cfpkt_extr_head(pkt, &linkid, 1);
459 break;
460 case CFCTRL_SRV_RFM:
461 /* Construct a frame, convert
462 * DatagramConnectionID
463 * to network format long and copy it out...
464 */
465 cfpkt_extr_head(pkt, &tmp32, 4);
466 linkparam.u.rfm.connid =
467 le32_to_cpu(tmp32);
468 cp = (u8 *) linkparam.u.rfm.volume;
469 for (cfpkt_extr_head(pkt, &tmp, 1);
470 cfpkt_more(pkt) && tmp != '\0';
471 cfpkt_extr_head(pkt, &tmp, 1))
472 *cp++ = tmp;
473 *cp = '\0';
474
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000475 if (CFCTRL_ERR_BIT & cmdrsp)
476 break;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000477 /* Link ID */
478 cfpkt_extr_head(pkt, &linkid, 1);
479
480 break;
481 case CFCTRL_SRV_UTIL:
482 /* Construct a frame, convert
483 * DatagramConnectionID
484 * to network format long and copy it out...
485 */
486 /* Fifosize KB */
487 cfpkt_extr_head(pkt, &tmp16, 2);
488 linkparam.u.utility.fifosize_kb =
489 le16_to_cpu(tmp16);
490 /* Fifosize bufs */
491 cfpkt_extr_head(pkt, &tmp16, 2);
492 linkparam.u.utility.fifosize_bufs =
493 le16_to_cpu(tmp16);
494 /* name */
495 cp = (u8 *) linkparam.u.utility.name;
496 caif_assert(sizeof(linkparam.u.utility.name)
497 >= UTILITY_NAME_LENGTH);
498 for (i = 0;
499 i < UTILITY_NAME_LENGTH
500 && cfpkt_more(pkt); i++) {
501 cfpkt_extr_head(pkt, &tmp, 1);
502 *cp++ = tmp;
503 }
504 /* Length */
505 cfpkt_extr_head(pkt, &len, 1);
506 linkparam.u.utility.paramlen = len;
507 /* Param Data */
508 cp = linkparam.u.utility.params;
509 while (cfpkt_more(pkt) && len--) {
510 cfpkt_extr_head(pkt, &tmp, 1);
511 *cp++ = tmp;
512 }
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000513 if (CFCTRL_ERR_BIT & cmdrsp)
514 break;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000515 /* Link ID */
516 cfpkt_extr_head(pkt, &linkid, 1);
517 /* Length */
518 cfpkt_extr_head(pkt, &len, 1);
519 /* Param Data */
520 cfpkt_extr_head(pkt, &param, len);
521 break;
522 default:
523 pr_warning("CAIF: %s(): Request setup "
524 "- invalid link type (%d)",
525 __func__, serv);
526 goto error;
527 }
528
529 rsp.cmd = cmd;
530 rsp.param = linkparam;
531 req = cfctrl_remove_req(cfctrl, &rsp);
532
533 if (CFCTRL_ERR_BIT == (CFCTRL_ERR_BIT & cmdrsp) ||
534 cfpkt_erroneous(pkt)) {
535 pr_err("CAIF: %s(): Invalid O/E bit or parse "
536 "error on CAIF control channel",
537 __func__);
538 cfctrl->res.reject_rsp(cfctrl->serv.layer.up,
539 0,
540 req ? req->client_layer
541 : NULL);
542 } else {
543 cfctrl->res.linksetup_rsp(cfctrl->serv.
544 layer.up, linkid,
545 serv, physlinkid,
546 req ? req->
547 client_layer : NULL);
548 }
549
550 if (req != NULL)
551 kfree(req);
552 }
553 break;
554 case CFCTRL_CMD_LINK_DESTROY:
555 cfpkt_extr_head(pkt, &linkid, 1);
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000556 cfctrl->res.linkdestroy_rsp(cfctrl->serv.layer.up, linkid);
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000557 break;
558 case CFCTRL_CMD_LINK_ERR:
559 pr_err("CAIF: %s(): Frame Error Indication received\n",
560 __func__);
561 cfctrl->res.linkerror_ind();
562 break;
563 case CFCTRL_CMD_ENUM:
564 cfctrl->res.enum_rsp();
565 break;
566 case CFCTRL_CMD_SLEEP:
567 cfctrl->res.sleep_rsp();
568 break;
569 case CFCTRL_CMD_WAKE:
570 cfctrl->res.wake_rsp();
571 break;
572 case CFCTRL_CMD_LINK_RECONF:
573 cfctrl->res.restart_rsp();
574 break;
575 case CFCTRL_CMD_RADIO_SET:
576 cfctrl->res.radioset_rsp();
577 break;
578 default:
579 pr_err("CAIF: %s(): Unrecognized Control Frame\n", __func__);
580 goto error;
581 break;
582 }
583 ret = 0;
584error:
585 cfpkt_destroy(pkt);
586 return ret;
587}
588
589static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
590 int phyid)
591{
592 struct cfctrl *this = container_obj(layr);
593 switch (ctrl) {
594 case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND:
595 case CAIF_CTRLCMD_FLOW_OFF_IND:
596 spin_lock(&this->info_list_lock);
Sjur Braendeland7aecf492010-05-21 02:16:08 +0000597 if (!list_empty(&this->list)) {
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000598 pr_debug("CAIF: %s(): Received flow off in "
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000599 "control layer", __func__);
600 }
601 spin_unlock(&this->info_list_lock);
602 break;
603 default:
604 break;
605 }
606}
607
608#ifndef CAIF_NO_LOOP
609static int handle_loop(struct cfctrl *ctrl, int cmd, struct cfpkt *pkt)
610{
611 static int last_linkid;
612 u8 linkid, linktype, tmp;
613 switch (cmd) {
614 case CFCTRL_CMD_LINK_SETUP:
615 spin_lock(&ctrl->loop_linkid_lock);
616 for (linkid = last_linkid + 1; linkid < 255; linkid++)
617 if (!ctrl->loop_linkused[linkid])
618 goto found;
619 for (linkid = last_linkid - 1; linkid > 0; linkid--)
620 if (!ctrl->loop_linkused[linkid])
621 goto found;
622 spin_unlock(&ctrl->loop_linkid_lock);
Sjur Braendeland8d545c82010-04-28 08:54:37 +0000623 pr_err("CAIF: %s(): Out of link-ids\n", __func__);
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000624 return -EINVAL;
625found:
626 if (!ctrl->loop_linkused[linkid])
627 ctrl->loop_linkused[linkid] = 1;
628
629 last_linkid = linkid;
630
631 cfpkt_add_trail(pkt, &linkid, 1);
632 spin_unlock(&ctrl->loop_linkid_lock);
633 cfpkt_peek_head(pkt, &linktype, 1);
634 if (linktype == CFCTRL_SRV_UTIL) {
635 tmp = 0x01;
636 cfpkt_add_trail(pkt, &tmp, 1);
637 cfpkt_add_trail(pkt, &tmp, 1);
638 }
639 break;
640
641 case CFCTRL_CMD_LINK_DESTROY:
642 spin_lock(&ctrl->loop_linkid_lock);
643 cfpkt_peek_head(pkt, &linkid, 1);
644 ctrl->loop_linkused[linkid] = 0;
645 spin_unlock(&ctrl->loop_linkid_lock);
646 break;
647 default:
648 break;
649 }
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +0000650 return 0;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000651}
652#endif