Updated Phonet Stack with adaptations from v29 to v33 for MBL

Signed-off-by: B Sampath Kumar <sampath.kumar@stericsson.com>

6 files changed, 282 insertions, 16 deletions

diff --git a/net/phonet/Makefile b/net/phonet/Makefile
index d62bbba649b..bdda5807382 100644..100755
--- a/net/phonet/Makefile
+++ b/net/phonet/Makefile
@@ -1,3 +1,5 @@
+EXTRA_CFLAGS += -Wall
+
 obj-$(CONFIG_PHONET) += phonet.o pn_pep.o
 
 phonet-objs := \
diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c
index 73aee7f2fcd..409616fde42 100644..100755
--- a/net/phonet/af_phonet.c
+++ b/net/phonet/af_phonet.c
@@ -37,6 +37,16 @@
 /* Transport protocol registration */
 static struct phonet_protocol *proto_tab[PHONET_NPROTO] __read_mostly;
 
+#define PHONET_DEBUG		0
+#define PHONET_PIPE_CTRLR
+
+#define PN_MODEM_MEDIA		0x26
+#define PN_MODEM_MEDIA_SIZE	1
+#define PN_PIPE_RES_ID		0xD9
+#define PNS_PIPE_DATA_MSG_ID	0x20
+
+struct net_device phonet_dev;
+
 static struct phonet_protocol *phonet_proto_get(int protocol)
 {
 	struct phonet_protocol *pp;
@@ -152,7 +162,7 @@ struct header_ops phonet_header_ops = {
 	.create = pn_header_create,
 	.parse = pn_header_parse,
 };
-EXPORT_SYMBOL(phonet_header_ops);
+/*EXPORT_SYMBOL(phonet_header_ops);*/
 
 /*
  * Prepends an ISI header and sends a datagram.
@@ -162,18 +172,20 @@ static int pn_send(struct sk_buff *skb, struct net_device *dev,
 {
 	struct phonethdr *ph;
 	int err;
+	unsigned char *lptr = kmalloc(2*1024, GFP_ATOMIC);
 
+#ifdef PHONET_DEV
 	if (skb->len + 2 > 0xffff /* Phonet length field limit */ ||
 	    skb->len + sizeof(struct phonethdr) > dev->mtu) {
 		err = -EMSGSIZE;
 		goto drop;
 	}
-
 	/* Broadcast sending is not implemented */
 	if (pn_addr(dst) == PNADDR_BROADCAST) {
 		err = -EOPNOTSUPP;
 		goto drop;
 	}
+#endif
 
 	skb_reset_transport_header(skb);
 	WARN_ON(skb_headroom(skb) & 1); /* HW assumes word alignment */
@@ -194,12 +206,56 @@ static int pn_send(struct sk_buff *skb, struct net_device *dev,
 	if (skb->pkt_type == PACKET_LOOPBACK) {
 		skb_reset_mac_header(skb);
 		skb_orphan(skb);
+
+#ifdef PHONET_PIPE_CTRLR
+#if (PHONET_DEBUG & 1)
+		int i;
+
+		printk(KERN_INFO "\n\nIn %s BEF memcpy loopback\n\n", __func__);
+
+		for (i = 0 ; i < skb->len ; i++)
+			printk(KERN_INFO "%02X ", skb->data[i]);
+
+		printk(KERN_INFO "\n\nIn %s END skb->data loopback\n\n", \
+				__func__);
+#endif
+
+		if ((char)*(skb->data + sizeof(struct phonethdr)) \
+				== PN_MODEM_MEDIA) {
+			skb->data += (sizeof(struct phonethdr) + \
+					PN_MODEM_MEDIA_SIZE);
+			skb->len -= (sizeof(struct phonethdr) - \
+					PN_MODEM_MEDIA_SIZE);
+		} else {
+			memcpy(lptr, (void *)(skb->data), \
+				     sizeof(struct phonethdr));
+			memcpy(lptr + sizeof(struct phonethdr), "\x26", \
+					PN_MODEM_MEDIA_SIZE);
+
+			__skb_push(skb, sizeof(struct phonethdr) + \
+					PN_MODEM_MEDIA_SIZE);
+			skb_reset_transport_header(skb);
+			skb_copy_to_linear_data(skb, lptr, \
+						sizeof(struct phonethdr) + \
+						PN_MODEM_MEDIA_SIZE);
+		}
+
+#if (PHONET_DEBUG & 1)
+		printk(KERN_INFO "\nIn %s AFT memcpy loopback\n\n", __func__);
+
+		for (i = 0 ; i < skb->len ; i++)
+			printk(KERN_INFO "%02X ", skb->data[i]);
+
+		printk(KERN_INFO "\nIn %s END skb->data loopback\n", __func__);
+#endif
+#endif
 		if (irq)
 			netif_rx(skb);
 		else
 			netif_rx_ni(skb);
 		err = 0;
 	} else {
+#ifdef PHONET_DEV
 		err = dev_hard_header(skb, dev, ntohs(skb->protocol),
 					NULL, NULL, skb->len);
 		if (err < 0) {
@@ -207,12 +263,92 @@ static int pn_send(struct sk_buff *skb, struct net_device *dev,
 			goto drop;
 		}
 		err = dev_queue_xmit(skb);
+#endif
+#if (PHONET_DEBUG & 1)
+		printk(KERN_INFO "\nSAMPATH:: PHONET:: before isa_write\n");
+		printk(KERN_INFO "SAMPATH:: len = %d\n%s \
+				SendBuf:: \n", skb->len, __func__);
+
+		for (i = 0 ; i < skb->len ; i++)
+			printk(KERN_INFO "%02X ", skb->data[i]);
+		printk(KERN_INFO "\n\n");
+#endif
+#ifdef PHONET_PIPE_CTRLR
+		/**
+		 * As the data coming from Phonet Library already has phonet
+		 * header appended to it and as the driver again tries to add,
+		 * the one added by phonet driver is bypassed. Also, for
+		 * PIPE_DATA msgs which come from TCP/IP stack, data and header
+		 * needs to be copied to a single buffer and passed to
+		 * SHM driver.
+		 */
+		if ((ph->pn_res == PN_PIPE_RES_ID) && \
+				(skb->data[8] == PNS_PIPE_DATA_MSG_ID)) {
+#if (PHONET_DEBUG & 1)
+			printk(KERN_INFO "SAMPATH:: rskb len = %d\n%s \
+					rskb SendBuf:: \n", \
+				(skb_shinfo(skb)->frag_list)->len, __func__);
+
+			for (i = 0 ; \
+			     i < (skb_shinfo(skb)->frag_list)->len ; \
+			     i++)
+				printk(KERN_INFO "%02X ", \
+					(skb_shinfo(skb)->frag_list)->data[i]);
+			printk(KERN_INFO "\n\n");
+			printk(KERN_INFO "\nPipe Data!!prepare headers!!!\n");
+#endif
+			memcpy(lptr, "\x26", PN_MODEM_MEDIA_SIZE);
+			memcpy(lptr + PN_MODEM_MEDIA_SIZE, \
+			       (void *)(skb->data), \
+			       sizeof(struct phonethdr) + 3);
+
+			__skb_push((skb_shinfo(skb)->frag_list), \
+					3 + sizeof(struct phonethdr) \
+					+ PN_MODEM_MEDIA_SIZE);
+			skb_reset_transport_header(\
+					(skb_shinfo(skb)->frag_list));
+			skb_copy_to_linear_data(\
+					(skb_shinfo(skb)->frag_list), \
+					lptr, \
+					3 + sizeof(struct phonethdr) \
+					+ PN_MODEM_MEDIA_SIZE);
+
+			err = isa_write(NULL, \
+					((skb_shinfo(skb)->frag_list)->data), \
+				(skb_shinfo(skb)->frag_list)->len + 8 + 3, \
+				NULL);
+
+			err = 0;
+
+#if (PHONET_DEBUG & 1)
+			printk(KERN_INFO "SAMPATH:: mod skb len = %d\n%s \
+				mod skb SendBuf:: \n", \
+				(skb_shinfo(skb)->frag_list)->len + 8, \
+				__func__);
+
+			for (i = 0 ; \
+			     i < (skb_shinfo(skb)->frag_list)->len + 8 + 3 ; \
+			     i++)
+				printk("%02X ", lptr[i]);
+			printk("\n\n");
+#endif
+		}
+#endif
+		else
+			err = isa_write(NULL, \
+					skb->data + sizeof(struct phonethdr), \
+					skb->len - sizeof(struct phonethdr), \
+					NULL);
+		kfree_skb(skb);
 	}
 
+	kfree(lptr);
 	return err;
+#ifdef PHONET_DEV
 drop:
 	kfree_skb(skb);
 	return err;
+#endif
 }
 
 static int pn_raw_send(const void *data, int len, struct net_device *dev,
@@ -221,10 +357,10 @@ static int pn_raw_send(const void *data, int len, struct net_device *dev,
 	struct sk_buff *skb = alloc_skb(MAX_PHONET_HEADER + len, GFP_ATOMIC);
 	if (skb == NULL)
 		return -ENOMEM;
-
+#ifdef PHONET_DEV
 	if (phonet_address_lookup(dev_net(dev), pn_addr(dst)) == 0)
 		skb->pkt_type = PACKET_LOOPBACK;
-
+#endif
 	skb_reserve(skb, MAX_PHONET_HEADER);
 	__skb_put(skb, len);
 	skb_copy_to_linear_data(skb, data, len);
@@ -238,14 +374,18 @@ static int pn_raw_send(const void *data, int len, struct net_device *dev,
 int pn_skb_send(struct sock *sk, struct sk_buff *skb,
 		const struct sockaddr_pn *target)
 {
-	struct net *net = sock_net(sk);
-	struct net_device *dev;
+	struct net_device *dev = &phonet_dev;
 	struct pn_sock *pn = pn_sk(sk);
 	int err;
 	u16 src;
-	u8 daddr = pn_sockaddr_get_addr(target), saddr = PN_NO_ADDR;
+	u8 saddr = PN_NO_ADDR;
+	u8 pipe_hdl;
 
 	err = -EHOSTUNREACH;
+#ifdef PHONET_DEV
+	struct net *net = sock_net(sk);
+	u8 daddr = pn_sockaddr_get_addr(target);
+
 	if (sk->sk_bound_dev_if)
 		dev = dev_get_by_index(net, sk->sk_bound_dev_if);
 	else if (phonet_address_lookup(net, daddr) == 0) {
@@ -258,13 +398,32 @@ int pn_skb_send(struct sock *sk, struct sk_buff *skb,
 		goto drop;
 
 	saddr = phonet_address_get(dev, daddr);
+#endif
+	saddr = 0x00;
+
 	if (saddr == PN_NO_ADDR)
 		goto drop;
 
 	src = pn->sobject;
+
+#if (PHONET_DEBUG & 1)
+	printk(KERN_INFO "\n\n\n src = %02X, target->spn_dev = %02X, \
+		target->spn_obj = %02X, target->spn_resource = %02X, \
+		dst = %02X\n\n\n", src, target->spn_dev, target->spn_obj, \
+				   target->spn_resource, \
+				   pn_sockaddr_get_object(target));
+#endif
 	if (!pn_addr(src))
 		src = pn_object(saddr, pn_obj(src));
 
+	(skb->data[0] == PN_MODEM_MEDIA) ? \
+		       (pipe_hdl = skb->data[10]) : (pipe_hdl = skb->data[2]);
+
+	if ((target->spn_dev == pipe_hdl) && (target->spn_obj == pipe_hdl)) {
+		src = pn_sockaddr_get_object(target);
+		skb->pkt_type = PACKET_LOOPBACK;
+	}
+
 	err = pn_send(skb, dev, pn_sockaddr_get_object(target),
 			src, pn_sockaddr_get_resource(target), 0);
 	dev_put(dev);
@@ -360,7 +519,18 @@ static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
 	struct phonethdr *ph;
 	struct sockaddr_pn sa;
 	u16 len;
+	u8 pipe_hdl, msg_id = 0;
 
+#if (PHONET_DEBUG & 1)
+	u16 i;
+
+	printk(KERN_INFO "\nSAMPATH:: %s: B4 pskb_pull: \
+			skb->len = %d\n\nskb->data:\n", __func__, skb->len);
+
+	for (i = 0 ; i < skb->len ; i++)
+		printk(KERN_INFO "%02X ", skb->data[i]);
+	printk(KERN_INFO "\n\n");
+#endif
 	/* check we have at least a full Phonet header */
 	if (!pskb_pull(skb, sizeof(struct phonethdr)))
 		goto out;
@@ -371,11 +541,33 @@ static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
 	if (len < 2)
 		goto out;
 	len -= 2;
-	if ((len > skb->len) || pskb_trim(skb, len))
+#if (PHONET_DEBUG & 1)
+	printk(KERN_INFO "\nSAMPATH:: %s: after pskb_pull: len = %d, \
+			skb->len = %d, ph->pn_length = %d\n\n \
+			skb->data:\n", __func__, len, skb->len, ph->pn_length);
+	for (i = 0 ; i < skb->len ; i++)
+		printk(KERN_INFO "%02X ", skb->data[i]);
+	printk(KERN_INFO "\n\n");
+#endif
+
+	if ((len > skb->len)/*|| pskb_trim(skb, len)*/)
 		goto out;
+
 	skb_reset_transport_header(skb);
 
+#ifdef PHONET_DEV
 	pn_skb_get_dst_sockaddr(skb, &sa);
+#else
+	{
+		struct phonethdr *ph = pn_hdr(skb);
+		u16 obj = (u16)((ph->pn_rdev << 8) | (ph->pn_robj & 0x3ff));
+		sa.spn_family = AF_PHONET;
+		sa.spn_dev = obj >> 8;
+		sa.spn_obj = obj & 0xff;
+		sa.spn_resource = ph->pn_res;
+		memset(sa.spn_zero, 0, sizeof(sa.spn_zero));
+	}
+#endif
 
 	/* check if this is broadcasted */
 	if (pn_sockaddr_get_addr(&sa) == PNADDR_BROADCAST) {
@@ -383,11 +575,47 @@ static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
 		goto out;
 	}
 
+	(skb->data[0] == PN_MODEM_MEDIA) ? \
+		       ((pipe_hdl = skb->data[10]), (msg_id = skb->data[9])) :\
+		       (pipe_hdl = skb->data[2]);
+
+	if ((skb->data[0] != PN_MODEM_MEDIA) 		|| \
+		((ph->pn_res == PN_PIPE_RES_ID) && \
+		 (msg_id == PNS_PIPE_DATA_MSG_ID)) 	|| \
+		((ph->pn_res == PN_PIPE_RES_ID) && \
+		 (msg_id == 0x60))) {
+		sa.spn_dev = sa.spn_obj = sa.spn_resource = pipe_hdl;
+	} else if (skb->data[0] == PN_MODEM_MEDIA) {
+		sa.spn_dev = sa.spn_obj = ph->pn_res;
+	}
+
+#ifdef PHONET_DEV
 	/* 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);
+#else
+	if (1) {
+		struct sock *sk = pn_find_sock_by_sa(/*net, */&sa);
+#endif
+
+		if ((ph->pn_res == PN_PIPE_RES_ID) && \
+				(msg_id == PNS_PIPE_DATA_MSG_ID)) {
+			skb->data += 8;
+			skb->len -= 8;
+			skb_reset_transport_header(skb);
+		}
 
+#if (PHONET_DEBUG & 1)
+		printk(KERN_INFO "\nSAMPATH:: %s: len = %d, skb->len = %d, \
+				    ph->pn_length = %d, sa.spn_dev = %d, \
+				    sa.spn_obj = %d, \
+			    sa.spn_res = %d\n\nskb->data:\n", __func__, \
+			    len, skb->len, ph->pn_length, sa.spn_dev, \
+			    sa.spn_obj, sa.spn_resource);
+		for (i = 0 ; i < skb->len ; i++)
+			printk(KERN_INFO "%02X ", skb->data[i]);
+#endif
 		if (sk)
 			return sk_receive_skb(sk, skb, 0);
 
diff --git a/net/phonet/datagram.c b/net/phonet/datagram.c
index 1bd38db4fe1..590fd85f716 100644..100755
--- a/net/phonet/datagram.c
+++ b/net/phonet/datagram.c
@@ -32,6 +32,11 @@
 #include <linux/phonet.h>
 #include <net/phonet/phonet.h>
 
+enum {
+	PHONET_BIND_NOT_DONE,
+	PHONET_BIND_DONE
+};
+
 static int pn_backlog_rcv(struct sock *sk, struct sk_buff *skb);
 
 /* associated socket ceases to exist */
@@ -52,6 +57,10 @@ static int pn_ioctl(struct sock *sk, int cmd, unsigned long arg)
 		answ = skb ? skb->len : 0;
 		release_sock(sk);
 		return put_user(answ, (int __user *)arg);
+	case SIOCPNSETBINDFLAG:
+		printk(KERN_INFO "\nSetting bind flag in kernel!!\n");
+		phonet_bind_check(PHONET_BIND_DONE);
+		return 0;
 	}
 
 	return -ENOIOCTLCMD;
diff --git a/net/phonet/pep.c b/net/phonet/pep.c
index e2a95762abd..0e0fbe4dbe0 100644..100755
--- a/net/phonet/pep.c
+++ b/net/phonet/pep.c
@@ -50,7 +50,8 @@
 #define CREDITS_MAX	10
 #define CREDITS_THR	7
 
-static const struct sockaddr_pn pipe_srv = {
+#define PHONET_DEBUG	0
+static /*const*/ struct sockaddr_pn pipe_srv = {
 	.spn_family = AF_PHONET,
 	.spn_resource = 0xD9, /* pipe service */
 };
@@ -89,12 +90,12 @@ static int pep_reply(struct sock *sk, struct sk_buff *oskb,
 	struct pnpipehdr *ph;
 	struct sk_buff *skb;
 
-	skb = alloc_skb(MAX_PNPIPE_HEADER + len, priority);
+	skb = alloc_skb(8 + MAX_PNPIPE_HEADER + len, priority);
 	if (!skb)
 		return -ENOMEM;
 	skb_set_owner_w(skb, sk);
 
-	skb_reserve(skb, MAX_PNPIPE_HEADER);
+	skb_reserve(skb, 8 + MAX_PNPIPE_HEADER);
 	__skb_put(skb, len);
 	skb_copy_to_linear_data(skb, data, len);
 	__skb_push(skb, sizeof(*ph));
@@ -105,6 +106,9 @@ static int pep_reply(struct sock *sk, struct sk_buff *oskb,
 	ph->pipe_handle = oph->pipe_handle;
 	ph->error_code = code;
 
+	pipe_srv.spn_dev = ph->pipe_handle;
+	pipe_srv.spn_obj = ph->pipe_handle;
+
 	return pn_skb_send(sk, skb, &pipe_srv);
 }
 
@@ -580,6 +584,9 @@ static int pep_do_rcv(struct sock *sk, struct sk_buff *skb)
 
 	pn_skb_get_dst_sockaddr(skb, &dst);
 
+	dst.spn_dev = hdr->pipe_handle;
+	dst.spn_obj = hdr->pipe_handle;
+
 	/* Look for an existing pipe handle */
 	sknode = pep_find_pipe(&pn->hlist, &dst, pipe_handle);
 	if (sknode)
@@ -849,6 +856,9 @@ static int pipe_skb_send(struct sock *sk, struct sk_buff *skb)
 		ph->message_id = PNS_PIPE_DATA;
 	ph->pipe_handle = pn->pipe_handle;
 
+	pipe_srv.spn_dev = 0x60;
+	pipe_srv.spn_obj = 0x30;
+
 	return pn_skb_send(sk, skb, &pipe_srv);
 }
 
diff --git a/net/phonet/pn_dev.c b/net/phonet/pn_dev.c
index 9b4ced6e096..a7857f11704 100644..100755
--- a/net/phonet/pn_dev.c
+++ b/net/phonet/pn_dev.c
@@ -212,7 +212,7 @@ u8 phonet_address_get(struct net_device *dev, u8 daddr)
 	}
 	return saddr;
 }
-
+#ifdef PHONET_DEV
 int phonet_address_lookup(struct net *net, u8 addr)
 {
 	struct phonet_device_list *pndevs = phonet_device_list(net);
@@ -235,7 +235,7 @@ found:
 	rcu_read_unlock();
 	return err;
 }
-
+#endif
 /* automatically configure a Phonet device, if supported */
 static int phonet_device_autoconf(struct net_device *dev)
 {
diff --git a/net/phonet/socket.c b/net/phonet/socket.c
index c785bfd0744..65d428e7b5c 100644..100755
--- a/net/phonet/socket.c
+++ b/net/phonet/socket.c
@@ -73,7 +73,12 @@ 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.
  */
+#ifdef PHONET_DEV
 struct sock *pn_find_sock_by_sa(struct net *net, const struct sockaddr_pn *spn)
+#else
+struct sock *pn_find_sock_by_sa(/*struct net *net, */ \
+		const struct sockaddr_pn *spn)
+#endif
 {
 	struct hlist_node *node;
 	struct sock *sknode;
@@ -87,9 +92,10 @@ struct sock *pn_find_sock_by_sa(struct net *net, const struct sockaddr_pn *spn)
 	sk_for_each(sknode, node, hlist) {
 		struct pn_sock *pn = pn_sk(sknode);
 		BUG_ON(!pn->sobject); /* unbound socket */
-
+#ifdef PHONET_DEV
 		if (!net_eq(sock_net(sknode), net))
 			continue;
+#endif
 		if (pn_port(obj)) {
 			/* Look up socket by port */
 			if (pn_port(pn->sobject) != pn_port(obj))
@@ -182,9 +188,10 @@ static int pn_socket_bind(struct socket *sock, struct sockaddr *addr, int len)
 
 	handle = pn_sockaddr_get_object((struct sockaddr_pn *)addr);
 	saddr = pn_addr(handle);
+#ifdef PHONET_DEV
 	if (saddr && phonet_address_lookup(sock_net(sk), saddr))
 		return -EADDRNOTAVAIL;
-
+#endif
 	lock_sock(sk);
 	if (sk->sk_state != TCP_CLOSE || pn_port(pn->sobject)) {
 		err = -EINVAL; /* attempt to rebind */
@@ -409,13 +416,15 @@ const struct proto_ops phonet_stream_ops = {
 	.mmap		= sock_no_mmap,
 	.sendpage	= sock_no_sendpage,
 };
-EXPORT_SYMBOL(phonet_stream_ops);
+/*EXPORT_SYMBOL(phonet_stream_ops);*/
 
 /* allocate port for a socket */
 int pn_sock_get_port(struct sock *sk, unsigned short sport)
 {
 	static int port_cur;
+#ifdef PHONET_DEV
 	struct net *net = sock_net(sk);
+#endif
 	struct pn_sock *pn = pn_sk(sk);
 	struct sockaddr_pn try_sa;
 	struct sock *tmpsk;
@@ -434,7 +443,11 @@ int pn_sock_get_port(struct sock *sk, unsigned short sport)
 				port_cur = pmin;
 
 			pn_sockaddr_set_port(&try_sa, port_cur);
+#ifdef PHONET_DEV
 			tmpsk = pn_find_sock_by_sa(net, &try_sa);
+#else
+			tmpsk = pn_find_sock_by_sa(/*net, In 2.6.29*/ &try_sa);
+#endif
 			if (tmpsk == NULL) {
 				sport = port_cur;
 				goto found;
@@ -444,7 +457,11 @@ int pn_sock_get_port(struct sock *sk, unsigned short sport)
 	} else {
 		/* try to find specific port */
 		pn_sockaddr_set_port(&try_sa, sport);
+#ifdef PHONET_DEV
 		tmpsk = pn_find_sock_by_sa(net, &try_sa);
+#else
+		tmpsk = pn_find_sock_by_sa(/*net, In 2.6.29*/ &try_sa);
+#endif
 		if (tmpsk == NULL)
 			/* No sock there! We can use that port... */
 			goto found;