net: phonet: Modifications for RMC PegaPCI.

* As submitted by RMC for modem support *

Bug 1054808

Change-Id: I6674df1870dea09e3a1b6035d2d8218e2f56de3b
Signed-off-by: Raj Jayaraman <rjayaraman@nvidia.com>
Reviewed-on: http://git-master/r/160031
(cherry picked from commit 60564998c44336e29d653e7d10a20555f0a5703b)
Reviewed-on: http://git-master/r/162291
Reviewed-by: Automatic_Commit_Validation_User
Reviewed-by: WK Tsai <wtsai@nvidia.com>
Reviewed-by: Steve Lin <stlin@nvidia.com>

4 files changed, 136 insertions, 11 deletions

diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c
index d65f699fbf34..be0c3d482f54 100644
--- a/net/phonet/af_phonet.c
+++ b/net/phonet/af_phonet.c
@@ -68,8 +68,10 @@ static int pn_socket_create(struct net *net, struct socket *sock, int protocol,
 	struct phonet_protocol *pnp;
 	int err;
 
-	if (!capable(CAP_SYS_ADMIN))
-		return -EPERM;
+/*PATCH
+ *	if (!capable(CAP_SYS_ADMIN))
+ *	return -EPERM;
+ */
 
 	if (protocol == 0) {
 		/* Default protocol selection */
@@ -112,6 +114,8 @@ static int pn_socket_create(struct net *net, struct socket *sock, int protocol,
 	pn->sobject = 0;
 	pn->dobject = 0;
 	pn->resource = 0;
+/*!*/	pn->resource_type = 0;
+/*!*/	pn->resource_subtype = 0;
 	sk->sk_prot->init(sk);
 	err = 0;
 
@@ -210,6 +214,7 @@ static int pn_send(struct sk_buff *skb, struct net_device *dev,
 
 	return err;
 drop:
+	printk(KERN_DEBUG "pn_send DROP\n");
 	kfree_skb(skb);
 	return err;
 }
@@ -287,6 +292,7 @@ int pn_skb_send(struct sock *sk, struct sk_buff *skb,
 	return err;
 
 drop:
+	printk(KERN_DEBUG "pn_skb_send DROP\n");
 	kfree_skb(skb);
 	if (dev)
 		dev_put(dev);
@@ -393,6 +399,12 @@ static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
 
 	pn_skb_get_dst_sockaddr(skb, &sa);
 
+	/* check if this is multicasted */
+	if (pn_sockaddr_get_object(&sa) == PNOBJECT_MULTICAST) {
+		pn_deliver_sock_broadcast(net, skb);
+		goto out;
+	}
+
 	/* check if this is broadcasted */
 	if (pn_sockaddr_get_addr(&sa) == PNADDR_BROADCAST) {
 		pn_deliver_sock_broadcast(net, skb);
@@ -402,14 +414,17 @@ static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
 	/* resource routing */
 	if (pn_sockaddr_get_object(&sa) == 0) {
 		struct sock *sk = pn_find_sock_by_res(net, sa.spn_resource);
-		if (sk)
+		if (sk) {
+			printk(KERN_DEBUG "phonet new resource routing!\n");
 			return sk_receive_skb(sk, skb, 0);
+		}
 	}
 
 	/* check if we are the destination */
 	if (phonet_address_lookup(net, pn_sockaddr_get_addr(&sa)) == 0) {
 		/* Phonet packet input */
-		struct sock *sk = pn_find_sock_by_sa(net, &sa);
+/*!*/		struct sock *sk = pn_find_sock_by_sa_and_skb(net, &sa, skb);
+		/*struct sock *sk = pn_find_sock_by_sa(net, &sa);*/
 
 		if (sk)
 			return sk_receive_skb(sk, skb, 0);
@@ -454,6 +469,7 @@ out_dev:
 
 out:
 	kfree_skb(skb);
+	printk(KERN_DEBUG "phonet_rcv Drop message!\n");
 	return NET_RX_DROP;
 }
 
diff --git a/net/phonet/pn_dev.c b/net/phonet/pn_dev.c
index bf5cf69c820a..b155ca02a2da 100644
--- a/net/phonet/pn_dev.c
+++ b/net/phonet/pn_dev.c
@@ -298,7 +298,7 @@ static int phonet_device_notify(struct notifier_block *me, unsigned long what,
 
 	switch (what) {
 	case NETDEV_REGISTER:
-		if (dev->type == ARPHRD_PHONET)
+		if (dev->type == ARPHRD_PHONET || dev->type == ARPHRD_MHI)
 			phonet_device_autoconf(dev);
 		break;
 	case NETDEV_UNREGISTER:
diff --git a/net/phonet/pn_netlink.c b/net/phonet/pn_netlink.c
index d61f6761777d..8aec4cbd15a2 100644
--- a/net/phonet/pn_netlink.c
+++ b/net/phonet/pn_netlink.c
@@ -70,9 +70,6 @@ static int addr_doit(struct sk_buff *skb, struct nlmsghdr *nlh, void *attr)
 	int err;
 	u8 pnaddr;
 
-	if (!capable(CAP_SYS_ADMIN))
-		return -EPERM;
-
 	ASSERT_RTNL();
 
 	err = nlmsg_parse(nlh, sizeof(*ifm), tb, IFA_MAX, ifa_phonet_policy);
@@ -228,9 +225,6 @@ static int route_doit(struct sk_buff *skb, struct nlmsghdr *nlh, void *attr)
 	int err;
 	u8 dst;
 
-	if (!capable(CAP_SYS_ADMIN))
-		return -EPERM;
-
 	ASSERT_RTNL();
 
 	err = nlmsg_parse(nlh, sizeof(*rtm), tb, RTA_MAX, rtm_phonet_policy);
diff --git a/net/phonet/socket.c b/net/phonet/socket.c
index 4c7eff30dfa9..e25f0c290387 100644
--- a/net/phonet/socket.c
+++ b/net/phonet/socket.c
@@ -74,6 +74,94 @@ static struct hlist_head *pn_hash_list(u16 obj)
  * 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_and_skb(struct net *net,
+			const struct sockaddr_pn *spn, struct sk_buff *skb)
+{
+	struct hlist_node *node;
+	struct sock *sknode;
+	struct sock *rval = NULL;
+	u16 obj = pn_sockaddr_get_object(spn);
+	u8 res = spn->spn_resource;
+	struct hlist_head *hlist = pnsocks.hlist;
+	unsigned h;
+	u8 type;
+	u8 subtype;
+
+	rcu_read_lock();
+
+	for (h = 0; h < PN_HASHSIZE; h++) {
+		sk_for_each_rcu(sknode, node, hlist) {
+			struct pn_sock *pn = pn_sk(sknode);
+			BUG_ON(!pn->sobject); /* unbound socket */
+			if (!net_eq(sock_net(sknode), net))
+				continue;
+
+			if ((PN_PREFIX == pn->resource) && (PN_PREFIX == res)) {
+				if (skb_shinfo(skb)->nr_frags) {
+					struct page *msg_page;
+					u8 *msg;
+					skb_frag_t *msg_frag = &skb_shinfo(skb)
+								->frags[0];
+
+					msg_page = skb_frag_page(msg_frag);
+					msg = page_address(msg_page);
+
+					type = msg[msg_frag->page_offset+2];
+					subtype = msg[msg_frag->page_offset+3];
+
+				} else {
+					type = *(skb->data + 2);
+					subtype = *(skb->data + 3);
+				}
+
+				if (type != pn->resource_type)
+					continue;
+
+				if (subtype != pn->resource_subtype) {
+					/* printk("phonet resource subtype
+						 don't match! type %d,
+						subtype %d pn->resource_subtype
+						 %d\n", type, subtype,
+						pn->resource_subtype); */
+					continue;
+				}
+				/* printk("phonet resource match! type %d,
+					 subtype %d\n", type, subtype); */
+			}
+
+			/* If port is zero, look up by resource */
+			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);
+			goto out;
+		}
+		hlist++;
+	}
+
+out:
+	rcu_read_unlock();
+
+	return rval;
+}
+
+/*
+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(struct net *net, const struct sockaddr_pn *spn)
 {
 	struct hlist_node *node;
@@ -371,6 +459,33 @@ static int pn_socket_ioctl(struct socket *sock, unsigned int cmd,
 	struct sock *sk = sock->sk;
 	struct pn_sock *pn = pn_sk(sk);
 
+	printk(KERN_DEBUG "pn_socket_ioctl with %s\n",
+		(cmd == SIOCCONFIGTYPE) ? "SIOCCONFIGTYPE" :
+		(cmd == SIOCCONFIGSUBTYPE) ? "SIOCCONFIGSUBTYPE" : "OTHER");
+
+	if (cmd == SIOCCONFIGTYPE) {
+		u16 type;
+		if (get_user(type, (__u16 __user *)arg))
+			return -EFAULT;
+
+		printk(KERN_DEBUG "Phonet register resource type %d on %x\n",
+					 type, pn);
+		pn->resource_type = type;
+		return 0;
+	}
+
+	if (cmd == SIOCCONFIGSUBTYPE) {
+		u16 subtype;
+
+		if (get_user(subtype, (__u16 __user *)arg))
+			return -EFAULT;
+
+		pn->resource_subtype = subtype;
+		/* printk(KERN_DEBUG "Phonet register resource subtype %d on %x
+					\n", subtype, pn); */
+		return 0;
+	}
+
 	if (cmd == SIOCPNGETOBJECT) {
 		struct net_device *dev;
 		u16 handle;