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