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/kernel.h> 10 #include <linux/stddef.h> 11 #include <linux/slab.h> 12 #include <linux/netdevice.h> 13 #include <linux/module.h> 14 #include <net/caif/caif_layer.h> 15 #include <net/caif/cfpkt.h> 16 #include <net/caif/cfcnfg.h> 17 #include <net/caif/cfctrl.h> 18 #include <net/caif/cfmuxl.h> 19 #include <net/caif/cffrml.h> 20 #include <net/caif/cfserl.h> 21 #include <net/caif/cfsrvl.h> 22 #include <net/caif/caif_dev.h> 23 24 #define container_obj(layr) container_of(layr, struct cfcnfg, layer) 25 26 /* Information about CAIF physical interfaces held by Config Module in order 27 * to manage physical interfaces 28 */ 29 struct cfcnfg_phyinfo { 30 struct list_head node; 31 bool up; 32 33 /* Pointer to the layer below the MUX (framing layer) */ 34 struct cflayer *frm_layer; 35 /* Pointer to the lowest actual physical layer */ 36 struct cflayer *phy_layer; 37 /* Unique identifier of the physical interface */ 38 unsigned int id; 39 /* Preference of the physical in interface */ 40 enum cfcnfg_phy_preference pref; 41 42 /* Information about the physical device */ 43 struct dev_info dev_info; 44 45 /* Interface index */ 46 int ifindex; 47 48 /* Use Start of frame extension */ 49 bool use_stx; 50 51 /* Use Start of frame checksum */ 52 bool use_fcs; 53 }; 54 55 struct cfcnfg { 56 struct cflayer layer; 57 struct cflayer *ctrl; 58 struct cflayer *mux; 59 struct list_head phys; 60 struct mutex lock; 61 }; 62 63 static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, 64 enum cfctrl_srv serv, u8 phyid, 65 struct cflayer *adapt_layer); 66 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id); 67 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id, 68 struct cflayer *adapt_layer); 69 static void cfctrl_resp_func(void); 70 static void cfctrl_enum_resp(void); 71 72 struct cfcnfg *cfcnfg_create(void) 73 { 74 struct cfcnfg *this; 75 struct cfctrl_rsp *resp; 76 77 might_sleep(); 78 79 /* Initiate this layer */ 80 this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC); 81 if (!this) { 82 pr_warn("Out of memory\n"); 83 return NULL; 84 } 85 this->mux = cfmuxl_create(); 86 if (!this->mux) 87 goto out_of_mem; 88 this->ctrl = cfctrl_create(); 89 if (!this->ctrl) 90 goto out_of_mem; 91 /* Initiate response functions */ 92 resp = cfctrl_get_respfuncs(this->ctrl); 93 resp->enum_rsp = cfctrl_enum_resp; 94 resp->linkerror_ind = cfctrl_resp_func; 95 resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp; 96 resp->sleep_rsp = cfctrl_resp_func; 97 resp->wake_rsp = cfctrl_resp_func; 98 resp->restart_rsp = cfctrl_resp_func; 99 resp->radioset_rsp = cfctrl_resp_func; 100 resp->linksetup_rsp = cfcnfg_linkup_rsp; 101 resp->reject_rsp = cfcnfg_reject_rsp; 102 INIT_LIST_HEAD(&this->phys); 103 104 cfmuxl_set_uplayer(this->mux, this->ctrl, 0); 105 layer_set_dn(this->ctrl, this->mux); 106 layer_set_up(this->ctrl, this); 107 mutex_init(&this->lock); 108 109 return this; 110 out_of_mem: 111 pr_warn("Out of memory\n"); 112 113 synchronize_rcu(); 114 115 kfree(this->mux); 116 kfree(this->ctrl); 117 kfree(this); 118 return NULL; 119 } 120 121 void cfcnfg_remove(struct cfcnfg *cfg) 122 { 123 might_sleep(); 124 if (cfg) { 125 synchronize_rcu(); 126 127 kfree(cfg->mux); 128 cfctrl_remove(cfg->ctrl); 129 kfree(cfg); 130 } 131 } 132 133 static void cfctrl_resp_func(void) 134 { 135 } 136 137 static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo_rcu(struct cfcnfg *cnfg, 138 u8 phyid) 139 { 140 struct cfcnfg_phyinfo *phy; 141 142 list_for_each_entry_rcu(phy, &cnfg->phys, node) 143 if (phy->id == phyid) 144 return phy; 145 return NULL; 146 } 147 148 static void cfctrl_enum_resp(void) 149 { 150 } 151 152 static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg, 153 enum cfcnfg_phy_preference phy_pref) 154 { 155 /* Try to match with specified preference */ 156 struct cfcnfg_phyinfo *phy; 157 158 list_for_each_entry_rcu(phy, &cnfg->phys, node) { 159 if (phy->up && phy->pref == phy_pref && 160 phy->frm_layer != NULL) 161 162 return &phy->dev_info; 163 } 164 165 /* Otherwise just return something */ 166 list_for_each_entry_rcu(phy, &cnfg->phys, node) 167 if (phy->up) 168 return &phy->dev_info; 169 170 return NULL; 171 } 172 173 static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi) 174 { 175 struct cfcnfg_phyinfo *phy; 176 177 list_for_each_entry_rcu(phy, &cnfg->phys, node) 178 if (phy->ifindex == ifi && phy->up) 179 return phy->id; 180 return -ENODEV; 181 } 182 183 int caif_disconnect_client(struct net *net, struct cflayer *adap_layer) 184 { 185 u8 channel_id = 0; 186 int ret = 0; 187 struct cflayer *servl = NULL; 188 struct cfcnfg *cfg = get_cfcnfg(net); 189 190 caif_assert(adap_layer != NULL); 191 192 channel_id = adap_layer->id; 193 if (adap_layer->dn == NULL || channel_id == 0) { 194 pr_err("adap_layer->dn == NULL or adap_layer->id is 0\n"); 195 ret = -ENOTCONN; 196 goto end; 197 } 198 199 servl = cfmuxl_remove_uplayer(cfg->mux, channel_id); 200 if (servl == NULL) { 201 pr_err("PROTOCOL ERROR - " 202 "Error removing service_layer Channel_Id(%d)", 203 channel_id); 204 ret = -EINVAL; 205 goto end; 206 } 207 208 ret = cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer); 209 210 end: 211 cfctrl_cancel_req(cfg->ctrl, adap_layer); 212 213 /* Do RCU sync before initiating cleanup */ 214 synchronize_rcu(); 215 if (adap_layer->ctrlcmd != NULL) 216 adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0); 217 return ret; 218 219 } 220 EXPORT_SYMBOL(caif_disconnect_client); 221 222 static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id) 223 { 224 } 225 226 static const int protohead[CFCTRL_SRV_MASK] = { 227 [CFCTRL_SRV_VEI] = 4, 228 [CFCTRL_SRV_DATAGRAM] = 7, 229 [CFCTRL_SRV_UTIL] = 4, 230 [CFCTRL_SRV_RFM] = 3, 231 [CFCTRL_SRV_DBG] = 3, 232 }; 233 234 235 static int caif_connect_req_to_link_param(struct cfcnfg *cnfg, 236 struct caif_connect_request *s, 237 struct cfctrl_link_param *l) 238 { 239 struct dev_info *dev_info; 240 enum cfcnfg_phy_preference pref; 241 int res; 242 243 memset(l, 0, sizeof(*l)); 244 /* In caif protocol low value is high priority */ 245 l->priority = CAIF_PRIO_MAX - s->priority + 1; 246 247 if (s->ifindex != 0) { 248 res = cfcnfg_get_id_from_ifi(cnfg, s->ifindex); 249 if (res < 0) 250 return res; 251 l->phyid = res; 252 } else { 253 switch (s->link_selector) { 254 case CAIF_LINK_HIGH_BANDW: 255 pref = CFPHYPREF_HIGH_BW; 256 break; 257 case CAIF_LINK_LOW_LATENCY: 258 pref = CFPHYPREF_LOW_LAT; 259 break; 260 default: 261 return -EINVAL; 262 } 263 dev_info = cfcnfg_get_phyid(cnfg, pref); 264 if (dev_info == NULL) 265 return -ENODEV; 266 l->phyid = dev_info->id; 267 } 268 switch (s->protocol) { 269 case CAIFPROTO_AT: 270 l->linktype = CFCTRL_SRV_VEI; 271 l->endpoint = (s->sockaddr.u.at.type >> 2) & 0x3; 272 l->chtype = s->sockaddr.u.at.type & 0x3; 273 break; 274 case CAIFPROTO_DATAGRAM: 275 l->linktype = CFCTRL_SRV_DATAGRAM; 276 l->chtype = 0x00; 277 l->u.datagram.connid = s->sockaddr.u.dgm.connection_id; 278 break; 279 case CAIFPROTO_DATAGRAM_LOOP: 280 l->linktype = CFCTRL_SRV_DATAGRAM; 281 l->chtype = 0x03; 282 l->endpoint = 0x00; 283 l->u.datagram.connid = s->sockaddr.u.dgm.connection_id; 284 break; 285 case CAIFPROTO_RFM: 286 l->linktype = CFCTRL_SRV_RFM; 287 l->u.datagram.connid = s->sockaddr.u.rfm.connection_id; 288 strncpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume, 289 sizeof(l->u.rfm.volume)-1); 290 l->u.rfm.volume[sizeof(l->u.rfm.volume)-1] = 0; 291 break; 292 case CAIFPROTO_UTIL: 293 l->linktype = CFCTRL_SRV_UTIL; 294 l->endpoint = 0x00; 295 l->chtype = 0x00; 296 strncpy(l->u.utility.name, s->sockaddr.u.util.service, 297 sizeof(l->u.utility.name)-1); 298 l->u.utility.name[sizeof(l->u.utility.name)-1] = 0; 299 caif_assert(sizeof(l->u.utility.name) > 10); 300 l->u.utility.paramlen = s->param.size; 301 if (l->u.utility.paramlen > sizeof(l->u.utility.params)) 302 l->u.utility.paramlen = sizeof(l->u.utility.params); 303 304 memcpy(l->u.utility.params, s->param.data, 305 l->u.utility.paramlen); 306 307 break; 308 case CAIFPROTO_DEBUG: 309 l->linktype = CFCTRL_SRV_DBG; 310 l->endpoint = s->sockaddr.u.dbg.service; 311 l->chtype = s->sockaddr.u.dbg.type; 312 break; 313 default: 314 return -EINVAL; 315 } 316 return 0; 317 } 318 319 int caif_connect_client(struct net *net, struct caif_connect_request *conn_req, 320 struct cflayer *adap_layer, int *ifindex, 321 int *proto_head, 322 int *proto_tail) 323 { 324 struct cflayer *frml; 325 struct cfcnfg_phyinfo *phy; 326 int err; 327 struct cfctrl_link_param param; 328 struct cfcnfg *cfg = get_cfcnfg(net); 329 caif_assert(cfg != NULL); 330 331 rcu_read_lock(); 332 err = caif_connect_req_to_link_param(cfg, conn_req, ¶m); 333 if (err) 334 goto unlock; 335 336 phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid); 337 if (!phy) { 338 err = -ENODEV; 339 goto unlock; 340 } 341 err = -EINVAL; 342 343 if (adap_layer == NULL) { 344 pr_err("adap_layer is zero\n"); 345 goto unlock; 346 } 347 if (adap_layer->receive == NULL) { 348 pr_err("adap_layer->receive is NULL\n"); 349 goto unlock; 350 } 351 if (adap_layer->ctrlcmd == NULL) { 352 pr_err("adap_layer->ctrlcmd == NULL\n"); 353 goto unlock; 354 } 355 356 err = -ENODEV; 357 frml = phy->frm_layer; 358 if (frml == NULL) { 359 pr_err("Specified PHY type does not exist!\n"); 360 goto unlock; 361 } 362 caif_assert(param.phyid == phy->id); 363 caif_assert(phy->frm_layer->id == 364 param.phyid); 365 caif_assert(phy->phy_layer->id == 366 param.phyid); 367 368 *ifindex = phy->ifindex; 369 *proto_tail = 2; 370 *proto_head = 371 372 protohead[param.linktype] + (phy->use_stx ? 1 : 0); 373 374 rcu_read_unlock(); 375 376 /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */ 377 cfctrl_enum_req(cfg->ctrl, param.phyid); 378 return cfctrl_linkup_request(cfg->ctrl, ¶m, adap_layer); 379 380 unlock: 381 rcu_read_unlock(); 382 return err; 383 } 384 EXPORT_SYMBOL(caif_connect_client); 385 386 static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id, 387 struct cflayer *adapt_layer) 388 { 389 if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL) 390 adapt_layer->ctrlcmd(adapt_layer, 391 CAIF_CTRLCMD_INIT_FAIL_RSP, 0); 392 } 393 394 static void 395 cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv, 396 u8 phyid, struct cflayer *adapt_layer) 397 { 398 struct cfcnfg *cnfg = container_obj(layer); 399 struct cflayer *servicel = NULL; 400 struct cfcnfg_phyinfo *phyinfo; 401 struct net_device *netdev; 402 403 rcu_read_lock(); 404 405 if (adapt_layer == NULL) { 406 pr_debug("link setup response but no client exist," 407 "send linkdown back\n"); 408 cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL); 409 goto unlock; 410 } 411 412 caif_assert(cnfg != NULL); 413 caif_assert(phyid != 0); 414 415 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid); 416 if (phyinfo == NULL) { 417 pr_err("ERROR: Link Layer Device dissapeared" 418 "while connecting\n"); 419 goto unlock; 420 } 421 422 caif_assert(phyinfo != NULL); 423 caif_assert(phyinfo->id == phyid); 424 caif_assert(phyinfo->phy_layer != NULL); 425 caif_assert(phyinfo->phy_layer->id == phyid); 426 427 adapt_layer->id = channel_id; 428 429 switch (serv) { 430 case CFCTRL_SRV_VEI: 431 servicel = cfvei_create(channel_id, &phyinfo->dev_info); 432 break; 433 case CFCTRL_SRV_DATAGRAM: 434 servicel = cfdgml_create(channel_id, 435 &phyinfo->dev_info); 436 break; 437 case CFCTRL_SRV_RFM: 438 netdev = phyinfo->dev_info.dev; 439 servicel = cfrfml_create(channel_id, &phyinfo->dev_info, 440 netdev->mtu); 441 break; 442 case CFCTRL_SRV_UTIL: 443 servicel = cfutill_create(channel_id, &phyinfo->dev_info); 444 break; 445 case CFCTRL_SRV_VIDEO: 446 servicel = cfvidl_create(channel_id, &phyinfo->dev_info); 447 break; 448 case CFCTRL_SRV_DBG: 449 servicel = cfdbgl_create(channel_id, &phyinfo->dev_info); 450 break; 451 default: 452 pr_err("Protocol error. Link setup response " 453 "- unknown channel type\n"); 454 goto unlock; 455 } 456 if (!servicel) { 457 pr_warn("Out of memory\n"); 458 goto unlock; 459 } 460 layer_set_dn(servicel, cnfg->mux); 461 cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id); 462 layer_set_up(servicel, adapt_layer); 463 layer_set_dn(adapt_layer, servicel); 464 465 rcu_read_unlock(); 466 467 servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0); 468 return; 469 unlock: 470 rcu_read_unlock(); 471 } 472 473 void 474 cfcnfg_add_phy_layer(struct cfcnfg *cnfg, enum cfcnfg_phy_type phy_type, 475 struct net_device *dev, struct cflayer *phy_layer, 476 enum cfcnfg_phy_preference pref, 477 bool fcs, bool stx) 478 { 479 struct cflayer *frml; 480 struct cflayer *phy_driver = NULL; 481 struct cfcnfg_phyinfo *phyinfo; 482 int i; 483 u8 phyid; 484 485 mutex_lock(&cnfg->lock); 486 487 /* CAIF protocol allow maximum 6 link-layers */ 488 for (i = 0; i < 7; i++) { 489 phyid = (dev->ifindex + i) & 0x7; 490 if (phyid == 0) 491 continue; 492 if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL) 493 goto got_phyid; 494 } 495 pr_warn("Too many CAIF Link Layers (max 6)\n"); 496 goto out; 497 498 got_phyid: 499 phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC); 500 501 switch (phy_type) { 502 case CFPHYTYPE_FRAG: 503 phy_driver = 504 cfserl_create(CFPHYTYPE_FRAG, phyid, stx); 505 if (!phy_driver) { 506 pr_warn("Out of memory\n"); 507 goto out; 508 } 509 break; 510 case CFPHYTYPE_CAIF: 511 phy_driver = NULL; 512 break; 513 default: 514 goto out; 515 } 516 phy_layer->id = phyid; 517 phyinfo->pref = pref; 518 phyinfo->id = phyid; 519 phyinfo->dev_info.id = phyid; 520 phyinfo->dev_info.dev = dev; 521 phyinfo->phy_layer = phy_layer; 522 phyinfo->ifindex = dev->ifindex; 523 phyinfo->use_stx = stx; 524 phyinfo->use_fcs = fcs; 525 526 phy_layer->type = phy_type; 527 frml = cffrml_create(phyid, fcs); 528 529 if (!frml) { 530 pr_warn("Out of memory\n"); 531 kfree(phyinfo); 532 goto out; 533 } 534 phyinfo->frm_layer = frml; 535 layer_set_up(frml, cnfg->mux); 536 537 if (phy_driver != NULL) { 538 phy_driver->id = phyid; 539 layer_set_dn(frml, phy_driver); 540 layer_set_up(phy_driver, frml); 541 layer_set_dn(phy_driver, phy_layer); 542 layer_set_up(phy_layer, phy_driver); 543 } else { 544 layer_set_dn(frml, phy_layer); 545 layer_set_up(phy_layer, frml); 546 } 547 548 list_add_rcu(&phyinfo->node, &cnfg->phys); 549 out: 550 mutex_unlock(&cnfg->lock); 551 } 552 EXPORT_SYMBOL(cfcnfg_add_phy_layer); 553 554 int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer, 555 bool up) 556 { 557 struct cfcnfg_phyinfo *phyinfo; 558 559 rcu_read_lock(); 560 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id); 561 if (phyinfo == NULL) { 562 rcu_read_unlock(); 563 return -ENODEV; 564 } 565 566 if (phyinfo->up == up) { 567 rcu_read_unlock(); 568 return 0; 569 } 570 phyinfo->up = up; 571 572 if (up) { 573 cffrml_hold(phyinfo->frm_layer); 574 cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer, 575 phy_layer->id); 576 } else { 577 cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id); 578 cffrml_put(phyinfo->frm_layer); 579 } 580 581 rcu_read_unlock(); 582 return 0; 583 } 584 EXPORT_SYMBOL(cfcnfg_set_phy_state); 585 586 int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer) 587 { 588 struct cflayer *frml, *frml_dn; 589 u16 phyid; 590 struct cfcnfg_phyinfo *phyinfo; 591 592 might_sleep(); 593 594 mutex_lock(&cnfg->lock); 595 596 phyid = phy_layer->id; 597 phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid); 598 599 if (phyinfo == NULL) { 600 mutex_unlock(&cnfg->lock); 601 return 0; 602 } 603 caif_assert(phyid == phyinfo->id); 604 caif_assert(phy_layer == phyinfo->phy_layer); 605 caif_assert(phy_layer->id == phyid); 606 caif_assert(phyinfo->frm_layer->id == phyid); 607 608 list_del_rcu(&phyinfo->node); 609 synchronize_rcu(); 610 611 /* Fail if reference count is not zero */ 612 if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) { 613 pr_info("Wait for device inuse\n"); 614 list_add_rcu(&phyinfo->node, &cnfg->phys); 615 mutex_unlock(&cnfg->lock); 616 return -EAGAIN; 617 } 618 619 frml = phyinfo->frm_layer; 620 frml_dn = frml->dn; 621 cffrml_set_uplayer(frml, NULL); 622 cffrml_set_dnlayer(frml, NULL); 623 if (phy_layer != frml_dn) { 624 layer_set_up(frml_dn, NULL); 625 layer_set_dn(frml_dn, NULL); 626 } 627 layer_set_up(phy_layer, NULL); 628 629 if (phyinfo->phy_layer != frml_dn) 630 kfree(frml_dn); 631 632 cffrml_free(frml); 633 kfree(phyinfo); 634 mutex_unlock(&cnfg->lock); 635 636 return 0; 637 } 638 EXPORT_SYMBOL(cfcnfg_del_phy_layer); 639