1 /* 2 * CAIF Interface registration. 3 * Copyright (C) ST-Ericsson AB 2010 4 * Author: Sjur Brendeland 5 * License terms: GNU General Public License (GPL) version 2 6 * 7 * Borrowed heavily from file: pn_dev.c. Thanks to Remi Denis-Courmont 8 * and Sakari Ailus <sakari.ailus@nokia.com> 9 */ 10 11 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ 12 13 #include <linux/kernel.h> 14 #include <linux/if_arp.h> 15 #include <linux/net.h> 16 #include <linux/netdevice.h> 17 #include <linux/mutex.h> 18 #include <linux/module.h> 19 #include <linux/spinlock.h> 20 #include <net/netns/generic.h> 21 #include <net/net_namespace.h> 22 #include <net/pkt_sched.h> 23 #include <net/caif/caif_device.h> 24 #include <net/caif/caif_layer.h> 25 #include <net/caif/caif_dev.h> 26 #include <net/caif/cfpkt.h> 27 #include <net/caif/cfcnfg.h> 28 #include <net/caif/cfserl.h> 29 30 MODULE_LICENSE("GPL"); 31 32 /* Used for local tracking of the CAIF net devices */ 33 struct caif_device_entry { 34 struct cflayer layer; 35 struct list_head list; 36 struct net_device *netdev; 37 int __percpu *pcpu_refcnt; 38 spinlock_t flow_lock; 39 struct sk_buff *xoff_skb; 40 void (*xoff_skb_dtor)(struct sk_buff *skb); 41 bool xoff; 42 }; 43 44 struct caif_device_entry_list { 45 struct list_head list; 46 /* Protects simulanous deletes in list */ 47 struct mutex lock; 48 }; 49 50 struct caif_net { 51 struct cfcnfg *cfg; 52 struct caif_device_entry_list caifdevs; 53 }; 54 55 static unsigned int caif_net_id; 56 static int q_high = 50; /* Percent */ 57 58 struct cfcnfg *get_cfcnfg(struct net *net) 59 { 60 struct caif_net *caifn; 61 caifn = net_generic(net, caif_net_id); 62 return caifn->cfg; 63 } 64 EXPORT_SYMBOL(get_cfcnfg); 65 66 static struct caif_device_entry_list *caif_device_list(struct net *net) 67 { 68 struct caif_net *caifn; 69 caifn = net_generic(net, caif_net_id); 70 return &caifn->caifdevs; 71 } 72 73 static void caifd_put(struct caif_device_entry *e) 74 { 75 this_cpu_dec(*e->pcpu_refcnt); 76 } 77 78 static void caifd_hold(struct caif_device_entry *e) 79 { 80 this_cpu_inc(*e->pcpu_refcnt); 81 } 82 83 static int caifd_refcnt_read(struct caif_device_entry *e) 84 { 85 int i, refcnt = 0; 86 for_each_possible_cpu(i) 87 refcnt += *per_cpu_ptr(e->pcpu_refcnt, i); 88 return refcnt; 89 } 90 91 /* Allocate new CAIF device. */ 92 static struct caif_device_entry *caif_device_alloc(struct net_device *dev) 93 { 94 struct caif_device_entry *caifd; 95 96 caifd = kzalloc(sizeof(*caifd), GFP_KERNEL); 97 if (!caifd) 98 return NULL; 99 caifd->pcpu_refcnt = alloc_percpu(int); 100 if (!caifd->pcpu_refcnt) { 101 kfree(caifd); 102 return NULL; 103 } 104 caifd->netdev = dev; 105 dev_hold(dev); 106 return caifd; 107 } 108 109 static struct caif_device_entry *caif_get(struct net_device *dev) 110 { 111 struct caif_device_entry_list *caifdevs = 112 caif_device_list(dev_net(dev)); 113 struct caif_device_entry *caifd; 114 115 list_for_each_entry_rcu(caifd, &caifdevs->list, list) { 116 if (caifd->netdev == dev) 117 return caifd; 118 } 119 return NULL; 120 } 121 122 static void caif_flow_cb(struct sk_buff *skb) 123 { 124 struct caif_device_entry *caifd; 125 void (*dtor)(struct sk_buff *skb) = NULL; 126 bool send_xoff; 127 128 WARN_ON(skb->dev == NULL); 129 130 rcu_read_lock(); 131 caifd = caif_get(skb->dev); 132 133 WARN_ON(caifd == NULL); 134 if (!caifd) { 135 rcu_read_unlock(); 136 return; 137 } 138 139 caifd_hold(caifd); 140 rcu_read_unlock(); 141 142 spin_lock_bh(&caifd->flow_lock); 143 send_xoff = caifd->xoff; 144 caifd->xoff = 0; 145 dtor = caifd->xoff_skb_dtor; 146 147 if (WARN_ON(caifd->xoff_skb != skb)) 148 skb = NULL; 149 150 caifd->xoff_skb = NULL; 151 caifd->xoff_skb_dtor = NULL; 152 153 spin_unlock_bh(&caifd->flow_lock); 154 155 if (dtor && skb) 156 dtor(skb); 157 158 if (send_xoff) 159 caifd->layer.up-> 160 ctrlcmd(caifd->layer.up, 161 _CAIF_CTRLCMD_PHYIF_FLOW_ON_IND, 162 caifd->layer.id); 163 caifd_put(caifd); 164 } 165 166 static int transmit(struct cflayer *layer, struct cfpkt *pkt) 167 { 168 int err, high = 0, qlen = 0; 169 struct caif_device_entry *caifd = 170 container_of(layer, struct caif_device_entry, layer); 171 struct sk_buff *skb; 172 struct netdev_queue *txq; 173 174 rcu_read_lock_bh(); 175 176 skb = cfpkt_tonative(pkt); 177 skb->dev = caifd->netdev; 178 skb_reset_network_header(skb); 179 skb->protocol = htons(ETH_P_CAIF); 180 181 /* Check if we need to handle xoff */ 182 if (likely(caifd->netdev->priv_flags & IFF_NO_QUEUE)) 183 goto noxoff; 184 185 if (unlikely(caifd->xoff)) 186 goto noxoff; 187 188 if (likely(!netif_queue_stopped(caifd->netdev))) { 189 /* If we run with a TX queue, check if the queue is too long*/ 190 txq = netdev_get_tx_queue(skb->dev, 0); 191 qlen = qdisc_qlen(rcu_dereference_bh(txq->qdisc)); 192 193 if (likely(qlen == 0)) 194 goto noxoff; 195 196 high = (caifd->netdev->tx_queue_len * q_high) / 100; 197 if (likely(qlen < high)) 198 goto noxoff; 199 } 200 201 /* Hold lock while accessing xoff */ 202 spin_lock_bh(&caifd->flow_lock); 203 if (caifd->xoff) { 204 spin_unlock_bh(&caifd->flow_lock); 205 goto noxoff; 206 } 207 208 /* 209 * Handle flow off, we do this by temporary hi-jacking this 210 * skb's destructor function, and replace it with our own 211 * flow-on callback. The callback will set flow-on and call 212 * the original destructor. 213 */ 214 215 pr_debug("queue has stopped(%d) or is full (%d > %d)\n", 216 netif_queue_stopped(caifd->netdev), 217 qlen, high); 218 caifd->xoff = 1; 219 caifd->xoff_skb = skb; 220 caifd->xoff_skb_dtor = skb->destructor; 221 skb->destructor = caif_flow_cb; 222 spin_unlock_bh(&caifd->flow_lock); 223 224 caifd->layer.up->ctrlcmd(caifd->layer.up, 225 _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND, 226 caifd->layer.id); 227 noxoff: 228 rcu_read_unlock_bh(); 229 230 err = dev_queue_xmit(skb); 231 if (err > 0) 232 err = -EIO; 233 234 return err; 235 } 236 237 /* 238 * Stuff received packets into the CAIF stack. 239 * On error, returns non-zero and releases the skb. 240 */ 241 static int receive(struct sk_buff *skb, struct net_device *dev, 242 struct packet_type *pkttype, struct net_device *orig_dev) 243 { 244 struct cfpkt *pkt; 245 struct caif_device_entry *caifd; 246 int err; 247 248 pkt = cfpkt_fromnative(CAIF_DIR_IN, skb); 249 250 rcu_read_lock(); 251 caifd = caif_get(dev); 252 253 if (!caifd || !caifd->layer.up || !caifd->layer.up->receive || 254 !netif_oper_up(caifd->netdev)) { 255 rcu_read_unlock(); 256 kfree_skb(skb); 257 return NET_RX_DROP; 258 } 259 260 /* Hold reference to netdevice while using CAIF stack */ 261 caifd_hold(caifd); 262 rcu_read_unlock(); 263 264 err = caifd->layer.up->receive(caifd->layer.up, pkt); 265 266 /* For -EILSEQ the packet is not freed so so it now */ 267 if (err == -EILSEQ) 268 cfpkt_destroy(pkt); 269 270 /* Release reference to stack upwards */ 271 caifd_put(caifd); 272 273 if (err != 0) 274 err = NET_RX_DROP; 275 return err; 276 } 277 278 static struct packet_type caif_packet_type __read_mostly = { 279 .type = cpu_to_be16(ETH_P_CAIF), 280 .func = receive, 281 }; 282 283 static void dev_flowctrl(struct net_device *dev, int on) 284 { 285 struct caif_device_entry *caifd; 286 287 rcu_read_lock(); 288 289 caifd = caif_get(dev); 290 if (!caifd || !caifd->layer.up || !caifd->layer.up->ctrlcmd) { 291 rcu_read_unlock(); 292 return; 293 } 294 295 caifd_hold(caifd); 296 rcu_read_unlock(); 297 298 caifd->layer.up->ctrlcmd(caifd->layer.up, 299 on ? 300 _CAIF_CTRLCMD_PHYIF_FLOW_ON_IND : 301 _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND, 302 caifd->layer.id); 303 caifd_put(caifd); 304 } 305 306 void caif_enroll_dev(struct net_device *dev, struct caif_dev_common *caifdev, 307 struct cflayer *link_support, int head_room, 308 struct cflayer **layer, 309 int (**rcv_func)(struct sk_buff *, struct net_device *, 310 struct packet_type *, 311 struct net_device *)) 312 { 313 struct caif_device_entry *caifd; 314 enum cfcnfg_phy_preference pref; 315 struct cfcnfg *cfg = get_cfcnfg(dev_net(dev)); 316 struct caif_device_entry_list *caifdevs; 317 318 caifdevs = caif_device_list(dev_net(dev)); 319 caifd = caif_device_alloc(dev); 320 if (!caifd) 321 return; 322 *layer = &caifd->layer; 323 spin_lock_init(&caifd->flow_lock); 324 325 switch (caifdev->link_select) { 326 case CAIF_LINK_HIGH_BANDW: 327 pref = CFPHYPREF_HIGH_BW; 328 break; 329 case CAIF_LINK_LOW_LATENCY: 330 pref = CFPHYPREF_LOW_LAT; 331 break; 332 default: 333 pref = CFPHYPREF_HIGH_BW; 334 break; 335 } 336 mutex_lock(&caifdevs->lock); 337 list_add_rcu(&caifd->list, &caifdevs->list); 338 339 strlcpy(caifd->layer.name, dev->name, 340 sizeof(caifd->layer.name)); 341 caifd->layer.transmit = transmit; 342 cfcnfg_add_phy_layer(cfg, 343 dev, 344 &caifd->layer, 345 pref, 346 link_support, 347 caifdev->use_fcs, 348 head_room); 349 mutex_unlock(&caifdevs->lock); 350 if (rcv_func) 351 *rcv_func = receive; 352 } 353 EXPORT_SYMBOL(caif_enroll_dev); 354 355 /* notify Caif of device events */ 356 static int caif_device_notify(struct notifier_block *me, unsigned long what, 357 void *ptr) 358 { 359 struct net_device *dev = netdev_notifier_info_to_dev(ptr); 360 struct caif_device_entry *caifd = NULL; 361 struct caif_dev_common *caifdev; 362 struct cfcnfg *cfg; 363 struct cflayer *layer, *link_support; 364 int head_room = 0; 365 struct caif_device_entry_list *caifdevs; 366 367 cfg = get_cfcnfg(dev_net(dev)); 368 caifdevs = caif_device_list(dev_net(dev)); 369 370 caifd = caif_get(dev); 371 if (caifd == NULL && dev->type != ARPHRD_CAIF) 372 return 0; 373 374 switch (what) { 375 case NETDEV_REGISTER: 376 if (caifd != NULL) 377 break; 378 379 caifdev = netdev_priv(dev); 380 381 link_support = NULL; 382 if (caifdev->use_frag) { 383 head_room = 1; 384 link_support = cfserl_create(dev->ifindex, 385 caifdev->use_stx); 386 if (!link_support) { 387 pr_warn("Out of memory\n"); 388 break; 389 } 390 } 391 caif_enroll_dev(dev, caifdev, link_support, head_room, 392 &layer, NULL); 393 caifdev->flowctrl = dev_flowctrl; 394 break; 395 396 case NETDEV_UP: 397 rcu_read_lock(); 398 399 caifd = caif_get(dev); 400 if (caifd == NULL) { 401 rcu_read_unlock(); 402 break; 403 } 404 405 caifd->xoff = 0; 406 cfcnfg_set_phy_state(cfg, &caifd->layer, true); 407 rcu_read_unlock(); 408 409 break; 410 411 case NETDEV_DOWN: 412 rcu_read_lock(); 413 414 caifd = caif_get(dev); 415 if (!caifd || !caifd->layer.up || !caifd->layer.up->ctrlcmd) { 416 rcu_read_unlock(); 417 return -EINVAL; 418 } 419 420 cfcnfg_set_phy_state(cfg, &caifd->layer, false); 421 caifd_hold(caifd); 422 rcu_read_unlock(); 423 424 caifd->layer.up->ctrlcmd(caifd->layer.up, 425 _CAIF_CTRLCMD_PHYIF_DOWN_IND, 426 caifd->layer.id); 427 428 spin_lock_bh(&caifd->flow_lock); 429 430 /* 431 * Replace our xoff-destructor with original destructor. 432 * We trust that skb->destructor *always* is called before 433 * the skb reference is invalid. The hijacked SKB destructor 434 * takes the flow_lock so manipulating the skb->destructor here 435 * should be safe. 436 */ 437 if (caifd->xoff_skb_dtor != NULL && caifd->xoff_skb != NULL) 438 caifd->xoff_skb->destructor = caifd->xoff_skb_dtor; 439 440 caifd->xoff = 0; 441 caifd->xoff_skb_dtor = NULL; 442 caifd->xoff_skb = NULL; 443 444 spin_unlock_bh(&caifd->flow_lock); 445 caifd_put(caifd); 446 break; 447 448 case NETDEV_UNREGISTER: 449 mutex_lock(&caifdevs->lock); 450 451 caifd = caif_get(dev); 452 if (caifd == NULL) { 453 mutex_unlock(&caifdevs->lock); 454 break; 455 } 456 list_del_rcu(&caifd->list); 457 458 /* 459 * NETDEV_UNREGISTER is called repeatedly until all reference 460 * counts for the net-device are released. If references to 461 * caifd is taken, simply ignore NETDEV_UNREGISTER and wait for 462 * the next call to NETDEV_UNREGISTER. 463 * 464 * If any packets are in flight down the CAIF Stack, 465 * cfcnfg_del_phy_layer will return nonzero. 466 * If no packets are in flight, the CAIF Stack associated 467 * with the net-device un-registering is freed. 468 */ 469 470 if (caifd_refcnt_read(caifd) != 0 || 471 cfcnfg_del_phy_layer(cfg, &caifd->layer) != 0) { 472 473 pr_info("Wait for device inuse\n"); 474 /* Enrole device if CAIF Stack is still in use */ 475 list_add_rcu(&caifd->list, &caifdevs->list); 476 mutex_unlock(&caifdevs->lock); 477 break; 478 } 479 480 synchronize_rcu(); 481 dev_put(caifd->netdev); 482 free_percpu(caifd->pcpu_refcnt); 483 kfree(caifd); 484 485 mutex_unlock(&caifdevs->lock); 486 break; 487 } 488 return 0; 489 } 490 491 static struct notifier_block caif_device_notifier = { 492 .notifier_call = caif_device_notify, 493 .priority = 0, 494 }; 495 496 /* Per-namespace Caif devices handling */ 497 static int caif_init_net(struct net *net) 498 { 499 struct caif_net *caifn = net_generic(net, caif_net_id); 500 INIT_LIST_HEAD(&caifn->caifdevs.list); 501 mutex_init(&caifn->caifdevs.lock); 502 503 caifn->cfg = cfcnfg_create(); 504 if (!caifn->cfg) 505 return -ENOMEM; 506 507 return 0; 508 } 509 510 static void caif_exit_net(struct net *net) 511 { 512 struct caif_device_entry *caifd, *tmp; 513 struct caif_device_entry_list *caifdevs = 514 caif_device_list(net); 515 struct cfcnfg *cfg = get_cfcnfg(net); 516 517 rtnl_lock(); 518 mutex_lock(&caifdevs->lock); 519 520 list_for_each_entry_safe(caifd, tmp, &caifdevs->list, list) { 521 int i = 0; 522 list_del_rcu(&caifd->list); 523 cfcnfg_set_phy_state(cfg, &caifd->layer, false); 524 525 while (i < 10 && 526 (caifd_refcnt_read(caifd) != 0 || 527 cfcnfg_del_phy_layer(cfg, &caifd->layer) != 0)) { 528 529 pr_info("Wait for device inuse\n"); 530 msleep(250); 531 i++; 532 } 533 synchronize_rcu(); 534 dev_put(caifd->netdev); 535 free_percpu(caifd->pcpu_refcnt); 536 kfree(caifd); 537 } 538 cfcnfg_remove(cfg); 539 540 mutex_unlock(&caifdevs->lock); 541 rtnl_unlock(); 542 } 543 544 static struct pernet_operations caif_net_ops = { 545 .init = caif_init_net, 546 .exit = caif_exit_net, 547 .id = &caif_net_id, 548 .size = sizeof(struct caif_net), 549 }; 550 551 /* Initialize Caif devices list */ 552 static int __init caif_device_init(void) 553 { 554 int result; 555 556 result = register_pernet_subsys(&caif_net_ops); 557 558 if (result) 559 return result; 560 561 register_netdevice_notifier(&caif_device_notifier); 562 dev_add_pack(&caif_packet_type); 563 564 return result; 565 } 566 567 static void __exit caif_device_exit(void) 568 { 569 unregister_netdevice_notifier(&caif_device_notifier); 570 dev_remove_pack(&caif_packet_type); 571 unregister_pernet_subsys(&caif_net_ops); 572 } 573 574 module_init(caif_device_init); 575 module_exit(caif_device_exit); 576