net: Add MHI support for RMC PegaPCI.
Raj Jayaraman [Sun, 16 Sep 2012 23:10:37 +0000 (16:10 -0700)]
* As submitted by RMC for modem support *

Bug 1054808

Change-Id: I37f027eaed75bddfdb4cec7dd03501f6749634e9
Signed-off-by: Raj Jayaraman <rjayaraman@nvidia.com>
Reviewed-on: http://git-master/r/160033
(cherry picked from commit 29bed237b4d4f7956f839411777d3855674d4bde)
Reviewed-on: http://git-master/r/162293
Reviewed-by: Automatic_Commit_Validation_User
Reviewed-by: WK Tsai <wtsai@nvidia.com>
Reviewed-by: Steve Lin <stlin@nvidia.com>

12 files changed:
net/Kconfig
net/Makefile
net/mhi/Kconfig [new file with mode: 0644]
net/mhi/Makefile [new file with mode: 0644]
net/mhi/l2mux.c [new file with mode: 0644]
net/mhi/l3mhdp.c [new file with mode: 0644]
net/mhi/l3mhi.c [new file with mode: 0644]
net/mhi/l3phonet.c [new file with mode: 0644]
net/mhi/mhi_dgram.c [new file with mode: 0644]
net/mhi/mhi_proto.c [new file with mode: 0644]
net/mhi/mhi_raw.c [new file with mode: 0644]
net/mhi/mhi_socket.c [new file with mode: 0644]

index 87fbda3..60a6d01 100644 (file)
@@ -223,6 +223,7 @@ source "drivers/net/appletalk/Kconfig"
 source "net/x25/Kconfig"
 source "net/lapb/Kconfig"
 source "net/phonet/Kconfig"
+source "net/mhi/Kconfig"
 source "net/ieee802154/Kconfig"
 source "net/mac802154/Kconfig"
 source "net/sched/Kconfig"
index 67d460a..6afdd27 100644 (file)
@@ -67,6 +67,7 @@ obj-$(CONFIG_WIMAX)           += wimax/
 obj-$(CONFIG_DNS_RESOLVER)     += dns_resolver/
 obj-$(CONFIG_CEPH_LIB)         += ceph/
 obj-$(CONFIG_BATMAN_ADV)       += batman-adv/
+obj-$(CONFIG_MHI)               += mhi/
 obj-$(CONFIG_NFC)              += nfc/
 obj-$(CONFIG_OPENVSWITCH)      += openvswitch/
 obj-$(CONFIG_VSOCKETS) += vmw_vsock/
diff --git a/net/mhi/Kconfig b/net/mhi/Kconfig
new file mode 100644 (file)
index 0000000..64965a7
--- /dev/null
@@ -0,0 +1,89 @@
+#
+# MHI protocol family and drivers
+#
+
+config MHI
+       bool "Modem-Host Interface"
+       default n
+       help
+         The Modem-Host Interface (MHI) is a packet-oriented transport protocol
+         developed by Renesas Mobile for use with their modems.
+
+         If unsure, say N.
+
+
+if MHI
+
+config MHI_L2MUX
+       tristate "L2 MUX Protocol Layer for MHI"
+       default y
+       help
+         L2 MUX is a protocol layer in the MHI stack. It is required
+         by the MHI L3 components.
+
+         To compile this driver as a module, choose M here: the module
+         will be called l2mux. If unsure, say Y.
+
+config MHI_L3MHI
+       tristate "L3 MHI Protocol Family (AF_MHI)"
+       select MHI_L2MUX
+       default y
+       help
+         AF_MHI provides datagram access to L2 channels in MHI,
+         developed by Renesas Mobile for use with their modems.
+
+         To compile this driver as a module, choose M here: the modules
+         will be called l3mhi and af_mhi. If unsure, say Y.
+
+config MHI_L3PHONET
+       tristate "L3 PHONET Protocol bridge (AF_PHONET)"
+       select MHI_L2MUX
+       select PHONET
+       default y
+       help
+         L3 PHONET protocol for MHI protocol family,
+         developed by Renesas Mobile for use with their modems.
+
+         This driver is a bridge between MHI L3 Phonet and Phonet Protocol Family.
+
+         To compile this driver as a module, choose M here: the module
+         will be called l3phonet. If unsure, say Y.
+
+config MHI_L3MHDP
+       tristate "L3 MHDP IP Tunneling Protocol"
+       select MHI_L2MUX
+       select INET_TUNNEL
+       default y
+       help
+         Tunneling means encapsulating data of one protocol type within
+         another protocol and sending it over a channel that understands the
+         encapsulating protocol. This particular tunneling driver implements
+         encapsulation of IP within MHDP (Modem Host Data Protocol), which
+         is used for communication between the APE and the Modem.
+
+         To compile this driver as a module, choose M here: the module
+         will be called l3mhdp. If unsure, say Y.
+
+
+config MHI_DEBUG
+       bool "MHI Debugging"
+       default n
+       help
+         Generate lots of debugging messages in the MHI stack.
+         This option is useful when developing MHI.
+         Otherwise it should be off.
+
+         If unsure, say N.
+
+config MHI_DUMP_FRAMES
+       bool "Dump MHI frames on L2 layer"
+       default n
+       help
+         Print out every frame passed through L2MUX into kernel log.
+         This option is useful when developing MHI. Otherwise it should be off.
+
+         If unsure, say N.
+
+
+endif
+
diff --git a/net/mhi/Makefile b/net/mhi/Makefile
new file mode 100644 (file)
index 0000000..64a2899
--- /dev/null
@@ -0,0 +1,10 @@
+
+obj-$(CONFIG_MHI_L3MHI)      += af_mhi.o
+
+af_mhi-objs                 := mhi_proto.o mhi_socket.o mhi_dgram.o mhi_raw.o
+
+obj-$(CONFIG_MHI_L2MUX)      += l2mux.o
+obj-$(CONFIG_MHI_L3MHI)      += l3mhi.o
+obj-$(CONFIG_MHI_L3MHDP)     += l3mhdp.o
+obj-$(CONFIG_MHI_L3PHONET)   += l3phonet.o
+
diff --git a/net/mhi/l2mux.c b/net/mhi/l2mux.c
new file mode 100644 (file)
index 0000000..b2bb0e4
--- /dev/null
@@ -0,0 +1,280 @@
+/*
+ * File: l2mux.c
+ *
+ * Modem-Host Interface (MHI) L2MUX layer
+ *
+ * Copyright (C) 2011 Renesas Mobile Corporation. All rights reserved.
+ *
+ * Author: Petri Mattila <petri.to.mattila@renesasmobile.com>
+ *
+ * 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 <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/if_mhi.h>
+#include <linux/mhi.h>
+#include <linux/l2mux.h>
+
+#include <net/af_mhi.h>
+
+#ifdef CONFIG_MHI_DEBUG
+# define DPRINTK(...)    printk(KERN_DEBUG "MHI/L2MUX: " __VA_ARGS__)
+#else
+# define DPRINTK(...)
+#endif
+
+
+/* Handle ONLY Non DIX types 0x00-0xff */
+#define ETH_NON_DIX_NPROTO   0x0100
+
+
+/* L2MUX master lock */
+static DEFINE_SPINLOCK(l2mux_lock);
+
+/* L3 ID -> RX function table */
+static l2mux_skb_fn *l2mux_id2rx_tab[MHI_L3_NPROTO] __read_mostly;
+
+/* Packet Type -> TX function table */
+static l2mux_skb_fn *l2mux_pt2tx_tab[ETH_NON_DIX_NPROTO] __read_mostly;
+
+
+int l2mux_netif_rx_register(int l3, l2mux_skb_fn *fn)
+{
+       int err = 0;
+
+       DPRINTK("l2mux_netif_rx_register(l3:%d, fn:%p)\n", l3, fn);
+
+       if (l3 < 0 || l3 >= MHI_L3_NPROTO)
+               return -EINVAL;
+
+       if (!fn)
+               return -EINVAL;
+
+       spin_lock(&l2mux_lock);
+       {
+               if (l2mux_id2rx_tab[l3] == NULL)
+                       l2mux_id2rx_tab[l3] = fn;
+               else
+                       err = -EBUSY;
+       }
+       spin_unlock(&l2mux_lock);
+
+       return err;
+}
+EXPORT_SYMBOL(l2mux_netif_rx_register);
+
+int l2mux_netif_rx_unregister(int l3)
+{
+       int err = 0;
+
+       DPRINTK("l2mux_netif_rx_unregister(l3:%d)\n", l3);
+
+       if (l3 < 0 || l3 >= MHI_L3_NPROTO)
+               return -EINVAL;
+
+       spin_lock(&l2mux_lock);
+       {
+               if (l2mux_id2rx_tab[l3])
+                       l2mux_id2rx_tab[l3] = NULL;
+               else
+                       err = -EPROTONOSUPPORT;
+       }
+       spin_unlock(&l2mux_lock);
+
+       return err;
+}
+EXPORT_SYMBOL(l2mux_netif_rx_unregister);
+
+int l2mux_netif_tx_register(int pt, l2mux_skb_fn *fn)
+{
+       int err = 0;
+
+       DPRINTK("l2mux_netif_tx_register(pt:%d, fn:%p)\n", pt, fn);
+
+       if (pt <= 0 || pt >= ETH_NON_DIX_NPROTO)
+               return -EINVAL;
+
+       if (!fn)
+               return -EINVAL;
+
+       spin_lock(&l2mux_lock);
+       {
+               if (l2mux_pt2tx_tab[pt] == NULL)
+                       l2mux_pt2tx_tab[pt] = fn;
+               else
+                       err = -EBUSY;
+       }
+       spin_unlock(&l2mux_lock);
+
+       return err;
+}
+EXPORT_SYMBOL(l2mux_netif_tx_register);
+
+int l2mux_netif_tx_unregister(int pt)
+{
+       int err = 0;
+
+       DPRINTK("l2mux_netif_tx_unregister(pt:%d)\n", pt);
+
+       if (pt <= 0 || pt >= ETH_NON_DIX_NPROTO)
+               return -EINVAL;
+
+       spin_lock(&l2mux_lock);
+       {
+               if (l2mux_pt2tx_tab[pt])
+                       l2mux_pt2tx_tab[pt] = NULL;
+               else
+                       err = -EPROTONOSUPPORT;
+       }
+       spin_unlock(&l2mux_lock);
+
+       return err;
+}
+EXPORT_SYMBOL(l2mux_netif_tx_unregister);
+
+int l2mux_skb_rx(struct sk_buff *skb, struct net_device *dev)
+{
+       struct l2muxhdr  *l2hdr;
+       unsigned          l3pid;
+       unsigned          l3len;
+       l2mux_skb_fn     *rxfn;
+
+       /* Set the device in the skb */
+       skb->dev = dev;
+
+       /* Set MAC header here */
+       skb_reset_mac_header(skb);
+
+       /* L2MUX header */
+       l2hdr = l2mux_hdr(skb);
+
+       /* proto id and length in L2 header */
+       l3pid = l2mux_get_proto(l2hdr);
+       l3len = l2mux_get_length(l2hdr);
+
+       DPRINTK("L2MUX: RX dev:%d skb_len:%d l3_len:%d l3_pid:%d\n",
+                      skb->dev->ifindex, skb->len, l3len, l3pid);
+
+#ifdef CONFIG_MHI_DUMP_FRAMES
+       {
+               u8 *ptr = skb->data;
+               int len = skb_headlen(skb);
+               int i;
+
+               printk(KERN_DEBUG "L2MUX: RX dev:%d skb_len:%d l3_len:%d l3_pid:%d\n",
+                      dev->ifindex, skb->len, l3len, l3pid);
+
+               for (i = 0; i < len; i++) {
+                       if (i%8 == 0)
+                               printk(KERN_DEBUG "L2MUX: RX [%04X] ", i);
+                       printk(" 0x%02X", ptr[i]);
+                       if (i%8 == 7 || i == len-1)
+                               printk("\n");
+               }
+       }
+#endif
+       /* check that the advertised length is correct */
+       if (l3len != skb->len - L2MUX_HDR_SIZE) {
+               printk(KERN_WARNING "L2MUX: l2mux_skb_rx: L3_id:%d - skb length mismatch L3:%d (+4) <> SKB:%d",
+                      l3pid, l3len, skb->len);
+               goto drop;
+       }
+
+       /* get RX function */
+       rxfn = l2mux_id2rx_tab[l3pid];
+
+       /* Not registered */
+       if (!rxfn)
+               goto drop;
+
+       /* Call the receiver function */
+       return rxfn(skb, dev);
+
+drop:
+       kfree_skb(skb);
+       return NET_RX_DROP;
+}
+EXPORT_SYMBOL(l2mux_skb_rx);
+
+int l2mux_skb_tx(struct sk_buff *skb, struct net_device *dev)
+{
+       l2mux_skb_fn *txfn;
+       unsigned type;
+
+       /* Packet type ETH_P_XXX */
+       type = ntohs(skb->protocol);
+
+#ifdef CONFIG_MHI_DUMP_FRAMES
+       {
+               u8 *ptr = skb->data;
+               int len = skb_headlen(skb);
+               int i;
+
+               printk(KERN_DEBUG "L2MUX: TX dev:%d skb_len:%d ETH_P:%d\n",
+                      dev->ifindex, skb->len, type);
+
+               for (i = 0; i < len; i++) {
+                       if (i%8 == 0)
+                               printk(KERN_DEBUG "L2MUX: TX [%04X] ", i);
+                       printk(" 0x%02X", ptr[i]);
+                       if (i%8 == 7 || i == len-1)
+                               printk("\n");
+               }
+       }
+#endif
+       /* Only handling non DIX types */
+       if (type <= 0 || type >= ETH_NON_DIX_NPROTO)
+               return -EINVAL;
+
+       /* TX function for this packet type */
+       txfn = l2mux_pt2tx_tab[type];
+
+       if (txfn)
+               return txfn(skb, dev);
+
+       return 0;
+}
+EXPORT_SYMBOL(l2mux_skb_tx);
+
+static int __init l2mux_init(void)
+{
+       int i;
+
+       DPRINTK("l2mux_init\n");
+
+       for (i = 0; i < MHI_L3_NPROTO; i++)
+               l2mux_id2rx_tab[i] = NULL;
+
+       for (i = 0; i < ETH_NON_DIX_NPROTO; i++)
+               l2mux_pt2tx_tab[i] = NULL;
+
+       return 0;
+}
+
+static void __exit l2mux_exit(void)
+{
+       DPRINTK("l2mux_exit\n");
+}
+
+module_init(l2mux_init);
+module_exit(l2mux_exit);
+
+MODULE_AUTHOR("Petri Mattila <petri.to.mattila@renesasmobile.com>");
+MODULE_DESCRIPTION("L2MUX for MHI Protocol Stack");
+MODULE_LICENSE("GPL");
+
diff --git a/net/mhi/l3mhdp.c b/net/mhi/l3mhdp.c
new file mode 100644 (file)
index 0000000..e3adb37
--- /dev/null
@@ -0,0 +1,832 @@
+/*
+ * File: l3mhdp.c
+ *
+ * MHDP - Modem Host Data Protocol for MHI protocol family.
+ *
+ * Copyright (C) 2011 Renesas Mobile Corporation. All rights reserved.
+ *
+ * Author:     Sugnan Prabhu S <sugnan.prabhu@renesasmobile.com>
+ *             Petri Mattila <petri.to.mattila@renesasmobile.com>
+ *
+ * Based on work by: Sam Lantinga (slouken@cs.ucdavis.edu)
+ *
+ * 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 <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/version.h>
+
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/l2mux.h>
+#include <linux/etherdevice.h>
+#include <linux/pkt_sched.h>
+
+#include <net/netns/generic.h>
+#include <net/mhi/mhdp.h>
+
+
+/* MHDP device MTU limits */
+#define MHDP_MTU_MAX           0x2400
+#define MHDP_MTU_MIN           0x44
+
+/* MHDP device names */
+#define MHDP_IFNAME            "rmnet%d"
+#define MHDP_CTL_IFNAME                "rmnetctl"
+
+/* Print every MHDP SKB content */
+/*#define MHDP_DEBUG_SKB*/
+
+
+#define EPRINTK(...)    printk(KERN_DEBUG "MHI/MHDP: " __VA_ARGS__)
+
+#ifdef CONFIG_MHI_DEBUG
+# define DPRINTK(...)    printk(KERN_DEBUG "MHI/MHDP: " __VA_ARGS__)
+#else
+# define DPRINTK(...)
+#endif
+
+#ifdef MHDP_DEBUG_SKB
+# define SKBPRINT(a, b)    __print_skb_content(a, b)
+#else
+# define SKBPRINT(a, b)
+#endif
+
+/* IPv6 support */
+#define VER_IPv4 0x04
+#define VER_IPv6 0x06
+#define ETH_IP_TYPE(x) (((0x00|(x>>4)) == VER_IPv4) ? ETH_P_IP : ETH_P_IPV6)
+
+int sysctl_mhdp_concat_nb_pkt __read_mostly;
+EXPORT_SYMBOL(sysctl_mhdp_concat_nb_pkt);
+
+/*** Type definitions ***/
+
+#define MAX_MHDPHDR_SIZE 12
+
+struct mhdp_tunnel {
+       struct mhdp_tunnel      *next;
+       struct net_device       *dev;
+       struct net_device       *master_dev;
+       struct sk_buff          *skb;
+       int pdn_id;
+       struct timer_list tx_timer;
+       struct sk_buff *skb_to_free[MAX_MHDPHDR_SIZE];
+       spinlock_t timer_lock;
+};
+
+struct mhdp_net {
+       struct mhdp_tunnel      *tunnels;
+       struct net_device       *ctl_dev;
+};
+
+struct packet_info {
+       uint32_t pdn_id;
+       uint32_t packet_offset;
+       uint32_t packet_length;
+};
+
+struct mhdp_hdr {
+       uint32_t packet_count;
+       struct packet_info info[MAX_MHDPHDR_SIZE];
+};
+
+
+/*** Prototypes ***/
+
+static void mhdp_netdev_setup(struct net_device *dev);
+
+static void mhdp_submit_queued_skb(struct mhdp_tunnel *tunnel);
+
+static int mhdp_netdev_event(struct notifier_block *this,
+                            unsigned long event, void *ptr);
+
+static void tx_timer_timeout(unsigned long arg);
+
+/*** Global Variables ***/
+
+static int  mhdp_net_id __read_mostly;
+
+static struct notifier_block mhdp_netdev_notifier = {
+       .notifier_call = mhdp_netdev_event,
+};
+
+/*** Funtions ***/
+
+#ifdef MHDP_DEBUG_SKB
+static void
+__print_skb_content(struct sk_buff *skb, const char *tag)
+{
+       struct page *page;
+       skb_frag_t *frag;
+       int len;
+       int i, j;
+       u8 *ptr;
+
+       /* Main SKB buffer */
+       ptr = (u8 *)skb->data;
+       len = skb_headlen(skb);
+
+       printk(KERN_DEBUG "MHDP: SKB buffer lenght %02u\n", len);
+       for (i = 0; i < len; i++) {
+               if (i%8 == 0)
+                       printk(KERN_DEBUG "%s DATA: ", tag);
+               printk(" 0x%02X", ptr[i]);
+               if (i%8 == 7 || i == len - 1)
+                       printk("\n");
+       }
+
+       /* SKB fragments */
+       for (i = 0; i < (skb_shinfo(skb)->nr_frags); i++) {
+               frag = &skb_shinfo(skb)->frags[i];
+               page = skb_frag_page(frag);
+
+               ptr = page_address(page);
+
+               for (j = 0; j < frag->size; j++) {
+                       if (j%8 == 0)
+                               printk(KERN_DEBUG "%s FRAG[%d]: ", tag, i);
+                       printk(" 0x%02X", ptr[frag->page_offset + j]);
+                       if (j%8 == 7 || j == frag->size - 1)
+                               printk("\n");
+               }
+       }
+}
+#endif
+
+
+static inline struct mhdp_net *
+mhdp_net_dev(struct net_device *dev)
+{
+       return net_generic(dev_net(dev), mhdp_net_id);
+}
+
+static void
+mhdp_tunnel_init(struct net_device *dev,
+                struct mhdp_tunnel_parm *parms,
+                struct net_device *master_dev)
+{
+       struct mhdp_net *mhdpn = mhdp_net_dev(dev);
+       struct mhdp_tunnel *tunnel = netdev_priv(dev);
+
+       DPRINTK("mhdp_tunnel_init: dev:%s", dev->name);
+
+       tunnel->next = mhdpn->tunnels;
+       mhdpn->tunnels = tunnel;
+
+       tunnel->dev         = dev;
+       tunnel->master_dev  = master_dev;
+       tunnel->skb         = NULL;
+       tunnel->pdn_id      = parms->pdn_id;
+
+       init_timer(&tunnel->tx_timer);
+       spin_lock_init(&tunnel->timer_lock);
+}
+
+static void
+mhdp_tunnel_destroy(struct net_device *dev)
+{
+       DPRINTK("mhdp_tunnel_destroy: dev:%s", dev->name);
+
+       unregister_netdevice(dev);
+}
+
+static void
+mhdp_destroy_tunnels(struct mhdp_net *mhdpn)
+{
+       struct mhdp_tunnel *tunnel;
+
+       for (tunnel = mhdpn->tunnels; (tunnel); tunnel = tunnel->next)
+               mhdp_tunnel_destroy(tunnel->dev);
+
+       mhdpn->tunnels = NULL;
+}
+
+static struct mhdp_tunnel *
+mhdp_locate_tunnel(struct mhdp_net *mhdpn, int pdn_id)
+{
+       struct mhdp_tunnel *tunnel;
+
+       for (tunnel = mhdpn->tunnels; tunnel; tunnel = tunnel->next)
+               if (tunnel->pdn_id == pdn_id)
+                       return tunnel;
+
+       return NULL;
+}
+
+static struct net_device *
+mhdp_add_tunnel(struct net *net, struct mhdp_tunnel_parm *parms)
+{
+       struct net_device *mhdp_dev, *master_dev;
+
+       DPRINTK("mhdp_add_tunnel: adding a tunnel to %s\n", parms->master);
+
+       master_dev = dev_get_by_name(net, parms->master);
+       if (!master_dev)
+               goto err_alloc_dev;
+
+       mhdp_dev = alloc_netdev(sizeof(struct mhdp_tunnel),
+                               MHDP_IFNAME, mhdp_netdev_setup);
+       if (!mhdp_dev)
+               goto err_alloc_dev;
+
+       dev_net_set(mhdp_dev, net);
+
+       if (dev_alloc_name(mhdp_dev, MHDP_IFNAME) < 0)
+               goto err_reg_dev;
+
+       strcpy(parms->name, mhdp_dev->name);
+
+       if (register_netdevice(mhdp_dev)) {
+               printk(KERN_ERR "MHDP: register_netdev failed\n");
+               goto err_reg_dev;
+       }
+
+       dev_hold(mhdp_dev);
+
+       mhdp_tunnel_init(mhdp_dev, parms, master_dev);
+
+       mhdp_dev->flags    |= IFF_SLAVE;
+       master_dev->flags  |= IFF_MASTER;
+
+       dev_put(master_dev);
+
+       return mhdp_dev;
+
+err_reg_dev:
+       free_netdev(mhdp_dev);
+err_alloc_dev:
+       return NULL;
+}
+
+
+static int
+mhdp_netdev_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+       struct net *net = dev_net(dev);
+       struct mhdp_net *mhdpn = mhdp_net_dev(dev);
+       struct mhdp_tunnel *tunnel, *pre_dev;
+       struct mhdp_tunnel_parm __user *u_parms;
+       struct mhdp_tunnel_parm k_parms;
+
+       int err = 0;
+
+       DPRINTK("mhdp tunnel ioctl %X", cmd);
+
+       switch (cmd) {
+
+       case SIOCADDPDNID:
+               u_parms = (struct mhdp_tunnel_parm *)ifr->ifr_data;
+               if (copy_from_user(&k_parms, u_parms,
+                                  sizeof(struct mhdp_tunnel_parm))) {
+                       DPRINTK("Error: Failed to copy data from user space");
+                       return -EFAULT;
+               }
+
+               DPRINTK("pdn_id:%d master_device:%s", k_parms.pdn_id,
+                                                       k_parms.master);
+
+               if (!mhdp_locate_tunnel(mhdpn, k_parms.pdn_id)) {
+                       if (mhdp_add_tunnel(net, &k_parms)) {
+                               if (copy_to_user(u_parms, &k_parms,
+                                        sizeof(struct mhdp_tunnel_parm)))
+                                       err = -EINVAL;
+                       } else {
+                               err = -EINVAL;
+                       }
+               } else {
+                       err = -EBUSY;
+               }
+               break;
+
+       case SIOCDELPDNID:
+               u_parms = (struct mhdp_tunnel_parm *)ifr->ifr_data;
+               if (copy_from_user(&k_parms, u_parms,
+                                       sizeof(struct mhdp_tunnel_parm))) {
+                       DPRINTK("Error: Failed to copy data from user space");
+                       return -EFAULT;
+               }
+
+               DPRINTK("pdn_id:%d", k_parms.pdn_id);
+
+               for (tunnel = mhdpn->tunnels, pre_dev = NULL;
+                       tunnel;
+                       pre_dev = tunnel, tunnel = tunnel->next) {
+                       if (tunnel->pdn_id == k_parms.pdn_id) {
+                               if (!pre_dev)
+                                       mhdpn->tunnels = mhdpn->tunnels->next;
+                               else
+                                       pre_dev->next = tunnel->next;
+
+                               mhdp_tunnel_destroy(tunnel->dev);
+                       }
+               }
+               break;
+
+       case SIOCRESETMHDP:
+               mhdp_destroy_tunnels(mhdpn);
+               break;
+
+       default:
+               err = -EINVAL;
+       }
+
+       return err;
+}
+
+static int
+mhdp_netdev_change_mtu(struct net_device *dev, int new_mtu)
+{
+       if (new_mtu < MHDP_MTU_MIN || new_mtu > MHDP_MTU_MAX)
+               return -EINVAL;
+
+       dev->mtu = new_mtu;
+
+       return 0;
+}
+
+static void
+mhdp_netdev_uninit(struct net_device *dev)
+{
+       dev_put(dev);
+}
+
+
+static void
+mhdp_submit_queued_skb(struct mhdp_tunnel *tunnel)
+{
+       struct sk_buff *skb = tunnel->skb;
+       struct l2muxhdr *l2hdr;
+       struct mhdp_hdr *mhdpHdr;
+       int i, nb_frags;
+
+       BUG_ON(!tunnel->master_dev);
+
+       if (skb) {
+               mhdpHdr = (struct mhdp_hdr *)tunnel->skb->data;
+               nb_frags = mhdpHdr->packet_count;
+
+               skb->protocol = htons(ETH_P_MHDP);
+               skb->priority = 1;
+
+               skb->dev = tunnel->master_dev;
+
+               skb_reset_network_header(skb);
+
+               skb_push(skb, L2MUX_HDR_SIZE);
+               skb_reset_mac_header(skb);
+
+               l2hdr = l2mux_hdr(skb);
+               l2mux_set_proto(l2hdr, MHI_L3_MHDP_UL);
+               l2mux_set_length(l2hdr, skb->len - L2MUX_HDR_SIZE);
+
+               SKBPRINT(skb, "MHDP: TX");
+
+               tunnel->dev->stats.tx_packets++;
+               tunnel->skb = NULL;
+
+               dev_queue_xmit(skb);
+
+               for (i = 0; i < nb_frags; i++)
+                       dev_kfree_skb(tunnel->skb_to_free[i]);
+       }
+}
+
+static int
+mhdp_netdev_rx(struct sk_buff *skb, struct net_device *dev)
+{
+       skb_frag_t *frag = NULL;
+       struct page *page = NULL;
+       struct sk_buff *newskb;
+       struct mhdp_hdr *mhdpHdr;
+       int offset, length;
+       int err = 0, i, pdn_id;
+       int mhdp_header_len;
+       struct mhdp_tunnel *tunnel = NULL;
+       int start = 0;
+       int has_frag = skb_shinfo(skb)->nr_frags;
+       uint32_t packet_count;
+       unsigned char ip_ver;
+
+       if (has_frag) {
+               frag = &skb_shinfo(skb)->frags[0];
+               page = skb_frag_page(frag);
+       }
+
+       if (skb_headlen(skb) > L2MUX_HDR_SIZE)
+               skb_pull(skb, L2MUX_HDR_SIZE);
+       else if (has_frag)
+               frag->page_offset += L2MUX_HDR_SIZE;
+
+       packet_count = *((unsigned char *)skb->data);
+
+       mhdp_header_len = sizeof(packet_count) +
+               (packet_count * sizeof(struct packet_info));
+
+       if (mhdp_header_len > skb_headlen(skb)) {
+               int skbheadlen = skb_headlen(skb);
+
+               DPRINTK("mhdp header length: %d, skb_headerlen: %d",
+                               mhdp_header_len, skbheadlen);
+
+               mhdpHdr = kmalloc(mhdp_header_len, GFP_ATOMIC);
+               if (mhdpHdr == NULL) {
+                       printk(KERN_ERR "%s: kmalloc failed.\n", __func__);
+                       return err;
+               }
+
+               if (skbheadlen == 0) {
+                       memcpy((__u8 *)mhdpHdr, page_address(page) +
+                                               frag->page_offset,
+                                               mhdp_header_len);
+
+               } else {
+                       memcpy((__u8 *)mhdpHdr, skb->data, skbheadlen);
+
+                       memcpy((__u8 *)mhdpHdr + skbheadlen,
+                              page_address(page) +
+                              frag->page_offset,
+                              mhdp_header_len - skbheadlen);
+
+                       start = mhdp_header_len - skbheadlen;
+               }
+
+               DPRINTK("page start: %d", start);
+       } else {
+               DPRINTK("skb->data has whole mhdp header");
+               mhdpHdr = (struct mhdp_hdr *)(((__u8 *)skb->data));
+       }
+
+       DPRINTK("MHDP PACKET COUNT : %d",  mhdpHdr->packet_count);
+
+       rcu_read_lock();
+
+       for (i = 0; i < mhdpHdr->packet_count; i++) {
+
+               DPRINTK(" packet_info[%d] - PDNID:%d, packet_offset: %d,
+                       packet_length: %d\n", i, mhdpHdr->info[i].pdn_id,
+                       mhdpHdr->info[i].packet_offset,
+                       mhdpHdr->info[i].packet_length);
+
+               pdn_id = mhdpHdr->info[i].pdn_id;
+               offset = mhdpHdr->info[i].packet_offset;
+               length = mhdpHdr->info[i].packet_length;
+
+               if (skb_headlen(skb) > (mhdp_header_len + offset)) {
+
+                       newskb = skb_clone(skb, GFP_ATOMIC);
+                       if (unlikely(!newskb))
+                               goto error;
+
+                       skb_pull(newskb, mhdp_header_len + offset);
+                       ip_ver = (u8)*newskb->data;
+
+               } else if (has_frag) {
+
+                       newskb = netdev_alloc_skb(dev, skb_headlen(skb));
+
+                       if (unlikely(!newskb))
+                               goto error;
+
+                       get_page(page);
+                       skb_add_rx_frag(newskb, skb_shinfo(newskb)->nr_frags,
+                       page,
+                       frag->page_offset +
+                       ((mhdp_header_len - skb_headlen(skb)) + offset),
+                       length, PAGE_SIZE);
+
+                       ip_ver = *((unsigned long *)page_address(page) +
+                       (frag->page_offset +
+                       ((mhdp_header_len - skb_headlen(skb)) + offset)));
+
+                       if ((ip_ver>>4) != VER_IPv4 &&
+                               (ip_ver>>4) != VER_IPv6)
+                               goto error;
+
+               } else {
+                       DPRINTK("Error in the data received");
+                       goto error;
+               }
+
+               skb_reset_network_header(newskb);
+
+               /* IPv6 Support - Check the IP version and set
+               ETH_P_IP or ETH_P_IPv6 for received packets */
+               newskb->protocol = htons(ETH_IP_TYPE(ip_ver));
+
+               newskb->pkt_type = PACKET_HOST;
+
+               skb_tunnel_rx(newskb, dev);
+
+               tunnel = mhdp_locate_tunnel(mhdp_net_dev(dev), pdn_id);
+               if (tunnel) {
+                       struct net_device_stats *stats = &tunnel->dev->stats;
+                       stats->rx_packets++;
+                       newskb->dev = tunnel->dev;
+                       SKBPRINT(newskb, "NEWSKB: RX");
+                       netif_rx(newskb);
+               }
+       }
+       rcu_read_unlock();
+
+error:
+       if (mhdp_header_len > skb_headlen(skb))
+               kfree(mhdpHdr);
+
+       dev_kfree_skb(skb);
+
+       return err;
+}
+
+static void tx_timer_timeout(unsigned long arg)
+{
+       struct mhdp_tunnel *tunnel = (struct mhdp_tunnel *) arg;
+
+       spin_lock(&tunnel->timer_lock);
+
+       mhdp_submit_queued_skb(tunnel);
+
+       spin_unlock(&tunnel->timer_lock);
+}
+
+
+static int
+mhdp_netdev_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+       struct mhdp_hdr *mhdpHdr;
+       struct mhdp_tunnel *tunnel = netdev_priv(dev);
+       struct net_device_stats *stats = &tunnel->dev->stats;
+       struct page *page = NULL;
+       int i;
+       int packet_count, offset, len;
+
+       spin_lock(&tunnel->timer_lock);
+
+       SKBPRINT(skb, "SKB: TX");
+
+       if (timer_pending(&tunnel->tx_timer))
+               del_timer(&tunnel->tx_timer);
+
+       if (tunnel->skb == NULL) {
+               tunnel->skb = netdev_alloc_skb(dev,
+                       L2MUX_HDR_SIZE + sizeof(struct mhdp_hdr) + ETH_HLEN);
+
+               if (!tunnel->skb) {
+                       EPRINTK("mhdp_netdev_xmit error1");
+                       BUG();
+               }
+
+               /* Place holder for the mhdp packet count */
+               len = skb_headroom(tunnel->skb) - L2MUX_HDR_SIZE - ETH_HLEN;
+
+               skb_push(tunnel->skb, len);
+               len -= 4;
+
+               memset(tunnel->skb->data, 0, len);
+
+               /*
+                * Need to replace following logic, with something better like
+                * __pskb_pull_tail or pskb_may_pull(tunnel->skb, len);
+                */
+               {
+                       tunnel->skb->tail -= len;
+                       tunnel->skb->len  -= len;
+               }
+
+
+               mhdpHdr = (struct mhdp_hdr *)tunnel->skb->data;
+               mhdpHdr->packet_count = 0;
+       }
+
+       /*
+        * skb_put cannot be called as the (data_len != 0)
+        */
+       {
+               tunnel->skb->tail += sizeof(struct packet_info);
+               tunnel->skb->len  += sizeof(struct packet_info);
+
+               DPRINTK("new - skb->tail:%lu skb->end:%lu skb->data_len:%lu",
+                               (unsigned long)tunnel->skb->tail,
+                               (unsigned long)tunnel->skb->end,
+                               (unsigned long)tunnel->skb->data_len);
+       }
+
+       mhdpHdr = (struct mhdp_hdr *)tunnel->skb->data;
+
+       tunnel->skb_to_free[mhdpHdr->packet_count] = skb;
+
+       packet_count = mhdpHdr->packet_count;
+       mhdpHdr->info[packet_count].pdn_id = tunnel->pdn_id;
+       if (packet_count == 0) {
+               mhdpHdr->info[packet_count].packet_offset = 0;
+       } else {
+               mhdpHdr->info[packet_count].packet_offset =
+                       mhdpHdr->info[packet_count - 1].packet_offset +
+                       mhdpHdr->info[packet_count - 1].packet_length;
+       }
+
+       mhdpHdr->info[packet_count].packet_length = skb->len;
+       mhdpHdr->packet_count++;
+
+       page = virt_to_page(skb->data);
+
+       if (page == NULL) {
+               EPRINTK("kmap_atomic_to_page returns NULL");
+               goto tx_error;
+       }
+
+       get_page(page);
+
+       offset = ((unsigned long)skb->data -
+                 (unsigned long)page_address(page));
+
+       skb_add_rx_frag(tunnel->skb, skb_shinfo(tunnel->skb)->nr_frags,
+                       page, offset, skb_headlen(skb), PAGE_SIZE);
+
+       if (skb_shinfo(skb)->nr_frags) {
+               for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+                       skb_frag_t *frag = &skb_shinfo(tunnel->skb)->frags[i];
+                       get_page(skb_frag_page(frag));
+                       skb_add_rx_frag(tunnel->skb,
+                                       skb_shinfo(tunnel->skb)->nr_frags,
+                                       skb_frag_page(frag), frag->page_offset,
+                                       frag->size, PAGE_SIZE);
+               }
+       }
+
+       if (mhdpHdr->packet_count == MAX_MHDPHDR_SIZE) {
+               mhdp_submit_queued_skb(tunnel);
+       } else {
+           tunnel->tx_timer.function = &tx_timer_timeout;
+           tunnel->tx_timer.data     = (unsigned long) tunnel;
+           tunnel->tx_timer.expires = jiffies + ((HZ + 999) / 1000) ;
+           add_timer(&tunnel->tx_timer);
+       }
+
+       spin_unlock(&tunnel->timer_lock);
+       return NETDEV_TX_OK;
+
+tx_error:
+       spin_unlock(&tunnel->timer_lock);
+       stats->tx_errors++;
+       dev_kfree_skb(skb);
+       return NETDEV_TX_OK;
+}
+
+
+static int
+mhdp_netdev_event(struct notifier_block *this, unsigned long event, void *ptr)
+{
+       struct net_device *event_dev = (struct net_device *)ptr;
+
+       DPRINTK("event_dev: %s, event: %lx\n",
+               event_dev ? event_dev->name : "None", event);
+
+       switch (event) {
+       case NETDEV_UNREGISTER:
+       {
+               struct mhdp_net *mhdpn = mhdp_net_dev(event_dev);
+               struct mhdp_tunnel *iter, *prev;
+
+               DPRINTK("event_dev: %s, event: %lx\n",
+                       event_dev ? event_dev->name : "None", event);
+
+               for (iter = mhdpn->tunnels, prev = NULL;
+                       iter; prev = iter, iter = iter->next) {
+                       if (event_dev == iter->master_dev) {
+                               if (!prev)
+                                       mhdpn->tunnels = mhdpn->tunnels->next;
+                               else
+                                       prev->next = iter->next;
+                               mhdp_tunnel_destroy(iter->dev);
+                       }
+               }
+       }
+       break;
+       }
+
+       return NOTIFY_DONE;
+}
+
+static const struct net_device_ops mhdp_netdev_ops = {
+       .ndo_uninit     = mhdp_netdev_uninit,
+       .ndo_start_xmit = mhdp_netdev_xmit,
+       .ndo_do_ioctl   = mhdp_netdev_ioctl,
+       .ndo_change_mtu = mhdp_netdev_change_mtu,
+};
+
+static void mhdp_netdev_setup(struct net_device *dev)
+{
+       dev->netdev_ops         = &mhdp_netdev_ops;
+       dev->destructor         = free_netdev;
+
+       dev->type               = ARPHRD_TUNNEL;
+       dev->hard_header_len    = L2MUX_HDR_SIZE + sizeof(struct mhdp_hdr);
+       dev->mtu                = ETH_DATA_LEN;
+       dev->flags              = IFF_NOARP;
+       dev->iflink             = 0;
+       dev->addr_len           = 4;
+       dev->features          |= (NETIF_F_NETNS_LOCAL | NETIF_F_FRAGLIST);
+}
+
+static int __net_init mhdp_init_net(struct net *net)
+{
+       struct mhdp_net *mhdpn = net_generic(net, mhdp_net_id);
+       int err;
+
+       mhdpn->tunnels = NULL;
+
+       mhdpn->ctl_dev = alloc_netdev(sizeof(struct mhdp_tunnel),
+                                     MHDP_CTL_IFNAME,
+                                     mhdp_netdev_setup);
+       if (!mhdpn->ctl_dev)
+               return -ENOMEM;
+
+       dev_net_set(mhdpn->ctl_dev, net);
+       dev_hold(mhdpn->ctl_dev);
+
+       err = register_netdev(mhdpn->ctl_dev);
+       if (err) {
+               printk(KERN_ERR MHDP_CTL_IFNAME " register failed");
+               free_netdev(mhdpn->ctl_dev);
+               return err;
+       }
+
+       return 0;
+}
+
+static void __net_exit mhdp_exit_net(struct net *net)
+{
+       struct mhdp_net *mhdpn = net_generic(net, mhdp_net_id);
+
+       rtnl_lock();
+       mhdp_destroy_tunnels(mhdpn);
+       unregister_netdevice(mhdpn->ctl_dev);
+       rtnl_unlock();
+}
+
+static struct pernet_operations mhdp_net_ops = {
+       .init = mhdp_init_net,
+       .exit = mhdp_exit_net,
+       .id   = &mhdp_net_id,
+       .size = sizeof(struct mhdp_net),
+};
+
+
+static int __init mhdp_init(void)
+{
+       int err;
+
+       err = l2mux_netif_rx_register(MHI_L3_MHDP_DL, mhdp_netdev_rx);
+       if (err)
+               goto rollback0;
+
+       err = register_pernet_device(&mhdp_net_ops);
+       if (err < 0)
+               goto rollback1;
+
+       err = register_netdevice_notifier(&mhdp_netdev_notifier);
+       if (err < 0)
+               goto rollback2;
+
+       return 0;
+
+rollback2:
+       unregister_pernet_device(&mhdp_net_ops);
+rollback1:
+       l2mux_netif_rx_unregister(MHI_L3_MHDP_DL);
+rollback0:
+       return err;
+}
+
+static void __exit mhdp_exit(void)
+{
+       l2mux_netif_rx_unregister(MHI_L3_MHDP_DL);
+       unregister_netdevice_notifier(&mhdp_netdev_notifier);
+       unregister_pernet_device(&mhdp_net_ops);
+}
+
+
+module_init(mhdp_init);
+module_exit(mhdp_exit);
+
+MODULE_AUTHOR("Sugnan Prabhu S <sugnan.prabhu@renesasmobile.com>");
+MODULE_DESCRIPTION("Modem Host Data Protocol for MHI");
+MODULE_LICENSE("GPL");
+
diff --git a/net/mhi/l3mhi.c b/net/mhi/l3mhi.c
new file mode 100644 (file)
index 0000000..086bad0
--- /dev/null
@@ -0,0 +1,132 @@
+/*
+ * File: l3mhi.c
+ *
+ * L2 channels to AF_MHI binding.
+ *
+ * Copyright (C) 2011 Renesas Mobile Corporation. All rights reserved.
+ *
+ * Author: Petri To Mattila <petri.to.mattila@renesasmobile.com>
+ *
+ * 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 <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/socket.h>
+#include <linux/mhi.h>
+#include <linux/l2mux.h>
+
+#include <net/af_mhi.h>
+#include <net/mhi/sock.h>
+#include <net/mhi/dgram.h>
+
+#define MAX_CHANNELS  256
+
+#ifdef CONFIG_MHI_DEBUG
+# define DPRINTK(...)    printk(KERN_DEBUG "L3MHI: " __VA_ARGS__)
+#else
+# define DPRINTK(...)
+#endif
+
+
+/* Module parameters - with defaults */
+static int l2chs[MAX_CHANNELS] = {
+       MHI_L3_FILE,
+       MHI_L3_XFILE,
+       MHI_L3_SECURITY,
+       MHI_L3_TEST,
+       MHI_L3_TEST_PRIO,
+       MHI_L3_THERMAL,
+       MHI_L3_HIGH_PRIO_TEST,
+       MHI_L3_MED_PRIO_TEST,
+       MHI_L3_LOW_PRIO_TEST
+};
+static int l2cnt = 9;
+
+
+
+/* Functions */
+
+static int
+mhi_netif_rx(struct sk_buff *skb, struct net_device *dev)
+{
+       skb->protocol = htons(ETH_P_MHI);
+
+       return netif_rx(skb);
+}
+
+
+/* Module registration */
+
+int __init l3mhi_init(void)
+{
+       int ch, i;
+       int err;
+
+       for (i = 0; i < l2cnt; i++) {
+               ch = l2chs[i];
+               if (ch >= 0 && ch < MHI_L3_NPROTO) {
+                       err = l2mux_netif_rx_register(ch, mhi_netif_rx);
+                       if (err)
+                               goto error;
+
+                       err = mhi_register_protocol(ch);
+                       if (err)
+                               goto error;
+               }
+       }
+
+       return 0;
+
+error:
+       for (i = 0; i < l2cnt; i++) {
+               ch = l2chs[i];
+               if (ch >= 0 && ch < MHI_L3_NPROTO) {
+                       if (mhi_protocol_registered(ch)) {
+                               l2mux_netif_rx_unregister(ch);
+                               mhi_unregister_protocol(ch);
+                       }
+               }
+       }
+
+       return err;
+}
+
+void __exit l3mhi_exit(void)
+{
+       int ch, i;
+
+       for (i = 0; i < l2cnt; i++) {
+               ch = l2chs[i];
+               if (ch >= 0 && ch < MHI_L3_NPROTO) {
+                       if (mhi_protocol_registered(ch)) {
+                               l2mux_netif_rx_unregister(ch);
+                               mhi_unregister_protocol(ch);
+                       }
+               }
+       }
+}
+
+
+module_init(l3mhi_init);
+module_exit(l3mhi_exit);
+
+module_param_array_named(l2_channels, l2chs, int, &l2cnt, 0444);
+
+MODULE_AUTHOR("Petri Mattila <petri.to.mattila@renesasmobile.com>");
+MODULE_DESCRIPTION("L3 MHI Binding");
+MODULE_LICENSE("GPL");
+
diff --git a/net/mhi/l3phonet.c b/net/mhi/l3phonet.c
new file mode 100644 (file)
index 0000000..8de7471
--- /dev/null
@@ -0,0 +1,118 @@
+/*
+ * File: l3phonet.c
+ *
+ * L2 PHONET channel to AF_PHONET binding.
+ *
+ * Copyright (C) 2011 Renesas Mobile Corporation. All rights reserved.
+ *
+ * Author: Petri To Mattila <petri.to.mattila@renesasmobile.com>
+ *
+ * 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 <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/socket.h>
+#include <linux/mhi.h>
+#include <linux/l2mux.h>
+
+
+/* Functions */
+
+static int
+mhi_pn_netif_rx(struct sk_buff *skb, struct net_device *dev)
+{
+       /* Set Protocol Family */
+       skb->protocol = htons(ETH_P_PHONET);
+
+       /* Remove L2MUX header and Phonet media byte */
+       skb_pull(skb, L2MUX_HDR_SIZE + 1);
+
+       /* Pass upwards to the Procotol Family */
+       return netif_rx(skb);
+}
+
+static int
+mhi_pn_netif_tx(struct sk_buff *skb, struct net_device *dev)
+{
+       struct l2muxhdr *l2hdr;
+       int l3len;
+       u8  *ptr;
+
+       /* Add media byte */
+       ptr = skb_push(skb, 1);
+
+       /* Set media byte */
+       ptr[0] = dev->dev_addr[0];
+
+       /* L3 length */
+       l3len = skb->len;
+
+       /* Add L2MUX header */
+       skb_push(skb, L2MUX_HDR_SIZE);
+
+       /* Mac header starts here */
+       skb_reset_mac_header(skb);
+
+       /* L2MUX header pointer */
+       l2hdr = l2mux_hdr(skb);
+
+       /* L3 Proto ID */
+       l2mux_set_proto(l2hdr, MHI_L3_PHONET);
+
+       /* L3 payload length */
+       l2mux_set_length(l2hdr, l3len);
+
+       return 0;
+}
+
+
+/* Module registration */
+
+int __init mhi_pn_init(void)
+{
+       int err;
+
+       err = l2mux_netif_rx_register(MHI_L3_PHONET, mhi_pn_netif_rx);
+       if (err)
+               goto err1;
+
+       err = l2mux_netif_tx_register(ETH_P_PHONET, mhi_pn_netif_tx);
+       if (err)
+               goto err2;
+
+       return 0;
+
+err2:
+       l2mux_netif_rx_unregister(MHI_L3_PHONET);
+err1:
+       return err;
+}
+
+void __exit mhi_pn_exit(void)
+{
+       l2mux_netif_rx_unregister(MHI_L3_PHONET);
+       l2mux_netif_tx_unregister(ETH_P_PHONET);
+}
+
+
+module_init(mhi_pn_init);
+module_exit(mhi_pn_exit);
+
+MODULE_AUTHOR("Petri Mattila <petri.to.mattila@renesasmobile.com>");
+MODULE_DESCRIPTION("MHI Phonet protocol family bridge");
+MODULE_LICENSE("GPL");
+
diff --git a/net/mhi/mhi_dgram.c b/net/mhi/mhi_dgram.c
new file mode 100644 (file)
index 0000000..b2089e0
--- /dev/null
@@ -0,0 +1,330 @@
+/*
+ * File: mhi_dgram.c
+ *
+ * Copyright (C) 2011 Renesas Mobile Corporation. All rights reserved.
+ *
+ * Author: Petri Mattila <petri.to.mattila@renesasmobile.com>
+ *
+ * DGRAM socket implementation for MHI protocol family.
+ *
+ * It uses the MHI socket framework in mhi_socket.c
+ *
+ * This implementation is the most basic frame passing interface.
+ * The user space can use the sendmsg() and recvmsg() system calls
+ * to access the frames. The socket is created with the socket()
+ * system call, e.g. socket(PF_MHI,SOCK_DGRAM,l2proto).
+ *
+ * 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 <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/socket.h>
+#include <linux/mhi.h>
+#include <linux/l2mux.h>
+
+#include <asm/ioctls.h>
+
+#include <net/af_mhi.h>
+#include <net/mhi/sock.h>
+#include <net/mhi/dgram.h>
+
+#ifdef CONFIG_MHI_DEBUG
+# define DPRINTK(...)    printk(KERN_DEBUG "MHI/DGRAM: " __VA_ARGS__)
+#else
+# define DPRINTK(...)
+#endif
+
+
+/*** Prototypes ***/
+
+static struct proto mhi_dgram_proto;
+
+static void mhi_dgram_destruct(struct sock *sk);
+
+
+/*** Functions ***/
+
+int mhi_dgram_sock_create(
+       struct net              *net,
+       struct socket           *sock,
+       int                     proto,
+       int                     kern)
+{
+       struct sock             *sk;
+       struct mhi_sock         *msk;
+
+       DPRINTK("mhi_dgram_sock_create: proto:%d type:%d\n",
+               proto, sock->type);
+
+       if (sock->type != SOCK_DGRAM)
+               return -EPROTONOSUPPORT;
+
+       if (proto == MHI_L3_ANY)
+               return -EPROTONOSUPPORT;
+
+       sk = sk_alloc(net, PF_MHI, GFP_KERNEL, &mhi_dgram_proto);
+       if (!sk)
+               return -ENOMEM;
+
+       sock_init_data(sock, sk);
+
+       sock->ops = &mhi_socket_ops;
+       sock->state = SS_UNCONNECTED;
+
+       sk->sk_protocol = proto;
+       sk->sk_destruct = mhi_dgram_destruct;
+       sk->sk_backlog_rcv = sk->sk_prot->backlog_rcv;
+
+       sk->sk_prot->init(sk);
+
+       msk = mhi_sk(sk);
+
+       msk->sk_l3proto = proto;
+       msk->sk_ifindex = -1;
+
+       return 0;
+}
+
+static int mhi_dgram_init(struct sock *sk)
+{
+       return 0;
+}
+
+static void mhi_dgram_destruct(struct sock *sk)
+{
+       skb_queue_purge(&sk->sk_receive_queue);
+}
+
+static void mhi_dgram_close(struct sock *sk, long timeout)
+{
+       sk_common_release(sk);
+}
+
+static int mhi_dgram_ioctl(struct sock *sk, int cmd, unsigned long arg)
+{
+       int err;
+
+       DPRINTK("mhi_dgram_ioctl: cmd:%d arg:%lu\n", cmd, arg);
+
+       switch (cmd) {
+       case SIOCOUTQ:
+               {
+                       int len;
+                       len = sk_wmem_alloc_get(sk);
+                       err = put_user(len, (int __user *)arg);
+               }
+               break;
+
+       case SIOCINQ:
+               {
+                       struct sk_buff *skb;
+                       int len;
+
+                       lock_sock(sk);
+                       {
+                               skb = skb_peek(&sk->sk_receive_queue);
+                               len = skb ? skb->len : 0;
+                       }
+                       release_sock(sk);
+
+                       err = put_user(len, (int __user *)arg);
+               }
+               break;
+
+       default:
+               err = -ENOIOCTLCMD;
+       }
+
+       return err;
+}
+
+static int mhi_dgram_sendmsg(
+       struct kiocb    *iocb,
+       struct sock     *sk,
+       struct msghdr   *msg,
+       size_t          len)
+{
+       struct mhi_sock         *msk = mhi_sk(sk);
+       struct net_device       *dev = NULL;
+       struct l2muxhdr         *l2hdr;
+       struct sk_buff          *skb;
+       unsigned                mflags;
+
+       int err = -EFAULT;
+       mflags = (MSG_DONTWAIT|MSG_EOR|MSG_NOSIGNAL|MSG_CMSG_COMPAT);
+
+       if (msg->msg_flags & ~mflags) {
+               printk(KERN_WARNING "%s: incompatible msg_flags: 0x%08X\n",
+                       msg->msg_flags, __func__);
+               err = -EOPNOTSUPP;
+               goto out;
+       }
+
+       skb = sock_alloc_send_skb(sk, len + L2MUX_HDR_SIZE + ETH_HLEN,
+                                 (msg->msg_flags & MSG_DONTWAIT), &err);
+       if (!skb) {
+               printk(KERN_ERR "%s: sock_alloc_send_skb failed: %d\n",
+                               err, __func__);
+               goto out;
+       }
+
+       skb_reserve(skb, L2MUX_HDR_SIZE + ETH_HLEN);
+       skb_reset_transport_header(skb);
+
+       err = memcpy_fromiovec((void *)skb_put(skb, len), msg->msg_iov, len);
+       if (err < 0) {
+               printk(KERN_ERR "%s: memcpy_fromiovec failed: %d\n",
+                               err, __func__);
+               goto drop;
+       }
+
+       if (msk->sk_ifindex)
+               dev = dev_get_by_index(sock_net(sk), msk->sk_ifindex);
+
+       if (!dev) {
+               printk(KERN_ERR "%s: no device for ifindex:%d\n",
+                               msk->sk_ifindex, __func__);
+               goto drop;
+       }
+
+       if (!(dev->flags & IFF_UP)) {
+               printk(KERN_ERR "%s: device %d not IFF_UP\n",
+                               msk->sk_ifindex, __func__);
+               err = -ENETDOWN;
+               goto drop;
+       }
+
+       if (len + L2MUX_HDR_SIZE > dev->mtu) {
+               err = -EMSGSIZE;
+               goto drop;
+       }
+
+       skb_reset_network_header(skb);
+
+       skb_push(skb, L2MUX_HDR_SIZE);
+       skb_reset_mac_header(skb);
+
+       l2hdr = l2mux_hdr(skb);
+       l2mux_set_proto(l2hdr, sk->sk_protocol);
+       l2mux_set_length(l2hdr, len);
+
+       err = mhi_skb_send(skb, dev, sk->sk_protocol);
+
+       goto put;
+
+drop:
+       kfree(skb);
+put:
+       if (dev)
+               dev_put(dev);
+out:
+       return err;
+}
+
+static int mhi_dgram_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;
+       int cnt, err;
+       unsigned mflags;
+
+       err = -EOPNOTSUPP;
+       mflags = (MSG_PEEK|MSG_TRUNC|MSG_DONTWAIT|MSG_NOSIGNAL|
+                                               MSG_CMSG_COMPAT);
+
+       if (flags & ~mflags) {
+               printk(KERN_WARNING "%s: incompatible socket flags: 0x%08X",
+                               flags, __func__);
+               goto out2;
+       }
+
+       if (addr_len)
+               addr_len[0] = 0;
+
+       skb = skb_recv_datagram(sk, flags, noblock, &err);
+       if (!skb)
+               goto out2;
+
+       cnt = skb->len - L2MUX_HDR_SIZE;
+       if (len < cnt) {
+               msg->msg_flags |= MSG_TRUNC;
+               cnt = len;
+       }
+
+       err = skb_copy_datagram_iovec(skb, L2MUX_HDR_SIZE, msg->msg_iov, cnt);
+       if (err)
+               goto out;
+
+       if (flags & MSG_TRUNC)
+               err = skb->len - L2MUX_HDR_SIZE;
+       else
+               err = cnt;
+
+out:
+       skb_free_datagram(sk, skb);
+out2:
+       return err;
+}
+
+static int mhi_dgram_backlog_rcv(struct sock *sk, struct sk_buff *skb)
+{
+       if (sock_queue_rcv_skb(sk, skb) < 0) {
+               kfree_skb(skb);
+               return NET_RX_DROP;
+       }
+
+       return NET_RX_SUCCESS;
+}
+
+
+static struct proto mhi_dgram_proto = {
+       .name           = "MHI-DGRAM",
+       .owner          = THIS_MODULE,
+       .close          = mhi_dgram_close,
+       .ioctl          = mhi_dgram_ioctl,
+       .init           = mhi_dgram_init,
+       .sendmsg        = mhi_dgram_sendmsg,
+       .recvmsg        = mhi_dgram_recvmsg,
+       .backlog_rcv    = mhi_dgram_backlog_rcv,
+       .hash           = mhi_sock_hash,
+       .unhash         = mhi_sock_unhash,
+       .obj_size       = sizeof(struct mhi_sock),
+};
+
+
+int mhi_dgram_proto_init(void)
+{
+       DPRINTK("mhi_dgram_proto_init\n");
+
+       return proto_register(&mhi_dgram_proto, 1);
+}
+
+void mhi_dgram_proto_exit(void)
+{
+       DPRINTK("mhi_dgram_proto_exit\n");
+
+       proto_unregister(&mhi_dgram_proto);
+}
+
+
diff --git a/net/mhi/mhi_proto.c b/net/mhi/mhi_proto.c
new file mode 100644 (file)
index 0000000..e67fbdc
--- /dev/null
@@ -0,0 +1,214 @@
+/*
+ * File: mhi_proto.c
+ *
+ * Copyright (C) 2011 Renesas Mobile Corporation. All rights reserved.
+ *
+ * Author: Petri Mattila <petri.to.mattila@renesasmobile.com>
+ *
+ * Modem-Host Interface (MHI) Protocol Family
+ *
+ * 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 <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/if_mhi.h>
+#include <linux/mhi.h>
+#include <linux/l2mux.h>
+
+#include <net/af_mhi.h>
+#include <net/mhi/sock.h>
+#include <net/mhi/dgram.h>
+#include <net/mhi/raw.h>
+
+#ifdef CONFIG_MHI_DEBUG
+# define DPRINTK(...)    printk(KERN_DEBUG "AF_MHI: " __VA_ARGS__)
+#else
+# define DPRINTK(...)
+#endif
+
+
+/* Supported L2 protocols */
+static __u8 mhi_protocols[MHI_L3_NPROTO] __read_mostly = { 0, };
+
+
+/*** Functions ***/
+
+int
+mhi_protocol_registered(int protocol)
+{
+       if (protocol >= 0 && protocol < MHI_L3_NPROTO)
+               return mhi_protocols[protocol];
+       if (protocol == MHI_L3_ANY)
+               return 1;
+
+       return 0;
+}
+EXPORT_SYMBOL(mhi_protocol_registered);
+
+int
+mhi_register_protocol(int protocol)
+{
+       DPRINTK("mhi_register_protocol: %d\n", protocol);
+
+       if (protocol < 0 || protocol >= MHI_L3_NPROTO)
+               return -EINVAL;
+
+       mhi_protocols[protocol] = 1;
+
+       return 0;
+}
+EXPORT_SYMBOL(mhi_register_protocol);
+
+int
+mhi_unregister_protocol(int protocol)
+{
+       DPRINTK("mhi_unregister_protocol: %d\n", protocol);
+
+       if (protocol < 0 || protocol >= MHI_L3_NPROTO)
+               return -EINVAL;
+
+       mhi_protocols[protocol] = 0;
+
+       return 0;
+}
+EXPORT_SYMBOL(mhi_unregister_protocol);
+
+
+int
+mhi_skb_send(
+       struct sk_buff          *skb,
+       struct net_device       *dev,
+       u8                       proto)
+{
+       int err = 0;
+
+       DPRINTK("mhi_skb_send: proto:%d skb_len:%d\n", proto, skb->len);
+
+       skb->protocol = htons(ETH_P_MHI);
+       skb->dev = dev;
+
+       if (skb->pkt_type == PACKET_LOOPBACK) {
+               skb_orphan(skb);
+               netif_rx_ni(skb);
+       } else {
+
+               if ((proto == MHI_L3_XFILE) ||
+                       (proto == MHI_L3_LOW_PRIO_TEST)) {
+                       skb->priority = 1; /* Low prio */
+               } else if ((proto == MHI_L3_AUDIO) ||
+                               (proto == MHI_L3_TEST_PRIO) ||
+                               (proto == MHI_L3_HIGH_PRIO_TEST)) {
+                       skb->priority = 6;      /* high prio */
+               } else {
+                       skb->priority = 0;      /* medium prio */
+               }
+               err = dev_queue_xmit(skb);
+       }
+
+       return err;
+}
+
+int
+mhi_skb_recv(
+       struct sk_buff          *skb,
+       struct net_device       *dev,
+       struct packet_type      *type,
+       struct net_device       *orig_dev)
+{
+       struct l2muxhdr         *l2hdr;
+
+       u8     l3pid;
+       u32    l3len;
+       int    err;
+
+       l2hdr = l2mux_hdr(skb);
+
+       l3pid = l2mux_get_proto(l2hdr);
+       l3len = l2mux_get_length(l2hdr);
+
+       DPRINTK("mhi_skb_recv: skb_len:%d l3pid:%d l3len:%d\n",
+                               skb->len, l3pid, l3len);
+
+       err = mhi_sock_rcv_multicast(skb, l3pid, l3len);
+
+       return err;
+}
+
+
+static struct packet_type mhi_packet_type __read_mostly = {
+       .type = cpu_to_be16(ETH_P_MHI),
+       .func = mhi_skb_recv,
+};
+
+
+static int __init mhi_proto_init(void)
+{
+       int err;
+
+       DPRINTK("mhi_proto_init\n");
+
+       err = mhi_sock_init();
+       if (err) {
+               printk(KERN_ALERT "MHI socket layer registration failed\n");
+               goto err0;
+       }
+
+       err = mhi_dgram_proto_init();
+       if (err) {
+               printk(KERN_ALERT "MHI DGRAM protocol layer reg failed\n");
+               goto err1;
+       }
+
+       err = mhi_raw_proto_init();
+       if (err) {
+               printk(KERN_ALERT "MHI RAW protocol layer reg failed\n");
+               goto err2;
+       }
+
+       dev_add_pack(&mhi_packet_type);
+
+       return 0;
+
+err2:
+       mhi_dgram_proto_exit();
+err1:
+       mhi_sock_exit();
+err0:
+       return err;
+}
+
+static void __exit mhi_proto_exit(void)
+{
+       DPRINTK("mhi_proto_exit\n");
+
+       dev_remove_pack(&mhi_packet_type);
+
+       mhi_dgram_proto_exit();
+       mhi_raw_proto_exit();
+       mhi_sock_exit();
+}
+
+module_init(mhi_proto_init);
+module_exit(mhi_proto_exit);
+
+MODULE_ALIAS_NETPROTO(PF_MHI);
+
+MODULE_AUTHOR("Petri Mattila <petri.to.mattila@renesasmobile.com>");
+MODULE_DESCRIPTION("MHI Protocol Family for Linux");
+MODULE_LICENSE("GPL");
+
diff --git a/net/mhi/mhi_raw.c b/net/mhi/mhi_raw.c
new file mode 100644 (file)
index 0000000..e152167
--- /dev/null
@@ -0,0 +1,326 @@
+/*
+ * File: mhi_raw.c
+ *
+ * Copyright (C) 2011 Renesas Mobile Corporation. All rights reserved.
+ *
+ * Author: Petri Mattila <petri.to.mattila@renesasmobile.com>
+ *
+ * RAW socket implementation for MHI protocol family.
+ *
+ * It uses the MHI socket framework in mhi_socket.c
+ *
+ * This implementation is the most basic frame passing interface.
+ * The user space can use the sendmsg() and recvmsg() system calls
+ * to access the frames. The socket is created with the socket()
+ * system call, e.g. socket(PF_MHI,SOCK_RAW,l2proto).
+ *
+ * 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 <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/socket.h>
+#include <linux/mhi.h>
+#include <linux/l2mux.h>
+
+#include <asm/ioctls.h>
+
+#include <net/af_mhi.h>
+#include <net/mhi/sock.h>
+#include <net/mhi/raw.h>
+
+#ifdef CONFIG_MHI_DEBUG
+# define DPRINTK(...)    printk(KERN_DEBUG "MHI/RAW: " __VA_ARGS__)
+#else
+# define DPRINTK(...)
+#endif
+
+
+/*** Prototypes ***/
+
+static struct proto mhi_raw_proto;
+
+static void mhi_raw_destruct(struct sock *sk);
+
+
+/*** Functions ***/
+
+int mhi_raw_sock_create(
+       struct net              *net,
+       struct socket           *sock,
+       int                      proto,
+       int                      kern)
+{
+       struct sock             *sk;
+       struct mhi_sock         *msk;
+
+       DPRINTK("mhi_raw_sock_create: proto:%d type:%d\n",
+               proto, sock->type);
+
+       if (sock->type != SOCK_RAW)
+               return -EPROTONOSUPPORT;
+
+       sk = sk_alloc(net, PF_MHI, GFP_KERNEL, &mhi_raw_proto);
+       if (!sk)
+               return -ENOMEM;
+
+       sock_init_data(sock, sk);
+
+       sock->ops = &mhi_socket_ops;
+       sock->state = SS_UNCONNECTED;
+
+       if (proto != MHI_L3_ANY)
+               sk->sk_protocol = proto;
+       else
+               sk->sk_protocol = 0;
+
+       sk->sk_destruct = mhi_raw_destruct;
+       sk->sk_backlog_rcv = sk->sk_prot->backlog_rcv;
+
+       sk->sk_prot->init(sk);
+
+       msk = mhi_sk(sk);
+
+       msk->sk_l3proto = proto;
+       msk->sk_ifindex = -1;
+
+       return 0;
+}
+
+static int mhi_raw_init(struct sock *sk)
+{
+       return 0;
+}
+
+static void mhi_raw_destruct(struct sock *sk)
+{
+       skb_queue_purge(&sk->sk_receive_queue);
+}
+
+static void mhi_raw_close(struct sock *sk, long timeout)
+{
+       sk_common_release(sk);
+}
+
+static int mhi_raw_ioctl(struct sock *sk, int cmd, unsigned long arg)
+{
+       int err;
+
+       DPRINTK("mhi_raw_ioctl: cmd:%d arg:%lu\n", cmd, arg);
+
+       switch (cmd) {
+       case SIOCOUTQ:
+               {
+                       int len;
+                       len = sk_wmem_alloc_get(sk);
+                       err = put_user(len, (int __user *)arg);
+               }
+               break;
+
+       case SIOCINQ:
+               {
+                       struct sk_buff *skb;
+                       int len;
+
+                       lock_sock(sk);
+                       {
+                               skb = skb_peek(&sk->sk_receive_queue);
+                               len = skb ? skb->len : 0;
+                       }
+                       release_sock(sk);
+
+                       err = put_user(len, (int __user *)arg);
+               }
+               break;
+
+       default:
+               err = -ENOIOCTLCMD;
+       }
+
+       return err;
+}
+
+static int mhi_raw_sendmsg(
+       struct kiocb            *iocb,
+       struct sock                     *sk,
+       struct msghdr           *msg,
+       size_t                          len)
+{
+       struct mhi_sock     *msk = mhi_sk(sk);
+       struct net_device   *dev = NULL;
+       struct sk_buff      *skb;
+
+       int err = -EFAULT;
+
+       if (msg->msg_flags &
+               ~(MSG_DONTWAIT|MSG_EOR|MSG_NOSIGNAL|MSG_CMSG_COMPAT)) {
+               printk(KERN_WARNING
+                       "mhi_raw_sendmsg: incompatible socket msg_flags: 0x%08X\n",
+                   msg->msg_flags);
+               err = -EOPNOTSUPP;
+               goto out;
+       }
+
+       skb = sock_alloc_send_skb(sk,
+                                               len,
+                                               (msg->msg_flags & MSG_DONTWAIT),
+                                               &err);
+       if (!skb) {
+               printk(KERN_ERR
+                       "mhi_raw_sendmsg: sock_alloc_send_skb failed: %d\n",
+                       err);
+               goto out;
+       }
+
+       err = memcpy_fromiovec((void *)skb_put(skb, len), msg->msg_iov, len);
+       if (err < 0) {
+               printk(KERN_ERR
+                       "mhi_raw_sendmsg: memcpy_fromiovec failed: %d\n",
+                       err);
+               goto drop;
+       }
+
+       if (msk->sk_ifindex)
+               dev = dev_get_by_index(sock_net(sk), msk->sk_ifindex);
+
+       if (!dev) {
+               printk(KERN_ERR
+                       "mhi_raw_sendmsg: no device for ifindex:%d\n",
+                       msk->sk_ifindex);
+               goto drop;
+       }
+
+       if (!(dev->flags & IFF_UP)) {
+               printk(KERN_ERR
+                       "mhi_raw_sendmsg: device %d not IFF_UP\n",
+                       msk->sk_ifindex);
+               err = -ENETDOWN;
+               goto drop;
+       }
+
+       if (len > dev->mtu) {
+               err = -EMSGSIZE;
+               goto drop;
+       }
+
+       skb_reset_network_header(skb);
+       skb_reset_mac_header(skb);
+
+       err = mhi_skb_send(skb, dev, sk->sk_protocol);
+
+       goto put;
+
+drop:
+       kfree(skb);
+put:
+       if (dev)
+               dev_put(dev);
+out:
+       return err;
+}
+
+static int mhi_raw_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;
+       int cnt, err;
+
+       err = -EOPNOTSUPP;
+
+       if (flags &
+               ~(MSG_PEEK|MSG_TRUNC|MSG_DONTWAIT|
+                       MSG_NOSIGNAL|MSG_CMSG_COMPAT)) {
+               printk(KERN_WARNING
+                       "mhi_raw_recvmsg: incompatible socket flags: 0x%08X",
+                       flags);
+               goto out2;
+       }
+
+       if (addr_len)
+               addr_len[0] = 0;
+
+       skb = skb_recv_datagram(sk, flags, noblock, &err);
+       if (!skb)
+               goto out2;
+
+       cnt = skb->len;
+       if (len < cnt) {
+               msg->msg_flags |= MSG_TRUNC;
+               cnt = len;
+       }
+
+       err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, cnt);
+       if (err)
+               goto out;
+
+       if (flags & MSG_TRUNC)
+               err = skb->len;
+       else
+               err = cnt;
+
+out:
+       skb_free_datagram(sk, skb);
+out2:
+       return err;
+}
+
+static int mhi_raw_backlog_rcv(struct sock *sk, struct sk_buff *skb)
+{
+       if (sock_queue_rcv_skb(sk, skb) < 0) {
+               kfree_skb(skb);
+               return NET_RX_DROP;
+       }
+
+       return NET_RX_SUCCESS;
+}
+
+
+static struct proto mhi_raw_proto = {
+       .name           = "MHI-RAW",
+       .owner          = THIS_MODULE,
+       .close          = mhi_raw_close,
+       .ioctl          = mhi_raw_ioctl,
+       .init           = mhi_raw_init,
+       .sendmsg        = mhi_raw_sendmsg,
+       .recvmsg        = mhi_raw_recvmsg,
+       .backlog_rcv    = mhi_raw_backlog_rcv,
+       .hash           = mhi_sock_hash,
+       .unhash         = mhi_sock_unhash,
+       .obj_size       = sizeof(struct mhi_sock),
+};
+
+
+int mhi_raw_proto_init(void)
+{
+       DPRINTK("mhi_raw_proto_init\n");
+
+       return proto_register(&mhi_raw_proto, 1);
+}
+
+void mhi_raw_proto_exit(void)
+{
+       DPRINTK("mhi_raw_proto_exit\n");
+
+       proto_unregister(&mhi_raw_proto);
+}
+
diff --git a/net/mhi/mhi_socket.c b/net/mhi/mhi_socket.c
new file mode 100644 (file)
index 0000000..7d8f1ba
--- /dev/null
@@ -0,0 +1,312 @@
+/*
+ * File: mhi_socket.c
+ *
+ * Copyright (C) 2011 Renesas Mobile Corporation. All rights reserved.
+ *
+ * Author: Petri Mattila <petri.to.mattila@renesasmobile.com>
+ *
+ * Socket layer implementation for AF_MHI.
+ *
+ * This module implements generic sockets for MHI.
+ * The protocol is implemented separately, like mhi_dgram.c.
+ *
+ * As MHI does not have addressed, the MHI interface is
+ * defined by sa_ifindex field in sockaddr_mhi.
+ *
+ * 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 <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/gfp.h>
+#include <linux/net.h>
+#include <linux/poll.h>
+#include <linux/mhi.h>
+#include <linux/l2mux.h>
+
+#include <net/tcp_states.h>
+#include <net/af_mhi.h>
+#include <net/mhi/sock.h>
+#include <net/mhi/dgram.h>
+#include <net/mhi/raw.h>
+
+#ifdef CONFIG_MHI_DEBUG
+# define DPRINTK(...)    printk(KERN_DEBUG "MHI/SOCKET: " __VA_ARGS__)
+#else
+# define DPRINTK(...)
+#endif
+
+
+/* Master lock for MHI sockets */
+static DEFINE_SPINLOCK(mhi_sock_lock);
+
+/* List of MHI sockets */
+static struct hlist_head mhi_sock_list;
+
+
+static int mhi_sock_create(
+       struct net              *net,
+       struct socket           *sock,
+       int                      proto,
+       int                      kern)
+{
+       int err = 0;
+
+       DPRINTK("mhi_sock_create: type:%d proto:%d\n",
+               sock->type, proto);
+
+       if (!capable(CAP_SYS_ADMIN) || !capable(CAP_NET_ADMIN)) {
+               printk(KERN_WARNING "AF_MHI: socket create failed: PERMISSION DENIED\n");
+               return -EPERM;
+       }
+
+       if (!mhi_protocol_registered(proto)) {
+               printk(KERN_WARNING "AF_MHI: socket create failed: No support for L2 channel %d\n",
+                                       proto);
+               return -EPROTONOSUPPORT;
+       }
+
+       if (sock->type == SOCK_DGRAM)
+               err = mhi_dgram_sock_create(net, sock, proto, kern);
+       else if (sock->type == SOCK_RAW)
+               err = mhi_raw_sock_create(net, sock, proto, kern);
+       else {
+               printk(KERN_WARNING "AF_MHI: trying to create a socket with unknown type %d\n",
+                                       sock->type);
+               err = -EPROTONOSUPPORT;
+       }
+
+       if (err)
+               printk(KERN_WARNING "AF_MHI: socket create failed: %d\n", err);
+
+       return err;
+}
+
+
+static int mhi_sock_release(struct socket *sock)
+{
+       if (sock->sk) {
+               DPRINTK("mhi_sock_release: proto:%d type:%d\n",
+                       sock->sk->sk_protocol, sock->type);
+
+               sock->sk->sk_prot->close(sock->sk, 0);
+               sock->sk = NULL;
+       }
+
+       return 0;
+}
+
+static int mhi_sock_bind(
+       struct socket           *sock,
+       struct sockaddr         *addr,
+       int                     len) {
+       struct sock             *sk  = sock->sk;
+       struct mhi_sock         *msk = mhi_sk(sk);
+       struct sockaddr_mhi     *sam = sa_mhi(addr);
+
+       int err = 0;
+
+       DPRINTK("mhi_sock_bind: proto:%d state:%d\n",
+               sk->sk_protocol, sk->sk_state);
+
+       if (sk->sk_prot->bind)
+               return sk->sk_prot->bind(sk, addr, len);
+
+       if (len < sizeof(struct sockaddr_mhi))
+               return -EINVAL;
+
+       lock_sock(sk);
+       {
+               if (sk->sk_state == TCP_CLOSE) {
+                       msk->sk_ifindex = sam->sa_ifindex;
+                       WARN_ON(sk_hashed(sk));
+                       sk->sk_prot->hash(sk);
+               } else {
+                       err = -EINVAL; /* attempt to rebind */
+               }
+       }
+       release_sock(sk);
+
+       return err;
+}
+
+int mhi_sock_rcv_unicast(
+       struct sk_buff     *skb,
+       u8                  l3proto,
+       u32                 l3length)
+{
+       struct hlist_node  *hnode;
+       struct sock        *sknode;
+       struct mhi_sock    *msk;
+
+       DPRINTK("%s: proto:%d, len:%d\n", l3proto, l3length, __func__);
+
+       spin_lock(&mhi_sock_lock);
+       {
+               sk_for_each(sknode, hnode, &mhi_sock_list) {
+                       msk = mhi_sk(sknode);
+                       if ((msk->sk_l3proto == MHI_L3_ANY ||
+                             msk->sk_l3proto == l3proto) &&
+                            (msk->sk_ifindex == skb->dev->ifindex)) {
+                               sock_hold(sknode);
+                               sk_receive_skb(sknode, skb, 0);
+                               skb = NULL;
+                               break;
+                       }
+               }
+       }
+       spin_unlock(&mhi_sock_lock);
+
+       if (skb)
+               kfree_skb(skb);
+
+       return NET_RX_SUCCESS;
+}
+
+int mhi_sock_rcv_multicast(
+       struct sk_buff     *skb,
+       u8                  l3proto,
+       u32                 l3length)
+{
+       struct hlist_node  *hnode;
+       struct sock        *sknode;
+       struct mhi_sock    *msk;
+       struct sk_buff     *clone;
+
+       DPRINTK("%s: proto:%d, len:%d\n", l3proto, l3length, __func__);
+
+       spin_lock(&mhi_sock_lock);
+       {
+               sk_for_each(sknode, hnode, &mhi_sock_list) {
+                       msk = mhi_sk(sknode);
+                       if ((msk->sk_l3proto == MHI_L3_ANY ||
+                             msk->sk_l3proto == l3proto) &&
+                            (msk->sk_ifindex == skb->dev->ifindex)) {
+                               clone = skb_clone(skb, GFP_ATOMIC);
+                               if (likely(clone)) {
+                                       sock_hold(sknode);
+                                       sk_receive_skb(sknode, clone, 0);
+                               }
+                       }
+               }
+       }
+       spin_unlock(&mhi_sock_lock);
+
+       kfree_skb(skb);
+
+       return NET_RX_SUCCESS;
+}
+
+int mhi_sock_sendmsg(
+       struct kiocb    *iocb,
+       struct socket   *sock,
+       struct msghdr   *msg,
+       size_t           len)
+{
+       DPRINTK("mhi_sock_sendmsg: len:%lu\n", len);
+       return sock->sk->sk_prot->sendmsg(iocb, sock->sk, msg, len);
+}
+
+int mhi_sock_recvmsg(
+       struct kiocb    *iocb,
+       struct socket   *sock,
+       struct msghdr   *msg,
+       size_t           len,
+       int              flags)
+{
+       int addrlen = 0;
+       int err;
+
+       err = sock->sk->sk_prot->recvmsg(iocb, sock->sk, msg, len,
+                                        flags & MSG_DONTWAIT,
+                                        flags & ~MSG_DONTWAIT,
+                                        &addrlen);
+
+       if (err >= 0)
+               msg->msg_namelen = addrlen;
+
+       return err;
+}
+
+
+void mhi_sock_hash(struct sock *sk)
+{
+       DPRINTK("mhi_sock_hash: proto:%d\n", sk->sk_protocol);
+
+       spin_lock_bh(&mhi_sock_lock);
+       sk_add_node(sk, &mhi_sock_list);
+       spin_unlock_bh(&mhi_sock_lock);
+}
+
+void mhi_sock_unhash(struct sock *sk)
+{
+       DPRINTK("mhi_sock_unhash: proto:%d\n", sk->sk_protocol);
+
+       spin_lock_bh(&mhi_sock_lock);
+       sk_del_node_init(sk);
+       spin_unlock_bh(&mhi_sock_lock);
+}
+
+
+const struct proto_ops mhi_socket_ops = {
+       .family         = AF_MHI,
+       .owner          = THIS_MODULE,
+       .release        = mhi_sock_release,
+       .bind           = mhi_sock_bind,
+       .connect        = sock_no_connect,
+       .socketpair     = sock_no_socketpair,
+       .accept         = sock_no_accept,
+       .getname        = sock_no_getname,
+       .poll           = datagram_poll,
+       .ioctl          = sock_no_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        = mhi_sock_sendmsg,
+       .recvmsg        = mhi_sock_recvmsg,
+       .mmap           = sock_no_mmap,
+       .sendpage       = sock_no_sendpage,
+};
+
+static const struct net_proto_family mhi_proto_family = {
+       .family = PF_MHI,
+       .create = mhi_sock_create,
+       .owner  = THIS_MODULE,
+};
+
+
+int mhi_sock_init(void)
+{
+       DPRINTK("mhi_sock_init\n");
+
+       INIT_HLIST_HEAD(&mhi_sock_list);
+       spin_lock_init(&mhi_sock_lock);
+
+       return sock_register(&mhi_proto_family);
+}
+
+void mhi_sock_exit(void)
+{
+       DPRINTK("mhi_sock_exit\n");
+
+       sock_unregister(PF_MHI);
+}
+