diff options
Diffstat (limited to 'net/caif')
| -rw-r--r-- | net/caif/Kconfig | 53 | ||||
| -rw-r--r-- | net/caif/Makefile | 16 | ||||
| -rw-r--r-- | net/caif/caif_dev.c | 580 | ||||
| -rw-r--r-- | net/caif/caif_socket.c | 1125 | ||||
| -rw-r--r-- | net/caif/caif_usb.c | 213 | ||||
| -rw-r--r-- | net/caif/cfcnfg.c | 612 | ||||
| -rw-r--r-- | net/caif/cfctrl.c | 635 | ||||
| -rw-r--r-- | net/caif/cfdbgl.c | 55 | ||||
| -rw-r--r-- | net/caif/cfdgml.c | 114 | ||||
| -rw-r--r-- | net/caif/cffrml.c | 197 | ||||
| -rw-r--r-- | net/caif/cfmuxl.c | 267 | ||||
| -rw-r--r-- | net/caif/cfpkt_skbuff.c | 388 | ||||
| -rw-r--r-- | net/caif/cfrfml.c | 302 | ||||
| -rw-r--r-- | net/caif/cfserl.c | 193 | ||||
| -rw-r--r-- | net/caif/cfsrvl.c | 221 | ||||
| -rw-r--r-- | net/caif/cfutill.c | 104 | ||||
| -rw-r--r-- | net/caif/cfveil.c | 101 | ||||
| -rw-r--r-- | net/caif/cfvidl.c | 65 | ||||
| -rw-r--r-- | net/caif/chnl_net.c | 538 |
19 files changed, 5779 insertions, 0 deletions
diff --git a/net/caif/Kconfig b/net/caif/Kconfig new file mode 100644 index 000000000..d3694953b --- /dev/null +++ b/net/caif/Kconfig @@ -0,0 +1,53 @@ +# +# CAIF net configurations +# + +menuconfig CAIF + tristate "CAIF support" + select CRC_CCITT + default n + ---help--- + The "Communication CPU to Application CPU Interface" (CAIF) is a packet + based connection-oriented MUX protocol developed by ST-Ericsson for use + with its modems. It is accessed from user space as sockets (PF_CAIF). + + Say Y (or M) here if you build for a phone product (e.g. Android or + MeeGo ) that uses CAIF as transport, if unsure say N. + + If you select to build it as module then CAIF_NETDEV also needs to be + built as modules. You will also need to say yes to any CAIF physical + devices that your platform requires. + + See Documentation/networking/caif for a further explanation on how to + use and configure CAIF. + +config CAIF_DEBUG + bool "Enable Debug" + depends on CAIF + default n + ---help--- + Enable the inclusion of debug code in the CAIF stack. + Be aware that doing this will impact performance. + If unsure say N. + +config CAIF_NETDEV + tristate "CAIF GPRS Network device" + depends on CAIF + default CAIF + ---help--- + Say Y if you will be using a CAIF based GPRS network device. + This can be either built-in or a loadable module, + If you select to build it as a built-in then the main CAIF device must + also be a built-in. + If unsure say Y. + +config CAIF_USB + tristate "CAIF USB support" + depends on CAIF + default n + ---help--- + Say Y if you are using CAIF over USB CDC NCM. + This can be either built-in or a loadable module, + If you select to build it as a built-in then the main CAIF device must + also be a built-in. + If unsure say N. diff --git a/net/caif/Makefile b/net/caif/Makefile new file mode 100644 index 000000000..4f6c0517c --- /dev/null +++ b/net/caif/Makefile @@ -0,0 +1,16 @@ +# SPDX-License-Identifier: GPL-2.0 +ccflags-$(CONFIG_CAIF_DEBUG) := -DDEBUG + +caif-y := caif_dev.o \ + cfcnfg.o cfmuxl.o cfctrl.o \ + cffrml.o cfveil.o cfdbgl.o\ + cfserl.o cfdgml.o \ + cfrfml.o cfvidl.o cfutill.o \ + cfsrvl.o cfpkt_skbuff.o + +obj-$(CONFIG_CAIF) += caif.o +obj-$(CONFIG_CAIF_NETDEV) += chnl_net.o +obj-$(CONFIG_CAIF) += caif_socket.o +obj-$(CONFIG_CAIF_USB) += caif_usb.o + +export-y := caif.o diff --git a/net/caif/caif_dev.c b/net/caif/caif_dev.c new file mode 100644 index 000000000..cc305d841 --- /dev/null +++ b/net/caif/caif_dev.c @@ -0,0 +1,580 @@ +/* + * CAIF Interface registration. + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + * + * Borrowed heavily from file: pn_dev.c. Thanks to Remi Denis-Courmont + * and Sakari Ailus <sakari.ailus@nokia.com> + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/kernel.h> +#include <linux/if_arp.h> +#include <linux/net.h> +#include <linux/netdevice.h> +#include <linux/mutex.h> +#include <linux/module.h> +#include <linux/spinlock.h> +#include <net/netns/generic.h> +#include <net/net_namespace.h> +#include <net/pkt_sched.h> +#include <net/caif/caif_device.h> +#include <net/caif/caif_layer.h> +#include <net/caif/caif_dev.h> +#include <net/caif/cfpkt.h> +#include <net/caif/cfcnfg.h> +#include <net/caif/cfserl.h> + +MODULE_LICENSE("GPL"); + +/* Used for local tracking of the CAIF net devices */ +struct caif_device_entry { + struct cflayer layer; + struct list_head list; + struct net_device *netdev; + int __percpu *pcpu_refcnt; + spinlock_t flow_lock; + struct sk_buff *xoff_skb; + void (*xoff_skb_dtor)(struct sk_buff *skb); + bool xoff; +}; + +struct caif_device_entry_list { + struct list_head list; + /* Protects simulanous deletes in list */ + struct mutex lock; +}; + +struct caif_net { + struct cfcnfg *cfg; + struct caif_device_entry_list caifdevs; +}; + +static unsigned int caif_net_id; +static int q_high = 50; /* Percent */ + +struct cfcnfg *get_cfcnfg(struct net *net) +{ + struct caif_net *caifn; + caifn = net_generic(net, caif_net_id); + return caifn->cfg; +} +EXPORT_SYMBOL(get_cfcnfg); + +static struct caif_device_entry_list *caif_device_list(struct net *net) +{ + struct caif_net *caifn; + caifn = net_generic(net, caif_net_id); + return &caifn->caifdevs; +} + +static void caifd_put(struct caif_device_entry *e) +{ + this_cpu_dec(*e->pcpu_refcnt); +} + +static void caifd_hold(struct caif_device_entry *e) +{ + this_cpu_inc(*e->pcpu_refcnt); +} + +static int caifd_refcnt_read(struct caif_device_entry *e) +{ + int i, refcnt = 0; + for_each_possible_cpu(i) + refcnt += *per_cpu_ptr(e->pcpu_refcnt, i); + return refcnt; +} + +/* Allocate new CAIF device. */ +static struct caif_device_entry *caif_device_alloc(struct net_device *dev) +{ + struct caif_device_entry *caifd; + + caifd = kzalloc(sizeof(*caifd), GFP_KERNEL); + if (!caifd) + return NULL; + caifd->pcpu_refcnt = alloc_percpu(int); + if (!caifd->pcpu_refcnt) { + kfree(caifd); + return NULL; + } + caifd->netdev = dev; + dev_hold(dev); + return caifd; +} + +static struct caif_device_entry *caif_get(struct net_device *dev) +{ + struct caif_device_entry_list *caifdevs = + caif_device_list(dev_net(dev)); + struct caif_device_entry *caifd; + + list_for_each_entry_rcu(caifd, &caifdevs->list, list) { + if (caifd->netdev == dev) + return caifd; + } + return NULL; +} + +static void caif_flow_cb(struct sk_buff *skb) +{ + struct caif_device_entry *caifd; + void (*dtor)(struct sk_buff *skb) = NULL; + bool send_xoff; + + WARN_ON(skb->dev == NULL); + + rcu_read_lock(); + caifd = caif_get(skb->dev); + + WARN_ON(caifd == NULL); + if (!caifd) { + rcu_read_unlock(); + return; + } + + caifd_hold(caifd); + rcu_read_unlock(); + + spin_lock_bh(&caifd->flow_lock); + send_xoff = caifd->xoff; + caifd->xoff = 0; + dtor = caifd->xoff_skb_dtor; + + if (WARN_ON(caifd->xoff_skb != skb)) + skb = NULL; + + caifd->xoff_skb = NULL; + caifd->xoff_skb_dtor = NULL; + + spin_unlock_bh(&caifd->flow_lock); + + if (dtor && skb) + dtor(skb); + + if (send_xoff) + caifd->layer.up-> + ctrlcmd(caifd->layer.up, + _CAIF_CTRLCMD_PHYIF_FLOW_ON_IND, + caifd->layer.id); + caifd_put(caifd); +} + +static int transmit(struct cflayer *layer, struct cfpkt *pkt) +{ + int err, high = 0, qlen = 0; + struct caif_device_entry *caifd = + container_of(layer, struct caif_device_entry, layer); + struct sk_buff *skb; + struct netdev_queue *txq; + + rcu_read_lock_bh(); + + skb = cfpkt_tonative(pkt); + skb->dev = caifd->netdev; + skb_reset_network_header(skb); + skb->protocol = htons(ETH_P_CAIF); + + /* Check if we need to handle xoff */ + if (likely(caifd->netdev->priv_flags & IFF_NO_QUEUE)) + goto noxoff; + + if (unlikely(caifd->xoff)) + goto noxoff; + + if (likely(!netif_queue_stopped(caifd->netdev))) { + /* If we run with a TX queue, check if the queue is too long*/ + txq = netdev_get_tx_queue(skb->dev, 0); + qlen = qdisc_qlen(rcu_dereference_bh(txq->qdisc)); + + if (likely(qlen == 0)) + goto noxoff; + + high = (caifd->netdev->tx_queue_len * q_high) / 100; + if (likely(qlen < high)) + goto noxoff; + } + + /* Hold lock while accessing xoff */ + spin_lock_bh(&caifd->flow_lock); + if (caifd->xoff) { + spin_unlock_bh(&caifd->flow_lock); + goto noxoff; + } + + /* + * Handle flow off, we do this by temporary hi-jacking this + * skb's destructor function, and replace it with our own + * flow-on callback. The callback will set flow-on and call + * the original destructor. + */ + + pr_debug("queue has stopped(%d) or is full (%d > %d)\n", + netif_queue_stopped(caifd->netdev), + qlen, high); + caifd->xoff = 1; + caifd->xoff_skb = skb; + caifd->xoff_skb_dtor = skb->destructor; + skb->destructor = caif_flow_cb; + spin_unlock_bh(&caifd->flow_lock); + + caifd->layer.up->ctrlcmd(caifd->layer.up, + _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND, + caifd->layer.id); +noxoff: + rcu_read_unlock_bh(); + + err = dev_queue_xmit(skb); + if (err > 0) + err = -EIO; + + return err; +} + +/* + * Stuff received packets into the CAIF stack. + * On error, returns non-zero and releases the skb. + */ +static int receive(struct sk_buff *skb, struct net_device *dev, + struct packet_type *pkttype, struct net_device *orig_dev) +{ + struct cfpkt *pkt; + struct caif_device_entry *caifd; + int err; + + pkt = cfpkt_fromnative(CAIF_DIR_IN, skb); + + rcu_read_lock(); + caifd = caif_get(dev); + + if (!caifd || !caifd->layer.up || !caifd->layer.up->receive || + !netif_oper_up(caifd->netdev)) { + rcu_read_unlock(); + kfree_skb(skb); + return NET_RX_DROP; + } + + /* Hold reference to netdevice while using CAIF stack */ + caifd_hold(caifd); + rcu_read_unlock(); + + err = caifd->layer.up->receive(caifd->layer.up, pkt); + + /* For -EILSEQ the packet is not freed so so it now */ + if (err == -EILSEQ) + cfpkt_destroy(pkt); + + /* Release reference to stack upwards */ + caifd_put(caifd); + + if (err != 0) + err = NET_RX_DROP; + return err; +} + +static struct packet_type caif_packet_type __read_mostly = { + .type = cpu_to_be16(ETH_P_CAIF), + .func = receive, +}; + +static void dev_flowctrl(struct net_device *dev, int on) +{ + struct caif_device_entry *caifd; + + rcu_read_lock(); + + caifd = caif_get(dev); + if (!caifd || !caifd->layer.up || !caifd->layer.up->ctrlcmd) { + rcu_read_unlock(); + return; + } + + caifd_hold(caifd); + rcu_read_unlock(); + + caifd->layer.up->ctrlcmd(caifd->layer.up, + on ? + _CAIF_CTRLCMD_PHYIF_FLOW_ON_IND : + _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND, + caifd->layer.id); + caifd_put(caifd); +} + +int caif_enroll_dev(struct net_device *dev, struct caif_dev_common *caifdev, + struct cflayer *link_support, int head_room, + struct cflayer **layer, + int (**rcv_func)(struct sk_buff *, struct net_device *, + struct packet_type *, + struct net_device *)) +{ + struct caif_device_entry *caifd; + enum cfcnfg_phy_preference pref; + struct cfcnfg *cfg = get_cfcnfg(dev_net(dev)); + struct caif_device_entry_list *caifdevs; + int res; + + caifdevs = caif_device_list(dev_net(dev)); + caifd = caif_device_alloc(dev); + if (!caifd) + return -ENOMEM; + *layer = &caifd->layer; + spin_lock_init(&caifd->flow_lock); + + switch (caifdev->link_select) { + case CAIF_LINK_HIGH_BANDW: + pref = CFPHYPREF_HIGH_BW; + break; + case CAIF_LINK_LOW_LATENCY: + pref = CFPHYPREF_LOW_LAT; + break; + default: + pref = CFPHYPREF_HIGH_BW; + break; + } + mutex_lock(&caifdevs->lock); + list_add_rcu(&caifd->list, &caifdevs->list); + + strlcpy(caifd->layer.name, dev->name, + sizeof(caifd->layer.name)); + caifd->layer.transmit = transmit; + res = cfcnfg_add_phy_layer(cfg, + dev, + &caifd->layer, + pref, + link_support, + caifdev->use_fcs, + head_room); + mutex_unlock(&caifdevs->lock); + if (rcv_func) + *rcv_func = receive; + return res; +} +EXPORT_SYMBOL(caif_enroll_dev); + +/* notify Caif of device events */ +static int caif_device_notify(struct notifier_block *me, unsigned long what, + void *ptr) +{ + struct net_device *dev = netdev_notifier_info_to_dev(ptr); + struct caif_device_entry *caifd = NULL; + struct caif_dev_common *caifdev; + struct cfcnfg *cfg; + struct cflayer *layer, *link_support; + int head_room = 0; + struct caif_device_entry_list *caifdevs; + int res; + + cfg = get_cfcnfg(dev_net(dev)); + caifdevs = caif_device_list(dev_net(dev)); + + caifd = caif_get(dev); + if (caifd == NULL && dev->type != ARPHRD_CAIF) + return 0; + + switch (what) { + case NETDEV_REGISTER: + if (caifd != NULL) + break; + + caifdev = netdev_priv(dev); + + link_support = NULL; + if (caifdev->use_frag) { + head_room = 1; + link_support = cfserl_create(dev->ifindex, + caifdev->use_stx); + if (!link_support) { + pr_warn("Out of memory\n"); + break; + } + } + res = caif_enroll_dev(dev, caifdev, link_support, head_room, + &layer, NULL); + if (res) + cfserl_release(link_support); + caifdev->flowctrl = dev_flowctrl; + break; + + case NETDEV_UP: + rcu_read_lock(); + + caifd = caif_get(dev); + if (caifd == NULL) { + rcu_read_unlock(); + break; + } + + caifd->xoff = 0; + cfcnfg_set_phy_state(cfg, &caifd->layer, true); + rcu_read_unlock(); + + break; + + case NETDEV_DOWN: + rcu_read_lock(); + + caifd = caif_get(dev); + if (!caifd || !caifd->layer.up || !caifd->layer.up->ctrlcmd) { + rcu_read_unlock(); + return -EINVAL; + } + + cfcnfg_set_phy_state(cfg, &caifd->layer, false); + caifd_hold(caifd); + rcu_read_unlock(); + + caifd->layer.up->ctrlcmd(caifd->layer.up, + _CAIF_CTRLCMD_PHYIF_DOWN_IND, + caifd->layer.id); + + spin_lock_bh(&caifd->flow_lock); + + /* + * Replace our xoff-destructor with original destructor. + * We trust that skb->destructor *always* is called before + * the skb reference is invalid. The hijacked SKB destructor + * takes the flow_lock so manipulating the skb->destructor here + * should be safe. + */ + if (caifd->xoff_skb_dtor != NULL && caifd->xoff_skb != NULL) + caifd->xoff_skb->destructor = caifd->xoff_skb_dtor; + + caifd->xoff = 0; + caifd->xoff_skb_dtor = NULL; + caifd->xoff_skb = NULL; + + spin_unlock_bh(&caifd->flow_lock); + caifd_put(caifd); + break; + + case NETDEV_UNREGISTER: + mutex_lock(&caifdevs->lock); + + caifd = caif_get(dev); + if (caifd == NULL) { + mutex_unlock(&caifdevs->lock); + break; + } + list_del_rcu(&caifd->list); + + /* + * NETDEV_UNREGISTER is called repeatedly until all reference + * counts for the net-device are released. If references to + * caifd is taken, simply ignore NETDEV_UNREGISTER and wait for + * the next call to NETDEV_UNREGISTER. + * + * If any packets are in flight down the CAIF Stack, + * cfcnfg_del_phy_layer will return nonzero. + * If no packets are in flight, the CAIF Stack associated + * with the net-device un-registering is freed. + */ + + if (caifd_refcnt_read(caifd) != 0 || + cfcnfg_del_phy_layer(cfg, &caifd->layer) != 0) { + + pr_info("Wait for device inuse\n"); + /* Enrole device if CAIF Stack is still in use */ + list_add_rcu(&caifd->list, &caifdevs->list); + mutex_unlock(&caifdevs->lock); + break; + } + + synchronize_rcu(); + dev_put(caifd->netdev); + free_percpu(caifd->pcpu_refcnt); + kfree(caifd); + + mutex_unlock(&caifdevs->lock); + break; + } + return 0; +} + +static struct notifier_block caif_device_notifier = { + .notifier_call = caif_device_notify, + .priority = 0, +}; + +/* Per-namespace Caif devices handling */ +static int caif_init_net(struct net *net) +{ + struct caif_net *caifn = net_generic(net, caif_net_id); + INIT_LIST_HEAD(&caifn->caifdevs.list); + mutex_init(&caifn->caifdevs.lock); + + caifn->cfg = cfcnfg_create(); + if (!caifn->cfg) + return -ENOMEM; + + return 0; +} + +static void caif_exit_net(struct net *net) +{ + struct caif_device_entry *caifd, *tmp; + struct caif_device_entry_list *caifdevs = + caif_device_list(net); + struct cfcnfg *cfg = get_cfcnfg(net); + + rtnl_lock(); + mutex_lock(&caifdevs->lock); + + list_for_each_entry_safe(caifd, tmp, &caifdevs->list, list) { + int i = 0; + list_del_rcu(&caifd->list); + cfcnfg_set_phy_state(cfg, &caifd->layer, false); + + while (i < 10 && + (caifd_refcnt_read(caifd) != 0 || + cfcnfg_del_phy_layer(cfg, &caifd->layer) != 0)) { + + pr_info("Wait for device inuse\n"); + msleep(250); + i++; + } + synchronize_rcu(); + dev_put(caifd->netdev); + free_percpu(caifd->pcpu_refcnt); + kfree(caifd); + } + cfcnfg_remove(cfg); + + mutex_unlock(&caifdevs->lock); + rtnl_unlock(); +} + +static struct pernet_operations caif_net_ops = { + .init = caif_init_net, + .exit = caif_exit_net, + .id = &caif_net_id, + .size = sizeof(struct caif_net), +}; + +/* Initialize Caif devices list */ +static int __init caif_device_init(void) +{ + int result; + + result = register_pernet_subsys(&caif_net_ops); + + if (result) + return result; + + register_netdevice_notifier(&caif_device_notifier); + dev_add_pack(&caif_packet_type); + + return result; +} + +static void __exit caif_device_exit(void) +{ + unregister_netdevice_notifier(&caif_device_notifier); + dev_remove_pack(&caif_packet_type); + unregister_pernet_subsys(&caif_net_ops); +} + +module_init(caif_device_init); +module_exit(caif_device_exit); diff --git a/net/caif/caif_socket.c b/net/caif/caif_socket.c new file mode 100644 index 000000000..348b8cb0b --- /dev/null +++ b/net/caif/caif_socket.c @@ -0,0 +1,1125 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/fs.h> +#include <linux/init.h> +#include <linux/module.h> +#include <linux/sched/signal.h> +#include <linux/spinlock.h> +#include <linux/mutex.h> +#include <linux/list.h> +#include <linux/wait.h> +#include <linux/poll.h> +#include <linux/tcp.h> +#include <linux/uaccess.h> +#include <linux/debugfs.h> +#include <linux/caif/caif_socket.h> +#include <linux/pkt_sched.h> +#include <net/sock.h> +#include <net/tcp_states.h> +#include <net/caif/caif_layer.h> +#include <net/caif/caif_dev.h> +#include <net/caif/cfpkt.h> + +MODULE_LICENSE("GPL"); +MODULE_ALIAS_NETPROTO(AF_CAIF); + +/* + * CAIF state is re-using the TCP socket states. + * caif_states stored in sk_state reflect the state as reported by + * the CAIF stack, while sk_socket->state is the state of the socket. + */ +enum caif_states { + CAIF_CONNECTED = TCP_ESTABLISHED, + CAIF_CONNECTING = TCP_SYN_SENT, + CAIF_DISCONNECTED = TCP_CLOSE +}; + +#define TX_FLOW_ON_BIT 1 +#define RX_FLOW_ON_BIT 2 + +struct caifsock { + struct sock sk; /* must be first member */ + struct cflayer layer; + u32 flow_state; + struct caif_connect_request conn_req; + struct mutex readlock; + struct dentry *debugfs_socket_dir; + int headroom, tailroom, maxframe; +}; + +static int rx_flow_is_on(struct caifsock *cf_sk) +{ + return test_bit(RX_FLOW_ON_BIT, + (void *) &cf_sk->flow_state); +} + +static int tx_flow_is_on(struct caifsock *cf_sk) +{ + return test_bit(TX_FLOW_ON_BIT, + (void *) &cf_sk->flow_state); +} + +static void set_rx_flow_off(struct caifsock *cf_sk) +{ + clear_bit(RX_FLOW_ON_BIT, + (void *) &cf_sk->flow_state); +} + +static void set_rx_flow_on(struct caifsock *cf_sk) +{ + set_bit(RX_FLOW_ON_BIT, + (void *) &cf_sk->flow_state); +} + +static void set_tx_flow_off(struct caifsock *cf_sk) +{ + clear_bit(TX_FLOW_ON_BIT, + (void *) &cf_sk->flow_state); +} + +static void set_tx_flow_on(struct caifsock *cf_sk) +{ + set_bit(TX_FLOW_ON_BIT, + (void *) &cf_sk->flow_state); +} + +static void caif_read_lock(struct sock *sk) +{ + struct caifsock *cf_sk; + cf_sk = container_of(sk, struct caifsock, sk); + mutex_lock(&cf_sk->readlock); +} + +static void caif_read_unlock(struct sock *sk) +{ + struct caifsock *cf_sk; + cf_sk = container_of(sk, struct caifsock, sk); + mutex_unlock(&cf_sk->readlock); +} + +static int sk_rcvbuf_lowwater(struct caifsock *cf_sk) +{ + /* A quarter of full buffer is used a low water mark */ + return cf_sk->sk.sk_rcvbuf / 4; +} + +static void caif_flow_ctrl(struct sock *sk, int mode) +{ + struct caifsock *cf_sk; + cf_sk = container_of(sk, struct caifsock, sk); + if (cf_sk->layer.dn && cf_sk->layer.dn->modemcmd) + cf_sk->layer.dn->modemcmd(cf_sk->layer.dn, mode); +} + +/* + * Copied from sock.c:sock_queue_rcv_skb(), but changed so packets are + * not dropped, but CAIF is sending flow off instead. + */ +static void caif_queue_rcv_skb(struct sock *sk, struct sk_buff *skb) +{ + int err; + unsigned long flags; + struct sk_buff_head *list = &sk->sk_receive_queue; + struct caifsock *cf_sk = container_of(sk, struct caifsock, sk); + bool queued = false; + + if (atomic_read(&sk->sk_rmem_alloc) + skb->truesize >= + (unsigned int)sk->sk_rcvbuf && rx_flow_is_on(cf_sk)) { + net_dbg_ratelimited("sending flow OFF (queue len = %d %d)\n", + atomic_read(&cf_sk->sk.sk_rmem_alloc), + sk_rcvbuf_lowwater(cf_sk)); + set_rx_flow_off(cf_sk); + caif_flow_ctrl(sk, CAIF_MODEMCMD_FLOW_OFF_REQ); + } + + err = sk_filter(sk, skb); + if (err) + goto out; + + if (!sk_rmem_schedule(sk, skb, skb->truesize) && rx_flow_is_on(cf_sk)) { + set_rx_flow_off(cf_sk); + net_dbg_ratelimited("sending flow OFF due to rmem_schedule\n"); + caif_flow_ctrl(sk, CAIF_MODEMCMD_FLOW_OFF_REQ); + } + skb->dev = NULL; + skb_set_owner_r(skb, sk); + spin_lock_irqsave(&list->lock, flags); + queued = !sock_flag(sk, SOCK_DEAD); + if (queued) + __skb_queue_tail(list, skb); + spin_unlock_irqrestore(&list->lock, flags); +out: + if (queued) + sk->sk_data_ready(sk); + else + kfree_skb(skb); +} + +/* Packet Receive Callback function called from CAIF Stack */ +static int caif_sktrecv_cb(struct cflayer *layr, struct cfpkt *pkt) +{ + struct caifsock *cf_sk; + struct sk_buff *skb; + + cf_sk = container_of(layr, struct caifsock, layer); + skb = cfpkt_tonative(pkt); + + if (unlikely(cf_sk->sk.sk_state != CAIF_CONNECTED)) { + kfree_skb(skb); + return 0; + } + caif_queue_rcv_skb(&cf_sk->sk, skb); + return 0; +} + +static void cfsk_hold(struct cflayer *layr) +{ + struct caifsock *cf_sk = container_of(layr, struct caifsock, layer); + sock_hold(&cf_sk->sk); +} + +static void cfsk_put(struct cflayer *layr) +{ + struct caifsock *cf_sk = container_of(layr, struct caifsock, layer); + sock_put(&cf_sk->sk); +} + +/* Packet Control Callback function called from CAIF */ +static void caif_ctrl_cb(struct cflayer *layr, + enum caif_ctrlcmd flow, + int phyid) +{ + struct caifsock *cf_sk = container_of(layr, struct caifsock, layer); + switch (flow) { + case CAIF_CTRLCMD_FLOW_ON_IND: + /* OK from modem to start sending again */ + set_tx_flow_on(cf_sk); + cf_sk->sk.sk_state_change(&cf_sk->sk); + break; + + case CAIF_CTRLCMD_FLOW_OFF_IND: + /* Modem asks us to shut up */ + set_tx_flow_off(cf_sk); + cf_sk->sk.sk_state_change(&cf_sk->sk); + break; + + case CAIF_CTRLCMD_INIT_RSP: + /* We're now connected */ + caif_client_register_refcnt(&cf_sk->layer, + cfsk_hold, cfsk_put); + cf_sk->sk.sk_state = CAIF_CONNECTED; + set_tx_flow_on(cf_sk); + cf_sk->sk.sk_shutdown = 0; + cf_sk->sk.sk_state_change(&cf_sk->sk); + break; + + case CAIF_CTRLCMD_DEINIT_RSP: + /* We're now disconnected */ + cf_sk->sk.sk_state = CAIF_DISCONNECTED; + cf_sk->sk.sk_state_change(&cf_sk->sk); + break; + + case CAIF_CTRLCMD_INIT_FAIL_RSP: + /* Connect request failed */ + cf_sk->sk.sk_err = ECONNREFUSED; + cf_sk->sk.sk_state = CAIF_DISCONNECTED; + cf_sk->sk.sk_shutdown = SHUTDOWN_MASK; + /* + * Socket "standards" seems to require POLLOUT to + * be set at connect failure. + */ + set_tx_flow_on(cf_sk); + cf_sk->sk.sk_state_change(&cf_sk->sk); + break; + + case CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND: + /* Modem has closed this connection, or device is down. */ + cf_sk->sk.sk_shutdown = SHUTDOWN_MASK; + cf_sk->sk.sk_err = ECONNRESET; + set_rx_flow_on(cf_sk); + cf_sk->sk.sk_error_report(&cf_sk->sk); + break; + + default: + pr_debug("Unexpected flow command %d\n", flow); + } +} + +static void caif_check_flow_release(struct sock *sk) +{ + struct caifsock *cf_sk = container_of(sk, struct caifsock, sk); + + if (rx_flow_is_on(cf_sk)) + return; + + if (atomic_read(&sk->sk_rmem_alloc) <= sk_rcvbuf_lowwater(cf_sk)) { + set_rx_flow_on(cf_sk); + caif_flow_ctrl(sk, CAIF_MODEMCMD_FLOW_ON_REQ); + } +} + +/* + * Copied from unix_dgram_recvmsg, but removed credit checks, + * changed locking, address handling and added MSG_TRUNC. + */ +static int caif_seqpkt_recvmsg(struct socket *sock, struct msghdr *m, + size_t len, int flags) + +{ + struct sock *sk = sock->sk; + struct sk_buff *skb; + int ret; + int copylen; + + ret = -EOPNOTSUPP; + if (flags & MSG_OOB) + goto read_error; + + skb = skb_recv_datagram(sk, flags, 0 , &ret); + if (!skb) + goto read_error; + copylen = skb->len; + if (len < copylen) { + m->msg_flags |= MSG_TRUNC; + copylen = len; + } + + ret = skb_copy_datagram_msg(skb, 0, m, copylen); + if (ret) + goto out_free; + + ret = (flags & MSG_TRUNC) ? skb->len : copylen; +out_free: + skb_free_datagram(sk, skb); + caif_check_flow_release(sk); + return ret; + +read_error: + return ret; +} + + +/* Copied from unix_stream_wait_data, identical except for lock call. */ +static long caif_stream_data_wait(struct sock *sk, long timeo) +{ + DEFINE_WAIT(wait); + lock_sock(sk); + + for (;;) { + prepare_to_wait(sk_sleep(sk), &wait, TASK_INTERRUPTIBLE); + + if (!skb_queue_empty(&sk->sk_receive_queue) || + sk->sk_err || + sk->sk_state != CAIF_CONNECTED || + sock_flag(sk, SOCK_DEAD) || + (sk->sk_shutdown & RCV_SHUTDOWN) || + signal_pending(current) || + !timeo) + break; + + sk_set_bit(SOCKWQ_ASYNC_WAITDATA, sk); + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + + if (sock_flag(sk, SOCK_DEAD)) + break; + + sk_clear_bit(SOCKWQ_ASYNC_WAITDATA, sk); + } + + finish_wait(sk_sleep(sk), &wait); + release_sock(sk); + return timeo; +} + + +/* + * Copied from unix_stream_recvmsg, but removed credit checks, + * changed locking calls, changed address handling. + */ +static int caif_stream_recvmsg(struct socket *sock, struct msghdr *msg, + size_t size, int flags) +{ + struct sock *sk = sock->sk; + int copied = 0; + int target; + int err = 0; + long timeo; + + err = -EOPNOTSUPP; + if (flags&MSG_OOB) + goto out; + + /* + * Lock the socket to prevent queue disordering + * while sleeps in memcpy_tomsg + */ + err = -EAGAIN; + if (sk->sk_state == CAIF_CONNECTING) + goto out; + + caif_read_lock(sk); + target = sock_rcvlowat(sk, flags&MSG_WAITALL, size); + timeo = sock_rcvtimeo(sk, flags&MSG_DONTWAIT); + + do { + int chunk; + struct sk_buff *skb; + + lock_sock(sk); + if (sock_flag(sk, SOCK_DEAD)) { + err = -ECONNRESET; + goto unlock; + } + skb = skb_dequeue(&sk->sk_receive_queue); + caif_check_flow_release(sk); + + if (skb == NULL) { + if (copied >= target) + goto unlock; + /* + * POSIX 1003.1g mandates this order. + */ + err = sock_error(sk); + if (err) + goto unlock; + err = -ECONNRESET; + if (sk->sk_shutdown & RCV_SHUTDOWN) + goto unlock; + + err = -EPIPE; + if (sk->sk_state != CAIF_CONNECTED) + goto unlock; + if (sock_flag(sk, SOCK_DEAD)) + goto unlock; + + release_sock(sk); + + err = -EAGAIN; + if (!timeo) + break; + + caif_read_unlock(sk); + + timeo = caif_stream_data_wait(sk, timeo); + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + goto out; + } + caif_read_lock(sk); + continue; +unlock: + release_sock(sk); + break; + } + release_sock(sk); + chunk = min_t(unsigned int, skb->len, size); + if (memcpy_to_msg(msg, skb->data, chunk)) { + skb_queue_head(&sk->sk_receive_queue, skb); + if (copied == 0) + copied = -EFAULT; + break; + } + copied += chunk; + size -= chunk; + + /* Mark read part of skb as used */ + if (!(flags & MSG_PEEK)) { + skb_pull(skb, chunk); + + /* put the skb back if we didn't use it up. */ + if (skb->len) { + skb_queue_head(&sk->sk_receive_queue, skb); + break; + } + kfree_skb(skb); + + } else { + /* + * It is questionable, see note in unix_dgram_recvmsg. + */ + /* put message back and return */ + skb_queue_head(&sk->sk_receive_queue, skb); + break; + } + } while (size); + caif_read_unlock(sk); + +out: + return copied ? : err; +} + +/* + * Copied from sock.c:sock_wait_for_wmem, but change to wait for + * CAIF flow-on and sock_writable. + */ +static long caif_wait_for_flow_on(struct caifsock *cf_sk, + int wait_writeable, long timeo, int *err) +{ + struct sock *sk = &cf_sk->sk; + DEFINE_WAIT(wait); + for (;;) { + *err = 0; + if (tx_flow_is_on(cf_sk) && + (!wait_writeable || sock_writeable(&cf_sk->sk))) + break; + *err = -ETIMEDOUT; + if (!timeo) + break; + *err = -ERESTARTSYS; + if (signal_pending(current)) + break; + prepare_to_wait(sk_sleep(sk), &wait, TASK_INTERRUPTIBLE); + *err = -ECONNRESET; + if (sk->sk_shutdown & SHUTDOWN_MASK) + break; + *err = -sk->sk_err; + if (sk->sk_err) + break; + *err = -EPIPE; + if (cf_sk->sk.sk_state != CAIF_CONNECTED) + break; + timeo = schedule_timeout(timeo); + } + finish_wait(sk_sleep(sk), &wait); + return timeo; +} + +/* + * Transmit a SKB. The device may temporarily request re-transmission + * by returning EAGAIN. + */ +static int transmit_skb(struct sk_buff *skb, struct caifsock *cf_sk, + int noblock, long timeo) +{ + struct cfpkt *pkt; + + pkt = cfpkt_fromnative(CAIF_DIR_OUT, skb); + memset(skb->cb, 0, sizeof(struct caif_payload_info)); + cfpkt_set_prio(pkt, cf_sk->sk.sk_priority); + + if (cf_sk->layer.dn == NULL) { + kfree_skb(skb); + return -EINVAL; + } + + return cf_sk->layer.dn->transmit(cf_sk->layer.dn, pkt); +} + +/* Copied from af_unix:unix_dgram_sendmsg, and adapted to CAIF */ +static int caif_seqpkt_sendmsg(struct socket *sock, struct msghdr *msg, + size_t len) +{ + struct sock *sk = sock->sk; + struct caifsock *cf_sk = container_of(sk, struct caifsock, sk); + int buffer_size; + int ret = 0; + struct sk_buff *skb = NULL; + int noblock; + long timeo; + caif_assert(cf_sk); + ret = sock_error(sk); + if (ret) + goto err; + + ret = -EOPNOTSUPP; + if (msg->msg_flags&MSG_OOB) + goto err; + + ret = -EOPNOTSUPP; + if (msg->msg_namelen) + goto err; + + ret = -EINVAL; + if (unlikely(msg->msg_iter.nr_segs == 0) || + unlikely(msg->msg_iter.iov->iov_base == NULL)) + goto err; + noblock = msg->msg_flags & MSG_DONTWAIT; + + timeo = sock_sndtimeo(sk, noblock); + timeo = caif_wait_for_flow_on(container_of(sk, struct caifsock, sk), + 1, timeo, &ret); + + if (ret) + goto err; + ret = -EPIPE; + if (cf_sk->sk.sk_state != CAIF_CONNECTED || + sock_flag(sk, SOCK_DEAD) || + (sk->sk_shutdown & RCV_SHUTDOWN)) + goto err; + + /* Error if trying to write more than maximum frame size. */ + ret = -EMSGSIZE; + if (len > cf_sk->maxframe && cf_sk->sk.sk_protocol != CAIFPROTO_RFM) + goto err; + + buffer_size = len + cf_sk->headroom + cf_sk->tailroom; + + ret = -ENOMEM; + skb = sock_alloc_send_skb(sk, buffer_size, noblock, &ret); + + if (!skb || skb_tailroom(skb) < buffer_size) + goto err; + + skb_reserve(skb, cf_sk->headroom); + + ret = memcpy_from_msg(skb_put(skb, len), msg, len); + + if (ret) + goto err; + ret = transmit_skb(skb, cf_sk, noblock, timeo); + if (ret < 0) + /* skb is already freed */ + return ret; + + return len; +err: + kfree_skb(skb); + return ret; +} + +/* + * Copied from unix_stream_sendmsg and adapted to CAIF: + * Changed removed permission handling and added waiting for flow on + * and other minor adaptations. + */ +static int caif_stream_sendmsg(struct socket *sock, struct msghdr *msg, + size_t len) +{ + struct sock *sk = sock->sk; + struct caifsock *cf_sk = container_of(sk, struct caifsock, sk); + int err, size; + struct sk_buff *skb; + int sent = 0; + long timeo; + + err = -EOPNOTSUPP; + if (unlikely(msg->msg_flags&MSG_OOB)) + goto out_err; + + if (unlikely(msg->msg_namelen)) + goto out_err; + + timeo = sock_sndtimeo(sk, msg->msg_flags & MSG_DONTWAIT); + timeo = caif_wait_for_flow_on(cf_sk, 1, timeo, &err); + + if (unlikely(sk->sk_shutdown & SEND_SHUTDOWN)) + goto pipe_err; + + while (sent < len) { + + size = len-sent; + + if (size > cf_sk->maxframe) + size = cf_sk->maxframe; + + /* If size is more than half of sndbuf, chop up message */ + if (size > ((sk->sk_sndbuf >> 1) - 64)) + size = (sk->sk_sndbuf >> 1) - 64; + + if (size > SKB_MAX_ALLOC) + size = SKB_MAX_ALLOC; + + skb = sock_alloc_send_skb(sk, + size + cf_sk->headroom + + cf_sk->tailroom, + msg->msg_flags&MSG_DONTWAIT, + &err); + if (skb == NULL) + goto out_err; + + skb_reserve(skb, cf_sk->headroom); + /* + * If you pass two values to the sock_alloc_send_skb + * it tries to grab the large buffer with GFP_NOFS + * (which can fail easily), and if it fails grab the + * fallback size buffer which is under a page and will + * succeed. [Alan] + */ + size = min_t(int, size, skb_tailroom(skb)); + + err = memcpy_from_msg(skb_put(skb, size), msg, size); + if (err) { + kfree_skb(skb); + goto out_err; + } + err = transmit_skb(skb, cf_sk, + msg->msg_flags&MSG_DONTWAIT, timeo); + if (err < 0) + /* skb is already freed */ + goto pipe_err; + + sent += size; + } + + return sent; + +pipe_err: + if (sent == 0 && !(msg->msg_flags&MSG_NOSIGNAL)) + send_sig(SIGPIPE, current, 0); + err = -EPIPE; +out_err: + return sent ? : err; +} + +static int setsockopt(struct socket *sock, + int lvl, int opt, char __user *ov, unsigned int ol) +{ + struct sock *sk = sock->sk; + struct caifsock *cf_sk = container_of(sk, struct caifsock, sk); + int linksel; + + if (cf_sk->sk.sk_socket->state != SS_UNCONNECTED) + return -ENOPROTOOPT; + + switch (opt) { + case CAIFSO_LINK_SELECT: + if (ol < sizeof(int)) + return -EINVAL; + if (lvl != SOL_CAIF) + goto bad_sol; + if (copy_from_user(&linksel, ov, sizeof(int))) + return -EINVAL; + lock_sock(&(cf_sk->sk)); + cf_sk->conn_req.link_selector = linksel; + release_sock(&cf_sk->sk); + return 0; + + case CAIFSO_REQ_PARAM: + if (lvl != SOL_CAIF) + goto bad_sol; + if (cf_sk->sk.sk_protocol != CAIFPROTO_UTIL) + return -ENOPROTOOPT; + lock_sock(&(cf_sk->sk)); + if (ol > sizeof(cf_sk->conn_req.param.data) || + copy_from_user(&cf_sk->conn_req.param.data, ov, ol)) { + release_sock(&cf_sk->sk); + return -EINVAL; + } + cf_sk->conn_req.param.size = ol; + release_sock(&cf_sk->sk); + return 0; + + default: + return -ENOPROTOOPT; + } + + return 0; +bad_sol: + return -ENOPROTOOPT; + +} + +/* + * caif_connect() - Connect a CAIF Socket + * Copied and modified af_irda.c:irda_connect(). + * + * Note : by consulting "errno", the user space caller may learn the cause + * of the failure. Most of them are visible in the function, others may come + * from subroutines called and are listed here : + * o -EAFNOSUPPORT: bad socket family or type. + * o -ESOCKTNOSUPPORT: bad socket type or protocol + * o -EINVAL: bad socket address, or CAIF link type + * o -ECONNREFUSED: remote end refused the connection. + * o -EINPROGRESS: connect request sent but timed out (or non-blocking) + * o -EISCONN: already connected. + * o -ETIMEDOUT: Connection timed out (send timeout) + * o -ENODEV: No link layer to send request + * o -ECONNRESET: Received Shutdown indication or lost link layer + * o -ENOMEM: Out of memory + * + * State Strategy: + * o sk_state: holds the CAIF_* protocol state, it's updated by + * caif_ctrl_cb. + * o sock->state: holds the SS_* socket state and is updated by connect and + * disconnect. + */ +static int caif_connect(struct socket *sock, struct sockaddr *uaddr, + int addr_len, int flags) +{ + struct sock *sk = sock->sk; + struct caifsock *cf_sk = container_of(sk, struct caifsock, sk); + long timeo; + int err; + int ifindex, headroom, tailroom; + unsigned int mtu; + struct net_device *dev; + + lock_sock(sk); + + err = -EINVAL; + if (addr_len < offsetofend(struct sockaddr, sa_family)) + goto out; + + err = -EAFNOSUPPORT; + if (uaddr->sa_family != AF_CAIF) + goto out; + + switch (sock->state) { + case SS_UNCONNECTED: + /* Normal case, a fresh connect */ + caif_assert(sk->sk_state == CAIF_DISCONNECTED); + break; + case SS_CONNECTING: + switch (sk->sk_state) { + case CAIF_CONNECTED: + sock->state = SS_CONNECTED; + err = -EISCONN; + goto out; + case CAIF_DISCONNECTED: + /* Reconnect allowed */ + break; + case CAIF_CONNECTING: + err = -EALREADY; + if (flags & O_NONBLOCK) + goto out; + goto wait_connect; + } + break; + case SS_CONNECTED: + caif_assert(sk->sk_state == CAIF_CONNECTED || + sk->sk_state == CAIF_DISCONNECTED); + if (sk->sk_shutdown & SHUTDOWN_MASK) { + /* Allow re-connect after SHUTDOWN_IND */ + caif_disconnect_client(sock_net(sk), &cf_sk->layer); + caif_free_client(&cf_sk->layer); + break; + } + /* No reconnect on a seqpacket socket */ + err = -EISCONN; + goto out; + case SS_DISCONNECTING: + case SS_FREE: + caif_assert(1); /*Should never happen */ + break; + } + sk->sk_state = CAIF_DISCONNECTED; + sock->state = SS_UNCONNECTED; + sk_stream_kill_queues(&cf_sk->sk); + + err = -EINVAL; + if (addr_len != sizeof(struct sockaddr_caif)) + goto out; + + memcpy(&cf_sk->conn_req.sockaddr, uaddr, + sizeof(struct sockaddr_caif)); + + /* Move to connecting socket, start sending Connect Requests */ + sock->state = SS_CONNECTING; + sk->sk_state = CAIF_CONNECTING; + + /* Check priority value comming from socket */ + /* if priority value is out of range it will be ajusted */ + if (cf_sk->sk.sk_priority > CAIF_PRIO_MAX) + cf_sk->conn_req.priority = CAIF_PRIO_MAX; + else if (cf_sk->sk.sk_priority < CAIF_PRIO_MIN) + cf_sk->conn_req.priority = CAIF_PRIO_MIN; + else + cf_sk->conn_req.priority = cf_sk->sk.sk_priority; + + /*ifindex = id of the interface.*/ + cf_sk->conn_req.ifindex = cf_sk->sk.sk_bound_dev_if; + + cf_sk->layer.receive = caif_sktrecv_cb; + + err = caif_connect_client(sock_net(sk), &cf_sk->conn_req, + &cf_sk->layer, &ifindex, &headroom, &tailroom); + + if (err < 0) { + cf_sk->sk.sk_socket->state = SS_UNCONNECTED; + cf_sk->sk.sk_state = CAIF_DISCONNECTED; + goto out; + } + + err = -ENODEV; + rcu_read_lock(); + dev = dev_get_by_index_rcu(sock_net(sk), ifindex); + if (!dev) { + rcu_read_unlock(); + goto out; + } + cf_sk->headroom = LL_RESERVED_SPACE_EXTRA(dev, headroom); + mtu = dev->mtu; + rcu_read_unlock(); + + cf_sk->tailroom = tailroom; + cf_sk->maxframe = mtu - (headroom + tailroom); + if (cf_sk->maxframe < 1) { + pr_warn("CAIF Interface MTU too small (%d)\n", dev->mtu); + err = -ENODEV; + goto out; + } + + err = -EINPROGRESS; +wait_connect: + + if (sk->sk_state != CAIF_CONNECTED && (flags & O_NONBLOCK)) + goto out; + + timeo = sock_sndtimeo(sk, flags & O_NONBLOCK); + + release_sock(sk); + err = -ERESTARTSYS; + timeo = wait_event_interruptible_timeout(*sk_sleep(sk), + sk->sk_state != CAIF_CONNECTING, + timeo); + lock_sock(sk); + if (timeo < 0) + goto out; /* -ERESTARTSYS */ + + err = -ETIMEDOUT; + if (timeo == 0 && sk->sk_state != CAIF_CONNECTED) + goto out; + if (sk->sk_state != CAIF_CONNECTED) { + sock->state = SS_UNCONNECTED; + err = sock_error(sk); + if (!err) + err = -ECONNREFUSED; + goto out; + } + sock->state = SS_CONNECTED; + err = 0; +out: + release_sock(sk); + return err; +} + +/* + * caif_release() - Disconnect a CAIF Socket + * Copied and modified af_irda.c:irda_release(). + */ +static int caif_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + struct caifsock *cf_sk = container_of(sk, struct caifsock, sk); + + if (!sk) + return 0; + + set_tx_flow_off(cf_sk); + + /* + * Ensure that packets are not queued after this point in time. + * caif_queue_rcv_skb checks SOCK_DEAD holding the queue lock, + * this ensures no packets when sock is dead. + */ + spin_lock_bh(&sk->sk_receive_queue.lock); + sock_set_flag(sk, SOCK_DEAD); + spin_unlock_bh(&sk->sk_receive_queue.lock); + sock->sk = NULL; + + WARN_ON(IS_ERR(cf_sk->debugfs_socket_dir)); + debugfs_remove_recursive(cf_sk->debugfs_socket_dir); + + lock_sock(&(cf_sk->sk)); + sk->sk_state = CAIF_DISCONNECTED; + sk->sk_shutdown = SHUTDOWN_MASK; + + caif_disconnect_client(sock_net(sk), &cf_sk->layer); + cf_sk->sk.sk_socket->state = SS_DISCONNECTING; + wake_up_interruptible_poll(sk_sleep(sk), EPOLLERR|EPOLLHUP); + + sock_orphan(sk); + sk_stream_kill_queues(&cf_sk->sk); + release_sock(sk); + sock_put(sk); + return 0; +} + +/* Copied from af_unix.c:unix_poll(), added CAIF tx_flow handling */ +static __poll_t caif_poll(struct file *file, + struct socket *sock, poll_table *wait) +{ + struct sock *sk = sock->sk; + __poll_t mask; + struct caifsock *cf_sk = container_of(sk, struct caifsock, sk); + + sock_poll_wait(file, sock, wait); + mask = 0; + + /* exceptional events? */ + if (sk->sk_err) + mask |= EPOLLERR; + if (sk->sk_shutdown == SHUTDOWN_MASK) + mask |= EPOLLHUP; + if (sk->sk_shutdown & RCV_SHUTDOWN) + mask |= EPOLLRDHUP; + + /* readable? */ + if (!skb_queue_empty_lockless(&sk->sk_receive_queue) || + (sk->sk_shutdown & RCV_SHUTDOWN)) + mask |= EPOLLIN | EPOLLRDNORM; + + /* + * we set writable also when the other side has shut down the + * connection. This prevents stuck sockets. + */ + if (sock_writeable(sk) && tx_flow_is_on(cf_sk)) + mask |= EPOLLOUT | EPOLLWRNORM | EPOLLWRBAND; + + return mask; +} + +static const struct proto_ops caif_seqpacket_ops = { + .family = PF_CAIF, + .owner = THIS_MODULE, + .release = caif_release, + .bind = sock_no_bind, + .connect = caif_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .getname = sock_no_getname, + .poll = caif_poll, + .ioctl = sock_no_ioctl, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = setsockopt, + .getsockopt = sock_no_getsockopt, + .sendmsg = caif_seqpkt_sendmsg, + .recvmsg = caif_seqpkt_recvmsg, + .mmap = sock_no_mmap, + .sendpage = sock_no_sendpage, +}; + +static const struct proto_ops caif_stream_ops = { + .family = PF_CAIF, + .owner = THIS_MODULE, + .release = caif_release, + .bind = sock_no_bind, + .connect = caif_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .getname = sock_no_getname, + .poll = caif_poll, + .ioctl = sock_no_ioctl, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = setsockopt, + .getsockopt = sock_no_getsockopt, + .sendmsg = caif_stream_sendmsg, + .recvmsg = caif_stream_recvmsg, + .mmap = sock_no_mmap, + .sendpage = sock_no_sendpage, +}; + +/* This function is called when a socket is finally destroyed. */ +static void caif_sock_destructor(struct sock *sk) +{ + struct caifsock *cf_sk = container_of(sk, struct caifsock, sk); + caif_assert(!refcount_read(&sk->sk_wmem_alloc)); + caif_assert(sk_unhashed(sk)); + caif_assert(!sk->sk_socket); + if (!sock_flag(sk, SOCK_DEAD)) { + pr_debug("Attempt to release alive CAIF socket: %p\n", sk); + return; + } + sk_stream_kill_queues(&cf_sk->sk); + caif_free_client(&cf_sk->layer); +} + +static int caif_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk = NULL; + struct caifsock *cf_sk = NULL; + static struct proto prot = {.name = "PF_CAIF", + .owner = THIS_MODULE, + .obj_size = sizeof(struct caifsock), + .useroffset = offsetof(struct caifsock, conn_req.param), + .usersize = sizeof_field(struct caifsock, conn_req.param) + }; + + if (!capable(CAP_SYS_ADMIN) && !capable(CAP_NET_ADMIN)) + return -EPERM; + /* + * The sock->type specifies the socket type to use. + * The CAIF socket is a packet stream in the sense + * that it is packet based. CAIF trusts the reliability + * of the link, no resending is implemented. + */ + if (sock->type == SOCK_SEQPACKET) + sock->ops = &caif_seqpacket_ops; + else if (sock->type == SOCK_STREAM) + sock->ops = &caif_stream_ops; + else + return -ESOCKTNOSUPPORT; + + if (protocol < 0 || protocol >= CAIFPROTO_MAX) + return -EPROTONOSUPPORT; + /* + * Set the socket state to unconnected. The socket state + * is really not used at all in the net/core or socket.c but the + * initialization makes sure that sock->state is not uninitialized. + */ + sk = sk_alloc(net, PF_CAIF, GFP_KERNEL, &prot, kern); + if (!sk) + return -ENOMEM; + + cf_sk = container_of(sk, struct caifsock, sk); + + /* Store the protocol */ + sk->sk_protocol = (unsigned char) protocol; + + /* Initialize default priority for well-known cases */ + switch (protocol) { + case CAIFPROTO_AT: + sk->sk_priority = TC_PRIO_CONTROL; + break; + case CAIFPROTO_RFM: + sk->sk_priority = TC_PRIO_INTERACTIVE_BULK; + break; + default: + sk->sk_priority = TC_PRIO_BESTEFFORT; + } + + /* + * Lock in order to try to stop someone from opening the socket + * too early. + */ + lock_sock(&(cf_sk->sk)); + + /* Initialize the nozero default sock structure data. */ + sock_init_data(sock, sk); + sk->sk_destruct = caif_sock_destructor; + + mutex_init(&cf_sk->readlock); /* single task reading lock */ + cf_sk->layer.ctrlcmd = caif_ctrl_cb; + cf_sk->sk.sk_socket->state = SS_UNCONNECTED; + cf_sk->sk.sk_state = CAIF_DISCONNECTED; + + set_tx_flow_off(cf_sk); + set_rx_flow_on(cf_sk); + + /* Set default options on configuration */ + cf_sk->conn_req.link_selector = CAIF_LINK_LOW_LATENCY; + cf_sk->conn_req.protocol = protocol; + release_sock(&cf_sk->sk); + return 0; +} + + +static const struct net_proto_family caif_family_ops = { + .family = PF_CAIF, + .create = caif_create, + .owner = THIS_MODULE, +}; + +static int __init caif_sktinit_module(void) +{ + return sock_register(&caif_family_ops); +} + +static void __exit caif_sktexit_module(void) +{ + sock_unregister(PF_CAIF); +} +module_init(caif_sktinit_module); +module_exit(caif_sktexit_module); diff --git a/net/caif/caif_usb.c b/net/caif/caif_usb.c new file mode 100644 index 000000000..76d49a1bc --- /dev/null +++ b/net/caif/caif_usb.c @@ -0,0 +1,213 @@ +/* + * CAIF USB handler + * Copyright (C) ST-Ericsson AB 2011 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + * + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/module.h> +#include <linux/netdevice.h> +#include <linux/slab.h> +#include <linux/mii.h> +#include <linux/usb.h> +#include <linux/usb/usbnet.h> +#include <linux/etherdevice.h> +#include <net/netns/generic.h> +#include <net/caif/caif_dev.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfpkt.h> +#include <net/caif/cfcnfg.h> + +MODULE_LICENSE("GPL"); + +#define CFUSB_PAD_DESCR_SZ 1 /* Alignment descriptor length */ +#define CFUSB_ALIGNMENT 4 /* Number of bytes to align. */ +#define CFUSB_MAX_HEADLEN (CFUSB_PAD_DESCR_SZ + CFUSB_ALIGNMENT-1) +#define STE_USB_VID 0x04cc /* USB Product ID for ST-Ericsson */ +#define STE_USB_PID_CAIF 0x230f /* Product id for CAIF Modems */ + +struct cfusbl { + struct cflayer layer; + u8 tx_eth_hdr[ETH_HLEN]; +}; + +static bool pack_added; + +static int cfusbl_receive(struct cflayer *layr, struct cfpkt *pkt) +{ + u8 hpad; + + /* Remove padding. */ + cfpkt_extr_head(pkt, &hpad, 1); + cfpkt_extr_head(pkt, NULL, hpad); + return layr->up->receive(layr->up, pkt); +} + +static int cfusbl_transmit(struct cflayer *layr, struct cfpkt *pkt) +{ + struct caif_payload_info *info; + u8 hpad; + u8 zeros[CFUSB_ALIGNMENT]; + struct sk_buff *skb; + struct cfusbl *usbl = container_of(layr, struct cfusbl, layer); + + skb = cfpkt_tonative(pkt); + + skb_reset_network_header(skb); + skb->protocol = htons(ETH_P_IP); + + info = cfpkt_info(pkt); + hpad = (info->hdr_len + CFUSB_PAD_DESCR_SZ) & (CFUSB_ALIGNMENT - 1); + + if (skb_headroom(skb) < ETH_HLEN + CFUSB_PAD_DESCR_SZ + hpad) { + pr_warn("Headroom to small\n"); + kfree_skb(skb); + return -EIO; + } + memset(zeros, 0, hpad); + + cfpkt_add_head(pkt, zeros, hpad); + cfpkt_add_head(pkt, &hpad, 1); + cfpkt_add_head(pkt, usbl->tx_eth_hdr, sizeof(usbl->tx_eth_hdr)); + return layr->dn->transmit(layr->dn, pkt); +} + +static void cfusbl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, + int phyid) +{ + if (layr->up && layr->up->ctrlcmd) + layr->up->ctrlcmd(layr->up, ctrl, layr->id); +} + +static struct cflayer *cfusbl_create(int phyid, u8 ethaddr[ETH_ALEN], + u8 braddr[ETH_ALEN]) +{ + struct cfusbl *this = kmalloc(sizeof(struct cfusbl), GFP_ATOMIC); + + if (!this) + return NULL; + + caif_assert(offsetof(struct cfusbl, layer) == 0); + + memset(&this->layer, 0, sizeof(this->layer)); + this->layer.receive = cfusbl_receive; + this->layer.transmit = cfusbl_transmit; + this->layer.ctrlcmd = cfusbl_ctrlcmd; + snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "usb%d", phyid); + this->layer.id = phyid; + + /* + * Construct TX ethernet header: + * 0-5 destination address + * 5-11 source address + * 12-13 protocol type + */ + ether_addr_copy(&this->tx_eth_hdr[ETH_ALEN], braddr); + ether_addr_copy(&this->tx_eth_hdr[ETH_ALEN], ethaddr); + this->tx_eth_hdr[12] = cpu_to_be16(ETH_P_802_EX1) & 0xff; + this->tx_eth_hdr[13] = (cpu_to_be16(ETH_P_802_EX1) >> 8) & 0xff; + pr_debug("caif ethernet TX-header dst:%pM src:%pM type:%02x%02x\n", + this->tx_eth_hdr, this->tx_eth_hdr + ETH_ALEN, + this->tx_eth_hdr[12], this->tx_eth_hdr[13]); + + return (struct cflayer *) this; +} + +static void cfusbl_release(struct cflayer *layer) +{ + kfree(layer); +} + +static struct packet_type caif_usb_type __read_mostly = { + .type = cpu_to_be16(ETH_P_802_EX1), +}; + +static int cfusbl_device_notify(struct notifier_block *me, unsigned long what, + void *ptr) +{ + struct net_device *dev = netdev_notifier_info_to_dev(ptr); + struct caif_dev_common common; + struct cflayer *layer, *link_support; + struct usbnet *usbnet; + struct usb_device *usbdev; + int res; + + /* Check whether we have a NCM device, and find its VID/PID. */ + if (!(dev->dev.parent && dev->dev.parent->driver && + strcmp(dev->dev.parent->driver->name, "cdc_ncm") == 0)) + return 0; + + usbnet = netdev_priv(dev); + usbdev = usbnet->udev; + + pr_debug("USB CDC NCM device VID:0x%4x PID:0x%4x\n", + le16_to_cpu(usbdev->descriptor.idVendor), + le16_to_cpu(usbdev->descriptor.idProduct)); + + /* Check for VID/PID that supports CAIF */ + if (!(le16_to_cpu(usbdev->descriptor.idVendor) == STE_USB_VID && + le16_to_cpu(usbdev->descriptor.idProduct) == STE_USB_PID_CAIF)) + return 0; + + if (what == NETDEV_UNREGISTER) + module_put(THIS_MODULE); + + if (what != NETDEV_REGISTER) + return 0; + + __module_get(THIS_MODULE); + + memset(&common, 0, sizeof(common)); + common.use_frag = false; + common.use_fcs = false; + common.use_stx = false; + common.link_select = CAIF_LINK_HIGH_BANDW; + common.flowctrl = NULL; + + link_support = cfusbl_create(dev->ifindex, dev->dev_addr, + dev->broadcast); + + if (!link_support) + return -ENOMEM; + + if (dev->num_tx_queues > 1) + pr_warn("USB device uses more than one tx queue\n"); + + res = caif_enroll_dev(dev, &common, link_support, CFUSB_MAX_HEADLEN, + &layer, &caif_usb_type.func); + if (res) + goto err; + + if (!pack_added) + dev_add_pack(&caif_usb_type); + pack_added = true; + + strlcpy(layer->name, dev->name, sizeof(layer->name)); + + return 0; +err: + cfusbl_release(link_support); + return res; +} + +static struct notifier_block caif_device_notifier = { + .notifier_call = cfusbl_device_notify, + .priority = 0, +}; + +static int __init cfusbl_init(void) +{ + return register_netdevice_notifier(&caif_device_notifier); +} + +static void __exit cfusbl_exit(void) +{ + unregister_netdevice_notifier(&caif_device_notifier); + dev_remove_pack(&caif_usb_type); +} + +module_init(cfusbl_init); +module_exit(cfusbl_exit); diff --git a/net/caif/cfcnfg.c b/net/caif/cfcnfg.c new file mode 100644 index 000000000..b456b79ab --- /dev/null +++ b/net/caif/cfcnfg.c @@ -0,0 +1,612 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/kernel.h> +#include <linux/stddef.h> +#include <linux/slab.h> +#include <linux/netdevice.h> +#include <linux/module.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfpkt.h> +#include <net/caif/cfcnfg.h> +#include <net/caif/cfctrl.h> +#include <net/caif/cfmuxl.h> +#include <net/caif/cffrml.h> +#include <net/caif/cfserl.h> +#include <net/caif/cfsrvl.h> +#include <net/caif/caif_dev.h> + +#define container_obj(layr) container_of(layr, struct cfcnfg, layer) + +/* Information about CAIF physical interfaces held by Config Module in order + * to manage physical interfaces + */ +struct cfcnfg_phyinfo { + struct list_head node; + bool up; + + /* Pointer to the layer below the MUX (framing layer) */ + struct cflayer *frm_layer; + /* Pointer to the lowest actual physical layer */ + struct cflayer *phy_layer; + /* Unique identifier of the physical interface */ + unsigned int id; + /* Preference of the physical in interface */ + enum cfcnfg_phy_preference pref; + + /* Information about the physical device */ + struct dev_info dev_info; + + /* Interface index */ + int ifindex; + + /* Protocol head room added for CAIF link layer */ + int head_room; + + /* Use Start of frame checksum */ + bool use_fcs; +}; + +struct cfcnfg { + struct cflayer layer; + struct cflayer *ctrl; + struct cflayer *mux; + struct list_head phys; + struct mutex lock; +}; + +static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, + enum cfctrl_srv serv, u8 phyid, + struct cflayer *adapt_layer); +static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id); +static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id, + struct cflayer *adapt_layer); +static void cfctrl_resp_func(void); +static void cfctrl_enum_resp(void); + +struct cfcnfg *cfcnfg_create(void) +{ + struct cfcnfg *this; + struct cfctrl_rsp *resp; + + might_sleep(); + + /* Initiate this layer */ + this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC); + if (!this) + return NULL; + this->mux = cfmuxl_create(); + if (!this->mux) + goto out_of_mem; + this->ctrl = cfctrl_create(); + if (!this->ctrl) + goto out_of_mem; + /* Initiate response functions */ + resp = cfctrl_get_respfuncs(this->ctrl); + resp->enum_rsp = cfctrl_enum_resp; + resp->linkerror_ind = cfctrl_resp_func; + resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp; + resp->sleep_rsp = cfctrl_resp_func; + resp->wake_rsp = cfctrl_resp_func; + resp->restart_rsp = cfctrl_resp_func; + resp->radioset_rsp = cfctrl_resp_func; + resp->linksetup_rsp = cfcnfg_linkup_rsp; + resp->reject_rsp = cfcnfg_reject_rsp; + INIT_LIST_HEAD(&this->phys); + + cfmuxl_set_uplayer(this->mux, this->ctrl, 0); + layer_set_dn(this->ctrl, this->mux); + layer_set_up(this->ctrl, this); + mutex_init(&this->lock); + + return this; +out_of_mem: + synchronize_rcu(); + + kfree(this->mux); + kfree(this->ctrl); + kfree(this); + return NULL; +} + +void cfcnfg_remove(struct cfcnfg *cfg) +{ + might_sleep(); + if (cfg) { + synchronize_rcu(); + + kfree(cfg->mux); + cfctrl_remove(cfg->ctrl); + kfree(cfg); + } +} + +static void cfctrl_resp_func(void) +{ +} + +static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo_rcu(struct cfcnfg *cnfg, + u8 phyid) +{ + struct cfcnfg_phyinfo *phy; + + list_for_each_entry_rcu(phy, &cnfg->phys, node) + if (phy->id == phyid) + return phy; + return NULL; +} + +static void cfctrl_enum_resp(void) +{ +} + +static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg, + enum cfcnfg_phy_preference phy_pref) +{ + /* Try to match with specified preference */ + struct cfcnfg_phyinfo *phy; + + list_for_each_entry_rcu(phy, &cnfg->phys, node) { + if (phy->up && phy->pref == phy_pref && + phy->frm_layer != NULL) + + return &phy->dev_info; + } + + /* Otherwise just return something */ + list_for_each_entry_rcu(phy, &cnfg->phys, node) + if (phy->up) + return &phy->dev_info; + + return NULL; +} + +static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi) +{ + struct cfcnfg_phyinfo *phy; + + list_for_each_entry_rcu(phy, &cnfg->phys, node) + if (phy->ifindex == ifi && phy->up) + return phy->id; + return -ENODEV; +} + +int caif_disconnect_client(struct net *net, struct cflayer *adap_layer) +{ + u8 channel_id; + struct cfcnfg *cfg = get_cfcnfg(net); + + caif_assert(adap_layer != NULL); + cfctrl_cancel_req(cfg->ctrl, adap_layer); + channel_id = adap_layer->id; + if (channel_id != 0) { + struct cflayer *servl; + servl = cfmuxl_remove_uplayer(cfg->mux, channel_id); + cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer); + if (servl != NULL) + layer_set_up(servl, NULL); + } else + pr_debug("nothing to disconnect\n"); + + /* Do RCU sync before initiating cleanup */ + synchronize_rcu(); + if (adap_layer->ctrlcmd != NULL) + adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0); + return 0; + +} +EXPORT_SYMBOL(caif_disconnect_client); + +static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id) +{ +} + +static const int protohead[CFCTRL_SRV_MASK] = { + [CFCTRL_SRV_VEI] = 4, + [CFCTRL_SRV_DATAGRAM] = 7, + [CFCTRL_SRV_UTIL] = 4, + [CFCTRL_SRV_RFM] = 3, + [CFCTRL_SRV_DBG] = 3, +}; + + +static int caif_connect_req_to_link_param(struct cfcnfg *cnfg, + struct caif_connect_request *s, + struct cfctrl_link_param *l) +{ + struct dev_info *dev_info; + enum cfcnfg_phy_preference pref; + int res; + + memset(l, 0, sizeof(*l)); + /* In caif protocol low value is high priority */ + l->priority = CAIF_PRIO_MAX - s->priority + 1; + + if (s->ifindex != 0) { + res = cfcnfg_get_id_from_ifi(cnfg, s->ifindex); + if (res < 0) + return res; + l->phyid = res; + } else { + switch (s->link_selector) { + case CAIF_LINK_HIGH_BANDW: + pref = CFPHYPREF_HIGH_BW; + break; + case CAIF_LINK_LOW_LATENCY: + pref = CFPHYPREF_LOW_LAT; + break; + default: + return -EINVAL; + } + dev_info = cfcnfg_get_phyid(cnfg, pref); + if (dev_info == NULL) + return -ENODEV; + l->phyid = dev_info->id; + } + switch (s->protocol) { + case CAIFPROTO_AT: + l->linktype = CFCTRL_SRV_VEI; + l->endpoint = (s->sockaddr.u.at.type >> 2) & 0x3; + l->chtype = s->sockaddr.u.at.type & 0x3; + break; + case CAIFPROTO_DATAGRAM: + l->linktype = CFCTRL_SRV_DATAGRAM; + l->chtype = 0x00; + l->u.datagram.connid = s->sockaddr.u.dgm.connection_id; + break; + case CAIFPROTO_DATAGRAM_LOOP: + l->linktype = CFCTRL_SRV_DATAGRAM; + l->chtype = 0x03; + l->endpoint = 0x00; + l->u.datagram.connid = s->sockaddr.u.dgm.connection_id; + break; + case CAIFPROTO_RFM: + l->linktype = CFCTRL_SRV_RFM; + l->u.datagram.connid = s->sockaddr.u.rfm.connection_id; + strlcpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume, + sizeof(l->u.rfm.volume)); + break; + case CAIFPROTO_UTIL: + l->linktype = CFCTRL_SRV_UTIL; + l->endpoint = 0x00; + l->chtype = 0x00; + strlcpy(l->u.utility.name, s->sockaddr.u.util.service, + sizeof(l->u.utility.name)); + caif_assert(sizeof(l->u.utility.name) > 10); + l->u.utility.paramlen = s->param.size; + if (l->u.utility.paramlen > sizeof(l->u.utility.params)) + l->u.utility.paramlen = sizeof(l->u.utility.params); + + memcpy(l->u.utility.params, s->param.data, + l->u.utility.paramlen); + + break; + case CAIFPROTO_DEBUG: + l->linktype = CFCTRL_SRV_DBG; + l->endpoint = s->sockaddr.u.dbg.service; + l->chtype = s->sockaddr.u.dbg.type; + break; + default: + return -EINVAL; + } + return 0; +} + +int caif_connect_client(struct net *net, struct caif_connect_request *conn_req, + struct cflayer *adap_layer, int *ifindex, + int *proto_head, int *proto_tail) +{ + struct cflayer *frml; + struct cfcnfg_phyinfo *phy; + int err; + struct cfctrl_link_param param; + struct cfcnfg *cfg = get_cfcnfg(net); + + rcu_read_lock(); + err = caif_connect_req_to_link_param(cfg, conn_req, ¶m); + if (err) + goto unlock; + + phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid); + if (!phy) { + err = -ENODEV; + goto unlock; + } + err = -EINVAL; + + if (adap_layer == NULL) { + pr_err("adap_layer is zero\n"); + goto unlock; + } + if (adap_layer->receive == NULL) { + pr_err("adap_layer->receive is NULL\n"); + goto unlock; + } + if (adap_layer->ctrlcmd == NULL) { + pr_err("adap_layer->ctrlcmd == NULL\n"); + goto unlock; + } + + err = -ENODEV; + frml = phy->frm_layer; + if (frml == NULL) { + pr_err("Specified PHY type does not exist!\n"); + goto unlock; + } + caif_assert(param.phyid == phy->id); + caif_assert(phy->frm_layer->id == + param.phyid); + caif_assert(phy->phy_layer->id == + param.phyid); + + *ifindex = phy->ifindex; + *proto_tail = 2; + *proto_head = protohead[param.linktype] + phy->head_room; + + rcu_read_unlock(); + + /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */ + cfctrl_enum_req(cfg->ctrl, param.phyid); + return cfctrl_linkup_request(cfg->ctrl, ¶m, adap_layer); + +unlock: + rcu_read_unlock(); + return err; +} +EXPORT_SYMBOL(caif_connect_client); + +static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id, + struct cflayer *adapt_layer) +{ + if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL) + adapt_layer->ctrlcmd(adapt_layer, + CAIF_CTRLCMD_INIT_FAIL_RSP, 0); +} + +static void +cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv, + u8 phyid, struct cflayer *adapt_layer) +{ + struct cfcnfg *cnfg = container_obj(layer); + struct cflayer *servicel = NULL; + struct cfcnfg_phyinfo *phyinfo; + struct net_device *netdev; + + if (channel_id == 0) { + pr_warn("received channel_id zero\n"); + if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL) + adapt_layer->ctrlcmd(adapt_layer, + CAIF_CTRLCMD_INIT_FAIL_RSP, 0); + return; + } + + rcu_read_lock(); + + if (adapt_layer == NULL) { + pr_debug("link setup response but no client exist, send linkdown back\n"); + cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL); + goto unlock; + } + + caif_assert(cnfg != NULL); + caif_assert(phyid != 0); + + phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid); + if (phyinfo == NULL) { + pr_err("ERROR: Link Layer Device disappeared while connecting\n"); + goto unlock; + } + + caif_assert(phyinfo != NULL); + caif_assert(phyinfo->id == phyid); + caif_assert(phyinfo->phy_layer != NULL); + caif_assert(phyinfo->phy_layer->id == phyid); + + adapt_layer->id = channel_id; + + switch (serv) { + case CFCTRL_SRV_VEI: + servicel = cfvei_create(channel_id, &phyinfo->dev_info); + break; + case CFCTRL_SRV_DATAGRAM: + servicel = cfdgml_create(channel_id, + &phyinfo->dev_info); + break; + case CFCTRL_SRV_RFM: + netdev = phyinfo->dev_info.dev; + servicel = cfrfml_create(channel_id, &phyinfo->dev_info, + netdev->mtu); + break; + case CFCTRL_SRV_UTIL: + servicel = cfutill_create(channel_id, &phyinfo->dev_info); + break; + case CFCTRL_SRV_VIDEO: + servicel = cfvidl_create(channel_id, &phyinfo->dev_info); + break; + case CFCTRL_SRV_DBG: + servicel = cfdbgl_create(channel_id, &phyinfo->dev_info); + break; + default: + pr_err("Protocol error. Link setup response - unknown channel type\n"); + goto unlock; + } + if (!servicel) + goto unlock; + layer_set_dn(servicel, cnfg->mux); + cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id); + layer_set_up(servicel, adapt_layer); + layer_set_dn(adapt_layer, servicel); + + rcu_read_unlock(); + + servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0); + return; +unlock: + rcu_read_unlock(); +} + +int +cfcnfg_add_phy_layer(struct cfcnfg *cnfg, + struct net_device *dev, struct cflayer *phy_layer, + enum cfcnfg_phy_preference pref, + struct cflayer *link_support, + bool fcs, int head_room) +{ + struct cflayer *frml; + struct cfcnfg_phyinfo *phyinfo = NULL; + int i, res = 0; + u8 phyid; + + mutex_lock(&cnfg->lock); + + /* CAIF protocol allow maximum 6 link-layers */ + for (i = 0; i < 7; i++) { + phyid = (dev->ifindex + i) & 0x7; + if (phyid == 0) + continue; + if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL) + goto got_phyid; + } + pr_warn("Too many CAIF Link Layers (max 6)\n"); + res = -EEXIST; + goto out; + +got_phyid: + phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC); + if (!phyinfo) { + res = -ENOMEM; + goto out_err; + } + + phy_layer->id = phyid; + phyinfo->pref = pref; + phyinfo->id = phyid; + phyinfo->dev_info.id = phyid; + phyinfo->dev_info.dev = dev; + phyinfo->phy_layer = phy_layer; + phyinfo->ifindex = dev->ifindex; + phyinfo->head_room = head_room; + phyinfo->use_fcs = fcs; + + frml = cffrml_create(phyid, fcs); + + if (!frml) { + res = -ENOMEM; + goto out_err; + } + phyinfo->frm_layer = frml; + layer_set_up(frml, cnfg->mux); + + if (link_support != NULL) { + link_support->id = phyid; + layer_set_dn(frml, link_support); + layer_set_up(link_support, frml); + layer_set_dn(link_support, phy_layer); + layer_set_up(phy_layer, link_support); + } else { + layer_set_dn(frml, phy_layer); + layer_set_up(phy_layer, frml); + } + + list_add_rcu(&phyinfo->node, &cnfg->phys); +out: + mutex_unlock(&cnfg->lock); + return res; + +out_err: + kfree(phyinfo); + mutex_unlock(&cnfg->lock); + return res; +} +EXPORT_SYMBOL(cfcnfg_add_phy_layer); + +int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer, + bool up) +{ + struct cfcnfg_phyinfo *phyinfo; + + rcu_read_lock(); + phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id); + if (phyinfo == NULL) { + rcu_read_unlock(); + return -ENODEV; + } + + if (phyinfo->up == up) { + rcu_read_unlock(); + return 0; + } + phyinfo->up = up; + + if (up) { + cffrml_hold(phyinfo->frm_layer); + cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer, + phy_layer->id); + } else { + cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id); + cffrml_put(phyinfo->frm_layer); + } + + rcu_read_unlock(); + return 0; +} +EXPORT_SYMBOL(cfcnfg_set_phy_state); + +int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer) +{ + struct cflayer *frml, *frml_dn; + u16 phyid; + struct cfcnfg_phyinfo *phyinfo; + + might_sleep(); + + mutex_lock(&cnfg->lock); + + phyid = phy_layer->id; + phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid); + + if (phyinfo == NULL) { + mutex_unlock(&cnfg->lock); + return 0; + } + caif_assert(phyid == phyinfo->id); + caif_assert(phy_layer == phyinfo->phy_layer); + caif_assert(phy_layer->id == phyid); + caif_assert(phyinfo->frm_layer->id == phyid); + + list_del_rcu(&phyinfo->node); + synchronize_rcu(); + + /* Fail if reference count is not zero */ + if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) { + pr_info("Wait for device inuse\n"); + list_add_rcu(&phyinfo->node, &cnfg->phys); + mutex_unlock(&cnfg->lock); + return -EAGAIN; + } + + frml = phyinfo->frm_layer; + frml_dn = frml->dn; + cffrml_set_uplayer(frml, NULL); + cffrml_set_dnlayer(frml, NULL); + if (phy_layer != frml_dn) { + layer_set_up(frml_dn, NULL); + layer_set_dn(frml_dn, NULL); + } + layer_set_up(phy_layer, NULL); + + if (phyinfo->phy_layer != frml_dn) + kfree(frml_dn); + + cffrml_free(frml); + kfree(phyinfo); + mutex_unlock(&cnfg->lock); + + return 0; +} +EXPORT_SYMBOL(cfcnfg_del_phy_layer); diff --git a/net/caif/cfctrl.c b/net/caif/cfctrl.c new file mode 100644 index 000000000..a1e85f032 --- /dev/null +++ b/net/caif/cfctrl.c @@ -0,0 +1,635 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/stddef.h> +#include <linux/spinlock.h> +#include <linux/slab.h> +#include <linux/pkt_sched.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfpkt.h> +#include <net/caif/cfctrl.h> + +#define container_obj(layr) container_of(layr, struct cfctrl, serv.layer) +#define UTILITY_NAME_LENGTH 16 +#define CFPKT_CTRL_PKT_LEN 20 + +#ifdef CAIF_NO_LOOP +static int handle_loop(struct cfctrl *ctrl, + int cmd, struct cfpkt *pkt){ + return -1; +} +#else +static int handle_loop(struct cfctrl *ctrl, + int cmd, struct cfpkt *pkt); +#endif +static int cfctrl_recv(struct cflayer *layr, struct cfpkt *pkt); +static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, + int phyid); + + +struct cflayer *cfctrl_create(void) +{ + struct dev_info dev_info; + struct cfctrl *this = + kzalloc(sizeof(struct cfctrl), GFP_ATOMIC); + if (!this) + return NULL; + caif_assert(offsetof(struct cfctrl, serv.layer) == 0); + memset(&dev_info, 0, sizeof(dev_info)); + dev_info.id = 0xff; + cfsrvl_init(&this->serv, 0, &dev_info, false); + atomic_set(&this->req_seq_no, 1); + atomic_set(&this->rsp_seq_no, 1); + this->serv.layer.receive = cfctrl_recv; + sprintf(this->serv.layer.name, "ctrl"); + this->serv.layer.ctrlcmd = cfctrl_ctrlcmd; +#ifndef CAIF_NO_LOOP + spin_lock_init(&this->loop_linkid_lock); + this->loop_linkid = 1; +#endif + spin_lock_init(&this->info_list_lock); + INIT_LIST_HEAD(&this->list); + return &this->serv.layer; +} + +void cfctrl_remove(struct cflayer *layer) +{ + struct cfctrl_request_info *p, *tmp; + struct cfctrl *ctrl = container_obj(layer); + + spin_lock_bh(&ctrl->info_list_lock); + list_for_each_entry_safe(p, tmp, &ctrl->list, list) { + list_del(&p->list); + kfree(p); + } + spin_unlock_bh(&ctrl->info_list_lock); + kfree(layer); +} + +static bool param_eq(const struct cfctrl_link_param *p1, + const struct cfctrl_link_param *p2) +{ + bool eq = + p1->linktype == p2->linktype && + p1->priority == p2->priority && + p1->phyid == p2->phyid && + p1->endpoint == p2->endpoint && p1->chtype == p2->chtype; + + if (!eq) + return false; + + switch (p1->linktype) { + case CFCTRL_SRV_VEI: + return true; + case CFCTRL_SRV_DATAGRAM: + return p1->u.datagram.connid == p2->u.datagram.connid; + case CFCTRL_SRV_RFM: + return + p1->u.rfm.connid == p2->u.rfm.connid && + strcmp(p1->u.rfm.volume, p2->u.rfm.volume) == 0; + case CFCTRL_SRV_UTIL: + return + p1->u.utility.fifosize_kb == p2->u.utility.fifosize_kb + && p1->u.utility.fifosize_bufs == + p2->u.utility.fifosize_bufs + && strcmp(p1->u.utility.name, p2->u.utility.name) == 0 + && p1->u.utility.paramlen == p2->u.utility.paramlen + && memcmp(p1->u.utility.params, p2->u.utility.params, + p1->u.utility.paramlen) == 0; + + case CFCTRL_SRV_VIDEO: + return p1->u.video.connid == p2->u.video.connid; + case CFCTRL_SRV_DBG: + return true; + case CFCTRL_SRV_DECM: + return false; + default: + return false; + } + return false; +} + +static bool cfctrl_req_eq(const struct cfctrl_request_info *r1, + const struct cfctrl_request_info *r2) +{ + if (r1->cmd != r2->cmd) + return false; + if (r1->cmd == CFCTRL_CMD_LINK_SETUP) + return param_eq(&r1->param, &r2->param); + else + return r1->channel_id == r2->channel_id; +} + +/* Insert request at the end */ +static void cfctrl_insert_req(struct cfctrl *ctrl, + struct cfctrl_request_info *req) +{ + spin_lock_bh(&ctrl->info_list_lock); + atomic_inc(&ctrl->req_seq_no); + req->sequence_no = atomic_read(&ctrl->req_seq_no); + list_add_tail(&req->list, &ctrl->list); + spin_unlock_bh(&ctrl->info_list_lock); +} + +/* Compare and remove request */ +static struct cfctrl_request_info *cfctrl_remove_req(struct cfctrl *ctrl, + struct cfctrl_request_info *req) +{ + struct cfctrl_request_info *p, *tmp, *first; + + first = list_first_entry(&ctrl->list, struct cfctrl_request_info, list); + + list_for_each_entry_safe(p, tmp, &ctrl->list, list) { + if (cfctrl_req_eq(req, p)) { + if (p != first) + pr_warn("Requests are not received in order\n"); + + atomic_set(&ctrl->rsp_seq_no, + p->sequence_no); + list_del(&p->list); + goto out; + } + } + p = NULL; +out: + return p; +} + +struct cfctrl_rsp *cfctrl_get_respfuncs(struct cflayer *layer) +{ + struct cfctrl *this = container_obj(layer); + return &this->res; +} + +static void init_info(struct caif_payload_info *info, struct cfctrl *cfctrl) +{ + info->hdr_len = 0; + info->channel_id = cfctrl->serv.layer.id; + info->dev_info = &cfctrl->serv.dev_info; +} + +void cfctrl_enum_req(struct cflayer *layer, u8 physlinkid) +{ + struct cfpkt *pkt; + struct cfctrl *cfctrl = container_obj(layer); + struct cflayer *dn = cfctrl->serv.layer.dn; + + if (!dn) { + pr_debug("not able to send enum request\n"); + return; + } + pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); + if (!pkt) + return; + caif_assert(offsetof(struct cfctrl, serv.layer) == 0); + init_info(cfpkt_info(pkt), cfctrl); + cfpkt_info(pkt)->dev_info->id = physlinkid; + cfctrl->serv.dev_info.id = physlinkid; + cfpkt_addbdy(pkt, CFCTRL_CMD_ENUM); + cfpkt_addbdy(pkt, physlinkid); + cfpkt_set_prio(pkt, TC_PRIO_CONTROL); + dn->transmit(dn, pkt); +} + +int cfctrl_linkup_request(struct cflayer *layer, + struct cfctrl_link_param *param, + struct cflayer *user_layer) +{ + struct cfctrl *cfctrl = container_obj(layer); + u32 tmp32; + u16 tmp16; + u8 tmp8; + struct cfctrl_request_info *req; + int ret; + char utility_name[16]; + struct cfpkt *pkt; + struct cflayer *dn = cfctrl->serv.layer.dn; + + if (!dn) { + pr_debug("not able to send linkup request\n"); + return -ENODEV; + } + + if (cfctrl_cancel_req(layer, user_layer) > 0) { + /* Slight Paranoia, check if already connecting */ + pr_err("Duplicate connect request for same client\n"); + WARN_ON(1); + return -EALREADY; + } + + pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); + if (!pkt) + return -ENOMEM; + cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_SETUP); + cfpkt_addbdy(pkt, (param->chtype << 4) | param->linktype); + cfpkt_addbdy(pkt, (param->priority << 3) | param->phyid); + cfpkt_addbdy(pkt, param->endpoint & 0x03); + + switch (param->linktype) { + case CFCTRL_SRV_VEI: + break; + case CFCTRL_SRV_VIDEO: + cfpkt_addbdy(pkt, (u8) param->u.video.connid); + break; + case CFCTRL_SRV_DBG: + break; + case CFCTRL_SRV_DATAGRAM: + tmp32 = cpu_to_le32(param->u.datagram.connid); + cfpkt_add_body(pkt, &tmp32, 4); + break; + case CFCTRL_SRV_RFM: + /* Construct a frame, convert DatagramConnectionID to network + * format long and copy it out... + */ + tmp32 = cpu_to_le32(param->u.rfm.connid); + cfpkt_add_body(pkt, &tmp32, 4); + /* Add volume name, including zero termination... */ + cfpkt_add_body(pkt, param->u.rfm.volume, + strlen(param->u.rfm.volume) + 1); + break; + case CFCTRL_SRV_UTIL: + tmp16 = cpu_to_le16(param->u.utility.fifosize_kb); + cfpkt_add_body(pkt, &tmp16, 2); + tmp16 = cpu_to_le16(param->u.utility.fifosize_bufs); + cfpkt_add_body(pkt, &tmp16, 2); + memset(utility_name, 0, sizeof(utility_name)); + strlcpy(utility_name, param->u.utility.name, + UTILITY_NAME_LENGTH); + cfpkt_add_body(pkt, utility_name, UTILITY_NAME_LENGTH); + tmp8 = param->u.utility.paramlen; + cfpkt_add_body(pkt, &tmp8, 1); + cfpkt_add_body(pkt, param->u.utility.params, + param->u.utility.paramlen); + break; + default: + pr_warn("Request setup of bad link type = %d\n", + param->linktype); + return -EINVAL; + } + req = kzalloc(sizeof(*req), GFP_KERNEL); + if (!req) + return -ENOMEM; + req->client_layer = user_layer; + req->cmd = CFCTRL_CMD_LINK_SETUP; + req->param = *param; + cfctrl_insert_req(cfctrl, req); + init_info(cfpkt_info(pkt), cfctrl); + /* + * NOTE:Always send linkup and linkdown request on the same + * device as the payload. Otherwise old queued up payload + * might arrive with the newly allocated channel ID. + */ + cfpkt_info(pkt)->dev_info->id = param->phyid; + cfpkt_set_prio(pkt, TC_PRIO_CONTROL); + ret = + dn->transmit(dn, pkt); + if (ret < 0) { + int count; + + count = cfctrl_cancel_req(&cfctrl->serv.layer, + user_layer); + if (count != 1) { + pr_err("Could not remove request (%d)", count); + return -ENODEV; + } + } + return 0; +} + +int cfctrl_linkdown_req(struct cflayer *layer, u8 channelid, + struct cflayer *client) +{ + int ret; + struct cfpkt *pkt; + struct cfctrl *cfctrl = container_obj(layer); + struct cflayer *dn = cfctrl->serv.layer.dn; + + if (!dn) { + pr_debug("not able to send link-down request\n"); + return -ENODEV; + } + pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); + if (!pkt) + return -ENOMEM; + cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_DESTROY); + cfpkt_addbdy(pkt, channelid); + init_info(cfpkt_info(pkt), cfctrl); + cfpkt_set_prio(pkt, TC_PRIO_CONTROL); + ret = + dn->transmit(dn, pkt); +#ifndef CAIF_NO_LOOP + cfctrl->loop_linkused[channelid] = 0; +#endif + return ret; +} + +int cfctrl_cancel_req(struct cflayer *layr, struct cflayer *adap_layer) +{ + struct cfctrl_request_info *p, *tmp; + struct cfctrl *ctrl = container_obj(layr); + int found = 0; + spin_lock_bh(&ctrl->info_list_lock); + + list_for_each_entry_safe(p, tmp, &ctrl->list, list) { + if (p->client_layer == adap_layer) { + list_del(&p->list); + kfree(p); + found++; + } + } + + spin_unlock_bh(&ctrl->info_list_lock); + return found; +} + +static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt) +{ + u8 cmdrsp; + u8 cmd; + int ret = -1; + u8 len; + u8 param[255]; + u8 linkid = 0; + struct cfctrl *cfctrl = container_obj(layer); + struct cfctrl_request_info rsp, *req; + + + cmdrsp = cfpkt_extr_head_u8(pkt); + cmd = cmdrsp & CFCTRL_CMD_MASK; + if (cmd != CFCTRL_CMD_LINK_ERR + && CFCTRL_RSP_BIT != (CFCTRL_RSP_BIT & cmdrsp) + && CFCTRL_ERR_BIT != (CFCTRL_ERR_BIT & cmdrsp)) { + if (handle_loop(cfctrl, cmd, pkt) != 0) + cmdrsp |= CFCTRL_ERR_BIT; + } + + switch (cmd) { + case CFCTRL_CMD_LINK_SETUP: + { + enum cfctrl_srv serv; + enum cfctrl_srv servtype; + u8 endpoint; + u8 physlinkid; + u8 prio; + u8 tmp; + u8 *cp; + int i; + struct cfctrl_link_param linkparam; + memset(&linkparam, 0, sizeof(linkparam)); + + tmp = cfpkt_extr_head_u8(pkt); + + serv = tmp & CFCTRL_SRV_MASK; + linkparam.linktype = serv; + + servtype = tmp >> 4; + linkparam.chtype = servtype; + + tmp = cfpkt_extr_head_u8(pkt); + physlinkid = tmp & 0x07; + prio = tmp >> 3; + + linkparam.priority = prio; + linkparam.phyid = physlinkid; + endpoint = cfpkt_extr_head_u8(pkt); + linkparam.endpoint = endpoint & 0x03; + + switch (serv) { + case CFCTRL_SRV_VEI: + case CFCTRL_SRV_DBG: + if (CFCTRL_ERR_BIT & cmdrsp) + break; + /* Link ID */ + linkid = cfpkt_extr_head_u8(pkt); + break; + case CFCTRL_SRV_VIDEO: + tmp = cfpkt_extr_head_u8(pkt); + linkparam.u.video.connid = tmp; + if (CFCTRL_ERR_BIT & cmdrsp) + break; + /* Link ID */ + linkid = cfpkt_extr_head_u8(pkt); + break; + + case CFCTRL_SRV_DATAGRAM: + linkparam.u.datagram.connid = + cfpkt_extr_head_u32(pkt); + if (CFCTRL_ERR_BIT & cmdrsp) + break; + /* Link ID */ + linkid = cfpkt_extr_head_u8(pkt); + break; + case CFCTRL_SRV_RFM: + /* Construct a frame, convert + * DatagramConnectionID + * to network format long and copy it out... + */ + linkparam.u.rfm.connid = + cfpkt_extr_head_u32(pkt); + cp = (u8 *) linkparam.u.rfm.volume; + for (tmp = cfpkt_extr_head_u8(pkt); + cfpkt_more(pkt) && tmp != '\0'; + tmp = cfpkt_extr_head_u8(pkt)) + *cp++ = tmp; + *cp = '\0'; + + if (CFCTRL_ERR_BIT & cmdrsp) + break; + /* Link ID */ + linkid = cfpkt_extr_head_u8(pkt); + + break; + case CFCTRL_SRV_UTIL: + /* Construct a frame, convert + * DatagramConnectionID + * to network format long and copy it out... + */ + /* Fifosize KB */ + linkparam.u.utility.fifosize_kb = + cfpkt_extr_head_u16(pkt); + /* Fifosize bufs */ + linkparam.u.utility.fifosize_bufs = + cfpkt_extr_head_u16(pkt); + /* name */ + cp = (u8 *) linkparam.u.utility.name; + caif_assert(sizeof(linkparam.u.utility.name) + >= UTILITY_NAME_LENGTH); + for (i = 0; + i < UTILITY_NAME_LENGTH + && cfpkt_more(pkt); i++) { + tmp = cfpkt_extr_head_u8(pkt); + *cp++ = tmp; + } + /* Length */ + len = cfpkt_extr_head_u8(pkt); + linkparam.u.utility.paramlen = len; + /* Param Data */ + cp = linkparam.u.utility.params; + while (cfpkt_more(pkt) && len--) { + tmp = cfpkt_extr_head_u8(pkt); + *cp++ = tmp; + } + if (CFCTRL_ERR_BIT & cmdrsp) + break; + /* Link ID */ + linkid = cfpkt_extr_head_u8(pkt); + /* Length */ + len = cfpkt_extr_head_u8(pkt); + /* Param Data */ + cfpkt_extr_head(pkt, ¶m, len); + break; + default: + pr_warn("Request setup, invalid type (%d)\n", + serv); + goto error; + } + + rsp.cmd = cmd; + rsp.param = linkparam; + spin_lock_bh(&cfctrl->info_list_lock); + req = cfctrl_remove_req(cfctrl, &rsp); + + if (CFCTRL_ERR_BIT == (CFCTRL_ERR_BIT & cmdrsp) || + cfpkt_erroneous(pkt)) { + pr_err("Invalid O/E bit or parse error " + "on CAIF control channel\n"); + cfctrl->res.reject_rsp(cfctrl->serv.layer.up, + 0, + req ? req->client_layer + : NULL); + } else { + cfctrl->res.linksetup_rsp(cfctrl->serv. + layer.up, linkid, + serv, physlinkid, + req ? req-> + client_layer : NULL); + } + + kfree(req); + + spin_unlock_bh(&cfctrl->info_list_lock); + } + break; + case CFCTRL_CMD_LINK_DESTROY: + linkid = cfpkt_extr_head_u8(pkt); + cfctrl->res.linkdestroy_rsp(cfctrl->serv.layer.up, linkid); + break; + case CFCTRL_CMD_LINK_ERR: + pr_err("Frame Error Indication received\n"); + cfctrl->res.linkerror_ind(); + break; + case CFCTRL_CMD_ENUM: + cfctrl->res.enum_rsp(); + break; + case CFCTRL_CMD_SLEEP: + cfctrl->res.sleep_rsp(); + break; + case CFCTRL_CMD_WAKE: + cfctrl->res.wake_rsp(); + break; + case CFCTRL_CMD_LINK_RECONF: + cfctrl->res.restart_rsp(); + break; + case CFCTRL_CMD_RADIO_SET: + cfctrl->res.radioset_rsp(); + break; + default: + pr_err("Unrecognized Control Frame\n"); + goto error; + } + ret = 0; +error: + cfpkt_destroy(pkt); + return ret; +} + +static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, + int phyid) +{ + struct cfctrl *this = container_obj(layr); + switch (ctrl) { + case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND: + case CAIF_CTRLCMD_FLOW_OFF_IND: + spin_lock_bh(&this->info_list_lock); + if (!list_empty(&this->list)) + pr_debug("Received flow off in control layer\n"); + spin_unlock_bh(&this->info_list_lock); + break; + case _CAIF_CTRLCMD_PHYIF_DOWN_IND: { + struct cfctrl_request_info *p, *tmp; + + /* Find all connect request and report failure */ + spin_lock_bh(&this->info_list_lock); + list_for_each_entry_safe(p, tmp, &this->list, list) { + if (p->param.phyid == phyid) { + list_del(&p->list); + p->client_layer->ctrlcmd(p->client_layer, + CAIF_CTRLCMD_INIT_FAIL_RSP, + phyid); + kfree(p); + } + } + spin_unlock_bh(&this->info_list_lock); + break; + } + default: + break; + } +} + +#ifndef CAIF_NO_LOOP +static int handle_loop(struct cfctrl *ctrl, int cmd, struct cfpkt *pkt) +{ + static int last_linkid; + static int dec; + u8 linkid, linktype, tmp; + switch (cmd) { + case CFCTRL_CMD_LINK_SETUP: + spin_lock_bh(&ctrl->loop_linkid_lock); + if (!dec) { + for (linkid = last_linkid + 1; linkid < 254; linkid++) + if (!ctrl->loop_linkused[linkid]) + goto found; + } + dec = 1; + for (linkid = last_linkid - 1; linkid > 1; linkid--) + if (!ctrl->loop_linkused[linkid]) + goto found; + spin_unlock_bh(&ctrl->loop_linkid_lock); + return -1; +found: + if (linkid < 10) + dec = 0; + + if (!ctrl->loop_linkused[linkid]) + ctrl->loop_linkused[linkid] = 1; + + last_linkid = linkid; + + cfpkt_add_trail(pkt, &linkid, 1); + spin_unlock_bh(&ctrl->loop_linkid_lock); + cfpkt_peek_head(pkt, &linktype, 1); + if (linktype == CFCTRL_SRV_UTIL) { + tmp = 0x01; + cfpkt_add_trail(pkt, &tmp, 1); + cfpkt_add_trail(pkt, &tmp, 1); + } + break; + + case CFCTRL_CMD_LINK_DESTROY: + spin_lock_bh(&ctrl->loop_linkid_lock); + cfpkt_peek_head(pkt, &linkid, 1); + ctrl->loop_linkused[linkid] = 0; + spin_unlock_bh(&ctrl->loop_linkid_lock); + break; + default: + break; + } + return 0; +} +#endif diff --git a/net/caif/cfdbgl.c b/net/caif/cfdbgl.c new file mode 100644 index 000000000..7aae0b568 --- /dev/null +++ b/net/caif/cfdbgl.c @@ -0,0 +1,55 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/stddef.h> +#include <linux/slab.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfsrvl.h> +#include <net/caif/cfpkt.h> + +#define container_obj(layr) ((struct cfsrvl *) layr) + +static int cfdbgl_receive(struct cflayer *layr, struct cfpkt *pkt); +static int cfdbgl_transmit(struct cflayer *layr, struct cfpkt *pkt); + +struct cflayer *cfdbgl_create(u8 channel_id, struct dev_info *dev_info) +{ + struct cfsrvl *dbg = kzalloc(sizeof(struct cfsrvl), GFP_ATOMIC); + if (!dbg) + return NULL; + caif_assert(offsetof(struct cfsrvl, layer) == 0); + cfsrvl_init(dbg, channel_id, dev_info, false); + dbg->layer.receive = cfdbgl_receive; + dbg->layer.transmit = cfdbgl_transmit; + snprintf(dbg->layer.name, CAIF_LAYER_NAME_SZ - 1, "dbg%d", channel_id); + return &dbg->layer; +} + +static int cfdbgl_receive(struct cflayer *layr, struct cfpkt *pkt) +{ + return layr->up->receive(layr->up, pkt); +} + +static int cfdbgl_transmit(struct cflayer *layr, struct cfpkt *pkt) +{ + struct cfsrvl *service = container_obj(layr); + struct caif_payload_info *info; + int ret; + + if (!cfsrvl_ready(service, &ret)) { + cfpkt_destroy(pkt); + return ret; + } + + /* Add info for MUX-layer to route the packet out */ + info = cfpkt_info(pkt); + info->channel_id = service->layer.id; + info->dev_info = &service->dev_info; + + return layr->dn->transmit(layr->dn, pkt); +} diff --git a/net/caif/cfdgml.c b/net/caif/cfdgml.c new file mode 100644 index 000000000..3bdddb32d --- /dev/null +++ b/net/caif/cfdgml.c @@ -0,0 +1,114 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/stddef.h> +#include <linux/spinlock.h> +#include <linux/slab.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfsrvl.h> +#include <net/caif/cfpkt.h> + + +#define container_obj(layr) ((struct cfsrvl *) layr) + +#define DGM_CMD_BIT 0x80 +#define DGM_FLOW_OFF 0x81 +#define DGM_FLOW_ON 0x80 +#define DGM_MTU 1500 + +static int cfdgml_receive(struct cflayer *layr, struct cfpkt *pkt); +static int cfdgml_transmit(struct cflayer *layr, struct cfpkt *pkt); + +struct cflayer *cfdgml_create(u8 channel_id, struct dev_info *dev_info) +{ + struct cfsrvl *dgm = kzalloc(sizeof(struct cfsrvl), GFP_ATOMIC); + if (!dgm) + return NULL; + caif_assert(offsetof(struct cfsrvl, layer) == 0); + cfsrvl_init(dgm, channel_id, dev_info, true); + dgm->layer.receive = cfdgml_receive; + dgm->layer.transmit = cfdgml_transmit; + snprintf(dgm->layer.name, CAIF_LAYER_NAME_SZ - 1, "dgm%d", channel_id); + dgm->layer.name[CAIF_LAYER_NAME_SZ - 1] = '\0'; + return &dgm->layer; +} + +static int cfdgml_receive(struct cflayer *layr, struct cfpkt *pkt) +{ + u8 cmd = -1; + u8 dgmhdr[3]; + int ret; + caif_assert(layr->up != NULL); + caif_assert(layr->receive != NULL); + caif_assert(layr->ctrlcmd != NULL); + + if (cfpkt_extr_head(pkt, &cmd, 1) < 0) { + pr_err("Packet is erroneous!\n"); + cfpkt_destroy(pkt); + return -EPROTO; + } + + if ((cmd & DGM_CMD_BIT) == 0) { + if (cfpkt_extr_head(pkt, &dgmhdr, 3) < 0) { + pr_err("Packet is erroneous!\n"); + cfpkt_destroy(pkt); + return -EPROTO; + } + ret = layr->up->receive(layr->up, pkt); + return ret; + } + + switch (cmd) { + case DGM_FLOW_OFF: /* FLOW OFF */ + layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0); + cfpkt_destroy(pkt); + return 0; + case DGM_FLOW_ON: /* FLOW ON */ + layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0); + cfpkt_destroy(pkt); + return 0; + default: + cfpkt_destroy(pkt); + pr_info("Unknown datagram control %d (0x%x)\n", cmd, cmd); + return -EPROTO; + } +} + +static int cfdgml_transmit(struct cflayer *layr, struct cfpkt *pkt) +{ + u8 packet_type; + u32 zero = 0; + struct caif_payload_info *info; + struct cfsrvl *service = container_obj(layr); + int ret; + + if (!cfsrvl_ready(service, &ret)) { + cfpkt_destroy(pkt); + return ret; + } + + /* STE Modem cannot handle more than 1500 bytes datagrams */ + if (cfpkt_getlen(pkt) > DGM_MTU) { + cfpkt_destroy(pkt); + return -EMSGSIZE; + } + + cfpkt_add_head(pkt, &zero, 3); + packet_type = 0x08; /* B9 set - UNCLASSIFIED */ + cfpkt_add_head(pkt, &packet_type, 1); + + /* Add info for MUX-layer to route the packet out. */ + info = cfpkt_info(pkt); + info->channel_id = service->layer.id; + /* To optimize alignment, we add up the size of CAIF header + * before payload. + */ + info->hdr_len = 4; + info->dev_info = &service->dev_info; + return layr->dn->transmit(layr->dn, pkt); +} diff --git a/net/caif/cffrml.c b/net/caif/cffrml.c new file mode 100644 index 000000000..434ba8557 --- /dev/null +++ b/net/caif/cffrml.c @@ -0,0 +1,197 @@ +/* + * CAIF Framing Layer. + * + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/stddef.h> +#include <linux/spinlock.h> +#include <linux/slab.h> +#include <linux/crc-ccitt.h> +#include <linux/netdevice.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfpkt.h> +#include <net/caif/cffrml.h> + +#define container_obj(layr) container_of(layr, struct cffrml, layer) + +struct cffrml { + struct cflayer layer; + bool dofcs; /* !< FCS active */ + int __percpu *pcpu_refcnt; +}; + +static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt); +static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt); +static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, + int phyid); + +static u32 cffrml_rcv_error; +static u32 cffrml_rcv_checsum_error; +struct cflayer *cffrml_create(u16 phyid, bool use_fcs) +{ + struct cffrml *this = kzalloc(sizeof(struct cffrml), GFP_ATOMIC); + if (!this) + return NULL; + this->pcpu_refcnt = alloc_percpu(int); + if (this->pcpu_refcnt == NULL) { + kfree(this); + return NULL; + } + + caif_assert(offsetof(struct cffrml, layer) == 0); + + this->layer.receive = cffrml_receive; + this->layer.transmit = cffrml_transmit; + this->layer.ctrlcmd = cffrml_ctrlcmd; + snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "frm%d", phyid); + this->dofcs = use_fcs; + this->layer.id = phyid; + return (struct cflayer *) this; +} + +void cffrml_free(struct cflayer *layer) +{ + struct cffrml *this = container_obj(layer); + free_percpu(this->pcpu_refcnt); + kfree(layer); +} + +void cffrml_set_uplayer(struct cflayer *this, struct cflayer *up) +{ + this->up = up; +} + +void cffrml_set_dnlayer(struct cflayer *this, struct cflayer *dn) +{ + this->dn = dn; +} + +static u16 cffrml_checksum(u16 chks, void *buf, u16 len) +{ + /* FIXME: FCS should be moved to glue in order to use OS-Specific + * solutions + */ + return crc_ccitt(chks, buf, len); +} + +static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt) +{ + u16 tmp; + u16 len; + u16 hdrchks; + int pktchks; + struct cffrml *this; + this = container_obj(layr); + + cfpkt_extr_head(pkt, &tmp, 2); + len = le16_to_cpu(tmp); + + /* Subtract for FCS on length if FCS is not used. */ + if (!this->dofcs) + len -= 2; + + if (cfpkt_setlen(pkt, len) < 0) { + ++cffrml_rcv_error; + pr_err("Framing length error (%d)\n", len); + cfpkt_destroy(pkt); + return -EPROTO; + } + /* + * Don't do extract if FCS is false, rather do setlen - then we don't + * get a cache-miss. + */ + if (this->dofcs) { + cfpkt_extr_trail(pkt, &tmp, 2); + hdrchks = le16_to_cpu(tmp); + pktchks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff); + if (pktchks != hdrchks) { + cfpkt_add_trail(pkt, &tmp, 2); + ++cffrml_rcv_error; + ++cffrml_rcv_checsum_error; + pr_info("Frame checksum error (0x%x != 0x%x)\n", + hdrchks, pktchks); + return -EILSEQ; + } + } + if (cfpkt_erroneous(pkt)) { + ++cffrml_rcv_error; + pr_err("Packet is erroneous!\n"); + cfpkt_destroy(pkt); + return -EPROTO; + } + + if (layr->up == NULL) { + pr_err("Layr up is missing!\n"); + cfpkt_destroy(pkt); + return -EINVAL; + } + + return layr->up->receive(layr->up, pkt); +} + +static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt) +{ + u16 chks; + u16 len; + __le16 data; + + struct cffrml *this = container_obj(layr); + if (this->dofcs) { + chks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff); + data = cpu_to_le16(chks); + cfpkt_add_trail(pkt, &data, 2); + } else { + cfpkt_pad_trail(pkt, 2); + } + len = cfpkt_getlen(pkt); + data = cpu_to_le16(len); + cfpkt_add_head(pkt, &data, 2); + cfpkt_info(pkt)->hdr_len += 2; + if (cfpkt_erroneous(pkt)) { + pr_err("Packet is erroneous!\n"); + cfpkt_destroy(pkt); + return -EPROTO; + } + + if (layr->dn == NULL) { + cfpkt_destroy(pkt); + return -ENODEV; + + } + return layr->dn->transmit(layr->dn, pkt); +} + +static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, + int phyid) +{ + if (layr->up && layr->up->ctrlcmd) + layr->up->ctrlcmd(layr->up, ctrl, layr->id); +} + +void cffrml_put(struct cflayer *layr) +{ + struct cffrml *this = container_obj(layr); + if (layr != NULL && this->pcpu_refcnt != NULL) + this_cpu_dec(*this->pcpu_refcnt); +} + +void cffrml_hold(struct cflayer *layr) +{ + struct cffrml *this = container_obj(layr); + if (layr != NULL && this->pcpu_refcnt != NULL) + this_cpu_inc(*this->pcpu_refcnt); +} + +int cffrml_refcnt_read(struct cflayer *layr) +{ + int i, refcnt = 0; + struct cffrml *this = container_obj(layr); + for_each_possible_cpu(i) + refcnt += *per_cpu_ptr(this->pcpu_refcnt, i); + return refcnt; +} diff --git a/net/caif/cfmuxl.c b/net/caif/cfmuxl.c new file mode 100644 index 000000000..510aa5a75 --- /dev/null +++ b/net/caif/cfmuxl.c @@ -0,0 +1,267 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/stddef.h> +#include <linux/spinlock.h> +#include <linux/slab.h> +#include <linux/rculist.h> +#include <net/caif/cfpkt.h> +#include <net/caif/cfmuxl.h> +#include <net/caif/cfsrvl.h> +#include <net/caif/cffrml.h> + +#define container_obj(layr) container_of(layr, struct cfmuxl, layer) + +#define CAIF_CTRL_CHANNEL 0 +#define UP_CACHE_SIZE 8 +#define DN_CACHE_SIZE 8 + +struct cfmuxl { + struct cflayer layer; + struct list_head srvl_list; + struct list_head frml_list; + struct cflayer *up_cache[UP_CACHE_SIZE]; + struct cflayer *dn_cache[DN_CACHE_SIZE]; + /* + * Set when inserting or removing downwards layers. + */ + spinlock_t transmit_lock; + + /* + * Set when inserting or removing upwards layers. + */ + spinlock_t receive_lock; + +}; + +static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt); +static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt); +static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, + int phyid); +static struct cflayer *get_up(struct cfmuxl *muxl, u16 id); + +struct cflayer *cfmuxl_create(void) +{ + struct cfmuxl *this = kzalloc(sizeof(struct cfmuxl), GFP_ATOMIC); + + if (!this) + return NULL; + this->layer.receive = cfmuxl_receive; + this->layer.transmit = cfmuxl_transmit; + this->layer.ctrlcmd = cfmuxl_ctrlcmd; + INIT_LIST_HEAD(&this->srvl_list); + INIT_LIST_HEAD(&this->frml_list); + spin_lock_init(&this->transmit_lock); + spin_lock_init(&this->receive_lock); + snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "mux"); + return &this->layer; +} + +int cfmuxl_set_dnlayer(struct cflayer *layr, struct cflayer *dn, u8 phyid) +{ + struct cfmuxl *muxl = (struct cfmuxl *) layr; + + spin_lock_bh(&muxl->transmit_lock); + list_add_rcu(&dn->node, &muxl->frml_list); + spin_unlock_bh(&muxl->transmit_lock); + return 0; +} + +static struct cflayer *get_from_id(struct list_head *list, u16 id) +{ + struct cflayer *lyr; + list_for_each_entry_rcu(lyr, list, node) { + if (lyr->id == id) + return lyr; + } + + return NULL; +} + +int cfmuxl_set_uplayer(struct cflayer *layr, struct cflayer *up, u8 linkid) +{ + struct cfmuxl *muxl = container_obj(layr); + struct cflayer *old; + + spin_lock_bh(&muxl->receive_lock); + + /* Two entries with same id is wrong, so remove old layer from mux */ + old = get_from_id(&muxl->srvl_list, linkid); + if (old != NULL) + list_del_rcu(&old->node); + + list_add_rcu(&up->node, &muxl->srvl_list); + spin_unlock_bh(&muxl->receive_lock); + + return 0; +} + +struct cflayer *cfmuxl_remove_dnlayer(struct cflayer *layr, u8 phyid) +{ + struct cfmuxl *muxl = container_obj(layr); + struct cflayer *dn; + int idx = phyid % DN_CACHE_SIZE; + + spin_lock_bh(&muxl->transmit_lock); + RCU_INIT_POINTER(muxl->dn_cache[idx], NULL); + dn = get_from_id(&muxl->frml_list, phyid); + if (dn == NULL) + goto out; + + list_del_rcu(&dn->node); + caif_assert(dn != NULL); +out: + spin_unlock_bh(&muxl->transmit_lock); + return dn; +} + +static struct cflayer *get_up(struct cfmuxl *muxl, u16 id) +{ + struct cflayer *up; + int idx = id % UP_CACHE_SIZE; + up = rcu_dereference(muxl->up_cache[idx]); + if (up == NULL || up->id != id) { + spin_lock_bh(&muxl->receive_lock); + up = get_from_id(&muxl->srvl_list, id); + rcu_assign_pointer(muxl->up_cache[idx], up); + spin_unlock_bh(&muxl->receive_lock); + } + return up; +} + +static struct cflayer *get_dn(struct cfmuxl *muxl, struct dev_info *dev_info) +{ + struct cflayer *dn; + int idx = dev_info->id % DN_CACHE_SIZE; + dn = rcu_dereference(muxl->dn_cache[idx]); + if (dn == NULL || dn->id != dev_info->id) { + spin_lock_bh(&muxl->transmit_lock); + dn = get_from_id(&muxl->frml_list, dev_info->id); + rcu_assign_pointer(muxl->dn_cache[idx], dn); + spin_unlock_bh(&muxl->transmit_lock); + } + return dn; +} + +struct cflayer *cfmuxl_remove_uplayer(struct cflayer *layr, u8 id) +{ + struct cflayer *up; + struct cfmuxl *muxl = container_obj(layr); + int idx = id % UP_CACHE_SIZE; + + if (id == 0) { + pr_warn("Trying to remove control layer\n"); + return NULL; + } + + spin_lock_bh(&muxl->receive_lock); + up = get_from_id(&muxl->srvl_list, id); + if (up == NULL) + goto out; + + RCU_INIT_POINTER(muxl->up_cache[idx], NULL); + list_del_rcu(&up->node); +out: + spin_unlock_bh(&muxl->receive_lock); + return up; +} + +static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt) +{ + int ret; + struct cfmuxl *muxl = container_obj(layr); + u8 id; + struct cflayer *up; + if (cfpkt_extr_head(pkt, &id, 1) < 0) { + pr_err("erroneous Caif Packet\n"); + cfpkt_destroy(pkt); + return -EPROTO; + } + rcu_read_lock(); + up = get_up(muxl, id); + + if (up == NULL) { + pr_debug("Received data on unknown link ID = %d (0x%x)" + " up == NULL", id, id); + cfpkt_destroy(pkt); + /* + * Don't return ERROR, since modem misbehaves and sends out + * flow on before linksetup response. + */ + + rcu_read_unlock(); + return /* CFGLU_EPROT; */ 0; + } + + /* We can't hold rcu_lock during receive, so take a ref count instead */ + cfsrvl_get(up); + rcu_read_unlock(); + + ret = up->receive(up, pkt); + + cfsrvl_put(up); + return ret; +} + +static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt) +{ + struct cfmuxl *muxl = container_obj(layr); + int err; + u8 linkid; + struct cflayer *dn; + struct caif_payload_info *info = cfpkt_info(pkt); + BUG_ON(!info); + + rcu_read_lock(); + + dn = get_dn(muxl, info->dev_info); + if (dn == NULL) { + pr_debug("Send data on unknown phy ID = %d (0x%x)\n", + info->dev_info->id, info->dev_info->id); + rcu_read_unlock(); + cfpkt_destroy(pkt); + return -ENOTCONN; + } + + info->hdr_len += 1; + linkid = info->channel_id; + cfpkt_add_head(pkt, &linkid, 1); + + /* We can't hold rcu_lock during receive, so take a ref count instead */ + cffrml_hold(dn); + + rcu_read_unlock(); + + err = dn->transmit(dn, pkt); + + cffrml_put(dn); + return err; +} + +static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, + int phyid) +{ + struct cfmuxl *muxl = container_obj(layr); + struct cflayer *layer; + + rcu_read_lock(); + list_for_each_entry_rcu(layer, &muxl->srvl_list, node) { + + if (cfsrvl_phyid_match(layer, phyid) && layer->ctrlcmd) { + + if ((ctrl == _CAIF_CTRLCMD_PHYIF_DOWN_IND || + ctrl == CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND) && + layer->id != 0) + cfmuxl_remove_uplayer(layr, layer->id); + + /* NOTE: ctrlcmd is not allowed to block */ + layer->ctrlcmd(layer, ctrl, phyid); + } + } + rcu_read_unlock(); +} diff --git a/net/caif/cfpkt_skbuff.c b/net/caif/cfpkt_skbuff.c new file mode 100644 index 000000000..38c2b7a89 --- /dev/null +++ b/net/caif/cfpkt_skbuff.c @@ -0,0 +1,388 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/string.h> +#include <linux/skbuff.h> +#include <linux/export.h> +#include <net/caif/cfpkt.h> + +#define PKT_PREFIX 48 +#define PKT_POSTFIX 2 +#define PKT_LEN_WHEN_EXTENDING 128 +#define PKT_ERROR(pkt, errmsg) \ +do { \ + cfpkt_priv(pkt)->erronous = true; \ + skb_reset_tail_pointer(&pkt->skb); \ + pr_warn(errmsg); \ +} while (0) + +struct cfpktq { + struct sk_buff_head head; + atomic_t count; + /* Lock protects count updates */ + spinlock_t lock; +}; + +/* + * net/caif/ is generic and does not + * understand SKB, so we do this typecast + */ +struct cfpkt { + struct sk_buff skb; +}; + +/* Private data inside SKB */ +struct cfpkt_priv_data { + struct dev_info dev_info; + bool erronous; +}; + +static inline struct cfpkt_priv_data *cfpkt_priv(struct cfpkt *pkt) +{ + return (struct cfpkt_priv_data *) pkt->skb.cb; +} + +static inline bool is_erronous(struct cfpkt *pkt) +{ + return cfpkt_priv(pkt)->erronous; +} + +static inline struct sk_buff *pkt_to_skb(struct cfpkt *pkt) +{ + return &pkt->skb; +} + +static inline struct cfpkt *skb_to_pkt(struct sk_buff *skb) +{ + return (struct cfpkt *) skb; +} + +struct cfpkt *cfpkt_fromnative(enum caif_direction dir, void *nativepkt) +{ + struct cfpkt *pkt = skb_to_pkt(nativepkt); + cfpkt_priv(pkt)->erronous = false; + return pkt; +} +EXPORT_SYMBOL(cfpkt_fromnative); + +void *cfpkt_tonative(struct cfpkt *pkt) +{ + return (void *) pkt; +} +EXPORT_SYMBOL(cfpkt_tonative); + +static struct cfpkt *cfpkt_create_pfx(u16 len, u16 pfx) +{ + struct sk_buff *skb; + + skb = alloc_skb(len + pfx, GFP_ATOMIC); + if (unlikely(skb == NULL)) + return NULL; + + skb_reserve(skb, pfx); + return skb_to_pkt(skb); +} + +inline struct cfpkt *cfpkt_create(u16 len) +{ + return cfpkt_create_pfx(len + PKT_POSTFIX, PKT_PREFIX); +} + +void cfpkt_destroy(struct cfpkt *pkt) +{ + struct sk_buff *skb = pkt_to_skb(pkt); + kfree_skb(skb); +} + +inline bool cfpkt_more(struct cfpkt *pkt) +{ + struct sk_buff *skb = pkt_to_skb(pkt); + return skb->len > 0; +} + +int cfpkt_peek_head(struct cfpkt *pkt, void *data, u16 len) +{ + struct sk_buff *skb = pkt_to_skb(pkt); + if (skb_headlen(skb) >= len) { + memcpy(data, skb->data, len); + return 0; + } + return !cfpkt_extr_head(pkt, data, len) && + !cfpkt_add_head(pkt, data, len); +} + +int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len) +{ + struct sk_buff *skb = pkt_to_skb(pkt); + u8 *from; + if (unlikely(is_erronous(pkt))) + return -EPROTO; + + if (unlikely(len > skb->len)) { + PKT_ERROR(pkt, "read beyond end of packet\n"); + return -EPROTO; + } + + if (unlikely(len > skb_headlen(skb))) { + if (unlikely(skb_linearize(skb) != 0)) { + PKT_ERROR(pkt, "linearize failed\n"); + return -EPROTO; + } + } + from = skb_pull(skb, len); + from -= len; + if (data) + memcpy(data, from, len); + return 0; +} +EXPORT_SYMBOL(cfpkt_extr_head); + +int cfpkt_extr_trail(struct cfpkt *pkt, void *dta, u16 len) +{ + struct sk_buff *skb = pkt_to_skb(pkt); + u8 *data = dta; + u8 *from; + if (unlikely(is_erronous(pkt))) + return -EPROTO; + + if (unlikely(skb_linearize(skb) != 0)) { + PKT_ERROR(pkt, "linearize failed\n"); + return -EPROTO; + } + if (unlikely(skb->data + len > skb_tail_pointer(skb))) { + PKT_ERROR(pkt, "read beyond end of packet\n"); + return -EPROTO; + } + from = skb_tail_pointer(skb) - len; + skb_trim(skb, skb->len - len); + memcpy(data, from, len); + return 0; +} + +int cfpkt_pad_trail(struct cfpkt *pkt, u16 len) +{ + return cfpkt_add_body(pkt, NULL, len); +} + +int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len) +{ + struct sk_buff *skb = pkt_to_skb(pkt); + struct sk_buff *lastskb; + u8 *to; + u16 addlen = 0; + + + if (unlikely(is_erronous(pkt))) + return -EPROTO; + + lastskb = skb; + + /* Check whether we need to add space at the tail */ + if (unlikely(skb_tailroom(skb) < len)) { + if (likely(len < PKT_LEN_WHEN_EXTENDING)) + addlen = PKT_LEN_WHEN_EXTENDING; + else + addlen = len; + } + + /* Check whether we need to change the SKB before writing to the tail */ + if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) { + + /* Make sure data is writable */ + if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) { + PKT_ERROR(pkt, "cow failed\n"); + return -EPROTO; + } + } + + /* All set to put the last SKB and optionally write data there. */ + to = pskb_put(skb, lastskb, len); + if (likely(data)) + memcpy(to, data, len); + return 0; +} + +inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data) +{ + return cfpkt_add_body(pkt, &data, 1); +} + +int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len) +{ + struct sk_buff *skb = pkt_to_skb(pkt); + struct sk_buff *lastskb; + u8 *to; + const u8 *data = data2; + int ret; + if (unlikely(is_erronous(pkt))) + return -EPROTO; + if (unlikely(skb_headroom(skb) < len)) { + PKT_ERROR(pkt, "no headroom\n"); + return -EPROTO; + } + + /* Make sure data is writable */ + ret = skb_cow_data(skb, 0, &lastskb); + if (unlikely(ret < 0)) { + PKT_ERROR(pkt, "cow failed\n"); + return ret; + } + + to = skb_push(skb, len); + memcpy(to, data, len); + return 0; +} +EXPORT_SYMBOL(cfpkt_add_head); + +inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len) +{ + return cfpkt_add_body(pkt, data, len); +} + +inline u16 cfpkt_getlen(struct cfpkt *pkt) +{ + struct sk_buff *skb = pkt_to_skb(pkt); + return skb->len; +} + +int cfpkt_iterate(struct cfpkt *pkt, + u16 (*iter_func)(u16, void *, u16), + u16 data) +{ + /* + * Don't care about the performance hit of linearizing, + * Checksum should not be used on high-speed interfaces anyway. + */ + if (unlikely(is_erronous(pkt))) + return -EPROTO; + if (unlikely(skb_linearize(&pkt->skb) != 0)) { + PKT_ERROR(pkt, "linearize failed\n"); + return -EPROTO; + } + return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt)); +} + +int cfpkt_setlen(struct cfpkt *pkt, u16 len) +{ + struct sk_buff *skb = pkt_to_skb(pkt); + + + if (unlikely(is_erronous(pkt))) + return -EPROTO; + + if (likely(len <= skb->len)) { + if (unlikely(skb->data_len)) + ___pskb_trim(skb, len); + else + skb_trim(skb, len); + + return cfpkt_getlen(pkt); + } + + /* Need to expand SKB */ + if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len))) + PKT_ERROR(pkt, "skb_pad_trail failed\n"); + + return cfpkt_getlen(pkt); +} + +struct cfpkt *cfpkt_append(struct cfpkt *dstpkt, + struct cfpkt *addpkt, + u16 expectlen) +{ + struct sk_buff *dst = pkt_to_skb(dstpkt); + struct sk_buff *add = pkt_to_skb(addpkt); + u16 addlen = skb_headlen(add); + u16 neededtailspace; + struct sk_buff *tmp; + u16 dstlen; + u16 createlen; + if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) { + return dstpkt; + } + if (expectlen > addlen) + neededtailspace = expectlen; + else + neededtailspace = addlen; + + if (dst->tail + neededtailspace > dst->end) { + /* Create a dumplicate of 'dst' with more tail space */ + struct cfpkt *tmppkt; + dstlen = skb_headlen(dst); + createlen = dstlen + neededtailspace; + tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX); + if (tmppkt == NULL) + return NULL; + tmp = pkt_to_skb(tmppkt); + skb_set_tail_pointer(tmp, dstlen); + tmp->len = dstlen; + memcpy(tmp->data, dst->data, dstlen); + cfpkt_destroy(dstpkt); + dst = tmp; + } + memcpy(skb_tail_pointer(dst), add->data, skb_headlen(add)); + cfpkt_destroy(addpkt); + dst->tail += addlen; + dst->len += addlen; + return skb_to_pkt(dst); +} + +struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos) +{ + struct sk_buff *skb2; + struct sk_buff *skb = pkt_to_skb(pkt); + struct cfpkt *tmppkt; + u8 *split = skb->data + pos; + u16 len2nd = skb_tail_pointer(skb) - split; + + if (unlikely(is_erronous(pkt))) + return NULL; + + if (skb->data + pos > skb_tail_pointer(skb)) { + PKT_ERROR(pkt, "trying to split beyond end of packet\n"); + return NULL; + } + + /* Create a new packet for the second part of the data */ + tmppkt = cfpkt_create_pfx(len2nd + PKT_PREFIX + PKT_POSTFIX, + PKT_PREFIX); + if (tmppkt == NULL) + return NULL; + skb2 = pkt_to_skb(tmppkt); + + + if (skb2 == NULL) + return NULL; + + /* Reduce the length of the original packet */ + skb_set_tail_pointer(skb, pos); + skb->len = pos; + + memcpy(skb2->data, split, len2nd); + skb2->tail += len2nd; + skb2->len += len2nd; + skb2->priority = skb->priority; + return skb_to_pkt(skb2); +} + +bool cfpkt_erroneous(struct cfpkt *pkt) +{ + return cfpkt_priv(pkt)->erronous; +} + +struct caif_payload_info *cfpkt_info(struct cfpkt *pkt) +{ + return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb; +} +EXPORT_SYMBOL(cfpkt_info); + +void cfpkt_set_prio(struct cfpkt *pkt, int prio) +{ + pkt_to_skb(pkt)->priority = prio; +} +EXPORT_SYMBOL(cfpkt_set_prio); diff --git a/net/caif/cfrfml.c b/net/caif/cfrfml.c new file mode 100644 index 000000000..b82440e1f --- /dev/null +++ b/net/caif/cfrfml.c @@ -0,0 +1,302 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/stddef.h> +#include <linux/spinlock.h> +#include <linux/slab.h> +#include <asm/unaligned.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfsrvl.h> +#include <net/caif/cfpkt.h> + +#define container_obj(layr) container_of(layr, struct cfrfml, serv.layer) +#define RFM_SEGMENTATION_BIT 0x01 +#define RFM_HEAD_SIZE 7 + +static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt); +static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt); + +struct cfrfml { + struct cfsrvl serv; + struct cfpkt *incomplete_frm; + int fragment_size; + u8 seghead[6]; + u16 pdu_size; + /* Protects serialized processing of packets */ + spinlock_t sync; +}; + +static void cfrfml_release(struct cflayer *layer) +{ + struct cfsrvl *srvl = container_of(layer, struct cfsrvl, layer); + struct cfrfml *rfml = container_obj(&srvl->layer); + + if (rfml->incomplete_frm) + cfpkt_destroy(rfml->incomplete_frm); + + kfree(srvl); +} + +struct cflayer *cfrfml_create(u8 channel_id, struct dev_info *dev_info, + int mtu_size) +{ + int tmp; + struct cfrfml *this = kzalloc(sizeof(struct cfrfml), GFP_ATOMIC); + + if (!this) + return NULL; + + cfsrvl_init(&this->serv, channel_id, dev_info, false); + this->serv.release = cfrfml_release; + this->serv.layer.receive = cfrfml_receive; + this->serv.layer.transmit = cfrfml_transmit; + + /* Round down to closest multiple of 16 */ + tmp = (mtu_size - RFM_HEAD_SIZE - 6) / 16; + tmp *= 16; + + this->fragment_size = tmp; + spin_lock_init(&this->sync); + snprintf(this->serv.layer.name, CAIF_LAYER_NAME_SZ, + "rfm%d", channel_id); + + return &this->serv.layer; +} + +static struct cfpkt *rfm_append(struct cfrfml *rfml, char *seghead, + struct cfpkt *pkt, int *err) +{ + struct cfpkt *tmppkt; + *err = -EPROTO; + /* n-th but not last segment */ + + if (cfpkt_extr_head(pkt, seghead, 6) < 0) + return NULL; + + /* Verify correct header */ + if (memcmp(seghead, rfml->seghead, 6) != 0) + return NULL; + + tmppkt = cfpkt_append(rfml->incomplete_frm, pkt, + rfml->pdu_size + RFM_HEAD_SIZE); + + /* If cfpkt_append failes input pkts are not freed */ + *err = -ENOMEM; + if (tmppkt == NULL) + return NULL; + + *err = 0; + return tmppkt; +} + +static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt) +{ + u8 tmp; + bool segmented; + int err; + u8 seghead[6]; + struct cfrfml *rfml; + struct cfpkt *tmppkt = NULL; + + caif_assert(layr->up != NULL); + caif_assert(layr->receive != NULL); + rfml = container_obj(layr); + spin_lock(&rfml->sync); + + err = -EPROTO; + if (cfpkt_extr_head(pkt, &tmp, 1) < 0) + goto out; + segmented = tmp & RFM_SEGMENTATION_BIT; + + if (segmented) { + if (rfml->incomplete_frm == NULL) { + /* Initial Segment */ + if (cfpkt_peek_head(pkt, rfml->seghead, 6) < 0) + goto out; + + rfml->pdu_size = get_unaligned_le16(rfml->seghead+4); + + if (cfpkt_erroneous(pkt)) + goto out; + rfml->incomplete_frm = pkt; + pkt = NULL; + } else { + + tmppkt = rfm_append(rfml, seghead, pkt, &err); + if (tmppkt == NULL) + goto out; + + if (cfpkt_erroneous(tmppkt)) + goto out; + + rfml->incomplete_frm = tmppkt; + + + if (cfpkt_erroneous(tmppkt)) + goto out; + } + err = 0; + goto out; + } + + if (rfml->incomplete_frm) { + + /* Last Segment */ + tmppkt = rfm_append(rfml, seghead, pkt, &err); + if (tmppkt == NULL) + goto out; + + if (cfpkt_erroneous(tmppkt)) + goto out; + + rfml->incomplete_frm = NULL; + pkt = tmppkt; + tmppkt = NULL; + + /* Verify that length is correct */ + err = -EPROTO; + if (rfml->pdu_size != cfpkt_getlen(pkt) - RFM_HEAD_SIZE + 1) + goto out; + } + + err = rfml->serv.layer.up->receive(rfml->serv.layer.up, pkt); + +out: + + if (err != 0) { + if (tmppkt) + cfpkt_destroy(tmppkt); + if (pkt) + cfpkt_destroy(pkt); + if (rfml->incomplete_frm) + cfpkt_destroy(rfml->incomplete_frm); + rfml->incomplete_frm = NULL; + + pr_info("Connection error %d triggered on RFM link\n", err); + + /* Trigger connection error upon failure.*/ + layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND, + rfml->serv.dev_info.id); + } + spin_unlock(&rfml->sync); + + if (unlikely(err == -EAGAIN)) + /* It is not possible to recover after drop of a fragment */ + err = -EIO; + + return err; +} + + +static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt) +{ + caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size + RFM_HEAD_SIZE); + + /* Add info for MUX-layer to route the packet out. */ + cfpkt_info(pkt)->channel_id = rfml->serv.layer.id; + + /* + * To optimize alignment, we add up the size of CAIF header before + * payload. + */ + cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE; + cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info; + + return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt); +} + +static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt) +{ + int err; + u8 seg; + u8 head[6]; + struct cfpkt *rearpkt = NULL; + struct cfpkt *frontpkt = pkt; + struct cfrfml *rfml = container_obj(layr); + + caif_assert(layr->dn != NULL); + caif_assert(layr->dn->transmit != NULL); + + if (!cfsrvl_ready(&rfml->serv, &err)) + goto out; + + err = -EPROTO; + if (cfpkt_getlen(pkt) <= RFM_HEAD_SIZE-1) + goto out; + + err = 0; + if (cfpkt_getlen(pkt) > rfml->fragment_size + RFM_HEAD_SIZE) + err = cfpkt_peek_head(pkt, head, 6); + + if (err < 0) + goto out; + + while (cfpkt_getlen(frontpkt) > rfml->fragment_size + RFM_HEAD_SIZE) { + + seg = 1; + err = -EPROTO; + + if (cfpkt_add_head(frontpkt, &seg, 1) < 0) + goto out; + /* + * On OOM error cfpkt_split returns NULL. + * + * NOTE: Segmented pdu is not correctly aligned. + * This has negative performance impact. + */ + + rearpkt = cfpkt_split(frontpkt, rfml->fragment_size); + if (rearpkt == NULL) + goto out; + + err = cfrfml_transmit_segment(rfml, frontpkt); + + if (err != 0) { + frontpkt = NULL; + goto out; + } + + frontpkt = rearpkt; + rearpkt = NULL; + + err = -ENOMEM; + if (frontpkt == NULL) + goto out; + err = -EPROTO; + if (cfpkt_add_head(frontpkt, head, 6) < 0) + goto out; + + } + + seg = 0; + err = -EPROTO; + + if (cfpkt_add_head(frontpkt, &seg, 1) < 0) + goto out; + + err = cfrfml_transmit_segment(rfml, frontpkt); + + frontpkt = NULL; +out: + + if (err != 0) { + pr_info("Connection error %d triggered on RFM link\n", err); + /* Trigger connection error upon failure.*/ + + layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND, + rfml->serv.dev_info.id); + + if (rearpkt) + cfpkt_destroy(rearpkt); + + if (frontpkt) + cfpkt_destroy(frontpkt); + } + + return err; +} diff --git a/net/caif/cfserl.c b/net/caif/cfserl.c new file mode 100644 index 000000000..af1e1e36d --- /dev/null +++ b/net/caif/cfserl.c @@ -0,0 +1,193 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/stddef.h> +#include <linux/spinlock.h> +#include <linux/slab.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfpkt.h> +#include <net/caif/cfserl.h> + +#define container_obj(layr) ((struct cfserl *) layr) + +#define CFSERL_STX 0x02 +#define SERIAL_MINIUM_PACKET_SIZE 4 +#define SERIAL_MAX_FRAMESIZE 4096 +struct cfserl { + struct cflayer layer; + struct cfpkt *incomplete_frm; + /* Protects parallel processing of incoming packets */ + spinlock_t sync; + bool usestx; +}; + +static int cfserl_receive(struct cflayer *layr, struct cfpkt *pkt); +static int cfserl_transmit(struct cflayer *layr, struct cfpkt *pkt); +static void cfserl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, + int phyid); + +void cfserl_release(struct cflayer *layer) +{ + kfree(layer); +} + +struct cflayer *cfserl_create(int instance, bool use_stx) +{ + struct cfserl *this = kzalloc(sizeof(struct cfserl), GFP_ATOMIC); + if (!this) + return NULL; + caif_assert(offsetof(struct cfserl, layer) == 0); + this->layer.receive = cfserl_receive; + this->layer.transmit = cfserl_transmit; + this->layer.ctrlcmd = cfserl_ctrlcmd; + this->usestx = use_stx; + spin_lock_init(&this->sync); + snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "ser1"); + return &this->layer; +} + +static int cfserl_receive(struct cflayer *l, struct cfpkt *newpkt) +{ + struct cfserl *layr = container_obj(l); + u16 pkt_len; + struct cfpkt *pkt = NULL; + struct cfpkt *tail_pkt = NULL; + u8 tmp8; + u16 tmp; + u8 stx = CFSERL_STX; + int ret; + u16 expectlen = 0; + + caif_assert(newpkt != NULL); + spin_lock(&layr->sync); + + if (layr->incomplete_frm != NULL) { + layr->incomplete_frm = + cfpkt_append(layr->incomplete_frm, newpkt, expectlen); + pkt = layr->incomplete_frm; + if (pkt == NULL) { + spin_unlock(&layr->sync); + return -ENOMEM; + } + } else { + pkt = newpkt; + } + layr->incomplete_frm = NULL; + + do { + /* Search for STX at start of pkt if STX is used */ + if (layr->usestx) { + cfpkt_extr_head(pkt, &tmp8, 1); + if (tmp8 != CFSERL_STX) { + while (cfpkt_more(pkt) + && tmp8 != CFSERL_STX) { + cfpkt_extr_head(pkt, &tmp8, 1); + } + if (!cfpkt_more(pkt)) { + cfpkt_destroy(pkt); + layr->incomplete_frm = NULL; + spin_unlock(&layr->sync); + return -EPROTO; + } + } + } + + pkt_len = cfpkt_getlen(pkt); + + /* + * pkt_len is the accumulated length of the packet data + * we have received so far. + * Exit if frame doesn't hold length. + */ + + if (pkt_len < 2) { + if (layr->usestx) + cfpkt_add_head(pkt, &stx, 1); + layr->incomplete_frm = pkt; + spin_unlock(&layr->sync); + return 0; + } + + /* + * Find length of frame. + * expectlen is the length we need for a full frame. + */ + cfpkt_peek_head(pkt, &tmp, 2); + expectlen = le16_to_cpu(tmp) + 2; + /* + * Frame error handling + */ + if (expectlen < SERIAL_MINIUM_PACKET_SIZE + || expectlen > SERIAL_MAX_FRAMESIZE) { + if (!layr->usestx) { + if (pkt != NULL) + cfpkt_destroy(pkt); + layr->incomplete_frm = NULL; + expectlen = 0; + spin_unlock(&layr->sync); + return -EPROTO; + } + continue; + } + + if (pkt_len < expectlen) { + /* Too little received data */ + if (layr->usestx) + cfpkt_add_head(pkt, &stx, 1); + layr->incomplete_frm = pkt; + spin_unlock(&layr->sync); + return 0; + } + + /* + * Enough data for at least one frame. + * Split the frame, if too long + */ + if (pkt_len > expectlen) + tail_pkt = cfpkt_split(pkt, expectlen); + else + tail_pkt = NULL; + + /* Send the first part of packet upwards.*/ + spin_unlock(&layr->sync); + ret = layr->layer.up->receive(layr->layer.up, pkt); + spin_lock(&layr->sync); + if (ret == -EILSEQ) { + if (layr->usestx) { + if (tail_pkt != NULL) + pkt = cfpkt_append(pkt, tail_pkt, 0); + /* Start search for next STX if frame failed */ + continue; + } else { + cfpkt_destroy(pkt); + pkt = NULL; + } + } + + pkt = tail_pkt; + + } while (pkt != NULL); + + spin_unlock(&layr->sync); + return 0; +} + +static int cfserl_transmit(struct cflayer *layer, struct cfpkt *newpkt) +{ + struct cfserl *layr = container_obj(layer); + u8 tmp8 = CFSERL_STX; + if (layr->usestx) + cfpkt_add_head(newpkt, &tmp8, 1); + return layer->dn->transmit(layer->dn, newpkt); +} + +static void cfserl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, + int phyid) +{ + layr->up->ctrlcmd(layr->up, ctrl, phyid); +} diff --git a/net/caif/cfsrvl.c b/net/caif/cfsrvl.c new file mode 100644 index 000000000..a6e115463 --- /dev/null +++ b/net/caif/cfsrvl.c @@ -0,0 +1,221 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/kernel.h> +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/slab.h> +#include <linux/module.h> +#include <linux/pkt_sched.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfsrvl.h> +#include <net/caif/cfpkt.h> +#include <net/caif/caif_dev.h> + +#define SRVL_CTRL_PKT_SIZE 1 +#define SRVL_FLOW_OFF 0x81 +#define SRVL_FLOW_ON 0x80 +#define SRVL_SET_PIN 0x82 +#define SRVL_CTRL_PKT_SIZE 1 + +#define container_obj(layr) container_of(layr, struct cfsrvl, layer) + +static void cfservl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, + int phyid) +{ + struct cfsrvl *service = container_obj(layr); + + if (layr->up == NULL || layr->up->ctrlcmd == NULL) + return; + + switch (ctrl) { + case CAIF_CTRLCMD_INIT_RSP: + service->open = true; + layr->up->ctrlcmd(layr->up, ctrl, phyid); + break; + case CAIF_CTRLCMD_DEINIT_RSP: + case CAIF_CTRLCMD_INIT_FAIL_RSP: + service->open = false; + layr->up->ctrlcmd(layr->up, ctrl, phyid); + break; + case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND: + if (phyid != service->dev_info.id) + break; + if (service->modem_flow_on) + layr->up->ctrlcmd(layr->up, + CAIF_CTRLCMD_FLOW_OFF_IND, phyid); + service->phy_flow_on = false; + break; + case _CAIF_CTRLCMD_PHYIF_FLOW_ON_IND: + if (phyid != service->dev_info.id) + return; + if (service->modem_flow_on) { + layr->up->ctrlcmd(layr->up, + CAIF_CTRLCMD_FLOW_ON_IND, + phyid); + } + service->phy_flow_on = true; + break; + case CAIF_CTRLCMD_FLOW_OFF_IND: + if (service->phy_flow_on) { + layr->up->ctrlcmd(layr->up, + CAIF_CTRLCMD_FLOW_OFF_IND, phyid); + } + service->modem_flow_on = false; + break; + case CAIF_CTRLCMD_FLOW_ON_IND: + if (service->phy_flow_on) { + layr->up->ctrlcmd(layr->up, + CAIF_CTRLCMD_FLOW_ON_IND, phyid); + } + service->modem_flow_on = true; + break; + case _CAIF_CTRLCMD_PHYIF_DOWN_IND: + /* In case interface is down, let's fake a remove shutdown */ + layr->up->ctrlcmd(layr->up, + CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND, phyid); + break; + case CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND: + layr->up->ctrlcmd(layr->up, ctrl, phyid); + break; + default: + pr_warn("Unexpected ctrl in cfsrvl (%d)\n", ctrl); + /* We have both modem and phy flow on, send flow on */ + layr->up->ctrlcmd(layr->up, ctrl, phyid); + service->phy_flow_on = true; + break; + } +} + +static int cfservl_modemcmd(struct cflayer *layr, enum caif_modemcmd ctrl) +{ + struct cfsrvl *service = container_obj(layr); + + caif_assert(layr != NULL); + caif_assert(layr->dn != NULL); + caif_assert(layr->dn->transmit != NULL); + + if (!service->supports_flowctrl) + return 0; + + switch (ctrl) { + case CAIF_MODEMCMD_FLOW_ON_REQ: + { + struct cfpkt *pkt; + struct caif_payload_info *info; + u8 flow_on = SRVL_FLOW_ON; + pkt = cfpkt_create(SRVL_CTRL_PKT_SIZE); + if (!pkt) + return -ENOMEM; + + if (cfpkt_add_head(pkt, &flow_on, 1) < 0) { + pr_err("Packet is erroneous!\n"); + cfpkt_destroy(pkt); + return -EPROTO; + } + info = cfpkt_info(pkt); + info->channel_id = service->layer.id; + info->hdr_len = 1; + info->dev_info = &service->dev_info; + cfpkt_set_prio(pkt, TC_PRIO_CONTROL); + return layr->dn->transmit(layr->dn, pkt); + } + case CAIF_MODEMCMD_FLOW_OFF_REQ: + { + struct cfpkt *pkt; + struct caif_payload_info *info; + u8 flow_off = SRVL_FLOW_OFF; + pkt = cfpkt_create(SRVL_CTRL_PKT_SIZE); + if (!pkt) + return -ENOMEM; + + if (cfpkt_add_head(pkt, &flow_off, 1) < 0) { + pr_err("Packet is erroneous!\n"); + cfpkt_destroy(pkt); + return -EPROTO; + } + info = cfpkt_info(pkt); + info->channel_id = service->layer.id; + info->hdr_len = 1; + info->dev_info = &service->dev_info; + cfpkt_set_prio(pkt, TC_PRIO_CONTROL); + return layr->dn->transmit(layr->dn, pkt); + } + default: + break; + } + return -EINVAL; +} + +static void cfsrvl_release(struct cflayer *layer) +{ + struct cfsrvl *service = container_of(layer, struct cfsrvl, layer); + kfree(service); +} + +void cfsrvl_init(struct cfsrvl *service, + u8 channel_id, + struct dev_info *dev_info, + bool supports_flowctrl) +{ + caif_assert(offsetof(struct cfsrvl, layer) == 0); + service->open = false; + service->modem_flow_on = true; + service->phy_flow_on = true; + service->layer.id = channel_id; + service->layer.ctrlcmd = cfservl_ctrlcmd; + service->layer.modemcmd = cfservl_modemcmd; + service->dev_info = *dev_info; + service->supports_flowctrl = supports_flowctrl; + service->release = cfsrvl_release; +} + +bool cfsrvl_ready(struct cfsrvl *service, int *err) +{ + if (!service->open) { + *err = -ENOTCONN; + return false; + } + return true; +} + +u8 cfsrvl_getphyid(struct cflayer *layer) +{ + struct cfsrvl *servl = container_obj(layer); + return servl->dev_info.id; +} + +bool cfsrvl_phyid_match(struct cflayer *layer, int phyid) +{ + struct cfsrvl *servl = container_obj(layer); + return servl->dev_info.id == phyid; +} + +void caif_free_client(struct cflayer *adap_layer) +{ + struct cfsrvl *servl; + if (adap_layer == NULL || adap_layer->dn == NULL) + return; + servl = container_obj(adap_layer->dn); + servl->release(&servl->layer); +} +EXPORT_SYMBOL(caif_free_client); + +void caif_client_register_refcnt(struct cflayer *adapt_layer, + void (*hold)(struct cflayer *lyr), + void (*put)(struct cflayer *lyr)) +{ + struct cfsrvl *service; + + if (WARN_ON(adapt_layer == NULL || adapt_layer->dn == NULL)) + return; + service = container_of(adapt_layer->dn, struct cfsrvl, layer); + service->hold = hold; + service->put = put; +} +EXPORT_SYMBOL(caif_client_register_refcnt); diff --git a/net/caif/cfutill.c b/net/caif/cfutill.c new file mode 100644 index 000000000..1728fa447 --- /dev/null +++ b/net/caif/cfutill.c @@ -0,0 +1,104 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/kernel.h> +#include <linux/types.h> +#include <linux/slab.h> +#include <linux/errno.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfsrvl.h> +#include <net/caif/cfpkt.h> + +#define container_obj(layr) ((struct cfsrvl *) layr) +#define UTIL_PAYLOAD 0x00 +#define UTIL_CMD_BIT 0x80 +#define UTIL_REMOTE_SHUTDOWN 0x82 +#define UTIL_FLOW_OFF 0x81 +#define UTIL_FLOW_ON 0x80 + +static int cfutill_receive(struct cflayer *layr, struct cfpkt *pkt); +static int cfutill_transmit(struct cflayer *layr, struct cfpkt *pkt); + +struct cflayer *cfutill_create(u8 channel_id, struct dev_info *dev_info) +{ + struct cfsrvl *util = kzalloc(sizeof(struct cfsrvl), GFP_ATOMIC); + if (!util) + return NULL; + caif_assert(offsetof(struct cfsrvl, layer) == 0); + cfsrvl_init(util, channel_id, dev_info, true); + util->layer.receive = cfutill_receive; + util->layer.transmit = cfutill_transmit; + snprintf(util->layer.name, CAIF_LAYER_NAME_SZ - 1, "util1"); + return &util->layer; +} + +static int cfutill_receive(struct cflayer *layr, struct cfpkt *pkt) +{ + u8 cmd = -1; + struct cfsrvl *service = container_obj(layr); + caif_assert(layr != NULL); + caif_assert(layr->up != NULL); + caif_assert(layr->up->receive != NULL); + caif_assert(layr->up->ctrlcmd != NULL); + if (cfpkt_extr_head(pkt, &cmd, 1) < 0) { + pr_err("Packet is erroneous!\n"); + cfpkt_destroy(pkt); + return -EPROTO; + } + + switch (cmd) { + case UTIL_PAYLOAD: + return layr->up->receive(layr->up, pkt); + case UTIL_FLOW_OFF: + layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0); + cfpkt_destroy(pkt); + return 0; + case UTIL_FLOW_ON: + layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0); + cfpkt_destroy(pkt); + return 0; + case UTIL_REMOTE_SHUTDOWN: /* Remote Shutdown Request */ + pr_err("REMOTE SHUTDOWN REQUEST RECEIVED\n"); + layr->ctrlcmd(layr, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND, 0); + service->open = false; + cfpkt_destroy(pkt); + return 0; + default: + cfpkt_destroy(pkt); + pr_warn("Unknown service control %d (0x%x)\n", cmd, cmd); + return -EPROTO; + } +} + +static int cfutill_transmit(struct cflayer *layr, struct cfpkt *pkt) +{ + u8 zero = 0; + struct caif_payload_info *info; + int ret; + struct cfsrvl *service = container_obj(layr); + caif_assert(layr != NULL); + caif_assert(layr->dn != NULL); + caif_assert(layr->dn->transmit != NULL); + + if (!cfsrvl_ready(service, &ret)) { + cfpkt_destroy(pkt); + return ret; + } + + cfpkt_add_head(pkt, &zero, 1); + /* Add info for MUX-layer to route the packet out. */ + info = cfpkt_info(pkt); + info->channel_id = service->layer.id; + /* + * To optimize alignment, we add up the size of CAIF header before + * payload. + */ + info->hdr_len = 1; + info->dev_info = &service->dev_info; + return layr->dn->transmit(layr->dn, pkt); +} diff --git a/net/caif/cfveil.c b/net/caif/cfveil.c new file mode 100644 index 000000000..262224581 --- /dev/null +++ b/net/caif/cfveil.c @@ -0,0 +1,101 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/stddef.h> +#include <linux/slab.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfsrvl.h> +#include <net/caif/cfpkt.h> + +#define VEI_PAYLOAD 0x00 +#define VEI_CMD_BIT 0x80 +#define VEI_FLOW_OFF 0x81 +#define VEI_FLOW_ON 0x80 +#define VEI_SET_PIN 0x82 + +#define container_obj(layr) container_of(layr, struct cfsrvl, layer) + +static int cfvei_receive(struct cflayer *layr, struct cfpkt *pkt); +static int cfvei_transmit(struct cflayer *layr, struct cfpkt *pkt); + +struct cflayer *cfvei_create(u8 channel_id, struct dev_info *dev_info) +{ + struct cfsrvl *vei = kzalloc(sizeof(struct cfsrvl), GFP_ATOMIC); + if (!vei) + return NULL; + caif_assert(offsetof(struct cfsrvl, layer) == 0); + cfsrvl_init(vei, channel_id, dev_info, true); + vei->layer.receive = cfvei_receive; + vei->layer.transmit = cfvei_transmit; + snprintf(vei->layer.name, CAIF_LAYER_NAME_SZ - 1, "vei%d", channel_id); + return &vei->layer; +} + +static int cfvei_receive(struct cflayer *layr, struct cfpkt *pkt) +{ + u8 cmd; + int ret; + caif_assert(layr->up != NULL); + caif_assert(layr->receive != NULL); + caif_assert(layr->ctrlcmd != NULL); + + + if (cfpkt_extr_head(pkt, &cmd, 1) < 0) { + pr_err("Packet is erroneous!\n"); + cfpkt_destroy(pkt); + return -EPROTO; + } + switch (cmd) { + case VEI_PAYLOAD: + ret = layr->up->receive(layr->up, pkt); + return ret; + case VEI_FLOW_OFF: + layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0); + cfpkt_destroy(pkt); + return 0; + case VEI_FLOW_ON: + layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0); + cfpkt_destroy(pkt); + return 0; + case VEI_SET_PIN: /* SET RS232 PIN */ + cfpkt_destroy(pkt); + return 0; + default: /* SET RS232 PIN */ + pr_warn("Unknown VEI control packet %d (0x%x)!\n", cmd, cmd); + cfpkt_destroy(pkt); + return -EPROTO; + } +} + +static int cfvei_transmit(struct cflayer *layr, struct cfpkt *pkt) +{ + u8 tmp = 0; + struct caif_payload_info *info; + int ret; + struct cfsrvl *service = container_obj(layr); + if (!cfsrvl_ready(service, &ret)) + goto err; + caif_assert(layr->dn != NULL); + caif_assert(layr->dn->transmit != NULL); + + if (cfpkt_add_head(pkt, &tmp, 1) < 0) { + pr_err("Packet is erroneous!\n"); + ret = -EPROTO; + goto err; + } + + /* Add info-> for MUX-layer to route the packet out. */ + info = cfpkt_info(pkt); + info->channel_id = service->layer.id; + info->hdr_len = 1; + info->dev_info = &service->dev_info; + return layr->dn->transmit(layr->dn, pkt); +err: + cfpkt_destroy(pkt); + return ret; +} diff --git a/net/caif/cfvidl.c b/net/caif/cfvidl.c new file mode 100644 index 000000000..b3b110e8a --- /dev/null +++ b/net/caif/cfvidl.c @@ -0,0 +1,65 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Author: Sjur Brendeland + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/kernel.h> +#include <linux/types.h> +#include <linux/slab.h> +#include <linux/errno.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfsrvl.h> +#include <net/caif/cfpkt.h> + +#define container_obj(layr) ((struct cfsrvl *) layr) + +static int cfvidl_receive(struct cflayer *layr, struct cfpkt *pkt); +static int cfvidl_transmit(struct cflayer *layr, struct cfpkt *pkt); + +struct cflayer *cfvidl_create(u8 channel_id, struct dev_info *dev_info) +{ + struct cfsrvl *vid = kzalloc(sizeof(struct cfsrvl), GFP_ATOMIC); + if (!vid) + return NULL; + caif_assert(offsetof(struct cfsrvl, layer) == 0); + + cfsrvl_init(vid, channel_id, dev_info, false); + vid->layer.receive = cfvidl_receive; + vid->layer.transmit = cfvidl_transmit; + snprintf(vid->layer.name, CAIF_LAYER_NAME_SZ - 1, "vid1"); + return &vid->layer; +} + +static int cfvidl_receive(struct cflayer *layr, struct cfpkt *pkt) +{ + u32 videoheader; + if (cfpkt_extr_head(pkt, &videoheader, 4) < 0) { + pr_err("Packet is erroneous!\n"); + cfpkt_destroy(pkt); + return -EPROTO; + } + return layr->up->receive(layr->up, pkt); +} + +static int cfvidl_transmit(struct cflayer *layr, struct cfpkt *pkt) +{ + struct cfsrvl *service = container_obj(layr); + struct caif_payload_info *info; + u32 videoheader = 0; + int ret; + + if (!cfsrvl_ready(service, &ret)) { + cfpkt_destroy(pkt); + return ret; + } + + cfpkt_add_head(pkt, &videoheader, 4); + /* Add info for MUX-layer to route the packet out */ + info = cfpkt_info(pkt); + info->channel_id = service->layer.id; + info->dev_info = &service->dev_info; + return layr->dn->transmit(layr->dn, pkt); +} diff --git a/net/caif/chnl_net.c b/net/caif/chnl_net.c new file mode 100644 index 000000000..8aeece7aa --- /dev/null +++ b/net/caif/chnl_net.c @@ -0,0 +1,538 @@ +/* + * Copyright (C) ST-Ericsson AB 2010 + * Authors: Sjur Brendeland + * Daniel Martensson + * License terms: GNU General Public License (GPL) version 2 + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ + +#include <linux/fs.h> +#include <linux/init.h> +#include <linux/module.h> +#include <linux/netdevice.h> +#include <linux/if_ether.h> +#include <linux/ip.h> +#include <linux/sched.h> +#include <linux/sockios.h> +#include <linux/caif/if_caif.h> +#include <net/rtnetlink.h> +#include <net/caif/caif_layer.h> +#include <net/caif/cfpkt.h> +#include <net/caif/caif_dev.h> + +/* GPRS PDP connection has MTU to 1500 */ +#define GPRS_PDP_MTU 1500 +/* 5 sec. connect timeout */ +#define CONNECT_TIMEOUT (5 * HZ) +#define CAIF_NET_DEFAULT_QUEUE_LEN 500 +#define UNDEF_CONNID 0xffffffff + +/*This list is protected by the rtnl lock. */ +static LIST_HEAD(chnl_net_list); + +MODULE_LICENSE("GPL"); +MODULE_ALIAS_RTNL_LINK("caif"); + +enum caif_states { + CAIF_CONNECTED = 1, + CAIF_CONNECTING, + CAIF_DISCONNECTED, + CAIF_SHUTDOWN +}; + +struct chnl_net { + struct cflayer chnl; + struct caif_connect_request conn_req; + struct list_head list_field; + struct net_device *netdev; + char name[256]; + wait_queue_head_t netmgmt_wq; + /* Flow status to remember and control the transmission. */ + bool flowenabled; + enum caif_states state; +}; + +static int chnl_recv_cb(struct cflayer *layr, struct cfpkt *pkt) +{ + struct sk_buff *skb; + struct chnl_net *priv; + int pktlen; + const u8 *ip_version; + u8 buf; + + priv = container_of(layr, struct chnl_net, chnl); + if (!priv) + return -EINVAL; + + skb = (struct sk_buff *) cfpkt_tonative(pkt); + + /* Get length of CAIF packet. */ + pktlen = skb->len; + + /* Pass some minimum information and + * send the packet to the net stack. + */ + skb->dev = priv->netdev; + + /* check the version of IP */ + ip_version = skb_header_pointer(skb, 0, 1, &buf); + if (!ip_version) { + kfree_skb(skb); + return -EINVAL; + } + + switch (*ip_version >> 4) { + case 4: + skb->protocol = htons(ETH_P_IP); + break; + case 6: + skb->protocol = htons(ETH_P_IPV6); + break; + default: + kfree_skb(skb); + priv->netdev->stats.rx_errors++; + return -EINVAL; + } + + /* If we change the header in loop mode, the checksum is corrupted. */ + if (priv->conn_req.protocol == CAIFPROTO_DATAGRAM_LOOP) + skb->ip_summed = CHECKSUM_UNNECESSARY; + else + skb->ip_summed = CHECKSUM_NONE; + + if (in_interrupt()) + netif_rx(skb); + else + netif_rx_ni(skb); + + /* Update statistics. */ + priv->netdev->stats.rx_packets++; + priv->netdev->stats.rx_bytes += pktlen; + + return 0; +} + +static int delete_device(struct chnl_net *dev) +{ + ASSERT_RTNL(); + if (dev->netdev) + unregister_netdevice(dev->netdev); + return 0; +} + +static void close_work(struct work_struct *work) +{ + struct chnl_net *dev = NULL; + struct list_head *list_node; + struct list_head *_tmp; + + rtnl_lock(); + list_for_each_safe(list_node, _tmp, &chnl_net_list) { + dev = list_entry(list_node, struct chnl_net, list_field); + if (dev->state == CAIF_SHUTDOWN) + dev_close(dev->netdev); + } + rtnl_unlock(); +} +static DECLARE_WORK(close_worker, close_work); + +static void chnl_hold(struct cflayer *lyr) +{ + struct chnl_net *priv = container_of(lyr, struct chnl_net, chnl); + dev_hold(priv->netdev); +} + +static void chnl_put(struct cflayer *lyr) +{ + struct chnl_net *priv = container_of(lyr, struct chnl_net, chnl); + dev_put(priv->netdev); +} + +static void chnl_flowctrl_cb(struct cflayer *layr, enum caif_ctrlcmd flow, + int phyid) +{ + struct chnl_net *priv = container_of(layr, struct chnl_net, chnl); + pr_debug("NET flowctrl func called flow: %s\n", + flow == CAIF_CTRLCMD_FLOW_ON_IND ? "ON" : + flow == CAIF_CTRLCMD_INIT_RSP ? "INIT" : + flow == CAIF_CTRLCMD_FLOW_OFF_IND ? "OFF" : + flow == CAIF_CTRLCMD_DEINIT_RSP ? "CLOSE/DEINIT" : + flow == CAIF_CTRLCMD_INIT_FAIL_RSP ? "OPEN_FAIL" : + flow == CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND ? + "REMOTE_SHUTDOWN" : "UNKNOWN CTRL COMMAND"); + + + + switch (flow) { + case CAIF_CTRLCMD_FLOW_OFF_IND: + priv->flowenabled = false; + netif_stop_queue(priv->netdev); + break; + case CAIF_CTRLCMD_DEINIT_RSP: + priv->state = CAIF_DISCONNECTED; + break; + case CAIF_CTRLCMD_INIT_FAIL_RSP: + priv->state = CAIF_DISCONNECTED; + wake_up_interruptible(&priv->netmgmt_wq); + break; + case CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND: + priv->state = CAIF_SHUTDOWN; + netif_tx_disable(priv->netdev); + schedule_work(&close_worker); + break; + case CAIF_CTRLCMD_FLOW_ON_IND: + priv->flowenabled = true; + netif_wake_queue(priv->netdev); + break; + case CAIF_CTRLCMD_INIT_RSP: + caif_client_register_refcnt(&priv->chnl, chnl_hold, chnl_put); + priv->state = CAIF_CONNECTED; + priv->flowenabled = true; + netif_wake_queue(priv->netdev); + wake_up_interruptible(&priv->netmgmt_wq); + break; + default: + break; + } +} + +static int chnl_net_start_xmit(struct sk_buff *skb, struct net_device *dev) +{ + struct chnl_net *priv; + struct cfpkt *pkt = NULL; + int len; + int result = -1; + /* Get our private data. */ + priv = netdev_priv(dev); + + if (skb->len > priv->netdev->mtu) { + pr_warn("Size of skb exceeded MTU\n"); + kfree_skb(skb); + dev->stats.tx_errors++; + return NETDEV_TX_OK; + } + + if (!priv->flowenabled) { + pr_debug("dropping packets flow off\n"); + kfree_skb(skb); + dev->stats.tx_dropped++; + return NETDEV_TX_OK; + } + + if (priv->conn_req.protocol == CAIFPROTO_DATAGRAM_LOOP) + swap(ip_hdr(skb)->saddr, ip_hdr(skb)->daddr); + + /* Store original SKB length. */ + len = skb->len; + + pkt = cfpkt_fromnative(CAIF_DIR_OUT, (void *) skb); + + /* Send the packet down the stack. */ + result = priv->chnl.dn->transmit(priv->chnl.dn, pkt); + if (result) { + dev->stats.tx_dropped++; + return NETDEV_TX_OK; + } + + /* Update statistics. */ + dev->stats.tx_packets++; + dev->stats.tx_bytes += len; + + return NETDEV_TX_OK; +} + +static int chnl_net_open(struct net_device *dev) +{ + struct chnl_net *priv = NULL; + int result = -1; + int llifindex, headroom, tailroom, mtu; + struct net_device *lldev; + ASSERT_RTNL(); + priv = netdev_priv(dev); + if (!priv) { + pr_debug("chnl_net_open: no priv\n"); + return -ENODEV; + } + + if (priv->state != CAIF_CONNECTING) { + priv->state = CAIF_CONNECTING; + result = caif_connect_client(dev_net(dev), &priv->conn_req, + &priv->chnl, &llifindex, + &headroom, &tailroom); + if (result != 0) { + pr_debug("err: " + "Unable to register and open device," + " Err:%d\n", + result); + goto error; + } + + lldev = __dev_get_by_index(dev_net(dev), llifindex); + + if (lldev == NULL) { + pr_debug("no interface?\n"); + result = -ENODEV; + goto error; + } + + dev->needed_tailroom = tailroom + lldev->needed_tailroom; + dev->hard_header_len = headroom + lldev->hard_header_len + + lldev->needed_tailroom; + + /* + * MTU, head-room etc is not know before we have a + * CAIF link layer device available. MTU calculation may + * override initial RTNL configuration. + * MTU is minimum of current mtu, link layer mtu pluss + * CAIF head and tail, and PDP GPRS contexts max MTU. + */ + mtu = min_t(int, dev->mtu, lldev->mtu - (headroom + tailroom)); + mtu = min_t(int, GPRS_PDP_MTU, mtu); + dev_set_mtu(dev, mtu); + + if (mtu < 100) { + pr_warn("CAIF Interface MTU too small (%d)\n", mtu); + result = -ENODEV; + goto error; + } + } + + rtnl_unlock(); /* Release RTNL lock during connect wait */ + + result = wait_event_interruptible_timeout(priv->netmgmt_wq, + priv->state != CAIF_CONNECTING, + CONNECT_TIMEOUT); + + rtnl_lock(); + + if (result == -ERESTARTSYS) { + pr_debug("wait_event_interruptible woken by a signal\n"); + result = -ERESTARTSYS; + goto error; + } + + if (result == 0) { + pr_debug("connect timeout\n"); + caif_disconnect_client(dev_net(dev), &priv->chnl); + priv->state = CAIF_DISCONNECTED; + pr_debug("state disconnected\n"); + result = -ETIMEDOUT; + goto error; + } + + if (priv->state != CAIF_CONNECTED) { + pr_debug("connect failed\n"); + result = -ECONNREFUSED; + goto error; + } + pr_debug("CAIF Netdevice connected\n"); + return 0; + +error: + caif_disconnect_client(dev_net(dev), &priv->chnl); + priv->state = CAIF_DISCONNECTED; + pr_debug("state disconnected\n"); + return result; + +} + +static int chnl_net_stop(struct net_device *dev) +{ + struct chnl_net *priv; + + ASSERT_RTNL(); + priv = netdev_priv(dev); + priv->state = CAIF_DISCONNECTED; + caif_disconnect_client(dev_net(dev), &priv->chnl); + return 0; +} + +static int chnl_net_init(struct net_device *dev) +{ + struct chnl_net *priv; + ASSERT_RTNL(); + priv = netdev_priv(dev); + strncpy(priv->name, dev->name, sizeof(priv->name)); + INIT_LIST_HEAD(&priv->list_field); + return 0; +} + +static void chnl_net_uninit(struct net_device *dev) +{ + struct chnl_net *priv; + ASSERT_RTNL(); + priv = netdev_priv(dev); + list_del_init(&priv->list_field); +} + +static const struct net_device_ops netdev_ops = { + .ndo_open = chnl_net_open, + .ndo_stop = chnl_net_stop, + .ndo_init = chnl_net_init, + .ndo_uninit = chnl_net_uninit, + .ndo_start_xmit = chnl_net_start_xmit, +}; + +static void chnl_net_destructor(struct net_device *dev) +{ + struct chnl_net *priv = netdev_priv(dev); + caif_free_client(&priv->chnl); +} + +static void ipcaif_net_setup(struct net_device *dev) +{ + struct chnl_net *priv; + dev->netdev_ops = &netdev_ops; + dev->needs_free_netdev = true; + dev->priv_destructor = chnl_net_destructor; + dev->flags |= IFF_NOARP; + dev->flags |= IFF_POINTOPOINT; + dev->mtu = GPRS_PDP_MTU; + dev->tx_queue_len = CAIF_NET_DEFAULT_QUEUE_LEN; + + priv = netdev_priv(dev); + priv->chnl.receive = chnl_recv_cb; + priv->chnl.ctrlcmd = chnl_flowctrl_cb; + priv->netdev = dev; + priv->conn_req.protocol = CAIFPROTO_DATAGRAM; + priv->conn_req.link_selector = CAIF_LINK_HIGH_BANDW; + priv->conn_req.priority = CAIF_PRIO_LOW; + /* Insert illegal value */ + priv->conn_req.sockaddr.u.dgm.connection_id = UNDEF_CONNID; + priv->flowenabled = false; + + init_waitqueue_head(&priv->netmgmt_wq); +} + + +static int ipcaif_fill_info(struct sk_buff *skb, const struct net_device *dev) +{ + struct chnl_net *priv; + u8 loop; + priv = netdev_priv(dev); + if (nla_put_u32(skb, IFLA_CAIF_IPV4_CONNID, + priv->conn_req.sockaddr.u.dgm.connection_id) || + nla_put_u32(skb, IFLA_CAIF_IPV6_CONNID, + priv->conn_req.sockaddr.u.dgm.connection_id)) + goto nla_put_failure; + loop = priv->conn_req.protocol == CAIFPROTO_DATAGRAM_LOOP; + if (nla_put_u8(skb, IFLA_CAIF_LOOPBACK, loop)) + goto nla_put_failure; + return 0; +nla_put_failure: + return -EMSGSIZE; + +} + +static void caif_netlink_parms(struct nlattr *data[], + struct caif_connect_request *conn_req) +{ + if (!data) { + pr_warn("no params data found\n"); + return; + } + if (data[IFLA_CAIF_IPV4_CONNID]) + conn_req->sockaddr.u.dgm.connection_id = + nla_get_u32(data[IFLA_CAIF_IPV4_CONNID]); + if (data[IFLA_CAIF_IPV6_CONNID]) + conn_req->sockaddr.u.dgm.connection_id = + nla_get_u32(data[IFLA_CAIF_IPV6_CONNID]); + if (data[IFLA_CAIF_LOOPBACK]) { + if (nla_get_u8(data[IFLA_CAIF_LOOPBACK])) + conn_req->protocol = CAIFPROTO_DATAGRAM_LOOP; + else + conn_req->protocol = CAIFPROTO_DATAGRAM; + } +} + +static int ipcaif_newlink(struct net *src_net, struct net_device *dev, + struct nlattr *tb[], struct nlattr *data[], + struct netlink_ext_ack *extack) +{ + int ret; + struct chnl_net *caifdev; + ASSERT_RTNL(); + caifdev = netdev_priv(dev); + caif_netlink_parms(data, &caifdev->conn_req); + + ret = register_netdevice(dev); + if (ret) + pr_warn("device rtml registration failed\n"); + else + list_add(&caifdev->list_field, &chnl_net_list); + + /* Use ifindex as connection id, and use loopback channel default. */ + if (caifdev->conn_req.sockaddr.u.dgm.connection_id == UNDEF_CONNID) { + caifdev->conn_req.sockaddr.u.dgm.connection_id = dev->ifindex; + caifdev->conn_req.protocol = CAIFPROTO_DATAGRAM_LOOP; + } + return ret; +} + +static int ipcaif_changelink(struct net_device *dev, struct nlattr *tb[], + struct nlattr *data[], + struct netlink_ext_ack *extack) +{ + struct chnl_net *caifdev; + ASSERT_RTNL(); + caifdev = netdev_priv(dev); + caif_netlink_parms(data, &caifdev->conn_req); + netdev_state_change(dev); + return 0; +} + +static size_t ipcaif_get_size(const struct net_device *dev) +{ + return + /* IFLA_CAIF_IPV4_CONNID */ + nla_total_size(4) + + /* IFLA_CAIF_IPV6_CONNID */ + nla_total_size(4) + + /* IFLA_CAIF_LOOPBACK */ + nla_total_size(2) + + 0; +} + +static const struct nla_policy ipcaif_policy[IFLA_CAIF_MAX + 1] = { + [IFLA_CAIF_IPV4_CONNID] = { .type = NLA_U32 }, + [IFLA_CAIF_IPV6_CONNID] = { .type = NLA_U32 }, + [IFLA_CAIF_LOOPBACK] = { .type = NLA_U8 } +}; + + +static struct rtnl_link_ops ipcaif_link_ops __read_mostly = { + .kind = "caif", + .priv_size = sizeof(struct chnl_net), + .setup = ipcaif_net_setup, + .maxtype = IFLA_CAIF_MAX, + .policy = ipcaif_policy, + .newlink = ipcaif_newlink, + .changelink = ipcaif_changelink, + .get_size = ipcaif_get_size, + .fill_info = ipcaif_fill_info, + +}; + +static int __init chnl_init_module(void) +{ + return rtnl_link_register(&ipcaif_link_ops); +} + +static void __exit chnl_exit_module(void) +{ + struct chnl_net *dev = NULL; + struct list_head *list_node; + struct list_head *_tmp; + rtnl_link_unregister(&ipcaif_link_ops); + rtnl_lock(); + list_for_each_safe(list_node, _tmp, &chnl_net_list) { + dev = list_entry(list_node, struct chnl_net, list_field); + list_del_init(list_node); + delete_device(dev); + } + rtnl_unlock(); +} + +module_init(chnl_init_module); +module_exit(chnl_exit_module); |