From 4b07b3f69a8471cdc142c51461a331226fef248a Mon Sep 17 00:00:00 2001 From: Remi Denis-Courmont Date: Mon, 22 Sep 2008 20:02:10 -0700 Subject: Phonet: PF_PHONET protocol family support This is the basis for the Phonet protocol families, and introduces the ETH_P_PHONET packet type and the PF_PHONET socket family. Signed-off-by: Remi Denis-Courmont Signed-off-by: David S. Miller --- net/phonet/af_phonet.c | 216 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 216 insertions(+) create mode 100644 net/phonet/af_phonet.c (limited to 'net/phonet') diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c new file mode 100644 index 000000000000..0cfea9bc994a --- /dev/null +++ b/net/phonet/af_phonet.c @@ -0,0 +1,216 @@ +/* + * File: af_phonet.c + * + * Phonet protocols family + * + * Copyright (C) 2008 Nokia Corporation. + * + * Contact: Remi Denis-Courmont + * Original author: Sakari Ailus + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ + +#include +#include +#include +#include + +#include +#include +#include + +static struct net_proto_family phonet_proto_family; +static struct phonet_protocol *phonet_proto_get(int protocol); +static inline void phonet_proto_put(struct phonet_protocol *pp); + +/* protocol family functions */ + +static int pn_socket_create(struct net *net, struct socket *sock, int protocol) +{ + struct phonet_protocol *pnp; + int err; + + if (net != &init_net) + return -EAFNOSUPPORT; + + if (!capable(CAP_SYS_ADMIN)) + return -EPERM; + + if (protocol == 0) { + /* Default protocol selection */ + switch (sock->type) { + case SOCK_DGRAM: + protocol = PN_PROTO_PHONET; + break; + default: + return -EPROTONOSUPPORT; + } + } + + pnp = phonet_proto_get(protocol); + if (pnp == NULL) + return -EPROTONOSUPPORT; + if (sock->type != pnp->sock_type) { + err = -EPROTONOSUPPORT; + goto out; + } + + /* TODO: create and init the struct sock */ + err = -EPROTONOSUPPORT; + +out: + phonet_proto_put(pnp); + return err; +} + +static struct net_proto_family phonet_proto_family = { + .family = AF_PHONET, + .create = pn_socket_create, + .owner = THIS_MODULE, +}; + +/* packet type functions */ + +/* + * Stuff received packets to associated sockets. + * On error, returns non-zero and releases the skb. + */ +static int phonet_rcv(struct sk_buff *skb, struct net_device *dev, + struct packet_type *pkttype, + struct net_device *orig_dev) +{ + struct phonethdr *ph; + struct sockaddr_pn sa; + u16 len; + + if (dev_net(dev) != &init_net) + goto out; + + /* check we have at least a full Phonet header */ + if (!pskb_pull(skb, sizeof(struct phonethdr))) + goto out; + + /* check that the advertised length is correct */ + ph = pn_hdr(skb); + len = get_unaligned_be16(&ph->pn_length); + if (len < 2) + goto out; + len -= 2; + if ((len > skb->len) || pskb_trim(skb, len)) + goto out; + skb_reset_transport_header(skb); + + pn_skb_get_dst_sockaddr(skb, &sa); + if (pn_sockaddr_get_addr(&sa) == 0) + goto out; /* currently, we cannot be device 0 */ + + /* TODO: put packets to sockets backlog */ + +out: + kfree_skb(skb); + return NET_RX_DROP; +} + +static struct packet_type phonet_packet_type = { + .type = __constant_htons(ETH_P_PHONET), + .dev = NULL, + .func = phonet_rcv, +}; + +/* Transport protocol registration */ +static struct phonet_protocol *proto_tab[PHONET_NPROTO] __read_mostly; +static DEFINE_SPINLOCK(proto_tab_lock); + +int __init_or_module phonet_proto_register(int protocol, + struct phonet_protocol *pp) +{ + int err = 0; + + if (protocol >= PHONET_NPROTO) + return -EINVAL; + + err = proto_register(pp->prot, 1); + if (err) + return err; + + spin_lock(&proto_tab_lock); + if (proto_tab[protocol]) + err = -EBUSY; + else + proto_tab[protocol] = pp; + spin_unlock(&proto_tab_lock); + + return err; +} +EXPORT_SYMBOL(phonet_proto_register); + +void phonet_proto_unregister(int protocol, struct phonet_protocol *pp) +{ + spin_lock(&proto_tab_lock); + BUG_ON(proto_tab[protocol] != pp); + proto_tab[protocol] = NULL; + spin_unlock(&proto_tab_lock); + proto_unregister(pp->prot); +} +EXPORT_SYMBOL(phonet_proto_unregister); + +static struct phonet_protocol *phonet_proto_get(int protocol) +{ + struct phonet_protocol *pp; + + if (protocol >= PHONET_NPROTO) + return NULL; + + spin_lock(&proto_tab_lock); + pp = proto_tab[protocol]; + if (pp && !try_module_get(pp->prot->owner)) + pp = NULL; + spin_unlock(&proto_tab_lock); + + return pp; +} + +static inline void phonet_proto_put(struct phonet_protocol *pp) +{ + module_put(pp->prot->owner); +} + +/* Module registration */ +static int __init phonet_init(void) +{ + int err; + + err = sock_register(&phonet_proto_family); + if (err) { + printk(KERN_ALERT + "phonet protocol family initialization failed\n"); + return err; + } + + dev_add_pack(&phonet_packet_type); + return 0; +} + +static void __exit phonet_exit(void) +{ + sock_unregister(AF_PHONET); + dev_remove_pack(&phonet_packet_type); +} + +module_init(phonet_init); +module_exit(phonet_exit); +MODULE_DESCRIPTION("Phonet protocol stack for Linux"); +MODULE_LICENSE("GPL"); -- cgit v1.2.3 From 8ead536dec142f27d5b5f72c3994eb39f4741717 Mon Sep 17 00:00:00 2001 From: Remi Denis-Courmont Date: Mon, 22 Sep 2008 20:03:00 -0700 Subject: Phonet: add CONFIG_PHONET Signed-off-by: Remi Denis-Courmont Signed-off-by: David S. Miller --- net/Kconfig | 1 + net/Makefile | 1 + net/phonet/Kconfig | 16 ++++++++++++++++ net/phonet/Makefile | 4 ++++ 4 files changed, 22 insertions(+) create mode 100644 net/phonet/Kconfig create mode 100644 net/phonet/Makefile (limited to 'net/phonet') diff --git a/net/Kconfig b/net/Kconfig index d87de48ba656..9103a16a77be 100644 --- a/net/Kconfig +++ b/net/Kconfig @@ -232,6 +232,7 @@ source "net/can/Kconfig" source "net/irda/Kconfig" source "net/bluetooth/Kconfig" source "net/rxrpc/Kconfig" +source "net/phonet/Kconfig" config FIB_RULES bool diff --git a/net/Makefile b/net/Makefile index 4f43e7f874f3..acaf819f24aa 100644 --- a/net/Makefile +++ b/net/Makefile @@ -42,6 +42,7 @@ obj-$(CONFIG_AF_RXRPC) += rxrpc/ obj-$(CONFIG_ATM) += atm/ obj-$(CONFIG_DECNET) += decnet/ obj-$(CONFIG_ECONET) += econet/ +obj-$(CONFIG_PHONET) += phonet/ ifneq ($(CONFIG_VLAN_8021Q),) obj-y += 8021q/ endif diff --git a/net/phonet/Kconfig b/net/phonet/Kconfig new file mode 100644 index 000000000000..51a5669573f2 --- /dev/null +++ b/net/phonet/Kconfig @@ -0,0 +1,16 @@ +# +# Phonet protocol +# + +config PHONET + tristate "Phonet protocols family" + help + The Phone Network protocol (PhoNet) is a packet-oriented + communication protocol developped by Nokia for use with its modems. + + This is required for Maemo to use cellular data connectivity (if + supported). It can also be used to control Nokia phones + from a Linux computer, although AT commands may be easier to use. + + To compile this driver as a module, choose M here: the module + will be called phonet. If unsure, say N. diff --git a/net/phonet/Makefile b/net/phonet/Makefile new file mode 100644 index 000000000000..5dbff68a6f36 --- /dev/null +++ b/net/phonet/Makefile @@ -0,0 +1,4 @@ +obj-$(CONFIG_PHONET) += phonet.o + +phonet-objs := \ + af_phonet.o -- cgit v1.2.3 From f8ff60283de2b6775d7a14619056a08e3083bd40 Mon Sep 17 00:00:00 2001 From: Remi Denis-Courmont Date: Mon, 22 Sep 2008 20:03:44 -0700 Subject: Phonet: network device and address handling MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This provides support for adding Phonet addresses to and removing Phonet addresses from network devices. Signed-off-by: Rémi Denis-Courmont Signed-off-by: David S. Miller --- include/net/phonet/pn_dev.h | 50 +++++++++++ net/phonet/Makefile | 1 + net/phonet/af_phonet.c | 3 + net/phonet/pn_dev.c | 208 ++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 262 insertions(+) create mode 100644 include/net/phonet/pn_dev.h create mode 100644 net/phonet/pn_dev.c (limited to 'net/phonet') diff --git a/include/net/phonet/pn_dev.h b/include/net/phonet/pn_dev.h new file mode 100644 index 000000000000..bbd2a836e04c --- /dev/null +++ b/include/net/phonet/pn_dev.h @@ -0,0 +1,50 @@ +/* + * File: pn_dev.h + * + * Phonet network device + * + * Copyright (C) 2008 Nokia Corporation. + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ + +#ifndef PN_DEV_H +#define PN_DEV_H + +struct phonet_device_list { + struct list_head list; + spinlock_t lock; +}; + +extern struct phonet_device_list pndevs; + +struct phonet_device { + struct list_head list; + struct net_device *netdev; + DECLARE_BITMAP(addrs, 64); +}; + +void phonet_device_init(void); +void phonet_device_exit(void); +struct net_device *phonet_device_get(struct net *net); + +int phonet_address_add(struct net_device *dev, u8 addr); +int phonet_address_del(struct net_device *dev, u8 addr); +u8 phonet_address_get(struct net_device *dev, u8 addr); +int phonet_address_lookup(u8 addr); + +#define PN_NO_ADDR 0xff + +#endif diff --git a/net/phonet/Makefile b/net/phonet/Makefile index 5dbff68a6f36..980a3866c9a3 100644 --- a/net/phonet/Makefile +++ b/net/phonet/Makefile @@ -1,4 +1,5 @@ obj-$(CONFIG_PHONET) += phonet.o phonet-objs := \ + pn_dev.o \ af_phonet.o diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c index 0cfea9bc994a..a8ba6f177b29 100644 --- a/net/phonet/af_phonet.c +++ b/net/phonet/af_phonet.c @@ -31,6 +31,7 @@ #include #include #include +#include static struct net_proto_family phonet_proto_family; static struct phonet_protocol *phonet_proto_get(int protocol); @@ -200,6 +201,7 @@ static int __init phonet_init(void) return err; } + phonet_device_init(); dev_add_pack(&phonet_packet_type); return 0; } @@ -208,6 +210,7 @@ static void __exit phonet_exit(void) { sock_unregister(AF_PHONET); dev_remove_pack(&phonet_packet_type); + phonet_device_exit(); } module_init(phonet_init); diff --git a/net/phonet/pn_dev.c b/net/phonet/pn_dev.c new file mode 100644 index 000000000000..53be9fc82aaa --- /dev/null +++ b/net/phonet/pn_dev.c @@ -0,0 +1,208 @@ +/* + * File: pn_dev.c + * + * Phonet network device + * + * Copyright (C) 2008 Nokia Corporation. + * + * Contact: Remi Denis-Courmont + * Original author: Sakari Ailus + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ + +#include +#include +#include +#include +#include +#include + +/* when accessing, remember to lock with spin_lock(&pndevs.lock); */ +struct phonet_device_list pndevs = { + .list = LIST_HEAD_INIT(pndevs.list), + .lock = __SPIN_LOCK_UNLOCKED(pndevs.lock), +}; + +/* Allocate new Phonet device. */ +static struct phonet_device *__phonet_device_alloc(struct net_device *dev) +{ + struct phonet_device *pnd = kmalloc(sizeof(*pnd), GFP_ATOMIC); + if (pnd == NULL) + return NULL; + pnd->netdev = dev; + bitmap_zero(pnd->addrs, 64); + + list_add(&pnd->list, &pndevs.list); + return pnd; +} + +static struct phonet_device *__phonet_get(struct net_device *dev) +{ + struct phonet_device *pnd; + + list_for_each_entry(pnd, &pndevs.list, list) { + if (pnd->netdev == dev) + return pnd; + } + return NULL; +} + +static void __phonet_device_free(struct phonet_device *pnd) +{ + list_del(&pnd->list); + kfree(pnd); +} + +struct net_device *phonet_device_get(struct net *net) +{ + struct phonet_device *pnd; + struct net_device *dev; + + spin_lock_bh(&pndevs.lock); + list_for_each_entry(pnd, &pndevs.list, list) { + dev = pnd->netdev; + BUG_ON(!dev); + + if (dev_net(dev) == net && + (dev->reg_state == NETREG_REGISTERED) && + ((pnd->netdev->flags & IFF_UP)) == IFF_UP) + break; + dev = NULL; + } + if (dev) + dev_hold(dev); + spin_unlock_bh(&pndevs.lock); + return dev; +} + +int phonet_address_add(struct net_device *dev, u8 addr) +{ + struct phonet_device *pnd; + int err = 0; + + spin_lock_bh(&pndevs.lock); + /* Find or create Phonet-specific device data */ + pnd = __phonet_get(dev); + if (pnd == NULL) + pnd = __phonet_device_alloc(dev); + if (unlikely(pnd == NULL)) + err = -ENOMEM; + else if (test_and_set_bit(addr >> 2, pnd->addrs)) + err = -EEXIST; + spin_unlock_bh(&pndevs.lock); + return err; +} + +int phonet_address_del(struct net_device *dev, u8 addr) +{ + struct phonet_device *pnd; + int err = 0; + + spin_lock_bh(&pndevs.lock); + pnd = __phonet_get(dev); + if (!pnd || !test_and_clear_bit(addr >> 2, pnd->addrs)) + err = -EADDRNOTAVAIL; + if (bitmap_empty(pnd->addrs, 64)) + __phonet_device_free(pnd); + spin_unlock_bh(&pndevs.lock); + return err; +} + +/* Gets a source address toward a destination, through a interface. */ +u8 phonet_address_get(struct net_device *dev, u8 addr) +{ + struct phonet_device *pnd; + + spin_lock_bh(&pndevs.lock); + pnd = __phonet_get(dev); + if (pnd) { + BUG_ON(bitmap_empty(pnd->addrs, 64)); + + /* Use same source address as destination, if possible */ + if (!test_bit(addr >> 2, pnd->addrs)) + addr = find_first_bit(pnd->addrs, 64) << 2; + } else + addr = PN_NO_ADDR; + spin_unlock_bh(&pndevs.lock); + return addr; +} + +int phonet_address_lookup(u8 addr) +{ + struct phonet_device *pnd; + + spin_lock_bh(&pndevs.lock); + list_for_each_entry(pnd, &pndevs.list, list) { + /* Don't allow unregistering devices! */ + if ((pnd->netdev->reg_state != NETREG_REGISTERED) || + ((pnd->netdev->flags & IFF_UP)) != IFF_UP) + continue; + + if (test_bit(addr >> 2, pnd->addrs)) { + spin_unlock_bh(&pndevs.lock); + return 0; + } + } + spin_unlock_bh(&pndevs.lock); + return -EADDRNOTAVAIL; +} + +/* notify Phonet of device events */ +static int phonet_device_notify(struct notifier_block *me, unsigned long what, + void *arg) +{ + struct net_device *dev = arg; + + if (what == NETDEV_UNREGISTER) { + struct phonet_device *pnd; + + /* Destroy phonet-specific device data */ + spin_lock_bh(&pndevs.lock); + pnd = __phonet_get(dev); + if (pnd) + __phonet_device_free(pnd); + spin_unlock_bh(&pndevs.lock); + } + return 0; + +} + +static struct notifier_block phonet_device_notifier = { + .notifier_call = phonet_device_notify, + .priority = 0, +}; + +/* Initialize Phonet devices list */ +void phonet_device_init(void) +{ + register_netdevice_notifier(&phonet_device_notifier); +} + +void phonet_device_exit(void) +{ + struct phonet_device *pnd, *n; + + rtnl_unregister_all(PF_PHONET); + rtnl_lock(); + spin_lock_bh(&pndevs.lock); + + list_for_each_entry_safe(pnd, n, &pndevs.list, list) + __phonet_device_free(pnd); + + spin_unlock_bh(&pndevs.lock); + rtnl_unlock(); + unregister_netdevice_notifier(&phonet_device_notifier); +} -- cgit v1.2.3 From 8fb397406f6470f79040c41eec49af20900a9e3b Mon Sep 17 00:00:00 2001 From: Remi Denis-Courmont Date: Mon, 22 Sep 2008 20:04:30 -0700 Subject: Phonet: Netlink interface This provides support for configuring Phonet addresses, notifying Phonet configuration changes, and dumping the configuration. Signed-off-by: Remi Denis-Courmont Signed-off-by: David S. Miller --- include/net/phonet/phonet.h | 1 + net/phonet/Makefile | 1 + net/phonet/af_phonet.c | 1 + net/phonet/pn_netlink.c | 186 ++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 189 insertions(+) create mode 100644 net/phonet/pn_netlink.c (limited to 'net/phonet') diff --git a/include/net/phonet/phonet.h b/include/net/phonet/phonet.h index c53f2abc0595..8b777943d201 100644 --- a/include/net/phonet/phonet.h +++ b/include/net/phonet/phonet.h @@ -71,4 +71,5 @@ struct phonet_protocol { int phonet_proto_register(int protocol, struct phonet_protocol *pp); void phonet_proto_unregister(int protocol, struct phonet_protocol *pp); +void phonet_netlink_register(void); #endif diff --git a/net/phonet/Makefile b/net/phonet/Makefile index 980a3866c9a3..4143c3e1dfdc 100644 --- a/net/phonet/Makefile +++ b/net/phonet/Makefile @@ -2,4 +2,5 @@ obj-$(CONFIG_PHONET) += phonet.o phonet-objs := \ pn_dev.o \ + pn_netlink.o \ af_phonet.o diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c index a8ba6f177b29..5c729ba56939 100644 --- a/net/phonet/af_phonet.c +++ b/net/phonet/af_phonet.c @@ -203,6 +203,7 @@ static int __init phonet_init(void) phonet_device_init(); dev_add_pack(&phonet_packet_type); + phonet_netlink_register(); return 0; } diff --git a/net/phonet/pn_netlink.c b/net/phonet/pn_netlink.c new file mode 100644 index 000000000000..b1ea19a230dd --- /dev/null +++ b/net/phonet/pn_netlink.c @@ -0,0 +1,186 @@ +/* + * File: pn_netlink.c + * + * Phonet netlink interface + * + * Copyright (C) 2008 Nokia Corporation. + * + * Contact: Remi Denis-Courmont + * Original author: Sakari Ailus + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ + +#include +#include +#include +#include +#include + +static int fill_addr(struct sk_buff *skb, struct net_device *dev, u8 addr, + u32 pid, u32 seq, int event); + +static void rtmsg_notify(int event, struct net_device *dev, u8 addr) +{ + struct sk_buff *skb; + int err = -ENOBUFS; + + skb = nlmsg_new(NLMSG_ALIGN(sizeof(struct ifaddrmsg)) + + nla_total_size(1), GFP_KERNEL); + if (skb == NULL) + goto errout; + err = fill_addr(skb, dev, addr, 0, 0, event); + if (err < 0) { + WARN_ON(err == -EMSGSIZE); + kfree_skb(skb); + goto errout; + } + err = rtnl_notify(skb, dev_net(dev), 0, + RTNLGRP_PHONET_IFADDR, NULL, GFP_KERNEL); +errout: + if (err < 0) + rtnl_set_sk_err(dev_net(dev), RTNLGRP_PHONET_IFADDR, err); +} + +static int newaddr_doit(struct sk_buff *skb, struct nlmsghdr *nlm, void *attr) +{ + struct rtattr **rta = attr; + struct ifaddrmsg *ifm = NLMSG_DATA(nlm); + struct net_device *dev; + int err; + u8 pnaddr; + + if (!capable(CAP_SYS_ADMIN)) + return -EPERM; + + ASSERT_RTNL(); + + if (rta[IFA_LOCAL - 1] == NULL) + return -EINVAL; + + dev = __dev_get_by_index(&init_net, ifm->ifa_index); + if (dev == NULL) + return -ENODEV; + + if (ifm->ifa_prefixlen > 0) + return -EINVAL; + + memcpy(&pnaddr, RTA_DATA(rta[IFA_LOCAL - 1]), 1); + + err = phonet_address_add(dev, pnaddr); + if (!err) + rtmsg_notify(RTM_NEWADDR, dev, pnaddr); + return err; +} + +static int deladdr_doit(struct sk_buff *skb, struct nlmsghdr *nlm, void *attr) +{ + struct rtattr **rta = attr; + struct ifaddrmsg *ifm = NLMSG_DATA(nlm); + struct net_device *dev; + int err; + u8 pnaddr; + + if (!capable(CAP_SYS_ADMIN)) + return -EPERM; + + ASSERT_RTNL(); + + if (rta[IFA_LOCAL - 1] == NULL) + return -EINVAL; + + dev = __dev_get_by_index(&init_net, ifm->ifa_index); + if (dev == NULL) + return -ENODEV; + + if (ifm->ifa_prefixlen > 0) + return -EADDRNOTAVAIL; + + memcpy(&pnaddr, RTA_DATA(rta[IFA_LOCAL - 1]), 1); + + err = phonet_address_del(dev, pnaddr); + if (!err) + rtmsg_notify(RTM_DELADDR, dev, pnaddr); + return err; +} + +static int fill_addr(struct sk_buff *skb, struct net_device *dev, u8 addr, + u32 pid, u32 seq, int event) +{ + struct ifaddrmsg *ifm; + struct nlmsghdr *nlh; + unsigned int orig_len = skb->len; + + nlh = NLMSG_PUT(skb, pid, seq, event, sizeof(struct ifaddrmsg)); + ifm = NLMSG_DATA(nlh); + ifm->ifa_family = AF_PHONET; + ifm->ifa_prefixlen = 0; + ifm->ifa_flags = IFA_F_PERMANENT; + ifm->ifa_scope = RT_SCOPE_HOST; + ifm->ifa_index = dev->ifindex; + RTA_PUT(skb, IFA_LOCAL, 1, &addr); + nlh->nlmsg_len = skb->len - orig_len; + + return 0; + +nlmsg_failure: +rtattr_failure: + skb_trim(skb, orig_len); + + return -1; +} + +static int getaddr_dumpit(struct sk_buff *skb, struct netlink_callback *cb) +{ + struct phonet_device *pnd; + int dev_idx = 0, dev_start_idx = cb->args[0]; + int addr_idx = 0, addr_start_idx = cb->args[1]; + + spin_lock_bh(&pndevs.lock); + list_for_each_entry(pnd, &pndevs.list, list) { + u8 addr; + + if (dev_idx > dev_start_idx) + addr_start_idx = 0; + if (dev_idx++ < dev_start_idx) + continue; + + addr_idx = 0; + for (addr = find_first_bit(pnd->addrs, 64); addr < 64; + addr = find_next_bit(pnd->addrs, 64, 1+addr)) { + if (addr_idx++ < addr_start_idx) + continue; + + if (fill_addr(skb, pnd->netdev, addr << 2, + NETLINK_CB(cb->skb).pid, + cb->nlh->nlmsg_seq, RTM_NEWADDR)) + goto out; + } + } + +out: + spin_unlock_bh(&pndevs.lock); + cb->args[0] = dev_idx; + cb->args[1] = addr_idx; + + return skb->len; +} + +void __init phonet_netlink_register(void) +{ + rtnl_register(PF_PHONET, RTM_NEWADDR, newaddr_doit, NULL); + rtnl_register(PF_PHONET, RTM_DELADDR, deladdr_doit, NULL); + rtnl_register(PF_PHONET, RTM_GETADDR, NULL, getaddr_dumpit); +} -- cgit v1.2.3 From ba113a94b7503ee23ffe819e7045134b0c1d31de Mon Sep 17 00:00:00 2001 From: Remi Denis-Courmont Date: Mon, 22 Sep 2008 20:05:19 -0700 Subject: Phonet: common socket glue This provides the socket API for the Phonet protocols family. Signed-off-by: Remi Denis-Courmont Signed-off-by: David S. Miller --- include/linux/phonet.h | 3 + include/net/phonet/phonet.h | 23 ++++ net/phonet/Makefile | 1 + net/phonet/af_phonet.c | 28 +++- net/phonet/socket.c | 311 ++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 363 insertions(+), 3 deletions(-) create mode 100644 net/phonet/socket.c (limited to 'net/phonet') diff --git a/include/linux/phonet.h b/include/linux/phonet.h index 6a764f8584a4..001c0e679099 100644 --- a/include/linux/phonet.h +++ b/include/linux/phonet.h @@ -32,6 +32,9 @@ #define PNADDR_ANY 0 #define PNPORT_RESOURCE_ROUTING 0 +/* ioctls */ +#define SIOCPNGETOBJECT (SIOCPROTOPRIVATE + 0) + /* Phonet protocol header */ struct phonethdr { __u8 pn_rdev; diff --git a/include/net/phonet/phonet.h b/include/net/phonet/phonet.h index 8b777943d201..2ae5cbb59b60 100644 --- a/include/net/phonet/phonet.h +++ b/include/net/phonet/phonet.h @@ -29,6 +29,28 @@ */ #define MAX_PHONET_HEADER 8 +/* + * Every Phonet* socket has this structure first in its + * protocol-specific structure under name c. + */ +struct pn_sock { + struct sock sk; + u16 sobject; + u8 resource; +}; + +static inline struct pn_sock *pn_sk(struct sock *sk) +{ + return (struct pn_sock *)sk; +} + +extern const struct proto_ops phonet_dgram_ops; + +struct sock *pn_find_sock_by_sa(const struct sockaddr_pn *sa); +void pn_sock_hash(struct sock *sk); +void pn_sock_unhash(struct sock *sk); +int pn_sock_get_port(struct sock *sk, unsigned short sport); + static inline struct phonethdr *pn_hdr(struct sk_buff *skb) { return (struct phonethdr *)skb_network_header(skb); @@ -64,6 +86,7 @@ void pn_skb_get_dst_sockaddr(struct sk_buff *skb, struct sockaddr_pn *sa) /* Protocols in Phonet protocol family. */ struct phonet_protocol { + const struct proto_ops *ops; struct proto *prot; int sock_type; }; diff --git a/net/phonet/Makefile b/net/phonet/Makefile index 4143c3e1dfdc..c1d671de7835 100644 --- a/net/phonet/Makefile +++ b/net/phonet/Makefile @@ -3,4 +3,5 @@ obj-$(CONFIG_PHONET) += phonet.o phonet-objs := \ pn_dev.o \ pn_netlink.o \ + socket.o \ af_phonet.o diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c index 5c729ba56939..ba54d53020ff 100644 --- a/net/phonet/af_phonet.c +++ b/net/phonet/af_phonet.c @@ -41,6 +41,8 @@ static inline void phonet_proto_put(struct phonet_protocol *pp); static int pn_socket_create(struct net *net, struct socket *sock, int protocol) { + struct sock *sk; + struct pn_sock *pn; struct phonet_protocol *pnp; int err; @@ -69,8 +71,22 @@ static int pn_socket_create(struct net *net, struct socket *sock, int protocol) goto out; } - /* TODO: create and init the struct sock */ - err = -EPROTONOSUPPORT; + sk = sk_alloc(net, PF_PHONET, GFP_KERNEL, pnp->prot); + if (sk == NULL) { + err = -ENOMEM; + goto out; + } + + sock_init_data(sock, sk); + sock->state = SS_UNCONNECTED; + sock->ops = pnp->ops; + sk->sk_backlog_rcv = sk->sk_prot->backlog_rcv; + sk->sk_protocol = protocol; + pn = pn_sk(sk); + pn->sobject = 0; + pn->resource = 0; + sk->sk_prot->init(sk); + err = 0; out: phonet_proto_put(pnp); @@ -94,6 +110,7 @@ static int phonet_rcv(struct sk_buff *skb, struct net_device *dev, struct net_device *orig_dev) { struct phonethdr *ph; + struct sock *sk; struct sockaddr_pn sa; u16 len; @@ -118,7 +135,12 @@ static int phonet_rcv(struct sk_buff *skb, struct net_device *dev, if (pn_sockaddr_get_addr(&sa) == 0) goto out; /* currently, we cannot be device 0 */ - /* TODO: put packets to sockets backlog */ + sk = pn_find_sock_by_sa(&sa); + if (sk == NULL) + goto out; + + /* Push data to the socket (or other sockets connected to it). */ + return sk_receive_skb(sk, skb, 0); out: kfree_skb(skb); diff --git a/net/phonet/socket.c b/net/phonet/socket.c new file mode 100644 index 000000000000..99a4945d565d --- /dev/null +++ b/net/phonet/socket.c @@ -0,0 +1,311 @@ +/* + * File: socket.c + * + * Phonet sockets + * + * Copyright (C) 2008 Nokia Corporation. + * + * Contact: Remi Denis-Courmont + * Original author: Sakari Ailus + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ + +#include +#include +#include +#include + +#include +#include +#include + +static int pn_socket_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + + if (sk) { + sock->sk = NULL; + sk->sk_prot->close(sk, 0); + } + return 0; +} + +static struct { + struct hlist_head hlist; + spinlock_t lock; +} pnsocks = { + .hlist = HLIST_HEAD_INIT, + .lock = __SPIN_LOCK_UNLOCKED(pnsocks.lock), +}; + +/* + * Find address based on socket address, match only certain fields. + * Also grab sock if it was found. Remember to sock_put it later. + */ +struct sock *pn_find_sock_by_sa(const struct sockaddr_pn *spn) +{ + struct hlist_node *node; + struct sock *sknode; + struct sock *rval = NULL; + u16 obj = pn_sockaddr_get_object(spn); + u8 res = spn->spn_resource; + + spin_lock_bh(&pnsocks.lock); + + sk_for_each(sknode, node, &pnsocks.hlist) { + struct pn_sock *pn = pn_sk(sknode); + BUG_ON(!pn->sobject); /* unbound socket */ + + if (pn_port(obj)) { + /* Look up socket by port */ + if (pn_port(pn->sobject) != pn_port(obj)) + continue; + } else { + /* If port is zero, look up by resource */ + if (pn->resource != res) + continue; + } + if (pn_addr(pn->sobject) + && pn_addr(pn->sobject) != pn_addr(obj)) + continue; + + rval = sknode; + sock_hold(sknode); + break; + } + + spin_unlock_bh(&pnsocks.lock); + + return rval; + +} + +void pn_sock_hash(struct sock *sk) +{ + spin_lock_bh(&pnsocks.lock); + sk_add_node(sk, &pnsocks.hlist); + spin_unlock_bh(&pnsocks.lock); +} +EXPORT_SYMBOL(pn_sock_hash); + +void pn_sock_unhash(struct sock *sk) +{ + spin_lock_bh(&pnsocks.lock); + sk_del_node_init(sk); + spin_unlock_bh(&pnsocks.lock); +} +EXPORT_SYMBOL(pn_sock_unhash); + +static int pn_socket_bind(struct socket *sock, struct sockaddr *addr, int len) +{ + struct sock *sk = sock->sk; + struct pn_sock *pn = pn_sk(sk); + struct sockaddr_pn *spn = (struct sockaddr_pn *)addr; + int err; + u16 handle; + u8 saddr; + + if (sk->sk_prot->bind) + return sk->sk_prot->bind(sk, addr, len); + + if (len < sizeof(struct sockaddr_pn)) + return -EINVAL; + if (spn->spn_family != AF_PHONET) + return -EAFNOSUPPORT; + + handle = pn_sockaddr_get_object((struct sockaddr_pn *)addr); + saddr = pn_addr(handle); + if (saddr && phonet_address_lookup(saddr)) + return -EADDRNOTAVAIL; + + lock_sock(sk); + if (sk->sk_state != TCP_CLOSE || pn_port(pn->sobject)) { + err = -EINVAL; /* attempt to rebind */ + goto out; + } + err = sk->sk_prot->get_port(sk, pn_port(handle)); + if (err) + goto out; + + /* get_port() sets the port, bind() sets the address if applicable */ + pn->sobject = pn_object(saddr, pn_port(pn->sobject)); + pn->resource = spn->spn_resource; + + /* Enable RX on the socket */ + sk->sk_prot->hash(sk); +out: + release_sock(sk); + return err; +} + +static int pn_socket_autobind(struct socket *sock) +{ + struct sockaddr_pn sa; + int err; + + memset(&sa, 0, sizeof(sa)); + sa.spn_family = AF_PHONET; + err = pn_socket_bind(sock, (struct sockaddr *)&sa, + sizeof(struct sockaddr_pn)); + if (err != -EINVAL) + return err; + BUG_ON(!pn_port(pn_sk(sock->sk)->sobject)); + return 0; /* socket was already bound */ +} + +static int pn_socket_getname(struct socket *sock, struct sockaddr *addr, + int *sockaddr_len, int peer) +{ + struct sock *sk = sock->sk; + struct pn_sock *pn = pn_sk(sk); + + memset(addr, 0, sizeof(struct sockaddr_pn)); + addr->sa_family = AF_PHONET; + if (!peer) /* Race with bind() here is userland's problem. */ + pn_sockaddr_set_object((struct sockaddr_pn *)addr, + pn->sobject); + + *sockaddr_len = sizeof(struct sockaddr_pn); + return 0; +} + +static int pn_socket_ioctl(struct socket *sock, unsigned int cmd, + unsigned long arg) +{ + struct sock *sk = sock->sk; + struct pn_sock *pn = pn_sk(sk); + + if (cmd == SIOCPNGETOBJECT) { + struct net_device *dev; + u16 handle; + u8 saddr; + + if (get_user(handle, (__u16 __user *)arg)) + return -EFAULT; + + lock_sock(sk); + if (sk->sk_bound_dev_if) + dev = dev_get_by_index(sock_net(sk), + sk->sk_bound_dev_if); + else + dev = phonet_device_get(sock_net(sk)); + if (dev && (dev->flags & IFF_UP)) + saddr = phonet_address_get(dev, pn_addr(handle)); + else + saddr = PN_NO_ADDR; + release_sock(sk); + + if (dev) + dev_put(dev); + if (saddr == PN_NO_ADDR) + return -EHOSTUNREACH; + + handle = pn_object(saddr, pn_port(pn->sobject)); + return put_user(handle, (__u16 __user *)arg); + } + + return sk->sk_prot->ioctl(sk, cmd, arg); +} + +static int pn_socket_sendmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *m, size_t total_len) +{ + struct sock *sk = sock->sk; + + if (pn_socket_autobind(sock)) + return -EAGAIN; + + return sk->sk_prot->sendmsg(iocb, sk, m, total_len); +} + +const struct proto_ops phonet_dgram_ops = { + .family = AF_PHONET, + .owner = THIS_MODULE, + .release = pn_socket_release, + .bind = pn_socket_bind, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .getname = pn_socket_getname, + .poll = datagram_poll, + .ioctl = pn_socket_ioctl, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt, +#ifdef CONFIG_COMPAT + .compat_setsockopt = sock_no_setsockopt, + .compat_getsockopt = sock_no_getsockopt, +#endif + .sendmsg = pn_socket_sendmsg, + .recvmsg = sock_common_recvmsg, + .mmap = sock_no_mmap, + .sendpage = sock_no_sendpage, +}; + +static DEFINE_MUTEX(port_mutex); + +/* allocate port for a socket */ +int pn_sock_get_port(struct sock *sk, unsigned short sport) +{ + static int port_cur; + struct pn_sock *pn = pn_sk(sk); + struct sockaddr_pn try_sa; + struct sock *tmpsk; + + memset(&try_sa, 0, sizeof(struct sockaddr_pn)); + try_sa.spn_family = AF_PHONET; + + mutex_lock(&port_mutex); + + if (!sport) { + /* search free port */ + int port, pmin = 0x40, pmax = 0x7f; + + for (port = pmin; port <= pmax; port++) { + port_cur++; + if (port_cur < pmin || port_cur > pmax) + port_cur = pmin; + + pn_sockaddr_set_port(&try_sa, port_cur); + tmpsk = pn_find_sock_by_sa(&try_sa); + if (tmpsk == NULL) { + sport = port_cur; + goto found; + } else + sock_put(tmpsk); + } + } else { + /* try to find specific port */ + pn_sockaddr_set_port(&try_sa, sport); + tmpsk = pn_find_sock_by_sa(&try_sa); + if (tmpsk == NULL) + /* No sock there! We can use that port... */ + goto found; + else + sock_put(tmpsk); + } + mutex_unlock(&port_mutex); + + /* the port must be in use already */ + return -EADDRINUSE; + +found: + mutex_unlock(&port_mutex); + pn->sobject = pn_object(pn_addr(pn->sobject), sport); + return 0; +} +EXPORT_SYMBOL(pn_sock_get_port); -- cgit v1.2.3 From 107d0d9b8d9a236883db72841fb61cedd5be845e Mon Sep 17 00:00:00 2001 From: Remi Denis-Courmont Date: Mon, 22 Sep 2008 20:05:57 -0700 Subject: Phonet: Phonet datagram transport protocol This provides the basic SOCK_DGRAM transport protocol for Phonet. Signed-off-by: Remi Denis-Courmont Signed-off-by: David S. Miller --- include/net/phonet/phonet.h | 6 ++ net/phonet/Makefile | 1 + net/phonet/af_phonet.c | 106 ++++++++++++++++++++++++ net/phonet/datagram.c | 197 ++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 310 insertions(+) create mode 100644 net/phonet/datagram.c (limited to 'net/phonet') diff --git a/include/net/phonet/phonet.h b/include/net/phonet/phonet.h index 2ae5cbb59b60..d3957d3be0fd 100644 --- a/include/net/phonet/phonet.h +++ b/include/net/phonet/phonet.h @@ -51,6 +51,9 @@ void pn_sock_hash(struct sock *sk); void pn_sock_unhash(struct sock *sk); int pn_sock_get_port(struct sock *sk, unsigned short sport); +int pn_skb_send(struct sock *sk, struct sk_buff *skb, + const struct sockaddr_pn *target); + static inline struct phonethdr *pn_hdr(struct sk_buff *skb) { return (struct phonethdr *)skb_network_header(skb); @@ -95,4 +98,7 @@ int phonet_proto_register(int protocol, struct phonet_protocol *pp); void phonet_proto_unregister(int protocol, struct phonet_protocol *pp); void phonet_netlink_register(void); +int isi_register(void); +void isi_unregister(void); + #endif diff --git a/net/phonet/Makefile b/net/phonet/Makefile index c1d671de7835..d218abc3f06a 100644 --- a/net/phonet/Makefile +++ b/net/phonet/Makefile @@ -4,4 +4,5 @@ phonet-objs := \ pn_dev.o \ pn_netlink.o \ socket.o \ + datagram.o \ af_phonet.o diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c index ba54d53020ff..e6771d3961cf 100644 --- a/net/phonet/af_phonet.c +++ b/net/phonet/af_phonet.c @@ -99,6 +99,101 @@ static struct net_proto_family phonet_proto_family = { .owner = THIS_MODULE, }; +/* + * Prepends an ISI header and sends a datagram. + */ +static int pn_send(struct sk_buff *skb, struct net_device *dev, + u16 dst, u16 src, u8 res) +{ + struct phonethdr *ph; + int err; + + if (skb->len + 2 > 0xffff) { + /* Phonet length field would overflow */ + err = -EMSGSIZE; + goto drop; + } + + skb_reset_transport_header(skb); + WARN_ON(skb_headroom(skb) & 1); /* HW assumes word alignment */ + skb_push(skb, sizeof(struct phonethdr)); + skb_reset_network_header(skb); + ph = pn_hdr(skb); + ph->pn_rdev = pn_dev(dst); + ph->pn_sdev = pn_dev(src); + ph->pn_res = res; + ph->pn_length = __cpu_to_be16(skb->len + 2 - sizeof(*ph)); + ph->pn_robj = pn_obj(dst); + ph->pn_sobj = pn_obj(src); + + skb->protocol = htons(ETH_P_PHONET); + skb->priority = 0; + skb->dev = dev; + + if (pn_addr(src) == pn_addr(dst)) { + skb_reset_mac_header(skb); + skb->pkt_type = PACKET_LOOPBACK; + skb_orphan(skb); + netif_rx_ni(skb); + err = 0; + } else { + err = dev_hard_header(skb, dev, ntohs(skb->protocol), + NULL, NULL, skb->len); + if (err < 0) { + err = -EHOSTUNREACH; + goto drop; + } + err = dev_queue_xmit(skb); + } + + return err; +drop: + kfree_skb(skb); + return err; +} + +/* + * Create a Phonet header for the skb and send it out. Returns + * non-zero error code if failed. The skb is freed then. + */ +int pn_skb_send(struct sock *sk, struct sk_buff *skb, + const struct sockaddr_pn *target) +{ + struct net_device *dev; + struct pn_sock *pn = pn_sk(sk); + int err; + u16 src; + u8 daddr = pn_sockaddr_get_addr(target), saddr = PN_NO_ADDR; + + err = -EHOSTUNREACH; + if (sk->sk_bound_dev_if) + dev = dev_get_by_index(sock_net(sk), sk->sk_bound_dev_if); + else + dev = phonet_device_get(sock_net(sk)); + if (!dev || !(dev->flags & IFF_UP)) + goto drop; + + saddr = phonet_address_get(dev, daddr); + if (saddr == PN_NO_ADDR) + goto drop; + + src = pn->sobject; + if (!pn_addr(src)) + src = pn_object(saddr, pn_obj(src)); + + err = pn_send(skb, dev, pn_sockaddr_get_object(target), + src, pn_sockaddr_get_resource(target)); + dev_put(dev); + return err; + +drop: + kfree_skb(skb); + if (dev) + dev_put(dev); + return err; +} +EXPORT_SYMBOL(pn_skb_send); + /* packet type functions */ /* @@ -226,11 +321,22 @@ static int __init phonet_init(void) phonet_device_init(); dev_add_pack(&phonet_packet_type); phonet_netlink_register(); + + err = isi_register(); + if (err) + goto err; return 0; + +err: + sock_unregister(AF_PHONET); + dev_remove_pack(&phonet_packet_type); + phonet_device_exit(); + return err; } static void __exit phonet_exit(void) { + isi_unregister(); sock_unregister(AF_PHONET); dev_remove_pack(&phonet_packet_type); phonet_device_exit(); diff --git a/net/phonet/datagram.c b/net/phonet/datagram.c new file mode 100644 index 000000000000..e087862ed7e4 --- /dev/null +++ b/net/phonet/datagram.c @@ -0,0 +1,197 @@ +/* + * File: datagram.c + * + * Datagram (ISI) Phonet sockets + * + * Copyright (C) 2008 Nokia Corporation. + * + * Contact: Remi Denis-Courmont + * Original author: Sakari Ailus + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ + +#include +#include +#include +#include + +#include +#include + +static int pn_backlog_rcv(struct sock *sk, struct sk_buff *skb); + +/* associated socket ceases to exist */ +static void pn_sock_close(struct sock *sk, long timeout) +{ + sk_common_release(sk); +} + +static int pn_ioctl(struct sock *sk, int cmd, unsigned long arg) +{ + struct sk_buff *skb; + int answ; + + switch (cmd) { + case SIOCINQ: + lock_sock(sk); + skb = skb_peek(&sk->sk_receive_queue); + answ = skb ? skb->len : 0; + release_sock(sk); + return put_user(answ, (int __user *)arg); + } + + return -ENOIOCTLCMD; +} + +/* Destroy socket. All references are gone. */ +static void pn_destruct(struct sock *sk) +{ + skb_queue_purge(&sk->sk_receive_queue); +} + +static int pn_init(struct sock *sk) +{ + sk->sk_destruct = pn_destruct; + return 0; +} + +static int pn_sendmsg(struct kiocb *iocb, struct sock *sk, + struct msghdr *msg, size_t len) +{ + struct sockaddr_pn *target; + struct sk_buff *skb; + int err; + + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + if (msg->msg_name == NULL) + return -EDESTADDRREQ; + + if (msg->msg_namelen < sizeof(struct sockaddr_pn)) + return -EINVAL; + + target = (struct sockaddr_pn *)msg->msg_name; + if (target->spn_family != AF_PHONET) + return -EAFNOSUPPORT; + + skb = sock_alloc_send_skb(sk, MAX_PHONET_HEADER + len, + msg->msg_flags & MSG_DONTWAIT, &err); + if (skb == NULL) + return err; + skb_reserve(skb, MAX_PHONET_HEADER); + + err = memcpy_fromiovec((void *)skb_put(skb, len), msg->msg_iov, len); + if (err < 0) { + kfree_skb(skb); + return err; + } + + /* + * Fill in the Phonet header and + * finally pass the packet forwards. + */ + err = pn_skb_send(sk, skb, target); + + /* If ok, return len. */ + return (err >= 0) ? len : err; +} + +static int pn_recvmsg(struct kiocb *iocb, struct sock *sk, + struct msghdr *msg, size_t len, int noblock, + int flags, int *addr_len) +{ + struct sk_buff *skb = NULL; + struct sockaddr_pn sa; + int rval = -EOPNOTSUPP; + int copylen; + + if (flags & MSG_OOB) + goto out_nofree; + + if (addr_len) + *addr_len = sizeof(sa); + + skb = skb_recv_datagram(sk, flags, noblock, &rval); + if (skb == NULL) + goto out_nofree; + + pn_skb_get_src_sockaddr(skb, &sa); + + copylen = skb->len; + if (len < copylen) { + msg->msg_flags |= MSG_TRUNC; + copylen = len; + } + + rval = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copylen); + if (rval) { + rval = -EFAULT; + goto out; + } + + rval = (flags & MSG_TRUNC) ? skb->len : copylen; + + if (msg->msg_name != NULL) + memcpy(msg->msg_name, &sa, sizeof(struct sockaddr_pn)); + +out: + skb_free_datagram(sk, skb); + +out_nofree: + return rval; +} + +/* Queue an skb for a sock. */ +static int pn_backlog_rcv(struct sock *sk, struct sk_buff *skb) +{ + int err = sock_queue_rcv_skb(sk, skb); + if (err < 0) + kfree_skb(skb); + return err ? NET_RX_DROP : NET_RX_SUCCESS; +} + +/* Module registration */ +static struct proto pn_proto = { + .close = pn_sock_close, + .ioctl = pn_ioctl, + .init = pn_init, + .sendmsg = pn_sendmsg, + .recvmsg = pn_recvmsg, + .backlog_rcv = pn_backlog_rcv, + .hash = pn_sock_hash, + .unhash = pn_sock_unhash, + .get_port = pn_sock_get_port, + .obj_size = sizeof(struct pn_sock), + .owner = THIS_MODULE, + .name = "PHONET", +}; + +static struct phonet_protocol pn_dgram_proto = { + .ops = &phonet_dgram_ops, + .prot = &pn_proto, + .sock_type = SOCK_DGRAM, +}; + +int __init isi_register(void) +{ + return phonet_proto_register(PN_PROTO_PHONET, &pn_dgram_proto); +} + +void __exit isi_unregister(void) +{ + phonet_proto_unregister(PN_PROTO_PHONET, &pn_dgram_proto); +} -- cgit v1.2.3 From 5f77076d75d35c9f5619e1f9d7e7428a627f65e6 Mon Sep 17 00:00:00 2001 From: Remi Denis-Courmont Date: Mon, 22 Sep 2008 20:08:04 -0700 Subject: Phonet: provide MAC header operations MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Rémi Denis-Courmont Signed-off-by: David S. Miller --- include/linux/if_phonet.h | 4 ++++ net/phonet/af_phonet.c | 29 +++++++++++++++++++++++++++++ 2 files changed, 33 insertions(+) (limited to 'net/phonet') diff --git a/include/linux/if_phonet.h b/include/linux/if_phonet.h index 22df25fbc4e2..7e989216ec17 100644 --- a/include/linux/if_phonet.h +++ b/include/linux/if_phonet.h @@ -12,3 +12,7 @@ /* 6 bytes header + 65535 bytes payload */ #define PHONET_MAX_MTU 65541 #define PHONET_DEV_MTU PHONET_MAX_MTU + +#ifdef __KERNEL__ +extern struct header_ops phonet_header_ops; +#endif diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c index e6771d3961cf..51397ff308bd 100644 --- a/net/phonet/af_phonet.c +++ b/net/phonet/af_phonet.c @@ -99,6 +99,35 @@ static struct net_proto_family phonet_proto_family = { .owner = THIS_MODULE, }; +/* Phonet device header operations */ +static int pn_header_create(struct sk_buff *skb, struct net_device *dev, + unsigned short type, const void *daddr, + const void *saddr, unsigned len) +{ + u8 *media = skb_push(skb, 1); + + if (type != ETH_P_PHONET) + return -1; + + if (!saddr) + saddr = dev->dev_addr; + *media = *(const u8 *)saddr; + return 1; +} + +static int pn_header_parse(const struct sk_buff *skb, unsigned char *haddr) +{ + const u8 *media = skb_mac_header(skb); + *haddr = *media; + return 1; +} + +struct header_ops phonet_header_ops = { + .create = pn_header_create, + .parse = pn_header_parse, +}; +EXPORT_SYMBOL(phonet_header_ops); + /* * Prepends an ISI header and sends a datagram. */ -- cgit v1.2.3 From 87ab4e20b445c6d2d2727ab4f96fa17f7259511e Mon Sep 17 00:00:00 2001 From: Remi Denis-Courmont Date: Mon, 22 Sep 2008 20:08:39 -0700 Subject: Phonet: proc interface for port range Phonet endpoints are bound to individual ports. This provides a /proc/sys/net/phonet (or sysctl) interface for selecting the range of automatically allocated ports (much like the ip_local_port_range with IPv4). Signed-off-by: Remi Denis-Courmont Signed-off-by: David S. Miller --- include/net/phonet/phonet.h | 3 ++ net/phonet/Makefile | 1 + net/phonet/af_phonet.c | 3 ++ net/phonet/socket.c | 3 +- net/phonet/sysctl.c | 113 ++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 122 insertions(+), 1 deletion(-) create mode 100644 net/phonet/sysctl.c (limited to 'net/phonet') diff --git a/include/net/phonet/phonet.h b/include/net/phonet/phonet.h index d3957d3be0fd..1c6f7e7d5fea 100644 --- a/include/net/phonet/phonet.h +++ b/include/net/phonet/phonet.h @@ -47,6 +47,7 @@ static inline struct pn_sock *pn_sk(struct sock *sk) extern const struct proto_ops phonet_dgram_ops; struct sock *pn_find_sock_by_sa(const struct sockaddr_pn *sa); +void phonet_get_local_port_range(int *min, int *max); void pn_sock_hash(struct sock *sk); void pn_sock_unhash(struct sock *sk); int pn_sock_get_port(struct sock *sk, unsigned short sport); @@ -97,6 +98,8 @@ struct phonet_protocol { int phonet_proto_register(int protocol, struct phonet_protocol *pp); void phonet_proto_unregister(int protocol, struct phonet_protocol *pp); +int phonet_sysctl_init(void); +void phonet_sysctl_exit(void); void phonet_netlink_register(void); int isi_register(void); void isi_unregister(void); diff --git a/net/phonet/Makefile b/net/phonet/Makefile index d218abc3f06a..ae9c3ed5be83 100644 --- a/net/phonet/Makefile +++ b/net/phonet/Makefile @@ -5,4 +5,5 @@ phonet-objs := \ pn_netlink.o \ socket.o \ datagram.o \ + sysctl.o \ af_phonet.o diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c index 51397ff308bd..50dc258d5aa2 100644 --- a/net/phonet/af_phonet.c +++ b/net/phonet/af_phonet.c @@ -350,6 +350,7 @@ static int __init phonet_init(void) phonet_device_init(); dev_add_pack(&phonet_packet_type); phonet_netlink_register(); + phonet_sysctl_init(); err = isi_register(); if (err) @@ -357,6 +358,7 @@ static int __init phonet_init(void) return 0; err: + phonet_sysctl_exit(); sock_unregister(AF_PHONET); dev_remove_pack(&phonet_packet_type); phonet_device_exit(); @@ -366,6 +368,7 @@ err: static void __exit phonet_exit(void) { isi_unregister(); + phonet_sysctl_exit(); sock_unregister(AF_PHONET); dev_remove_pack(&phonet_packet_type); phonet_device_exit(); diff --git a/net/phonet/socket.c b/net/phonet/socket.c index 99a4945d565d..dfd4061646db 100644 --- a/net/phonet/socket.c +++ b/net/phonet/socket.c @@ -273,8 +273,9 @@ int pn_sock_get_port(struct sock *sk, unsigned short sport) if (!sport) { /* search free port */ - int port, pmin = 0x40, pmax = 0x7f; + int port, pmin, pmax; + phonet_get_local_port_range(&pmin, &pmax); for (port = pmin; port <= pmax; port++) { port_cur++; if (port_cur < pmin || port_cur > pmax) diff --git a/net/phonet/sysctl.c b/net/phonet/sysctl.c new file mode 100644 index 000000000000..600a4309b8c8 --- /dev/null +++ b/net/phonet/sysctl.c @@ -0,0 +1,113 @@ +/* + * File: sysctl.c + * + * Phonet /proc/sys/net/phonet interface implementation + * + * Copyright (C) 2008 Nokia Corporation. + * + * Contact: Remi Denis-Courmont + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ + +#include +#include +#include +#include + +#define DYNAMIC_PORT_MIN 0x40 +#define DYNAMIC_PORT_MAX 0x7f + +static DEFINE_SEQLOCK(local_port_range_lock); +static int local_port_range_min[2] = {0, 0}; +static int local_port_range_max[2] = {1023, 1023}; +static int local_port_range[2] = {DYNAMIC_PORT_MIN, DYNAMIC_PORT_MAX}; +static struct ctl_table_header *phonet_table_hrd; + +static void set_local_port_range(int range[2]) +{ + write_seqlock(&local_port_range_lock); + local_port_range[0] = range[0]; + local_port_range[1] = range[1]; + write_sequnlock(&local_port_range_lock); +} + +void phonet_get_local_port_range(int *min, int *max) +{ + unsigned seq; + do { + seq = read_seqbegin(&local_port_range_lock); + if (min) + *min = local_port_range[0]; + if (max) + *max = local_port_range[1]; + } while (read_seqretry(&local_port_range_lock, seq)); +} + +static int proc_local_port_range(ctl_table *table, int write, struct file *filp, + void __user *buffer, + size_t *lenp, loff_t *ppos) +{ + int ret; + int range[2] = {local_port_range[0], local_port_range[1]}; + ctl_table tmp = { + .data = &range, + .maxlen = sizeof(range), + .mode = table->mode, + .extra1 = &local_port_range_min, + .extra2 = &local_port_range_max, + }; + + ret = proc_dointvec_minmax(&tmp, write, filp, buffer, lenp, ppos); + + if (write && ret == 0) { + if (range[1] < range[0]) + ret = -EINVAL; + else + set_local_port_range(range); + } + + return ret; +} + +static struct ctl_table phonet_table[] = { + { + .ctl_name = CTL_UNNUMBERED, + .procname = "local_port_range", + .data = &local_port_range, + .maxlen = sizeof(local_port_range), + .mode = 0644, + .proc_handler = &proc_local_port_range, + .strategy = NULL, + }, + { .ctl_name = 0 } +}; + +struct ctl_path phonet_ctl_path[] = { + { .procname = "net", .ctl_name = CTL_NET, }, + { .procname = "phonet", .ctl_name = CTL_UNNUMBERED, }, + { }, +}; + +int __init phonet_sysctl_init(void) +{ + phonet_table_hrd = register_sysctl_paths(phonet_ctl_path, phonet_table); + return phonet_table_hrd == NULL ? -ENOMEM : 0; +} + +void phonet_sysctl_exit(void) +{ + unregister_sysctl_table(phonet_table_hrd); +} -- cgit v1.2.3 From be0c52bfed7f7828494fa00060efd5d758e92580 Mon Sep 17 00:00:00 2001 From: Remi Denis-Courmont Date: Mon, 22 Sep 2008 20:09:13 -0700 Subject: Phonet: emit errors when a packet cannot be delivered locally When there is no listener socket for a received packet, send an error back to the sender. Signed-off-by: Remi Denis-Courmont Signed-off-by: David S. Miller --- include/linux/phonet.h | 32 +++++++++++++++ include/net/phonet/phonet.h | 5 +++ net/phonet/af_phonet.c | 96 +++++++++++++++++++++++++++++++++++++++++++-- 3 files changed, 129 insertions(+), 4 deletions(-) (limited to 'net/phonet') diff --git a/include/linux/phonet.h b/include/linux/phonet.h index 001c0e679099..3a027f588a4a 100644 --- a/include/linux/phonet.h +++ b/include/linux/phonet.h @@ -45,6 +45,38 @@ struct phonethdr { __u8 pn_sobj; } __attribute__((packed)); +/* Common Phonet payload header */ +struct phonetmsg { + __u8 pn_trans_id; /* transaction ID */ + __u8 pn_msg_id; /* message type */ + union { + struct { + __u8 pn_submsg_id; /* message subtype */ + __u8 pn_data[5]; + } base; + struct { + __u16 pn_e_res_id; /* extended resource ID */ + __u8 pn_e_submsg_id; /* message subtype */ + __u8 pn_e_data[3]; + } ext; + } pn_msg_u; +}; +#define PN_COMMON_MESSAGE 0xF0 +#define PN_PREFIX 0xE0 /* resource for extended messages */ +#define pn_submsg_id pn_msg_u.base.pn_submsg_id +#define pn_e_submsg_id pn_msg_u.ext.pn_e_submsg_id +#define pn_e_res_id pn_msg_u.ext.pn_e_res_id +#define pn_data pn_msg_u.base.pn_data +#define pn_e_data pn_msg_u.ext.pn_e_data + +/* data for unreachable errors */ +#define PN_COMM_SERVICE_NOT_IDENTIFIED_RESP 0x01 +#define PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP 0x14 +#define pn_orig_msg_id pn_data[0] +#define pn_status pn_data[1] +#define pn_e_orig_msg_id pn_e_data[0] +#define pn_e_status pn_e_data[1] + /* Phonet socket address structure */ struct sockaddr_pn { sa_family_t spn_family; diff --git a/include/net/phonet/phonet.h b/include/net/phonet/phonet.h index 1c6f7e7d5fea..d4e72508e145 100644 --- a/include/net/phonet/phonet.h +++ b/include/net/phonet/phonet.h @@ -60,6 +60,11 @@ static inline struct phonethdr *pn_hdr(struct sk_buff *skb) return (struct phonethdr *)skb_network_header(skb); } +static inline struct phonetmsg *pn_msg(struct sk_buff *skb) +{ + return (struct phonetmsg *)skb_transport_header(skb); +} + /* * Get the other party's sockaddr from received skb. The skb begins * with a Phonet header. diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c index 50dc258d5aa2..1d8df6b7e3df 100644 --- a/net/phonet/af_phonet.c +++ b/net/phonet/af_phonet.c @@ -132,7 +132,7 @@ EXPORT_SYMBOL(phonet_header_ops); * Prepends an ISI header and sends a datagram. */ static int pn_send(struct sk_buff *skb, struct net_device *dev, - u16 dst, u16 src, u8 res) + u16 dst, u16 src, u8 res, u8 irq) { struct phonethdr *ph; int err; @@ -163,7 +163,10 @@ static int pn_send(struct sk_buff *skb, struct net_device *dev, skb_reset_mac_header(skb); skb->pkt_type = PACKET_LOOPBACK; skb_orphan(skb); - netif_rx_ni(skb); + if (irq) + netif_rx(skb); + else + netif_rx_ni(skb); err = 0; } else { err = dev_hard_header(skb, dev, ntohs(skb->protocol), @@ -181,6 +184,19 @@ drop: return err; } +static int pn_raw_send(const void *data, int len, struct net_device *dev, + u16 dst, u16 src, u8 res) +{ + struct sk_buff *skb = alloc_skb(MAX_PHONET_HEADER + len, GFP_ATOMIC); + if (skb == NULL) + return -ENOMEM; + + skb_reserve(skb, MAX_PHONET_HEADER); + __skb_put(skb, len); + skb_copy_to_linear_data(skb, data, len); + return pn_send(skb, dev, dst, src, res, 1); +} + /* * Create a Phonet header for the skb and send it out. Returns * non-zero error code if failed. The skb is freed then. @@ -211,7 +227,7 @@ int pn_skb_send(struct sock *sk, struct sk_buff *skb, src = pn_object(saddr, pn_obj(src)); err = pn_send(skb, dev, pn_sockaddr_get_object(target), - src, pn_sockaddr_get_resource(target)); + src, pn_sockaddr_get_resource(target), 0); dev_put(dev); return err; @@ -223,6 +239,73 @@ drop: } EXPORT_SYMBOL(pn_skb_send); +/* Do not send an error message in response to an error message */ +static inline int can_respond(struct sk_buff *skb) +{ + const struct phonethdr *ph; + const struct phonetmsg *pm; + u8 submsg_id; + + if (!pskb_may_pull(skb, 3)) + return 0; + + ph = pn_hdr(skb); + if (phonet_address_get(skb->dev, ph->pn_rdev) != ph->pn_rdev) + return 0; /* we are not the destination */ + if (ph->pn_res == PN_PREFIX && !pskb_may_pull(skb, 5)) + return 0; + + ph = pn_hdr(skb); /* re-acquires the pointer */ + pm = pn_msg(skb); + if (pm->pn_msg_id != PN_COMMON_MESSAGE) + return 1; + submsg_id = (ph->pn_res == PN_PREFIX) + ? pm->pn_e_submsg_id : pm->pn_submsg_id; + if (submsg_id != PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP && + pm->pn_e_submsg_id != PN_COMM_SERVICE_NOT_IDENTIFIED_RESP) + return 1; + return 0; +} + +static int send_obj_unreachable(struct sk_buff *rskb) +{ + const struct phonethdr *oph = pn_hdr(rskb); + const struct phonetmsg *opm = pn_msg(rskb); + struct phonetmsg resp; + + memset(&resp, 0, sizeof(resp)); + resp.pn_trans_id = opm->pn_trans_id; + resp.pn_msg_id = PN_COMMON_MESSAGE; + if (oph->pn_res == PN_PREFIX) { + resp.pn_e_res_id = opm->pn_e_res_id; + resp.pn_e_submsg_id = PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP; + resp.pn_e_orig_msg_id = opm->pn_msg_id; + resp.pn_e_status = 0; + } else { + resp.pn_submsg_id = PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP; + resp.pn_orig_msg_id = opm->pn_msg_id; + resp.pn_status = 0; + } + return pn_raw_send(&resp, sizeof(resp), rskb->dev, + pn_object(oph->pn_sdev, oph->pn_sobj), + pn_object(oph->pn_rdev, oph->pn_robj), + oph->pn_res); +} + +static int send_reset_indications(struct sk_buff *rskb) +{ + struct phonethdr *oph = pn_hdr(rskb); + static const u8 data[4] = { + 0x00 /* trans ID */, 0x10 /* subscribe msg */, + 0x00 /* subscription count */, 0x00 /* dummy */ + }; + + return pn_raw_send(data, sizeof(data), rskb->dev, + pn_object(oph->pn_sdev, 0x00), + pn_object(oph->pn_rdev, oph->pn_robj), 0x10); +} + + /* packet type functions */ /* @@ -260,8 +343,13 @@ static int phonet_rcv(struct sk_buff *skb, struct net_device *dev, goto out; /* currently, we cannot be device 0 */ sk = pn_find_sock_by_sa(&sa); - if (sk == NULL) + if (sk == NULL) { + if (can_respond(skb)) { + send_obj_unreachable(skb); + send_reset_indications(skb); + } goto out; + } /* Push data to the socket (or other sockets connected to it). */ return sk_receive_skb(sk, skb, 0); -- cgit v1.2.3 From 8980713b97393b21a50d11408a22d2caa87d016a Mon Sep 17 00:00:00 2001 From: Rémi Denis-Courmont Date: Tue, 30 Sep 2008 02:51:18 -0700 Subject: Phonet: Netlink factorization and cleanup MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Rémi Denis-Courmont Signed-off-by: David S. Miller --- net/phonet/pn_netlink.c | 91 +++++++++++++++++++------------------------------ 1 file changed, 35 insertions(+), 56 deletions(-) (limited to 'net/phonet') diff --git a/net/phonet/pn_netlink.c b/net/phonet/pn_netlink.c index b1ea19a230dd..b1770d66bc8d 100644 --- a/net/phonet/pn_netlink.c +++ b/net/phonet/pn_netlink.c @@ -54,11 +54,16 @@ errout: rtnl_set_sk_err(dev_net(dev), RTNLGRP_PHONET_IFADDR, err); } -static int newaddr_doit(struct sk_buff *skb, struct nlmsghdr *nlm, void *attr) +static const struct nla_policy ifa_phonet_policy[IFA_MAX+1] = { + [IFA_LOCAL] = { .type = NLA_U8 }, +}; + +static int addr_doit(struct sk_buff *skb, struct nlmsghdr *nlh, void *attr) { - struct rtattr **rta = attr; - struct ifaddrmsg *ifm = NLMSG_DATA(nlm); + struct net *net = sock_net(skb->sk); + struct nlattr *tb[IFA_MAX+1]; struct net_device *dev; + struct ifaddrmsg *ifm; int err; u8 pnaddr; @@ -67,52 +72,28 @@ static int newaddr_doit(struct sk_buff *skb, struct nlmsghdr *nlm, void *attr) ASSERT_RTNL(); - if (rta[IFA_LOCAL - 1] == NULL) - return -EINVAL; - - dev = __dev_get_by_index(&init_net, ifm->ifa_index); - if (dev == NULL) - return -ENODEV; + err = nlmsg_parse(nlh, sizeof(*ifm), tb, IFA_MAX, ifa_phonet_policy); + if (err < 0) + return err; - if (ifm->ifa_prefixlen > 0) + ifm = nlmsg_data(nlh); + if (tb[IFA_LOCAL] == NULL) return -EINVAL; - - memcpy(&pnaddr, RTA_DATA(rta[IFA_LOCAL - 1]), 1); - - err = phonet_address_add(dev, pnaddr); - if (!err) - rtmsg_notify(RTM_NEWADDR, dev, pnaddr); - return err; -} - -static int deladdr_doit(struct sk_buff *skb, struct nlmsghdr *nlm, void *attr) -{ - struct rtattr **rta = attr; - struct ifaddrmsg *ifm = NLMSG_DATA(nlm); - struct net_device *dev; - int err; - u8 pnaddr; - - if (!capable(CAP_SYS_ADMIN)) - return -EPERM; - - ASSERT_RTNL(); - - if (rta[IFA_LOCAL - 1] == NULL) + pnaddr = nla_get_u8(tb[IFA_LOCAL]); + if (pnaddr & 3) + /* Phonet addresses only have 6 high-order bits */ return -EINVAL; - dev = __dev_get_by_index(&init_net, ifm->ifa_index); + dev = __dev_get_by_index(net, ifm->ifa_index); if (dev == NULL) return -ENODEV; - if (ifm->ifa_prefixlen > 0) - return -EADDRNOTAVAIL; - - memcpy(&pnaddr, RTA_DATA(rta[IFA_LOCAL - 1]), 1); - - err = phonet_address_del(dev, pnaddr); + if (nlh->nlmsg_type == RTM_NEWADDR) + err = phonet_address_add(dev, pnaddr); + else + err = phonet_address_del(dev, pnaddr); if (!err) - rtmsg_notify(RTM_DELADDR, dev, pnaddr); + rtmsg_notify(nlh->nlmsg_type, dev, pnaddr); return err; } @@ -121,25 +102,23 @@ static int fill_addr(struct sk_buff *skb, struct net_device *dev, u8 addr, { struct ifaddrmsg *ifm; struct nlmsghdr *nlh; - unsigned int orig_len = skb->len; - nlh = NLMSG_PUT(skb, pid, seq, event, sizeof(struct ifaddrmsg)); - ifm = NLMSG_DATA(nlh); + nlh = nlmsg_put(skb, pid, seq, event, sizeof(*ifm), 0); + if (nlh == NULL) + return -EMSGSIZE; + + ifm = nlmsg_data(nlh); ifm->ifa_family = AF_PHONET; ifm->ifa_prefixlen = 0; ifm->ifa_flags = IFA_F_PERMANENT; - ifm->ifa_scope = RT_SCOPE_HOST; + ifm->ifa_scope = RT_SCOPE_LINK; ifm->ifa_index = dev->ifindex; - RTA_PUT(skb, IFA_LOCAL, 1, &addr); - nlh->nlmsg_len = skb->len - orig_len; - - return 0; - -nlmsg_failure: -rtattr_failure: - skb_trim(skb, orig_len); + NLA_PUT_U8(skb, IFA_LOCAL, addr); + return nlmsg_end(skb, nlh); - return -1; +nla_put_failure: + nlmsg_cancel(skb, nlh); + return -EMSGSIZE; } static int getaddr_dumpit(struct sk_buff *skb, struct netlink_callback *cb) @@ -180,7 +159,7 @@ out: void __init phonet_netlink_register(void) { - rtnl_register(PF_PHONET, RTM_NEWADDR, newaddr_doit, NULL); - rtnl_register(PF_PHONET, RTM_DELADDR, deladdr_doit, NULL); + rtnl_register(PF_PHONET, RTM_NEWADDR, addr_doit, NULL); + rtnl_register(PF_PHONET, RTM_DELADDR, addr_doit, NULL); rtnl_register(PF_PHONET, RTM_GETADDR, NULL, getaddr_dumpit); } -- cgit v1.2.3 From 25532824fb727744a302edb25c6a6ac10b82cb63 Mon Sep 17 00:00:00 2001 From: Rémi Denis-Courmont Date: Sun, 5 Oct 2008 11:14:27 -0700 Subject: Phonet: modules auto-loading support MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Rémi Denis-Courmont Signed-off-by: David S. Miller --- net/phonet/af_phonet.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) (limited to 'net/phonet') diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c index 1d8df6b7e3df..0a74aeaf5adf 100644 --- a/net/phonet/af_phonet.c +++ b/net/phonet/af_phonet.c @@ -64,6 +64,11 @@ static int pn_socket_create(struct net *net, struct socket *sock, int protocol) } pnp = phonet_proto_get(protocol); +#ifdef CONFIG_KMOD + if (pnp == NULL && + request_module("net-pf-%d-proto-%d", PF_PHONET, protocol) == 0) + pnp = phonet_proto_get(protocol); +#endif if (pnp == NULL) return -EPROTONOSUPPORT; if (sock->type != pnp->sock_type) { @@ -94,7 +99,7 @@ out: } static struct net_proto_family phonet_proto_family = { - .family = AF_PHONET, + .family = PF_PHONET, .create = pn_socket_create, .owner = THIS_MODULE, }; @@ -447,7 +452,7 @@ static int __init phonet_init(void) err: phonet_sysctl_exit(); - sock_unregister(AF_PHONET); + sock_unregister(PF_PHONET); dev_remove_pack(&phonet_packet_type); phonet_device_exit(); return err; @@ -457,7 +462,7 @@ static void __exit phonet_exit(void) { isi_unregister(); phonet_sysctl_exit(); - sock_unregister(AF_PHONET); + sock_unregister(PF_PHONET); dev_remove_pack(&phonet_packet_type); phonet_device_exit(); } @@ -466,3 +471,4 @@ module_init(phonet_init); module_exit(phonet_exit); MODULE_DESCRIPTION("Phonet protocol stack for Linux"); MODULE_LICENSE("GPL"); +MODULE_ALIAS_NETPROTO(PF_PHONET); -- cgit v1.2.3 From 9995a32b4d14dcda2f8df58030526bee91114c16 Mon Sep 17 00:00:00 2001 From: Rémi Denis-Courmont Date: Sun, 5 Oct 2008 11:14:48 -0700 Subject: Phonet: connected sockets glue MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Rémi Denis-Courmont Acked-by: Arnaldo Carvalho de Melo Signed-off-by: David S. Miller --- include/net/phonet/pep.h | 43 +++++++++++++++++++++ net/phonet/socket.c | 97 ++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 140 insertions(+) create mode 100644 include/net/phonet/pep.h (limited to 'net/phonet') diff --git a/include/net/phonet/pep.h b/include/net/phonet/pep.h new file mode 100644 index 000000000000..b2f8c54c5333 --- /dev/null +++ b/include/net/phonet/pep.h @@ -0,0 +1,43 @@ +/* + * File: pep.h + * + * Phonet Pipe End Point sockets definitions + * + * Copyright (C) 2008 Nokia Corporation. + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ + +#ifndef NET_PHONET_PEP_H +#define NET_PHONET_PEP_H + +struct pep_sock { + struct pn_sock pn_sk; + + /* Listening socket stuff: */ + struct hlist_head ackq; + + /* Connected socket stuff: */ + u8 tx_credits; +}; + +static inline struct pep_sock *pep_sk(struct sock *sk) +{ + return (struct pep_sock *)sk; +} + +extern const struct proto_ops phonet_stream_ops; + +#endif diff --git a/net/phonet/socket.c b/net/phonet/socket.c index dfd4061646db..cea1136cbe46 100644 --- a/net/phonet/socket.c +++ b/net/phonet/socket.c @@ -25,11 +25,13 @@ #include #include +#include #include #include #include #include +#include #include static int pn_socket_release(struct socket *sock) @@ -166,6 +168,24 @@ static int pn_socket_autobind(struct socket *sock) return 0; /* socket was already bound */ } +static int pn_socket_accept(struct socket *sock, struct socket *newsock, + int flags) +{ + struct sock *sk = sock->sk; + struct sock *newsk; + int err; + + newsk = sk->sk_prot->accept(sk, flags, &err); + if (!newsk) + return err; + + lock_sock(newsk); + sock_graft(newsk, newsock); + newsock->state = SS_CONNECTED; + release_sock(newsk); + return 0; +} + static int pn_socket_getname(struct socket *sock, struct sockaddr *addr, int *sockaddr_len, int peer) { @@ -182,6 +202,33 @@ static int pn_socket_getname(struct socket *sock, struct sockaddr *addr, return 0; } +static unsigned int pn_socket_poll(struct file *file, struct socket *sock, + poll_table *wait) +{ + struct sock *sk = sock->sk; + struct pep_sock *pn = pep_sk(sk); + unsigned int mask = 0; + + poll_wait(file, &sock->wait, wait); + + switch (sk->sk_state) { + case TCP_LISTEN: + return hlist_empty(&pn->ackq) ? 0 : POLLIN; + case TCP_CLOSE: + return POLLERR; + } + + if (!skb_queue_empty(&sk->sk_receive_queue)) + mask |= POLLIN | POLLRDNORM; + else if (sk->sk_state == TCP_CLOSE_WAIT) + return POLLHUP; + + if (sk->sk_state == TCP_ESTABLISHED && pn->tx_credits) + mask |= POLLOUT | POLLWRNORM | POLLWRBAND; + + return mask; +} + static int pn_socket_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) { @@ -220,6 +267,30 @@ static int pn_socket_ioctl(struct socket *sock, unsigned int cmd, return sk->sk_prot->ioctl(sk, cmd, arg); } +static int pn_socket_listen(struct socket *sock, int backlog) +{ + struct sock *sk = sock->sk; + int err = 0; + + if (sock->state != SS_UNCONNECTED) + return -EINVAL; + if (pn_socket_autobind(sock)) + return -ENOBUFS; + + lock_sock(sk); + if (sk->sk_state != TCP_CLOSE) { + err = -EINVAL; + goto out; + } + + sk->sk_state = TCP_LISTEN; + sk->sk_ack_backlog = 0; + sk->sk_max_ack_backlog = backlog; +out: + release_sock(sk); + return err; +} + static int pn_socket_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *m, size_t total_len) { @@ -256,6 +327,32 @@ const struct proto_ops phonet_dgram_ops = { .sendpage = sock_no_sendpage, }; +const struct proto_ops phonet_stream_ops = { + .family = AF_PHONET, + .owner = THIS_MODULE, + .release = pn_socket_release, + .bind = pn_socket_bind, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = pn_socket_accept, + .getname = pn_socket_getname, + .poll = pn_socket_poll, + .ioctl = pn_socket_ioctl, + .listen = pn_socket_listen, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt, +#ifdef CONFIG_COMPAT + .compat_setsockopt = sock_no_setsockopt, + .compat_getsockopt = compat_sock_no_getsockopt, +#endif + .sendmsg = pn_socket_sendmsg, + .recvmsg = sock_common_recvmsg, + .mmap = sock_no_mmap, + .sendpage = sock_no_sendpage, +}; +EXPORT_SYMBOL(phonet_stream_ops); + static DEFINE_MUTEX(port_mutex); /* allocate port for a socket */ -- cgit v1.2.3 From 9641458d3ec42def729fde64669abf07f3220cd5 Mon Sep 17 00:00:00 2001 From: Rémi Denis-Courmont Date: Sun, 5 Oct 2008 11:15:13 -0700 Subject: Phonet: Pipe End Point for Phonet Pipes protocol MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This protocol provides some connection handling and negotiated congestion control. Nokia cellular modems use it for bulk transfers. It provides packet boundaries (hence SOCK_SEQPACKET). Congestion control is per packet rather per byte, so we do not re-use the generic socket memory accounting. Signed-off-by: Rémi Denis-Courmont Signed-off-by: David S. Miller --- include/linux/phonet.h | 4 +- include/net/phonet/pep.h | 114 ++++++ net/phonet/Makefile | 4 +- net/phonet/af_phonet.c | 3 + net/phonet/pep.c | 908 +++++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 1031 insertions(+), 2 deletions(-) create mode 100644 net/phonet/pep.c (limited to 'net/phonet') diff --git a/include/linux/phonet.h b/include/linux/phonet.h index 3a027f588a4a..f92185242078 100644 --- a/include/linux/phonet.h +++ b/include/linux/phonet.h @@ -27,7 +27,9 @@ #define PN_PROTO_TRANSPORT 0 /* Phonet datagram socket */ #define PN_PROTO_PHONET 1 -#define PHONET_NPROTO 2 +/* Phonet pipe */ +#define PN_PROTO_PIPE 2 +#define PHONET_NPROTO 3 #define PNADDR_ANY 0 #define PNPORT_RESOURCE_ROUTING 0 diff --git a/include/net/phonet/pep.h b/include/net/phonet/pep.h index b2f8c54c5333..fb024e186860 100644 --- a/include/net/phonet/pep.h +++ b/include/net/phonet/pep.h @@ -26,11 +26,21 @@ struct pep_sock { struct pn_sock pn_sk; + /* XXX: union-ify listening vs connected stuff ? */ /* Listening socket stuff: */ struct hlist_head ackq; + struct hlist_head hlist; /* Connected socket stuff: */ + struct sock *listener; + u16 peer_type; /* peer type/subtype */ + u8 pipe_handle; + + u8 rx_credits; u8 tx_credits; + u8 rx_fc; /* RX flow control */ + u8 tx_fc; /* TX flow control */ + u8 init_enable; /* auto-enable at creation */ }; static inline struct pep_sock *pep_sk(struct sock *sk) @@ -40,4 +50,108 @@ static inline struct pep_sock *pep_sk(struct sock *sk) extern const struct proto_ops phonet_stream_ops; +/* Pipe protocol definitions */ +struct pnpipehdr { + u8 utid; /* transaction ID */ + u8 message_id; + u8 pipe_handle; + union { + u8 state_after_connect; /* connect request */ + u8 state_after_reset; /* reset request */ + u8 error_code; /* any response */ + u8 pep_type; /* status indication */ + u8 data[1]; + }; +}; +#define other_pep_type data[1] + +static inline struct pnpipehdr *pnp_hdr(struct sk_buff *skb) +{ + return (struct pnpipehdr *)skb_transport_header(skb); +} + +#define MAX_PNPIPE_HEADER (MAX_PHONET_HEADER + 4) + +enum { + PNS_PIPE_DATA = 0x20, + + PNS_PEP_CONNECT_REQ = 0x40, + PNS_PEP_CONNECT_RESP, + PNS_PEP_DISCONNECT_REQ, + PNS_PEP_DISCONNECT_RESP, + PNS_PEP_RESET_REQ, + PNS_PEP_RESET_RESP, + PNS_PEP_ENABLE_REQ, + PNS_PEP_ENABLE_RESP, + PNS_PEP_CTRL_REQ, + PNS_PEP_CTRL_RESP, + PNS_PEP_DISABLE_REQ = 0x4C, + PNS_PEP_DISABLE_RESP, + + PNS_PEP_STATUS_IND = 0x60, + PNS_PIPE_CREATED_IND, + PNS_PIPE_RESET_IND = 0x63, + PNS_PIPE_ENABLED_IND, + PNS_PIPE_REDIRECTED_IND, + PNS_PIPE_DISABLED_IND = 0x66, +}; + +#define PN_PIPE_INVALID_HANDLE 0xff +#define PN_PEP_TYPE_COMMON 0x00 + +/* Phonet pipe status indication */ +enum { + PN_PEP_IND_FLOW_CONTROL, + PN_PEP_IND_ID_MCFC_GRANT_CREDITS, +}; + +/* Phonet pipe error codes */ +enum { + PN_PIPE_NO_ERROR, + PN_PIPE_ERR_INVALID_PARAM, + PN_PIPE_ERR_INVALID_HANDLE, + PN_PIPE_ERR_INVALID_CTRL_ID, + PN_PIPE_ERR_NOT_ALLOWED, + PN_PIPE_ERR_PEP_IN_USE, + PN_PIPE_ERR_OVERLOAD, + PN_PIPE_ERR_DEV_DISCONNECTED, + PN_PIPE_ERR_TIMEOUT, + PN_PIPE_ERR_ALL_PIPES_IN_USE, + PN_PIPE_ERR_GENERAL, + PN_PIPE_ERR_NOT_SUPPORTED, +}; + +/* Phonet pipe states */ +enum { + PN_PIPE_DISABLE, + PN_PIPE_ENABLE, +}; + +/* Phonet pipe sub-block types */ +enum { + PN_PIPE_SB_CREATE_REQ_PEP_SUB_TYPE, + PN_PIPE_SB_CONNECT_REQ_PEP_SUB_TYPE, + PN_PIPE_SB_REDIRECT_REQ_PEP_SUB_TYPE, + PN_PIPE_SB_NEGOTIATED_FC, + PN_PIPE_SB_REQUIRED_FC_TX, + PN_PIPE_SB_PREFERRED_FC_RX, +}; + +/* Phonet pipe flow control models */ +enum { + PN_NO_FLOW_CONTROL, + PN_LEGACY_FLOW_CONTROL, + PN_ONE_CREDIT_FLOW_CONTROL, + PN_MULTI_CREDIT_FLOW_CONTROL, +}; + +#define pn_flow_safe(fc) ((fc) >> 1) + +/* Phonet pipe flow control states */ +enum { + PEP_IND_EMPTY, + PEP_IND_BUSY, + PEP_IND_READY, +}; + #endif diff --git a/net/phonet/Makefile b/net/phonet/Makefile index ae9c3ed5be83..505df2a04a85 100644 --- a/net/phonet/Makefile +++ b/net/phonet/Makefile @@ -1,4 +1,4 @@ -obj-$(CONFIG_PHONET) += phonet.o +obj-$(CONFIG_PHONET) += phonet.o pn_pep.o phonet-objs := \ pn_dev.o \ @@ -7,3 +7,5 @@ phonet-objs := \ datagram.o \ sysctl.o \ af_phonet.o + +pn_pep-objs := pep.o diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c index 0a74aeaf5adf..9e9c6fce11aa 100644 --- a/net/phonet/af_phonet.c +++ b/net/phonet/af_phonet.c @@ -58,6 +58,9 @@ static int pn_socket_create(struct net *net, struct socket *sock, int protocol) case SOCK_DGRAM: protocol = PN_PROTO_PHONET; break; + case SOCK_SEQPACKET: + protocol = PN_PROTO_PIPE; + break; default: return -EPROTONOSUPPORT; } diff --git a/net/phonet/pep.c b/net/phonet/pep.c new file mode 100644 index 000000000000..c5dfecb207d2 --- /dev/null +++ b/net/phonet/pep.c @@ -0,0 +1,908 @@ +/* + * File: pep.c + * + * Phonet pipe protocol end point socket + * + * Copyright (C) 2008 Nokia Corporation. + * + * Author: Rémi Denis-Courmont + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ + +#include +#include +#include +#include +#include + +#include +#include +#include + +/* sk_state values: + * TCP_CLOSE sock not in use yet + * TCP_CLOSE_WAIT disconnected pipe + * TCP_LISTEN listening pipe endpoint + * TCP_SYN_RECV connected pipe in disabled state + * TCP_ESTABLISHED connected pipe in enabled state + * + * pep_sock locking: + * - sk_state, ackq, hlist: sock lock needed + * - listener: read only + * - pipe_handle: read only + */ + +#define CREDITS_MAX 10 +#define CREDITS_THR 7 + +static const struct sockaddr_pn pipe_srv = { + .spn_family = AF_PHONET, + .spn_resource = 0xD9, /* pipe service */ +}; + +#define pep_sb_size(s) (((s) + 5) & ~3) /* 2-bytes head, 32-bits aligned */ + +/* Get the next TLV sub-block. */ +static unsigned char *pep_get_sb(struct sk_buff *skb, u8 *ptype, u8 *plen, + void *buf) +{ + void *data = NULL; + struct { + u8 sb_type; + u8 sb_len; + } *ph, h; + int buflen = *plen; + + ph = skb_header_pointer(skb, 0, 2, &h); + if (ph == NULL || ph->sb_len < 2 || !pskb_may_pull(skb, ph->sb_len)) + return NULL; + ph->sb_len -= 2; + *ptype = ph->sb_type; + *plen = ph->sb_len; + + if (buflen > ph->sb_len) + buflen = ph->sb_len; + data = skb_header_pointer(skb, 2, buflen, buf); + __skb_pull(skb, 2 + ph->sb_len); + return data; +} + +static int pep_reply(struct sock *sk, struct sk_buff *oskb, + u8 code, const void *data, int len, gfp_t priority) +{ + const struct pnpipehdr *oph = pnp_hdr(oskb); + struct pnpipehdr *ph; + struct sk_buff *skb; + + skb = alloc_skb(MAX_PNPIPE_HEADER + len, priority); + if (!skb) + return -ENOMEM; + skb_set_owner_w(skb, sk); + + skb_reserve(skb, MAX_PNPIPE_HEADER); + __skb_put(skb, len); + skb_copy_to_linear_data(skb, data, len); + __skb_push(skb, sizeof(*ph)); + skb_reset_transport_header(skb); + ph = pnp_hdr(skb); + ph->utid = oph->utid; + ph->message_id = oph->message_id + 1; /* REQ -> RESP */ + ph->pipe_handle = oph->pipe_handle; + ph->error_code = code; + + return pn_skb_send(sk, skb, &pipe_srv); +} + +#define PAD 0x00 +static int pep_accept_conn(struct sock *sk, struct sk_buff *skb) +{ + static const u8 data[20] = { + PAD, PAD, PAD, 2 /* sub-blocks */, + PN_PIPE_SB_REQUIRED_FC_TX, pep_sb_size(5), 3, PAD, + PN_MULTI_CREDIT_FLOW_CONTROL, + PN_ONE_CREDIT_FLOW_CONTROL, + PN_LEGACY_FLOW_CONTROL, + PAD, + PN_PIPE_SB_PREFERRED_FC_RX, pep_sb_size(5), 3, PAD, + PN_MULTI_CREDIT_FLOW_CONTROL, + PN_ONE_CREDIT_FLOW_CONTROL, + PN_LEGACY_FLOW_CONTROL, + PAD, + }; + + might_sleep(); + return pep_reply(sk, skb, PN_PIPE_NO_ERROR, data, sizeof(data), + GFP_KERNEL); +} + +static int pep_reject_conn(struct sock *sk, struct sk_buff *skb, u8 code) +{ + static const u8 data[4] = { PAD, PAD, PAD, 0 /* sub-blocks */ }; + WARN_ON(code == PN_PIPE_NO_ERROR); + return pep_reply(sk, skb, code, data, sizeof(data), GFP_ATOMIC); +} + +/* Control requests are not sent by the pipe service and have a specific + * message format. */ +static int pep_ctrlreq_error(struct sock *sk, struct sk_buff *oskb, u8 code) +{ + const struct pnpipehdr *oph = pnp_hdr(oskb); + struct sk_buff *skb; + struct pnpipehdr *ph; + struct sockaddr_pn dst; + + skb = alloc_skb(MAX_PNPIPE_HEADER + 4, GFP_ATOMIC); + if (!skb) + return -ENOMEM; + skb_set_owner_w(skb, sk); + + skb_reserve(skb, MAX_PHONET_HEADER); + ph = (struct pnpipehdr *)skb_put(skb, sizeof(*ph) + 4); + + ph->utid = oph->utid; + ph->message_id = PNS_PEP_CTRL_RESP; + ph->pipe_handle = oph->pipe_handle; + ph->data[0] = oph->data[1]; /* CTRL id */ + ph->data[1] = oph->data[0]; /* PEP type */ + ph->data[2] = code; /* error code, at an usual offset */ + ph->data[3] = PAD; + ph->data[4] = PAD; + + pn_skb_get_src_sockaddr(oskb, &dst); + return pn_skb_send(sk, skb, &dst); +} + +static int pipe_snd_status(struct sock *sk, u8 type, u8 status, gfp_t priority) +{ + struct pep_sock *pn = pep_sk(sk); + struct pnpipehdr *ph; + struct sk_buff *skb; + + skb = alloc_skb(MAX_PNPIPE_HEADER + 4, priority); + if (!skb) + return -ENOMEM; + skb_set_owner_w(skb, sk); + + skb_reserve(skb, MAX_PNPIPE_HEADER + 4); + __skb_push(skb, sizeof(*ph) + 4); + skb_reset_transport_header(skb); + ph = pnp_hdr(skb); + ph->utid = 0; + ph->message_id = PNS_PEP_STATUS_IND; + ph->pipe_handle = pn->pipe_handle; + ph->pep_type = PN_PEP_TYPE_COMMON; + ph->data[1] = type; + ph->data[2] = PAD; + ph->data[3] = PAD; + ph->data[4] = status; + + return pn_skb_send(sk, skb, &pipe_srv); +} + +/* Send our RX flow control information to the sender. + * Socket must be locked. */ +static void pipe_grant_credits(struct sock *sk) +{ + struct pep_sock *pn = pep_sk(sk); + + BUG_ON(sk->sk_state != TCP_ESTABLISHED); + + switch (pn->rx_fc) { + case PN_LEGACY_FLOW_CONTROL: /* TODO */ + break; + case PN_ONE_CREDIT_FLOW_CONTROL: + pipe_snd_status(sk, PN_PEP_IND_FLOW_CONTROL, + PEP_IND_READY, GFP_ATOMIC); + pn->rx_credits = 1; + break; + case PN_MULTI_CREDIT_FLOW_CONTROL: + if ((pn->rx_credits + CREDITS_THR) > CREDITS_MAX) + break; + if (pipe_snd_status(sk, PN_PEP_IND_ID_MCFC_GRANT_CREDITS, + CREDITS_MAX - pn->rx_credits, + GFP_ATOMIC) == 0) + pn->rx_credits = CREDITS_MAX; + break; + } +} + +static int pipe_rcv_status(struct sock *sk, struct sk_buff *skb) +{ + struct pep_sock *pn = pep_sk(sk); + struct pnpipehdr *hdr = pnp_hdr(skb); + + if (!pskb_may_pull(skb, sizeof(*hdr) + 4)) + return -EINVAL; + + if (hdr->data[0] != PN_PEP_TYPE_COMMON) { + LIMIT_NETDEBUG(KERN_DEBUG"Phonet unknown PEP type: %u\n", + (unsigned)hdr->data[0]); + return -EOPNOTSUPP; + } + + switch (hdr->data[1]) { + case PN_PEP_IND_FLOW_CONTROL: + switch (pn->tx_fc) { + case PN_LEGACY_FLOW_CONTROL: + switch (hdr->data[4]) { + case PEP_IND_BUSY: + pn->tx_credits = 0; + break; + case PEP_IND_READY: + pn->tx_credits = 1; + break; + } + break; + case PN_ONE_CREDIT_FLOW_CONTROL: + if (hdr->data[4] == PEP_IND_READY) + pn->tx_credits = 1; + break; + } + break; + + case PN_PEP_IND_ID_MCFC_GRANT_CREDITS: + if (pn->tx_fc != PN_MULTI_CREDIT_FLOW_CONTROL) + break; + if (pn->tx_credits + hdr->data[4] > 0xff) + pn->tx_credits = 0xff; + else + pn->tx_credits += hdr->data[4]; + break; + + default: + LIMIT_NETDEBUG(KERN_DEBUG"Phonet unknown PEP indication: %u\n", + (unsigned)hdr->data[1]); + return -EOPNOTSUPP; + } + if (pn->tx_credits) + sk->sk_write_space(sk); + return 0; +} + +static int pipe_rcv_created(struct sock *sk, struct sk_buff *skb) +{ + struct pep_sock *pn = pep_sk(sk); + struct pnpipehdr *hdr = pnp_hdr(skb); + u8 n_sb = hdr->data[0]; + + pn->rx_fc = pn->tx_fc = PN_LEGACY_FLOW_CONTROL; + __skb_pull(skb, sizeof(*hdr)); + while (n_sb > 0) { + u8 type, buf[2], len = sizeof(buf); + u8 *data = pep_get_sb(skb, &type, &len, buf); + + if (data == NULL) + return -EINVAL; + switch (type) { + case PN_PIPE_SB_NEGOTIATED_FC: + if (len < 2 || (data[0] | data[1]) > 3) + break; + pn->tx_fc = data[0] & 3; + pn->rx_fc = data[1] & 3; + break; + } + n_sb--; + } + return 0; +} + +/* Queue an skb to a connected sock. + * Socket lock must be held. */ +static int pipe_do_rcv(struct sock *sk, struct sk_buff *skb) +{ + struct pep_sock *pn = pep_sk(sk); + struct pnpipehdr *hdr = pnp_hdr(skb); + int err = 0; + + BUG_ON(sk->sk_state == TCP_CLOSE_WAIT); + + switch (hdr->message_id) { + case PNS_PEP_CONNECT_REQ: + pep_reject_conn(sk, skb, PN_PIPE_ERR_PEP_IN_USE); + break; + + case PNS_PEP_DISCONNECT_REQ: + pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC); + sk->sk_state = TCP_CLOSE_WAIT; + if (!sock_flag(sk, SOCK_DEAD)) + sk->sk_state_change(sk); + break; + + case PNS_PEP_ENABLE_REQ: + /* Wait for PNS_PIPE_(ENABLED|REDIRECTED)_IND */ + pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC); + break; + + case PNS_PEP_RESET_REQ: + switch (hdr->state_after_reset) { + case PN_PIPE_DISABLE: + pn->init_enable = 0; + break; + case PN_PIPE_ENABLE: + pn->init_enable = 1; + break; + default: /* not allowed to send an error here!? */ + err = -EINVAL; + goto out; + } + /* fall through */ + case PNS_PEP_DISABLE_REQ: + pn->tx_credits = 0; + pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC); + break; + + case PNS_PEP_CTRL_REQ: + /* TODO */ + pep_ctrlreq_error(sk, skb, PN_PIPE_NO_ERROR); + break; + + case PNS_PIPE_DATA: + __skb_pull(skb, 3); /* Pipe data header */ + if (!pn_flow_safe(pn->rx_fc)) { + err = sock_queue_rcv_skb(sk, skb); + if (!err) + return 0; + break; + } + + if (pn->rx_credits == 0) { + err = -ENOBUFS; + break; + } + pn->rx_credits--; + skb->dev = NULL; + skb_set_owner_r(skb, sk); + err = skb->len; + skb_queue_tail(&sk->sk_receive_queue, skb); + if (!sock_flag(sk, SOCK_DEAD)) + sk->sk_data_ready(sk, err); + return 0; + + case PNS_PEP_STATUS_IND: + pipe_rcv_status(sk, skb); + break; + + case PNS_PIPE_REDIRECTED_IND: + err = pipe_rcv_created(sk, skb); + break; + + case PNS_PIPE_CREATED_IND: + err = pipe_rcv_created(sk, skb); + if (err) + break; + /* fall through */ + case PNS_PIPE_RESET_IND: + if (!pn->init_enable) + break; + /* fall through */ + case PNS_PIPE_ENABLED_IND: + if (!pn_flow_safe(pn->tx_fc)) { + pn->tx_credits = 1; + sk->sk_write_space(sk); + } + if (sk->sk_state == TCP_ESTABLISHED) + break; /* Nothing to do */ + sk->sk_state = TCP_ESTABLISHED; + pipe_grant_credits(sk); + break; + + case PNS_PIPE_DISABLED_IND: + sk->sk_state = TCP_SYN_RECV; + pn->rx_credits = 0; + break; + + default: + LIMIT_NETDEBUG(KERN_DEBUG"Phonet unknown PEP message: %u\n", + hdr->message_id); + err = -EINVAL; + } +out: + kfree_skb(skb); + return err; +} + +/* Destroy connected sock. */ +static void pipe_destruct(struct sock *sk) +{ + skb_queue_purge(&sk->sk_receive_queue); +} + +static int pep_connreq_rcv(struct sock *sk, struct sk_buff *skb) +{ + struct sock *newsk; + struct pep_sock *newpn, *pn = pep_sk(sk); + struct pnpipehdr *hdr; + struct sockaddr_pn dst; + u16 peer_type; + u8 pipe_handle, enabled, n_sb; + + if (!pskb_pull(skb, sizeof(*hdr) + 4)) + return -EINVAL; + + hdr = pnp_hdr(skb); + pipe_handle = hdr->pipe_handle; + switch (hdr->state_after_connect) { + case PN_PIPE_DISABLE: + enabled = 0; + break; + case PN_PIPE_ENABLE: + enabled = 1; + break; + default: + pep_reject_conn(sk, skb, PN_PIPE_ERR_INVALID_PARAM); + return -EINVAL; + } + peer_type = hdr->other_pep_type << 8; + + if (unlikely(sk->sk_state != TCP_LISTEN) || sk_acceptq_is_full(sk)) { + pep_reject_conn(sk, skb, PN_PIPE_ERR_PEP_IN_USE); + return -ENOBUFS; + } + + /* Parse sub-blocks (options) */ + n_sb = hdr->data[4]; + while (n_sb > 0) { + u8 type, buf[1], len = sizeof(buf); + const u8 *data = pep_get_sb(skb, &type, &len, buf); + + if (data == NULL) + return -EINVAL; + switch (type) { + case PN_PIPE_SB_CONNECT_REQ_PEP_SUB_TYPE: + if (len < 1) + return -EINVAL; + peer_type = (peer_type & 0xff00) | data[0]; + break; + } + n_sb--; + } + + skb = skb_clone(skb, GFP_ATOMIC); + if (!skb) + return -ENOMEM; + + /* Create a new to-be-accepted sock */ + newsk = sk_alloc(sock_net(sk), PF_PHONET, GFP_ATOMIC, sk->sk_prot); + if (!newsk) { + kfree_skb(skb); + return -ENOMEM; + } + sock_init_data(NULL, newsk); + newsk->sk_state = TCP_SYN_RECV; + newsk->sk_backlog_rcv = pipe_do_rcv; + newsk->sk_protocol = sk->sk_protocol; + newsk->sk_destruct = pipe_destruct; + + newpn = pep_sk(newsk); + pn_skb_get_dst_sockaddr(skb, &dst); + newpn->pn_sk.sobject = pn_sockaddr_get_object(&dst); + newpn->pn_sk.resource = pn->pn_sk.resource; + newpn->pipe_handle = pipe_handle; + newpn->peer_type = peer_type; + newpn->rx_credits = newpn->tx_credits = 0; + newpn->rx_fc = newpn->tx_fc = PN_LEGACY_FLOW_CONTROL; + newpn->init_enable = enabled; + + BUG_ON(!skb_queue_empty(&newsk->sk_receive_queue)); + skb_queue_head(&newsk->sk_receive_queue, skb); + if (!sock_flag(sk, SOCK_DEAD)) + sk->sk_data_ready(sk, 0); + + sk_acceptq_added(sk); + sk_add_node(newsk, &pn->ackq); + return 0; +} + +/* Listening sock must be locked */ +static struct sock *pep_find_pipe(const struct hlist_head *hlist, + const struct sockaddr_pn *dst, + u8 pipe_handle) +{ + struct hlist_node *node; + struct sock *sknode; + u16 dobj = pn_sockaddr_get_object(dst); + + sk_for_each(sknode, node, hlist) { + struct pep_sock *pnnode = pep_sk(sknode); + + /* Ports match, but addresses might not: */ + if (pnnode->pn_sk.sobject != dobj) + continue; + if (pnnode->pipe_handle != pipe_handle) + continue; + if (sknode->sk_state == TCP_CLOSE_WAIT) + continue; + + sock_hold(sknode); + return sknode; + } + return NULL; +} + +/* + * Deliver an skb to a listening sock. + * Socket lock must be held. + * We then queue the skb to the right connected sock (if any). + */ +static int pep_do_rcv(struct sock *sk, struct sk_buff *skb) +{ + struct pep_sock *pn = pep_sk(sk); + struct sock *sknode; + struct pnpipehdr *hdr = pnp_hdr(skb); + struct sockaddr_pn dst; + int err = NET_RX_SUCCESS; + u8 pipe_handle; + + if (!pskb_may_pull(skb, sizeof(*hdr))) + goto drop; + + hdr = pnp_hdr(skb); + pipe_handle = hdr->pipe_handle; + if (pipe_handle == PN_PIPE_INVALID_HANDLE) + goto drop; + + pn_skb_get_dst_sockaddr(skb, &dst); + + /* Look for an existing pipe handle */ + sknode = pep_find_pipe(&pn->hlist, &dst, pipe_handle); + if (sknode) + return sk_receive_skb(sknode, skb, 1); + + /* Look for a pipe handle pending accept */ + sknode = pep_find_pipe(&pn->ackq, &dst, pipe_handle); + if (sknode) { + sock_put(sknode); + if (net_ratelimit()) + printk(KERN_WARNING"Phonet unconnected PEP ignored"); + err = NET_RX_DROP; + goto drop; + } + + switch (hdr->message_id) { + case PNS_PEP_CONNECT_REQ: + err = pep_connreq_rcv(sk, skb); + break; + + case PNS_PEP_DISCONNECT_REQ: + pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC); + break; + + case PNS_PEP_CTRL_REQ: + pep_ctrlreq_error(sk, skb, PN_PIPE_INVALID_HANDLE); + break; + + case PNS_PEP_RESET_REQ: + case PNS_PEP_ENABLE_REQ: + case PNS_PEP_DISABLE_REQ: + /* invalid handle is not even allowed here! */ + default: + err = NET_RX_DROP; + } +drop: + kfree_skb(skb); + return err; +} + +/* associated socket ceases to exist */ +static void pep_sock_close(struct sock *sk, long timeout) +{ + struct pep_sock *pn = pep_sk(sk); + + sk_common_release(sk); + + lock_sock(sk); + if (sk->sk_state == TCP_LISTEN) { + /* Destroy the listen queue */ + struct sock *sknode; + struct hlist_node *p, *n; + + sk_for_each_safe(sknode, p, n, &pn->ackq) + sk_del_node_init(sknode); + sk->sk_state = TCP_CLOSE; + } + release_sock(sk); +} + +static int pep_wait_connreq(struct sock *sk, int noblock) +{ + struct task_struct *tsk = current; + struct pep_sock *pn = pep_sk(sk); + long timeo = sock_rcvtimeo(sk, noblock); + + for (;;) { + DEFINE_WAIT(wait); + + if (sk->sk_state != TCP_LISTEN) + return -EINVAL; + if (!hlist_empty(&pn->ackq)) + break; + if (!timeo) + return -EWOULDBLOCK; + if (signal_pending(tsk)) + return sock_intr_errno(timeo); + + prepare_to_wait_exclusive(&sk->sk_socket->wait, &wait, + TASK_INTERRUPTIBLE); + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + finish_wait(&sk->sk_socket->wait, &wait); + } + + return 0; +} + +static struct sock *pep_sock_accept(struct sock *sk, int flags, int *errp) +{ + struct pep_sock *pn = pep_sk(sk); + struct sock *newsk = NULL; + struct sk_buff *oskb; + int err; + + lock_sock(sk); + err = pep_wait_connreq(sk, flags & O_NONBLOCK); + if (err) + goto out; + + newsk = __sk_head(&pn->ackq); + + oskb = skb_dequeue(&newsk->sk_receive_queue); + err = pep_accept_conn(newsk, oskb); + if (err) { + skb_queue_head(&newsk->sk_receive_queue, oskb); + newsk = NULL; + goto out; + } + + sock_hold(sk); + pep_sk(newsk)->listener = sk; + + sock_hold(newsk); + sk_del_node_init(newsk); + sk_acceptq_removed(sk); + sk_add_node(newsk, &pn->hlist); + __sock_put(newsk); + +out: + release_sock(sk); + *errp = err; + return newsk; +} + +static int pep_ioctl(struct sock *sk, int cmd, unsigned long arg) +{ + int answ; + + switch (cmd) { + case SIOCINQ: + if (sk->sk_state == TCP_LISTEN) + return -EINVAL; + + lock_sock(sk); + if (!skb_queue_empty(&sk->sk_receive_queue)) + answ = skb_peek(&sk->sk_receive_queue)->len; + else + answ = 0; + release_sock(sk); + return put_user(answ, (int __user *)arg); + } + + return -ENOIOCTLCMD; +} + +static int pep_init(struct sock *sk) +{ + struct pep_sock *pn = pep_sk(sk); + + INIT_HLIST_HEAD(&pn->ackq); + INIT_HLIST_HEAD(&pn->hlist); + pn->pipe_handle = PN_PIPE_INVALID_HANDLE; + return 0; +} + +static int pep_sendmsg(struct kiocb *iocb, struct sock *sk, + struct msghdr *msg, size_t len) +{ + struct pep_sock *pn = pep_sk(sk); + struct sk_buff *skb = NULL; + struct pnpipehdr *ph; + long timeo; + int flags = msg->msg_flags; + int err, done; + + if (msg->msg_flags & MSG_OOB || !(msg->msg_flags & MSG_EOR)) + return -EOPNOTSUPP; + + lock_sock(sk); + timeo = sock_sndtimeo(sk, flags & MSG_DONTWAIT); + if ((1 << sk->sk_state) & (TCPF_LISTEN|TCPF_CLOSE)) { + err = -ENOTCONN; + goto out; + } + if (sk->sk_state != TCP_ESTABLISHED) { + /* Wait until the pipe gets to enabled state */ +disabled: + err = sk_stream_wait_connect(sk, &timeo); + if (err) + goto out; + + if (sk->sk_state == TCP_CLOSE_WAIT) { + err = -ECONNRESET; + goto out; + } + } + BUG_ON(sk->sk_state != TCP_ESTABLISHED); + + /* Wait until flow control allows TX */ + done = pn->tx_credits > 0; + while (!done) { + DEFINE_WAIT(wait); + + if (!timeo) { + err = -EAGAIN; + goto out; + } + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + goto out; + } + + prepare_to_wait(&sk->sk_socket->wait, &wait, + TASK_INTERRUPTIBLE); + done = sk_wait_event(sk, &timeo, pn->tx_credits > 0); + finish_wait(&sk->sk_socket->wait, &wait); + + if (sk->sk_state != TCP_ESTABLISHED) + goto disabled; + } + + if (!skb) { + skb = sock_alloc_send_skb(sk, MAX_PNPIPE_HEADER + len, + flags & MSG_DONTWAIT, &err); + if (skb == NULL) + goto out; + skb_reserve(skb, MAX_PHONET_HEADER + 3); + + if (sk->sk_state != TCP_ESTABLISHED || !pn->tx_credits) + goto disabled; /* sock_alloc_send_skb might sleep */ + } + + err = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len); + if (err < 0) + goto out; + + __skb_push(skb, 3); + skb_reset_transport_header(skb); + ph = pnp_hdr(skb); + ph->utid = 0; + ph->message_id = PNS_PIPE_DATA; + ph->pipe_handle = pn->pipe_handle; + if (pn_flow_safe(pn->tx_fc)) /* credit-based flow control */ + pn->tx_credits--; + + err = pn_skb_send(sk, skb, &pipe_srv); + if (err >= 0) + err = len; /* success! */ + skb = NULL; +out: + release_sock(sk); + kfree_skb(skb); + return err; +} + +static int pep_recvmsg(struct kiocb *iocb, struct sock *sk, + struct msghdr *msg, size_t len, int noblock, + int flags, int *addr_len) +{ + struct sk_buff *skb; + int err; + + if (unlikely(flags & MSG_OOB)) + return -EOPNOTSUPP; + if (unlikely(1 << sk->sk_state & (TCPF_LISTEN | TCPF_CLOSE))) + return -ENOTCONN; + + skb = skb_recv_datagram(sk, flags, noblock, &err); + lock_sock(sk); + if (skb == NULL) { + if (err == -ENOTCONN && sk->sk_state == TCP_CLOSE_WAIT) + err = -ECONNRESET; + release_sock(sk); + return err; + } + + if (sk->sk_state == TCP_ESTABLISHED) + pipe_grant_credits(sk); + release_sock(sk); + + msg->msg_flags |= MSG_EOR; + + if (skb->len > len) + msg->msg_flags |= MSG_TRUNC; + else + len = skb->len; + + err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, len); + if (!err) + err = (flags & MSG_TRUNC) ? skb->len : len; + + skb_free_datagram(sk, skb); + return err; +} + +static void pep_sock_unhash(struct sock *sk) +{ + struct pep_sock *pn = pep_sk(sk); + struct sock *skparent = NULL; + + lock_sock(sk); + if ((1 << sk->sk_state) & ~(TCPF_CLOSE|TCPF_LISTEN)) { + skparent = pn->listener; + sk_del_node_init(sk); + release_sock(sk); + + sk = skparent; + pn = pep_sk(skparent); + lock_sock(sk); + } + /* Unhash a listening sock only when it is closed + * and all of its active connected pipes are closed. */ + if (hlist_empty(&pn->hlist)) + pn_sock_unhash(&pn->pn_sk.sk); + release_sock(sk); + + if (skparent) + sock_put(skparent); +} + +static struct proto pep_proto = { + .close = pep_sock_close, + .accept = pep_sock_accept, + .ioctl = pep_ioctl, + .init = pep_init, + .sendmsg = pep_sendmsg, + .recvmsg = pep_recvmsg, + .backlog_rcv = pep_do_rcv, + .hash = pn_sock_hash, + .unhash = pep_sock_unhash, + .get_port = pn_sock_get_port, + .obj_size = sizeof(struct pep_sock), + .owner = THIS_MODULE, + .name = "PNPIPE", +}; + +static struct phonet_protocol pep_pn_proto = { + .ops = &phonet_stream_ops, + .prot = &pep_proto, + .sock_type = SOCK_SEQPACKET, +}; + +static int __init pep_register(void) +{ + return phonet_proto_register(PN_PROTO_PIPE, &pep_pn_proto); +} + +static void __exit pep_unregister(void) +{ + phonet_proto_unregister(PN_PROTO_PIPE, &pep_pn_proto); +} + +module_init(pep_register); +module_exit(pep_unregister); +MODULE_AUTHOR("Remi Denis-Courmont, Nokia"); +MODULE_DESCRIPTION("Phonet pipe protocol"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS_NET_PF_PROTO(PF_PHONET, PN_PROTO_PIPE); -- cgit v1.2.3 From c41bd97f815720f9404f97da0c4f4400b52c243d Mon Sep 17 00:00:00 2001 From: Rémi Denis-Courmont Date: Sun, 5 Oct 2008 11:15:43 -0700 Subject: Phonet: receive pipe control requests as out-of-band data MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Rémi Denis-Courmont Signed-off-by: David S. Miller --- include/net/phonet/pep.h | 2 ++ net/phonet/pep.c | 65 ++++++++++++++++++++++++++++++++++-------------- net/phonet/socket.c | 4 ++- 3 files changed, 52 insertions(+), 19 deletions(-) (limited to 'net/phonet') diff --git a/include/net/phonet/pep.h b/include/net/phonet/pep.h index fb024e186860..4d79564850ae 100644 --- a/include/net/phonet/pep.h +++ b/include/net/phonet/pep.h @@ -33,6 +33,8 @@ struct pep_sock { /* Connected socket stuff: */ struct sock *listener; + struct sk_buff_head ctrlreq_queue; +#define PNPIPE_CTRLREQ_MAX 10 u16 peer_type; /* peer type/subtype */ u8 pipe_handle; diff --git a/net/phonet/pep.c b/net/phonet/pep.c index c5dfecb207d2..d564d07cb797 100644 --- a/net/phonet/pep.c +++ b/net/phonet/pep.c @@ -137,14 +137,15 @@ static int pep_reject_conn(struct sock *sk, struct sk_buff *skb, u8 code) /* Control requests are not sent by the pipe service and have a specific * message format. */ -static int pep_ctrlreq_error(struct sock *sk, struct sk_buff *oskb, u8 code) +static int pep_ctrlreq_error(struct sock *sk, struct sk_buff *oskb, u8 code, + gfp_t priority) { const struct pnpipehdr *oph = pnp_hdr(oskb); struct sk_buff *skb; struct pnpipehdr *ph; struct sockaddr_pn dst; - skb = alloc_skb(MAX_PNPIPE_HEADER + 4, GFP_ATOMIC); + skb = alloc_skb(MAX_PNPIPE_HEADER + 4, priority); if (!skb) return -ENOMEM; skb_set_owner_w(skb, sk); @@ -305,6 +306,7 @@ static int pipe_do_rcv(struct sock *sk, struct sk_buff *skb) { struct pep_sock *pn = pep_sk(sk); struct pnpipehdr *hdr = pnp_hdr(skb); + struct sk_buff_head *queue; int err = 0; BUG_ON(sk->sk_state == TCP_CLOSE_WAIT); @@ -345,9 +347,11 @@ static int pipe_do_rcv(struct sock *sk, struct sk_buff *skb) break; case PNS_PEP_CTRL_REQ: - /* TODO */ - pep_ctrlreq_error(sk, skb, PN_PIPE_NO_ERROR); - break; + if (skb_queue_len(&pn->ctrlreq_queue) >= PNPIPE_CTRLREQ_MAX) + break; + __skb_pull(skb, 4); + queue = &pn->ctrlreq_queue; + goto queue; case PNS_PIPE_DATA: __skb_pull(skb, 3); /* Pipe data header */ @@ -363,13 +367,8 @@ static int pipe_do_rcv(struct sock *sk, struct sk_buff *skb) break; } pn->rx_credits--; - skb->dev = NULL; - skb_set_owner_r(skb, sk); - err = skb->len; - skb_queue_tail(&sk->sk_receive_queue, skb); - if (!sock_flag(sk, SOCK_DEAD)) - sk->sk_data_ready(sk, err); - return 0; + queue = &sk->sk_receive_queue; + goto queue; case PNS_PEP_STATUS_IND: pipe_rcv_status(sk, skb); @@ -412,12 +411,24 @@ static int pipe_do_rcv(struct sock *sk, struct sk_buff *skb) out: kfree_skb(skb); return err; + +queue: + skb->dev = NULL; + skb_set_owner_r(skb, sk); + err = skb->len; + skb_queue_tail(queue, skb); + if (!sock_flag(sk, SOCK_DEAD)) + sk->sk_data_ready(sk, err); + return 0; } /* Destroy connected sock. */ static void pipe_destruct(struct sock *sk) { + struct pep_sock *pn = pep_sk(sk); + skb_queue_purge(&sk->sk_receive_queue); + skb_queue_purge(&pn->ctrlreq_queue); } static int pep_connreq_rcv(struct sock *sk, struct sk_buff *skb) @@ -490,6 +501,7 @@ static int pep_connreq_rcv(struct sock *sk, struct sk_buff *skb) pn_skb_get_dst_sockaddr(skb, &dst); newpn->pn_sk.sobject = pn_sockaddr_get_object(&dst); newpn->pn_sk.resource = pn->pn_sk.resource; + skb_queue_head_init(&newpn->ctrlreq_queue); newpn->pipe_handle = pipe_handle; newpn->peer_type = peer_type; newpn->rx_credits = newpn->tx_credits = 0; @@ -581,7 +593,7 @@ static int pep_do_rcv(struct sock *sk, struct sk_buff *skb) break; case PNS_PEP_CTRL_REQ: - pep_ctrlreq_error(sk, skb, PN_PIPE_INVALID_HANDLE); + pep_ctrlreq_error(sk, skb, PN_PIPE_INVALID_HANDLE, GFP_ATOMIC); break; case PNS_PEP_RESET_REQ: @@ -684,6 +696,7 @@ out: static int pep_ioctl(struct sock *sk, int cmd, unsigned long arg) { + struct pep_sock *pn = pep_sk(sk); int answ; switch (cmd) { @@ -692,7 +705,10 @@ static int pep_ioctl(struct sock *sk, int cmd, unsigned long arg) return -EINVAL; lock_sock(sk); - if (!skb_queue_empty(&sk->sk_receive_queue)) + if (sock_flag(sk, SOCK_URGINLINE) + && !skb_queue_empty(&pn->ctrlreq_queue)) + answ = skb_peek(&pn->ctrlreq_queue)->len; + else if (!skb_queue_empty(&sk->sk_receive_queue)) answ = skb_peek(&sk->sk_receive_queue)->len; else answ = 0; @@ -709,6 +725,7 @@ static int pep_init(struct sock *sk) INIT_HLIST_HEAD(&pn->ackq); INIT_HLIST_HEAD(&pn->hlist); + skb_queue_head_init(&pn->ctrlreq_queue); pn->pipe_handle = PN_PIPE_INVALID_HANDLE; return 0; } @@ -810,11 +827,24 @@ static int pep_recvmsg(struct kiocb *iocb, struct sock *sk, struct sk_buff *skb; int err; - if (unlikely(flags & MSG_OOB)) - return -EOPNOTSUPP; if (unlikely(1 << sk->sk_state & (TCPF_LISTEN | TCPF_CLOSE))) return -ENOTCONN; + if ((flags & MSG_OOB) || sock_flag(sk, SOCK_URGINLINE)) { + /* Dequeue and acknowledge control request */ + struct pep_sock *pn = pep_sk(sk); + + skb = skb_dequeue(&pn->ctrlreq_queue); + if (skb) { + pep_ctrlreq_error(sk, skb, PN_PIPE_NO_ERROR, + GFP_KERNEL); + msg->msg_flags |= MSG_OOB; + goto copy; + } + if (flags & MSG_OOB) + return -EINVAL; + } + skb = skb_recv_datagram(sk, flags, noblock, &err); lock_sock(sk); if (skb == NULL) { @@ -827,9 +857,8 @@ static int pep_recvmsg(struct kiocb *iocb, struct sock *sk, if (sk->sk_state == TCP_ESTABLISHED) pipe_grant_credits(sk); release_sock(sk); - +copy: msg->msg_flags |= MSG_EOR; - if (skb->len > len) msg->msg_flags |= MSG_TRUNC; else diff --git a/net/phonet/socket.c b/net/phonet/socket.c index cea1136cbe46..a9c3d1f2e9db 100644 --- a/net/phonet/socket.c +++ b/net/phonet/socket.c @@ -220,7 +220,9 @@ static unsigned int pn_socket_poll(struct file *file, struct socket *sock, if (!skb_queue_empty(&sk->sk_receive_queue)) mask |= POLLIN | POLLRDNORM; - else if (sk->sk_state == TCP_CLOSE_WAIT) + if (!skb_queue_empty(&pn->ctrlreq_queue)) + mask |= POLLPRI; + if (!mask && sk->sk_state == TCP_CLOSE_WAIT) return POLLHUP; if (sk->sk_state == TCP_ESTABLISHED && pn->tx_credits) -- cgit v1.2.3 From 02a47617cdce440f60c71a51f3a93f9f5fcc5a7a Mon Sep 17 00:00:00 2001 From: Rémi Denis-Courmont Date: Sun, 5 Oct 2008 11:16:16 -0700 Subject: Phonet: implement GPRS virtual interface over PEP socket MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Rémi Denis-Courmont Signed-off-by: David S. Miller --- include/linux/phonet.h | 8 ++ include/linux/socket.h | 1 + include/net/phonet/gprs.h | 38 +++++ include/net/phonet/pep.h | 1 + net/phonet/Makefile | 2 +- net/phonet/pep-gprs.c | 347 ++++++++++++++++++++++++++++++++++++++++++++++ net/phonet/pep.c | 161 +++++++++++++++++++-- net/phonet/socket.c | 8 +- 8 files changed, 550 insertions(+), 16 deletions(-) create mode 100644 include/net/phonet/gprs.h create mode 100644 net/phonet/pep-gprs.c (limited to 'net/phonet') diff --git a/include/linux/phonet.h b/include/linux/phonet.h index f92185242078..c9609f9aedac 100644 --- a/include/linux/phonet.h +++ b/include/linux/phonet.h @@ -31,9 +31,17 @@ #define PN_PROTO_PIPE 2 #define PHONET_NPROTO 3 +/* Socket options for SOL_PNPIPE level */ +#define PNPIPE_ENCAP 1 +#define PNPIPE_IFINDEX 2 + #define PNADDR_ANY 0 #define PNPORT_RESOURCE_ROUTING 0 +/* Values for PNPIPE_ENCAP option */ +#define PNPIPE_ENCAP_NONE 0 +#define PNPIPE_ENCAP_IP 1 + /* ioctls */ #define SIOCPNGETOBJECT (SIOCPROTOPRIVATE + 0) diff --git a/include/linux/socket.h b/include/linux/socket.h index 818ca33bf79f..20fc4bbfca42 100644 --- a/include/linux/socket.h +++ b/include/linux/socket.h @@ -297,6 +297,7 @@ struct ucred { #define SOL_RXRPC 272 #define SOL_PPPOL2TP 273 #define SOL_BLUETOOTH 274 +#define SOL_PNPIPE 275 /* IPX options */ #define IPX_TYPE 1 diff --git a/include/net/phonet/gprs.h b/include/net/phonet/gprs.h new file mode 100644 index 000000000000..928daf595beb --- /dev/null +++ b/include/net/phonet/gprs.h @@ -0,0 +1,38 @@ +/* + * File: pep_gprs.h + * + * GPRS over Phonet pipe end point socket + * + * Copyright (C) 2008 Nokia Corporation. + * + * Author: Rémi Denis-Courmont + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ + +#ifndef NET_PHONET_GPRS_H +#define NET_PHONET_GPRS_H + +struct sock; +struct sk_buff; + +int pep_writeable(struct sock *sk); +int pep_write(struct sock *sk, struct sk_buff *skb); +struct sk_buff *pep_read(struct sock *sk); + +int gprs_attach(struct sock *sk); +void gprs_detach(struct sock *sk); + +#endif diff --git a/include/net/phonet/pep.h b/include/net/phonet/pep.h index 4d79564850ae..fcd793030e4d 100644 --- a/include/net/phonet/pep.h +++ b/include/net/phonet/pep.h @@ -35,6 +35,7 @@ struct pep_sock { struct sock *listener; struct sk_buff_head ctrlreq_queue; #define PNPIPE_CTRLREQ_MAX 10 + int ifindex; u16 peer_type; /* peer type/subtype */ u8 pipe_handle; diff --git a/net/phonet/Makefile b/net/phonet/Makefile index 505df2a04a85..d62bbba649b3 100644 --- a/net/phonet/Makefile +++ b/net/phonet/Makefile @@ -8,4 +8,4 @@ phonet-objs := \ sysctl.o \ af_phonet.o -pn_pep-objs := pep.o +pn_pep-objs := pep.o pep-gprs.o diff --git a/net/phonet/pep-gprs.c b/net/phonet/pep-gprs.c new file mode 100644 index 000000000000..9978afbd9f2a --- /dev/null +++ b/net/phonet/pep-gprs.c @@ -0,0 +1,347 @@ +/* + * File: pep-gprs.c + * + * GPRS over Phonet pipe end point socket + * + * Copyright (C) 2008 Nokia Corporation. + * + * Author: Rémi Denis-Courmont + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA + * 02110-1301 USA + */ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#define GPRS_DEFAULT_MTU 1400 + +struct gprs_dev { + struct sock *sk; + void (*old_state_change)(struct sock *); + void (*old_data_ready)(struct sock *, int); + void (*old_write_space)(struct sock *); + + struct net_device *net; + struct net_device_stats stats; + + struct sk_buff_head tx_queue; + struct work_struct tx_work; + spinlock_t tx_lock; + unsigned tx_max; +}; + +static int gprs_type_trans(struct sk_buff *skb) +{ + const u8 *pvfc; + u8 buf; + + pvfc = skb_header_pointer(skb, 0, 1, &buf); + if (!pvfc) + return 0; + /* Look at IP version field */ + switch (*pvfc >> 4) { + case 4: + return htons(ETH_P_IP); + case 6: + return htons(ETH_P_IPV6); + } + return 0; +} + +/* + * Socket callbacks + */ + +static void gprs_state_change(struct sock *sk) +{ + struct gprs_dev *dev = sk->sk_user_data; + + if (sk->sk_state == TCP_CLOSE_WAIT) { + netif_stop_queue(dev->net); + netif_carrier_off(dev->net); + } +} + +static int gprs_recv(struct gprs_dev *dev, struct sk_buff *skb) +{ + int err = 0; + u16 protocol = gprs_type_trans(skb); + + if (!protocol) { + err = -EINVAL; + goto drop; + } + + if (likely(skb_headroom(skb) & 3)) { + struct sk_buff *rskb, *fs; + int flen = 0; + + /* Phonet Pipe data header is misaligned (3 bytes), + * so wrap the IP packet as a single fragment of an head-less + * socket buffer. The network stack will pull what it needs, + * but at least, the whole IP payload is not memcpy'd. */ + rskb = netdev_alloc_skb(dev->net, 0); + if (!rskb) { + err = -ENOBUFS; + goto drop; + } + skb_shinfo(rskb)->frag_list = skb; + rskb->len += skb->len; + rskb->data_len += rskb->len; + rskb->truesize += rskb->len; + + /* Avoid nested fragments */ + for (fs = skb_shinfo(skb)->frag_list; fs; fs = fs->next) + flen += fs->len; + skb->next = skb_shinfo(skb)->frag_list; + skb_shinfo(skb)->frag_list = NULL; + skb->len -= flen; + skb->data_len -= flen; + skb->truesize -= flen; + + skb = rskb; + } + + skb->protocol = protocol; + skb_reset_mac_header(skb); + skb->dev = dev->net; + + if (likely(dev->net->flags & IFF_UP)) { + dev->stats.rx_packets++; + dev->stats.rx_bytes += skb->len; + netif_rx(skb); + skb = NULL; + } else + err = -ENODEV; + +drop: + if (skb) { + dev_kfree_skb(skb); + dev->stats.rx_dropped++; + } + return err; +} + +static void gprs_data_ready(struct sock *sk, int len) +{ + struct gprs_dev *dev = sk->sk_user_data; + struct sk_buff *skb; + + while ((skb = pep_read(sk)) != NULL) { + skb_orphan(skb); + gprs_recv(dev, skb); + } +} + +static void gprs_write_space(struct sock *sk) +{ + struct gprs_dev *dev = sk->sk_user_data; + unsigned credits = pep_writeable(sk); + + spin_lock_bh(&dev->tx_lock); + dev->tx_max = credits; + if (credits > skb_queue_len(&dev->tx_queue)) + netif_wake_queue(dev->net); + spin_unlock_bh(&dev->tx_lock); +} + +/* + * Network device callbacks + */ + +static int gprs_xmit(struct sk_buff *skb, struct net_device *net) +{ + struct gprs_dev *dev = netdev_priv(net); + + switch (skb->protocol) { + case htons(ETH_P_IP): + case htons(ETH_P_IPV6): + break; + default: + dev_kfree_skb(skb); + return 0; + } + + spin_lock(&dev->tx_lock); + if (likely(skb_queue_len(&dev->tx_queue) < dev->tx_max)) { + skb_queue_tail(&dev->tx_queue, skb); + skb = NULL; + } + if (skb_queue_len(&dev->tx_queue) >= dev->tx_max) + netif_stop_queue(net); + spin_unlock(&dev->tx_lock); + + schedule_work(&dev->tx_work); + if (unlikely(skb)) + dev_kfree_skb(skb); + return 0; +} + +static void gprs_tx(struct work_struct *work) +{ + struct gprs_dev *dev = container_of(work, struct gprs_dev, tx_work); + struct sock *sk = dev->sk; + struct sk_buff *skb; + + while ((skb = skb_dequeue(&dev->tx_queue)) != NULL) { + int err; + + dev->stats.tx_bytes += skb->len; + dev->stats.tx_packets++; + + skb_orphan(skb); + skb_set_owner_w(skb, sk); + + lock_sock(sk); + err = pep_write(sk, skb); + if (err) { + LIMIT_NETDEBUG(KERN_WARNING"%s: TX error (%d)\n", + dev->net->name, err); + dev->stats.tx_aborted_errors++; + dev->stats.tx_errors++; + } + release_sock(sk); + } + + lock_sock(sk); + gprs_write_space(sk); + release_sock(sk); +} + +static int gprs_set_mtu(struct net_device *net, int new_mtu) +{ + if ((new_mtu < 576) || (new_mtu > (PHONET_MAX_MTU - 11))) + return -EINVAL; + + net->mtu = new_mtu; + return 0; +} + +static struct net_device_stats *gprs_get_stats(struct net_device *net) +{ + struct gprs_dev *dev = netdev_priv(net); + + return &dev->stats; +} + +static void gprs_setup(struct net_device *net) +{ + net->features = NETIF_F_FRAGLIST; + net->type = ARPHRD_NONE; + net->flags = IFF_POINTOPOINT | IFF_NOARP; + net->mtu = GPRS_DEFAULT_MTU; + net->hard_header_len = 0; + net->addr_len = 0; + net->tx_queue_len = 10; + + net->destructor = free_netdev; + net->hard_start_xmit = gprs_xmit; /* mandatory */ + net->change_mtu = gprs_set_mtu; + net->get_stats = gprs_get_stats; +} + +/* + * External interface + */ + +/* + * Attach a GPRS interface to a datagram socket. + * Returns the interface index on success, negative error code on error. + */ +int gprs_attach(struct sock *sk) +{ + static const char ifname[] = "gprs%d"; + struct gprs_dev *dev; + struct net_device *net; + int err; + + if (unlikely(sk->sk_type == SOCK_STREAM)) + return -EINVAL; /* need packet boundaries */ + + /* Create net device */ + net = alloc_netdev(sizeof(*dev), ifname, gprs_setup); + if (!net) + return -ENOMEM; + dev = netdev_priv(net); + dev->net = net; + dev->tx_max = 0; + spin_lock_init(&dev->tx_lock); + skb_queue_head_init(&dev->tx_queue); + INIT_WORK(&dev->tx_work, gprs_tx); + + netif_stop_queue(net); + err = register_netdev(net); + if (err) { + free_netdev(net); + return err; + } + + lock_sock(sk); + if (unlikely(sk->sk_user_data)) { + err = -EBUSY; + goto out_rel; + } + if (unlikely((1 << sk->sk_state & (TCPF_CLOSE|TCPF_LISTEN)) || + sock_flag(sk, SOCK_DEAD))) { + err = -EINVAL; + goto out_rel; + } + sk->sk_user_data = dev; + dev->old_state_change = sk->sk_state_change; + dev->old_data_ready = sk->sk_data_ready; + dev->old_write_space = sk->sk_write_space; + sk->sk_state_change = gprs_state_change; + sk->sk_data_ready = gprs_data_ready; + sk->sk_write_space = gprs_write_space; + release_sock(sk); + + sock_hold(sk); + dev->sk = sk; + + printk(KERN_DEBUG"%s: attached\n", net->name); + gprs_write_space(sk); /* kick off TX */ + return net->ifindex; + +out_rel: + release_sock(sk); + unregister_netdev(net); + return err; +} + +void gprs_detach(struct sock *sk) +{ + struct gprs_dev *dev = sk->sk_user_data; + struct net_device *net = dev->net; + + lock_sock(sk); + sk->sk_user_data = NULL; + sk->sk_state_change = dev->old_state_change; + sk->sk_data_ready = dev->old_data_ready; + sk->sk_write_space = dev->old_write_space; + release_sock(sk); + + printk(KERN_DEBUG"%s: detached\n", net->name); + unregister_netdev(net); + flush_scheduled_work(); + sock_put(sk); + skb_queue_purge(&dev->tx_queue); +} diff --git a/net/phonet/pep.c b/net/phonet/pep.c index d564d07cb797..bc6d50f83249 100644 --- a/net/phonet/pep.c +++ b/net/phonet/pep.c @@ -31,6 +31,7 @@ #include #include #include +#include /* sk_state values: * TCP_CLOSE sock not in use yet @@ -612,6 +613,7 @@ drop: static void pep_sock_close(struct sock *sk, long timeout) { struct pep_sock *pn = pep_sk(sk); + int ifindex = 0; sk_common_release(sk); @@ -625,7 +627,12 @@ static void pep_sock_close(struct sock *sk, long timeout) sk_del_node_init(sknode); sk->sk_state = TCP_CLOSE; } + ifindex = pn->ifindex; + pn->ifindex = 0; release_sock(sk); + + if (ifindex) + gprs_detach(sk); } static int pep_wait_connreq(struct sock *sk, int noblock) @@ -730,12 +737,107 @@ static int pep_init(struct sock *sk) return 0; } +static int pep_setsockopt(struct sock *sk, int level, int optname, + char __user *optval, int optlen) +{ + struct pep_sock *pn = pep_sk(sk); + int val = 0, err = 0; + + if (level != SOL_PNPIPE) + return -ENOPROTOOPT; + if (optlen >= sizeof(int)) { + if (get_user(val, (int __user *) optval)) + return -EFAULT; + } + + lock_sock(sk); + switch (optname) { + case PNPIPE_ENCAP: + if (val && val != PNPIPE_ENCAP_IP) { + err = -EINVAL; + break; + } + if (!pn->ifindex == !val) + break; /* Nothing to do! */ + if (!capable(CAP_NET_ADMIN)) { + err = -EPERM; + break; + } + if (val) { + release_sock(sk); + err = gprs_attach(sk); + if (err > 0) { + pn->ifindex = err; + err = 0; + } + } else { + pn->ifindex = 0; + release_sock(sk); + gprs_detach(sk); + err = 0; + } + goto out_norel; + default: + err = -ENOPROTOOPT; + } + release_sock(sk); + +out_norel: + return err; +} + +static int pep_getsockopt(struct sock *sk, int level, int optname, + char __user *optval, int __user *optlen) +{ + struct pep_sock *pn = pep_sk(sk); + int len, val; + + if (level != SOL_PNPIPE) + return -ENOPROTOOPT; + if (get_user(len, optlen)) + return -EFAULT; + + switch (optname) { + case PNPIPE_ENCAP: + val = pn->ifindex ? PNPIPE_ENCAP_IP : PNPIPE_ENCAP_NONE; + break; + case PNPIPE_IFINDEX: + val = pn->ifindex; + break; + default: + return -ENOPROTOOPT; + } + + len = min_t(unsigned int, sizeof(int), len); + if (put_user(len, optlen)) + return -EFAULT; + if (put_user(val, (int __user *) optval)) + return -EFAULT; + return 0; +} + +static int pipe_skb_send(struct sock *sk, struct sk_buff *skb) +{ + struct pep_sock *pn = pep_sk(sk); + struct pnpipehdr *ph; + + skb_push(skb, 3); + skb_reset_transport_header(skb); + ph = pnp_hdr(skb); + ph->utid = 0; + ph->message_id = PNS_PIPE_DATA; + ph->pipe_handle = pn->pipe_handle; + if (pn_flow_safe(pn->tx_fc) && pn->tx_credits) + pn->tx_credits--; + + return pn_skb_send(sk, skb, &pipe_srv); +} + static int pep_sendmsg(struct kiocb *iocb, struct sock *sk, struct msghdr *msg, size_t len) { struct pep_sock *pn = pep_sk(sk); struct sk_buff *skb = NULL; - struct pnpipehdr *ph; long timeo; int flags = msg->msg_flags; int err, done; @@ -801,16 +903,7 @@ disabled: if (err < 0) goto out; - __skb_push(skb, 3); - skb_reset_transport_header(skb); - ph = pnp_hdr(skb); - ph->utid = 0; - ph->message_id = PNS_PIPE_DATA; - ph->pipe_handle = pn->pipe_handle; - if (pn_flow_safe(pn->tx_fc)) /* credit-based flow control */ - pn->tx_credits--; - - err = pn_skb_send(sk, skb, &pipe_srv); + err = pipe_skb_send(sk, skb); if (err >= 0) err = len; /* success! */ skb = NULL; @@ -820,6 +913,50 @@ out: return err; } +int pep_writeable(struct sock *sk) +{ + struct pep_sock *pn = pep_sk(sk); + + return (sk->sk_state == TCP_ESTABLISHED) ? pn->tx_credits : 0; +} + +int pep_write(struct sock *sk, struct sk_buff *skb) +{ + struct sk_buff *rskb, *fs; + int flen = 0; + + rskb = alloc_skb(MAX_PNPIPE_HEADER, GFP_ATOMIC); + if (!rskb) { + kfree_skb(skb); + return -ENOMEM; + } + skb_shinfo(rskb)->frag_list = skb; + rskb->len += skb->len; + rskb->data_len += rskb->len; + rskb->truesize += rskb->len; + + /* Avoid nested fragments */ + for (fs = skb_shinfo(skb)->frag_list; fs; fs = fs->next) + flen += fs->len; + skb->next = skb_shinfo(skb)->frag_list; + skb_shinfo(skb)->frag_list = NULL; + skb->len -= flen; + skb->data_len -= flen; + skb->truesize -= flen; + + skb_reserve(rskb, MAX_PHONET_HEADER + 3); + return pipe_skb_send(sk, rskb); +} + +struct sk_buff *pep_read(struct sock *sk) +{ + struct sk_buff *skb = skb_dequeue(&sk->sk_receive_queue); + + if (sk->sk_state == TCP_ESTABLISHED) + pipe_grant_credits(sk); + return skb; +} + static int pep_recvmsg(struct kiocb *iocb, struct sock *sk, struct msghdr *msg, size_t len, int noblock, int flags, int *addr_len) @@ -902,6 +1039,8 @@ static struct proto pep_proto = { .accept = pep_sock_accept, .ioctl = pep_ioctl, .init = pep_init, + .setsockopt = pep_setsockopt, + .getsockopt = pep_getsockopt, .sendmsg = pep_sendmsg, .recvmsg = pep_recvmsg, .backlog_rcv = pep_do_rcv, diff --git a/net/phonet/socket.c b/net/phonet/socket.c index a9c3d1f2e9db..d81740187fb4 100644 --- a/net/phonet/socket.c +++ b/net/phonet/socket.c @@ -342,11 +342,11 @@ const struct proto_ops phonet_stream_ops = { .ioctl = pn_socket_ioctl, .listen = pn_socket_listen, .shutdown = sock_no_shutdown, - .setsockopt = sock_no_setsockopt, - .getsockopt = sock_no_getsockopt, + .setsockopt = sock_common_setsockopt, + .getsockopt = sock_common_getsockopt, #ifdef CONFIG_COMPAT - .compat_setsockopt = sock_no_setsockopt, - .compat_getsockopt = compat_sock_no_getsockopt, + .compat_setsockopt = compat_sock_common_setsockopt, + .compat_getsockopt = compat_sock_common_getsockopt, #endif .sendmsg = pn_socket_sendmsg, .recvmsg = sock_common_recvmsg, -- cgit v1.2.3