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
7 * Borrowed heavily from file: pn_dev.c. Thanks to Remi Denis-Courmont
8 * and Sakari Ailus <sakari.ailus@nokia.com>
11 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
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>
30 MODULE_LICENSE("GPL");
32 /* Used for local tracking of the CAIF net devices */
33 struct caif_device_entry
{
35 struct list_head list
;
36 struct net_device
*netdev
;
37 int __percpu
*pcpu_refcnt
;
39 struct sk_buff
*xoff_skb
;
40 void (*xoff_skb_dtor
)(struct sk_buff
*skb
);
44 struct caif_device_entry_list
{
45 struct list_head list
;
46 /* Protects simulanous deletes in list */
52 struct caif_device_entry_list caifdevs
;
55 static unsigned int caif_net_id
;
56 static int q_high
= 50; /* Percent */
58 struct cfcnfg
*get_cfcnfg(struct net
*net
)
60 struct caif_net
*caifn
;
61 caifn
= net_generic(net
, caif_net_id
);
64 EXPORT_SYMBOL(get_cfcnfg
);
66 static struct caif_device_entry_list
*caif_device_list(struct net
*net
)
68 struct caif_net
*caifn
;
69 caifn
= net_generic(net
, caif_net_id
);
70 return &caifn
->caifdevs
;
73 static void caifd_put(struct caif_device_entry
*e
)
75 this_cpu_dec(*e
->pcpu_refcnt
);
78 static void caifd_hold(struct caif_device_entry
*e
)
80 this_cpu_inc(*e
->pcpu_refcnt
);
83 static int caifd_refcnt_read(struct caif_device_entry
*e
)
86 for_each_possible_cpu(i
)
87 refcnt
+= *per_cpu_ptr(e
->pcpu_refcnt
, i
);
91 /* Allocate new CAIF device. */
92 static struct caif_device_entry
*caif_device_alloc(struct net_device
*dev
)
94 struct caif_device_entry
*caifd
;
96 caifd
= kzalloc(sizeof(*caifd
), GFP_KERNEL
);
99 caifd
->pcpu_refcnt
= alloc_percpu(int);
100 if (!caifd
->pcpu_refcnt
) {
109 static struct caif_device_entry
*caif_get(struct net_device
*dev
)
111 struct caif_device_entry_list
*caifdevs
=
112 caif_device_list(dev_net(dev
));
113 struct caif_device_entry
*caifd
;
115 list_for_each_entry_rcu(caifd
, &caifdevs
->list
, list
) {
116 if (caifd
->netdev
== dev
)
122 static void caif_flow_cb(struct sk_buff
*skb
)
124 struct caif_device_entry
*caifd
;
125 void (*dtor
)(struct sk_buff
*skb
) = NULL
;
128 WARN_ON(skb
->dev
== NULL
);
131 caifd
= caif_get(skb
->dev
);
133 WARN_ON(caifd
== NULL
);
142 spin_lock_bh(&caifd
->flow_lock
);
143 send_xoff
= caifd
->xoff
;
145 dtor
= caifd
->xoff_skb_dtor
;
147 if (WARN_ON(caifd
->xoff_skb
!= skb
))
150 caifd
->xoff_skb
= NULL
;
151 caifd
->xoff_skb_dtor
= NULL
;
153 spin_unlock_bh(&caifd
->flow_lock
);
160 ctrlcmd(caifd
->layer
.up
,
161 _CAIF_CTRLCMD_PHYIF_FLOW_ON_IND
,
166 static int transmit(struct cflayer
*layer
, struct cfpkt
*pkt
)
168 int err
, high
= 0, qlen
= 0;
169 struct caif_device_entry
*caifd
=
170 container_of(layer
, struct caif_device_entry
, layer
);
172 struct netdev_queue
*txq
;
176 skb
= cfpkt_tonative(pkt
);
177 skb
->dev
= caifd
->netdev
;
178 skb_reset_network_header(skb
);
179 skb
->protocol
= htons(ETH_P_CAIF
);
181 /* Check if we need to handle xoff */
182 if (likely(caifd
->netdev
->priv_flags
& IFF_NO_QUEUE
))
185 if (unlikely(caifd
->xoff
))
188 if (likely(!netif_queue_stopped(caifd
->netdev
))) {
191 /* If we run with a TX queue, check if the queue is too long*/
192 txq
= netdev_get_tx_queue(skb
->dev
, 0);
193 sch
= rcu_dereference_bh(txq
->qdisc
);
194 if (likely(qdisc_is_empty(sch
)))
197 /* can check for explicit qdisc len value only !NOLOCK,
198 * always set flow off otherwise
200 high
= (caifd
->netdev
->tx_queue_len
* q_high
) / 100;
201 if (!(sch
->flags
& TCQ_F_NOLOCK
) && likely(sch
->q
.qlen
< high
))
205 /* Hold lock while accessing xoff */
206 spin_lock_bh(&caifd
->flow_lock
);
208 spin_unlock_bh(&caifd
->flow_lock
);
213 * Handle flow off, we do this by temporary hi-jacking this
214 * skb's destructor function, and replace it with our own
215 * flow-on callback. The callback will set flow-on and call
216 * the original destructor.
219 pr_debug("queue has stopped(%d) or is full (%d > %d)\n",
220 netif_queue_stopped(caifd
->netdev
),
223 caifd
->xoff_skb
= skb
;
224 caifd
->xoff_skb_dtor
= skb
->destructor
;
225 skb
->destructor
= caif_flow_cb
;
226 spin_unlock_bh(&caifd
->flow_lock
);
228 caifd
->layer
.up
->ctrlcmd(caifd
->layer
.up
,
229 _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND
,
232 rcu_read_unlock_bh();
234 err
= dev_queue_xmit(skb
);
242 * Stuff received packets into the CAIF stack.
243 * On error, returns non-zero and releases the skb.
245 static int receive(struct sk_buff
*skb
, struct net_device
*dev
,
246 struct packet_type
*pkttype
, struct net_device
*orig_dev
)
249 struct caif_device_entry
*caifd
;
252 pkt
= cfpkt_fromnative(CAIF_DIR_IN
, skb
);
255 caifd
= caif_get(dev
);
257 if (!caifd
|| !caifd
->layer
.up
|| !caifd
->layer
.up
->receive
||
258 !netif_oper_up(caifd
->netdev
)) {
264 /* Hold reference to netdevice while using CAIF stack */
268 err
= caifd
->layer
.up
->receive(caifd
->layer
.up
, pkt
);
270 /* For -EILSEQ the packet is not freed so so it now */
274 /* Release reference to stack upwards */
282 static struct packet_type caif_packet_type __read_mostly
= {
283 .type
= cpu_to_be16(ETH_P_CAIF
),
287 static void dev_flowctrl(struct net_device
*dev
, int on
)
289 struct caif_device_entry
*caifd
;
293 caifd
= caif_get(dev
);
294 if (!caifd
|| !caifd
->layer
.up
|| !caifd
->layer
.up
->ctrlcmd
) {
302 caifd
->layer
.up
->ctrlcmd(caifd
->layer
.up
,
304 _CAIF_CTRLCMD_PHYIF_FLOW_ON_IND
:
305 _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND
,
310 void caif_enroll_dev(struct net_device
*dev
, struct caif_dev_common
*caifdev
,
311 struct cflayer
*link_support
, int head_room
,
312 struct cflayer
**layer
,
313 int (**rcv_func
)(struct sk_buff
*, struct net_device
*,
314 struct packet_type
*,
315 struct net_device
*))
317 struct caif_device_entry
*caifd
;
318 enum cfcnfg_phy_preference pref
;
319 struct cfcnfg
*cfg
= get_cfcnfg(dev_net(dev
));
320 struct caif_device_entry_list
*caifdevs
;
322 caifdevs
= caif_device_list(dev_net(dev
));
323 caifd
= caif_device_alloc(dev
);
326 *layer
= &caifd
->layer
;
327 spin_lock_init(&caifd
->flow_lock
);
329 switch (caifdev
->link_select
) {
330 case CAIF_LINK_HIGH_BANDW
:
331 pref
= CFPHYPREF_HIGH_BW
;
333 case CAIF_LINK_LOW_LATENCY
:
334 pref
= CFPHYPREF_LOW_LAT
;
337 pref
= CFPHYPREF_HIGH_BW
;
340 mutex_lock(&caifdevs
->lock
);
341 list_add_rcu(&caifd
->list
, &caifdevs
->list
);
343 strlcpy(caifd
->layer
.name
, dev
->name
,
344 sizeof(caifd
->layer
.name
));
345 caifd
->layer
.transmit
= transmit
;
346 cfcnfg_add_phy_layer(cfg
,
353 mutex_unlock(&caifdevs
->lock
);
357 EXPORT_SYMBOL(caif_enroll_dev
);
359 /* notify Caif of device events */
360 static int caif_device_notify(struct notifier_block
*me
, unsigned long what
,
363 struct net_device
*dev
= netdev_notifier_info_to_dev(ptr
);
364 struct caif_device_entry
*caifd
= NULL
;
365 struct caif_dev_common
*caifdev
;
367 struct cflayer
*layer
, *link_support
;
369 struct caif_device_entry_list
*caifdevs
;
371 cfg
= get_cfcnfg(dev_net(dev
));
372 caifdevs
= caif_device_list(dev_net(dev
));
374 caifd
= caif_get(dev
);
375 if (caifd
== NULL
&& dev
->type
!= ARPHRD_CAIF
)
379 case NETDEV_REGISTER
:
383 caifdev
= netdev_priv(dev
);
386 if (caifdev
->use_frag
) {
388 link_support
= cfserl_create(dev
->ifindex
,
391 pr_warn("Out of memory\n");
395 caif_enroll_dev(dev
, caifdev
, link_support
, head_room
,
397 caifdev
->flowctrl
= dev_flowctrl
;
403 caifd
= caif_get(dev
);
410 cfcnfg_set_phy_state(cfg
, &caifd
->layer
, true);
418 caifd
= caif_get(dev
);
419 if (!caifd
|| !caifd
->layer
.up
|| !caifd
->layer
.up
->ctrlcmd
) {
424 cfcnfg_set_phy_state(cfg
, &caifd
->layer
, false);
428 caifd
->layer
.up
->ctrlcmd(caifd
->layer
.up
,
429 _CAIF_CTRLCMD_PHYIF_DOWN_IND
,
432 spin_lock_bh(&caifd
->flow_lock
);
435 * Replace our xoff-destructor with original destructor.
436 * We trust that skb->destructor *always* is called before
437 * the skb reference is invalid. The hijacked SKB destructor
438 * takes the flow_lock so manipulating the skb->destructor here
441 if (caifd
->xoff_skb_dtor
!= NULL
&& caifd
->xoff_skb
!= NULL
)
442 caifd
->xoff_skb
->destructor
= caifd
->xoff_skb_dtor
;
445 caifd
->xoff_skb_dtor
= NULL
;
446 caifd
->xoff_skb
= NULL
;
448 spin_unlock_bh(&caifd
->flow_lock
);
452 case NETDEV_UNREGISTER
:
453 mutex_lock(&caifdevs
->lock
);
455 caifd
= caif_get(dev
);
457 mutex_unlock(&caifdevs
->lock
);
460 list_del_rcu(&caifd
->list
);
463 * NETDEV_UNREGISTER is called repeatedly until all reference
464 * counts for the net-device are released. If references to
465 * caifd is taken, simply ignore NETDEV_UNREGISTER and wait for
466 * the next call to NETDEV_UNREGISTER.
468 * If any packets are in flight down the CAIF Stack,
469 * cfcnfg_del_phy_layer will return nonzero.
470 * If no packets are in flight, the CAIF Stack associated
471 * with the net-device un-registering is freed.
474 if (caifd_refcnt_read(caifd
) != 0 ||
475 cfcnfg_del_phy_layer(cfg
, &caifd
->layer
) != 0) {
477 pr_info("Wait for device inuse\n");
478 /* Enrole device if CAIF Stack is still in use */
479 list_add_rcu(&caifd
->list
, &caifdevs
->list
);
480 mutex_unlock(&caifdevs
->lock
);
485 dev_put(caifd
->netdev
);
486 free_percpu(caifd
->pcpu_refcnt
);
489 mutex_unlock(&caifdevs
->lock
);
495 static struct notifier_block caif_device_notifier
= {
496 .notifier_call
= caif_device_notify
,
500 /* Per-namespace Caif devices handling */
501 static int caif_init_net(struct net
*net
)
503 struct caif_net
*caifn
= net_generic(net
, caif_net_id
);
504 INIT_LIST_HEAD(&caifn
->caifdevs
.list
);
505 mutex_init(&caifn
->caifdevs
.lock
);
507 caifn
->cfg
= cfcnfg_create();
514 static void caif_exit_net(struct net
*net
)
516 struct caif_device_entry
*caifd
, *tmp
;
517 struct caif_device_entry_list
*caifdevs
=
518 caif_device_list(net
);
519 struct cfcnfg
*cfg
= get_cfcnfg(net
);
522 mutex_lock(&caifdevs
->lock
);
524 list_for_each_entry_safe(caifd
, tmp
, &caifdevs
->list
, list
) {
526 list_del_rcu(&caifd
->list
);
527 cfcnfg_set_phy_state(cfg
, &caifd
->layer
, false);
530 (caifd_refcnt_read(caifd
) != 0 ||
531 cfcnfg_del_phy_layer(cfg
, &caifd
->layer
) != 0)) {
533 pr_info("Wait for device inuse\n");
538 dev_put(caifd
->netdev
);
539 free_percpu(caifd
->pcpu_refcnt
);
544 mutex_unlock(&caifdevs
->lock
);
548 static struct pernet_operations caif_net_ops
= {
549 .init
= caif_init_net
,
550 .exit
= caif_exit_net
,
552 .size
= sizeof(struct caif_net
),
555 /* Initialize Caif devices list */
556 static int __init
caif_device_init(void)
560 result
= register_pernet_subsys(&caif_net_ops
);
565 register_netdevice_notifier(&caif_device_notifier
);
566 dev_add_pack(&caif_packet_type
);
571 static void __exit
caif_device_exit(void)
573 unregister_netdevice_notifier(&caif_device_notifier
);
574 dev_remove_pack(&caif_packet_type
);
575 unregister_pernet_subsys(&caif_net_ops
);
578 module_init(caif_device_init
);
579 module_exit(caif_device_exit
);