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