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