usb: gadget: phonet: Add Phonet over ACM for RMC

* As submitted by RMC for modem support *

Add Phonet over ACM support for RMC PegaPCI

Bug 1066582
Bug 1167013

Change-Id: Id7883e30767d47200a1ed8ccb4e79f01a30c7a06
Signed-off-by: BH Hsieh <bhsieh@nvidia.com>
Reviewed-on: http://git-master/r/160149
(cherry picked from commit 17ad36b016454666bf3c81036fe3d4e6987be591)
Reviewed-on: http://git-master/r/162772
GVS: Gerrit_Virtual_Submit
Reviewed-by: Steve Lin <stlin@nvidia.com>

4 files changed, 731 insertions, 2 deletions

diff --git a/net/phonet/Kconfig b/net/phonet/Kconfig
index 6ec7d55b1769..dba14b02dd09 100644
--- a/net/phonet/Kconfig
+++ b/net/phonet/Kconfig
@@ -14,3 +14,12 @@ config PHONET
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called phonet. If unsure, say N.
+
+
+if PHONET
+config PHONET_DEBUG
+        boolean "Debug support for PHONET drivers"
+        depends on DEBUG_KERNEL
+        help
+          Say "yes" to enable phonet debug messaging
+endif
diff --git a/net/phonet/Makefile b/net/phonet/Makefile
index e10b1b182ce3..f219b2dd4755 100644
--- a/net/phonet/Makefile
+++ b/net/phonet/Makefile
@@ -6,6 +6,7 @@ phonet-y := \
 	socket.o \
 	datagram.o \
 	sysctl.o \
-	af_phonet.o
+	af_phonet.o \
+	ld_phonet.o
 
 pn_pep-y := pep.o pep-gprs.o
diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c
index a6ecd4796d5a..da25b8a99317 100644
--- a/net/phonet/af_phonet.c
+++ b/net/phonet/af_phonet.c
@@ -34,6 +34,53 @@
 #include <net/phonet/phonet.h>
 #include <net/phonet/pn_dev.h>
 
+#ifdef ACTIVATE_PHONET_DEBUG
+
+enum phonet_debug_state phonet_dbg_state = OFF;
+
+static ssize_t phonet_show(struct kobject *kobj, struct kobj_attribute *attr,
+	char *buf)
+{
+	printk(KERN_DEBUG "phonet show:\n");
+
+	switch (phonet_dbg_state) {
+	case ON:
+		return sprintf(buf, "on\n");
+	case OFF:
+		return sprintf(buf, "off\n");
+	case DATA:
+		return sprintf(buf, "data\n");
+	default:
+		return -ENODEV;
+	}
+
+	return -ENODEV;
+	/*return sprintf(buf, "%hu\n", phonet_dbg_state);*/
+}
+
+static ssize_t phonet_store(struct kobject *kobj, struct kobj_attribute *attr,
+	const char *buf, size_t n)
+{
+	if (sysfs_streq(buf, "on")) {
+		phonet_dbg_state = ON;
+		printk(KERN_DEBUG "Phonet traces activated\nBe Careful do not trace Dmesg in MTDn");
+	} else if (sysfs_streq(buf, "off")) {
+		phonet_dbg_state = OFF;
+	} else if (sysfs_streq(buf, "off")) {
+		phonet_dbg_state = OFF;
+	} else if (sysfs_streq(buf, "data")) {
+		phonet_dbg_state = DATA;
+	} else {
+		printk(KERN_DEBUG "please use  on/off/data\n");
+	}
+	return -EINVAL;
+}
+
+static struct kobj_attribute phonet_attr =
+	__ATTR(phonet_dbg, 0644, phonet_show, phonet_store);
+#endif
+
+
 /* Transport protocol registration */
 static struct phonet_protocol *proto_tab[PHONET_NPROTO] __read_mostly;
 
@@ -166,7 +213,7 @@ static int pn_send(struct sk_buff *skb, struct net_device *dev,
 			u16 dst, u16 src, u8 res, u8 irq)
 {
 	struct phonethdr *ph;
-	int err;
+	int err, i;
 
 	if (skb->len + 2 > 0xffff /* Phonet length field limit */ ||
 	    skb->len + sizeof(struct phonethdr) > dev->mtu) {
@@ -196,6 +243,16 @@ static int pn_send(struct sk_buff *skb, struct net_device *dev,
 	skb->priority = 0;
 	skb->dev = dev;
 
+	PN_PRINTK("pn_send rdev %x sdev %x res %x robj %x sobj %x netdev=%s\n",
+		ph->pn_rdev, ph->pn_sdev, ph->pn_res,
+		ph->pn_robj, ph->pn_sobj, dev->name);
+	PN_DATA_PRINTK("PHONET : skb  data = %d\nPHONET :", skb->len);
+	for (i = 1; i <= skb->len; i++) {
+		PN_DATA_PRINTK(" %02x", skb->data[i-1]);
+		if ((i%8) == 0)
+			PN_DATA_PRINTK("\n");
+	}
+
 	if (skb->pkt_type == PACKET_LOOPBACK) {
 		skb_reset_mac_header(skb);
 		skb_orphan(skb);
@@ -382,6 +439,7 @@ static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
 	struct phonethdr *ph;
 	struct sockaddr_pn sa;
 	u16 len;
+	int i;
 
 	/* check we have at least a full Phonet header */
 	if (!pskb_pull(skb, sizeof(struct phonethdr)))
@@ -399,6 +457,16 @@ static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
 
 	pn_skb_get_dst_sockaddr(skb, &sa);
 
+	PN_PRINTK("PN rcv: hdr rdev %x sdev %x res %x robj %x sobj %x dev=%s\n",
+		ph->pn_rdev, ph->pn_sdev, ph->pn_res,
+		ph->pn_robj, ph->pn_sobj, dev->name);
+	PN_DATA_PRINTK("PHONET : skb  data = %d\nPHONET :", skb->len);
+	for (i = 1; i <= skb->len; i++) {
+		PN_DATA_PRINTK(" %02x", skb->data[i-1]);
+		if ((i%8) == 0)
+			PN_DATA_PRINTK("\n");
+	}
+
 	/* check if this is multicasted */
 	if (pn_sockaddr_get_object(&sa) == PNOBJECT_MULTICAST) {
 		pn_deliver_sock_broadcast(net, skb);
@@ -519,6 +587,12 @@ static int __init phonet_init(void)
 {
 	int err;
 
+#ifdef ACTIVATE_PHONET_DEBUG
+	err = sysfs_create_file(kernel_kobj, &phonet_attr.attr);
+	if (err)
+		printk(KERN_DEBUG "phonet sysfs_create_file failed: %d\n", err);
+#endif
+
 	err = phonet_device_init();
 	if (err)
 		return err;
diff --git a/net/phonet/ld_phonet.c b/net/phonet/ld_phonet.c
new file mode 100644
index 000000000000..10af8c030bda
--- /dev/null
+++ b/net/phonet/ld_phonet.c
@@ -0,0 +1,645 @@
+/*
+ * Phonet device TTY line discipline
+ *
+ * Copyright (c) 1999-2002 RMC
+ *
+ *
+ *
+ */
+
+/*
+ * 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.
+ */
+
+#include <linux/uaccess.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/serio.h>
+#include <linux/tty.h>
+
+#include <asm/unaligned.h>
+#include <net/sock.h>
+#include <linux/errno.h>
+
+#include <linux/if_arp.h>
+#include <linux/if_phonet.h>
+#include <linux/phonet.h>
+#include <net/phonet/phonet.h>
+#include <net/phonet/pn_dev.h>
+
+MODULE_AUTHOR("david RMC");
+MODULE_DESCRIPTION("Phonet TTY line discipline");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_LDISC(N_PHONET);
+
+#define SEND_QUEUE_LOW 10
+#define SEND_QUEUE_HIGH 100
+#define PHONET_SENDING	        1 /* Bit 1 = 0x02*/
+#define PHONET_FLOW_OFF_SENT	4 /* Bit 4 = 0x10 */
+#define MAX_WRITE_CHUNK	       8192
+#define ISI_MSG_HEADER_SIZE 6
+#define MAX_BUFF_SIZE 20000
+
+#define LD_PHONET_NEW_ISI_MSG     0
+#define LD_PHONET_ISI_MSG_LEN     1
+#define LD_PHONET_ISI_MSG_NO_LEN  2
+
+#define PN_MEDIA_USB    0x1B
+
+struct ld_phonet {
+	struct tty_struct *tty;
+	wait_queue_head_t wait;
+	spinlock_t lock;
+	unsigned long flags;
+	struct sk_buff *skb;
+	unsigned long len;
+	unsigned long lentorcv;
+	unsigned long datarcv ;
+	unsigned long state;
+	struct net_device *dev;
+	struct list_head node;
+	struct sk_buff_head head;
+	char *tty_name;
+	int  ld_phonet_state;
+	int n_Data_Processed;
+	int n_Data_Sent;
+	int n_Remaining_Data;
+	bool link_up;
+	int nb_try_to_tx;
+};
+
+
+static int ld_pn_net_open(struct net_device *dev)
+{
+	netif_wake_queue(dev);
+	return 0;
+}
+
+static int ld_pn_net_close(struct net_device *dev)
+{
+	netif_stop_queue(dev);
+	return 0;
+}
+
+static int ld_pn_handle_tx(struct ld_phonet *ld_pn)
+{
+	struct tty_struct *tty = ld_pn->tty;
+	struct sk_buff *skb;
+	int tty_wr, len, room, i;
+	PN_PRINTK("Write Data in tty\n");
+	if (tty == NULL)
+		return 0;
+	/* Enter critical section */
+	if (test_and_set_bit(PHONET_SENDING, &ld_pn->state))
+		return 0;
+
+	/* skb_peek is safe because handle_tx is called after skb_queue_tail */
+	while ((skb = skb_peek(&ld_pn->head)) != NULL) {
+
+		/* Make sure you don't write too much */
+		len = skb->len;
+		room = tty_write_room(tty);
+
+		if (!room) {
+			if (ld_pn->nb_try_to_tx++ > 40) {
+				ld_pn->link_up = false;
+				/* Flush TX queue */
+				while ((skb = \
+				skb_dequeue(&ld_pn->head)) != NULL) {
+					skb->dev->stats.tx_dropped++;
+					if (in_interrupt())
+						dev_kfree_skb_irq(skb);
+					else
+						kfree_skb(skb);
+				}
+			}
+			break;
+		}
+
+		/* Get room => reset nb_try_to_tx counter */
+		ld_pn->nb_try_to_tx = 0;
+
+		if (room > MAX_WRITE_CHUNK)
+			room = MAX_WRITE_CHUNK;
+		if (len > room)
+			len = room;
+
+
+		tty_wr = tty->ops->write(tty, skb->data, len);
+		ld_pn->dev->stats.tx_packets++;
+		ld_pn->dev->stats.tx_bytes += tty_wr;
+		PN_DATA_PRINTK("PHONET: write data in tty\n");
+		for (i = 1; i <= len; i++) {
+			PN_DATA_PRINTK(" %02x", skb->data[i-1]);
+			if ((i%8) == 0)
+				PN_DATA_PRINTK("\n");
+		}
+		PN_DATA_PRINTK("\n");
+		/* Error on TTY ?! */
+		if (tty_wr < 0)
+			goto error;
+		/* Reduce buffer written, and discard if empty */
+		skb_pull(skb, tty_wr);
+		if (skb->len == 0) {
+			struct sk_buff *tmp = skb_dequeue(&ld_pn->head);
+			BUG_ON(tmp != skb);
+			if (in_interrupt())
+				dev_kfree_skb_irq(skb);
+			else
+				kfree_skb(skb);
+		}
+	}
+	/* Send flow off if queue is empty */
+	clear_bit(PHONET_SENDING, &ld_pn->state);
+
+	return 0;
+error:
+	clear_bit(PHONET_SENDING, &ld_pn->state);
+	return tty_wr;
+}
+
+
+
+static int ld_pn_net_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct ld_phonet *ld_pn;
+	u8 *ptr;
+
+	BUG_ON(dev == NULL);
+	ld_pn = netdev_priv(dev);
+	/* Add special Pattern before each ISI message */
+	ptr = skb_push(skb, 6);
+	ptr[0] = 0xdd;
+	ptr[1] = 0x7f;
+	ptr[2] = 0x21;
+	ptr[3] = 0x9a;
+	ptr[4] = skb->data[10];
+	ptr[5] = skb->data[11];
+
+	if (ld_pn->link_up == true) {
+		skb_queue_tail(&ld_pn->head, skb);
+		return ld_pn_handle_tx(ld_pn);
+	} else {
+		if (tty_write_room(ld_pn->tty)) {
+			/* link is up again */
+			ld_pn->link_up = true;
+			ld_pn->nb_try_to_tx = 0;
+
+			skb_queue_tail(&ld_pn->head, skb);
+			return ld_pn_handle_tx(ld_pn);
+		} else {
+			if (in_interrupt())
+				dev_kfree_skb_irq(skb);
+			else
+				kfree_skb(skb);
+			dev->stats.tx_dropped++;
+			return NETDEV_TX_OK;
+			}
+		}
+}
+
+static int
+ld_pn_net_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	int ret = 0;
+	switch (cmd) {
+	case SIOCPNGAUTOCONF:
+		ret = phonet_address_add(dev, PN_MEDIA_USB);
+		if (ret)
+			return ret;
+		phonet_address_notify(RTM_NEWADDR, dev, PN_MEDIA_USB);
+		phonet_route_add(dev, PN_DEV_PC);
+		dev_open(dev);
+		netif_carrier_on(dev);
+		/* Return NOIOCTLCMD so Phonet won't do it again */
+		return -ENOIOCTLCMD;
+	}
+	return -ENOIOCTLCMD;
+}
+
+static int ld_pn_net_mtu(struct net_device *dev, int new_mtu)
+{
+	if ((new_mtu < PHONET_MIN_MTU) || (new_mtu > PHONET_MAX_MTU))
+		return -EINVAL;
+	dev->mtu = new_mtu;
+	return 0;
+}
+
+
+
+static const struct net_device_ops ld_pn_netdev_ops = {
+	.ndo_open	= ld_pn_net_open,
+	.ndo_stop	= ld_pn_net_close,
+	.ndo_start_xmit	= ld_pn_net_xmit,
+	.ndo_do_ioctl   = ld_pn_net_ioctl,
+	.ndo_change_mtu	= ld_pn_net_mtu,
+};
+
+
+
+
+
+static void ld_pn_net_setup(struct net_device *dev)
+{
+	dev->features		= 0;
+	dev->type		= ARPHRD_PHONET;
+	dev->flags		= IFF_POINTOPOINT | IFF_NOARP;
+	dev->mtu		= PHONET_DEV_MTU;
+	dev->hard_header_len	= 1;
+	dev->dev_addr[0]	= PN_MEDIA_USB;
+	dev->addr_len		= 1;
+	dev->tx_queue_len	= 5;
+
+	dev->netdev_ops		= &ld_pn_netdev_ops;
+	dev->destructor		= free_netdev;
+	dev->header_ops		= &phonet_header_ops;
+};
+
+
+/*****************************************
+*** TTY
+******************************************/
+static int ld_phonet_ldisc_open(struct tty_struct *tty)
+{
+
+	struct ld_phonet *ld_pn;
+	struct net_device *dev;
+	int err = 0;
+	/* Create net device */
+	dev = alloc_netdev(sizeof(*ld_pn), "upnlink%d", ld_pn_net_setup);
+	if (!dev)
+		return -ENOMEM;
+
+	ld_pn = netdev_priv(dev);
+	spin_lock_init(&ld_pn->lock);
+	netif_carrier_off(dev);
+	skb_queue_head_init(&ld_pn->head);
+	ld_pn->tty = tty;
+	tty->disc_data = ld_pn;
+	tty->receive_room = 65536;
+	ld_pn->dev = dev;
+	ld_pn->skb = NULL;
+	ld_pn->len = 0;
+	ld_pn->lentorcv = 0;
+	ld_pn->datarcv = 0 ;
+	ld_pn->ld_phonet_state = LD_PHONET_NEW_ISI_MSG;
+	ld_pn->n_Data_Processed = 0;
+	ld_pn->n_Data_Sent = 0;
+	ld_pn->n_Remaining_Data = 0;
+	ld_pn->link_up = true;
+	ld_pn->nb_try_to_tx = 0;
+
+	err = register_netdev(dev);
+
+	if (err)
+		free_netdev(dev);
+
+
+	return err;
+
+}
+
+
+
+static void ld_phonet_ldisc_close(struct tty_struct *tty)
+{
+	struct ld_phonet *ld_pn = tty->disc_data;
+
+	tty->disc_data = NULL;
+	ld_pn->tty = NULL;
+	unregister_netdev(ld_pn->dev);
+	/*free_netdev(ld_pn->dev); David a checker*/
+}
+
+static void ld_phonet_ldisc_initiate_transfer \
+(struct ld_phonet *ld_pn, const unsigned char *cp, int count)
+{
+
+	struct sk_buff *skb = NULL;
+	unsigned int msglen = 0;
+	int i = 0;
+
+	struct phonethdr *ph = NULL;
+
+	/* Check if there is still data in cp */
+	while (ld_pn->n_Data_Processed < count) {
+		/* Check if extract length is possible */
+		if (count > ISI_MSG_HEADER_SIZE) {
+			/* Extract length */
+			/* Move one byte since media parameter
+				 is not there in phonethdr structure */
+			ph = (struct phonethdr *) \
+			(cp + ld_pn->n_Data_Processed + sizeof(char));
+			msglen = get_unaligned_be16(&ph->pn_length);
+			ld_pn->len = msglen + ISI_MSG_HEADER_SIZE;
+
+			/* Alloc SKBuff */
+			skb = netdev_alloc_skb(ld_pn->dev, ld_pn->len);
+			if (NULL == skb) {
+				/* TBD handle error */
+				return;
+			}
+
+			skb->dev = ld_pn->dev;
+			skb->protocol = htons(ETH_P_PHONET);
+			skb_reset_mac_header(skb);
+			ld_pn->skb = skb;
+
+			/* check if we receive complete
+				 data in this usb frame */
+			if (ld_pn->len <= count) {
+				/* We receive complete data
+					in this usb frame */
+				/* copy the ISI buffer */
+				memcpy(skb_put(skb, ld_pn->len), \
+				cp + ld_pn->n_Data_Processed, ld_pn->len);
+				ld_pn->n_Data_Processed += ld_pn->len;
+
+				/* Send to Phonet */
+				ld_pn->dev->stats.rx_packets++;
+				ld_pn->dev->stats.rx_bytes += skb->len;
+				__skb_pull(skb, 1);
+				/* we remove media id
+				(Why ? because we always do it ;-)) */
+
+				netif_rx(skb);
+				ld_pn->n_Data_Sent += ld_pn->len;
+
+				/* TBD : Reset pointers */
+			} else {
+				/* We receive only partial ISI message */
+				/* Copy the partial ISI message */
+				memcpy(skb_put(skb, count - \
+				ld_pn->n_Data_Sent), cp + \
+				ld_pn->n_Data_Processed, count - \
+				ld_pn->n_Data_Sent);
+				ld_pn->ld_phonet_state = LD_PHONET_ISI_MSG_LEN;
+				ld_pn->n_Remaining_Data = ld_pn->len - \
+				(count - ld_pn->n_Data_Sent);
+				ld_pn->n_Data_Processed += count - \
+				ld_pn->n_Data_Sent;
+
+				return;
+			}
+		} else {
+			/* Not able to extract length since received
+				 usb frame length is
+				less than ISI message header size */
+
+			/* Alloc SKBuff with max size */
+			skb = netdev_alloc_skb(ld_pn->dev, MAX_BUFF_SIZE);
+			if (NULL == skb) {
+				/* TBD handle error */
+				return;
+			}
+
+			skb->dev = ld_pn->dev;
+			skb->protocol = htons(ETH_P_PHONET);
+			skb_reset_mac_header(skb);
+			ld_pn->skb = skb;
+
+			/* Copy available data */
+			memcpy(skb_put(skb, count - ld_pn->n_Data_Sent), \
+			cp + ld_pn->n_Data_Processed, count - \
+			ld_pn->n_Data_Sent);
+			ld_pn->ld_phonet_state = LD_PHONET_ISI_MSG_NO_LEN;
+			ld_pn->n_Data_Processed += count - ld_pn->n_Data_Sent;
+			ld_pn->len += count - ld_pn->n_Data_Sent;
+
+			return;
+		}
+	}
+	/* No more data in cp */
+	ld_pn->ld_phonet_state = LD_PHONET_NEW_ISI_MSG;
+	ld_pn->len = 0;
+	ld_pn->n_Data_Processed = 0;
+	ld_pn->n_Data_Sent = 0;
+	ld_pn->n_Remaining_Data = 0;
+
+	return;
+}
+
+
+static void ld_phonet_ldisc_receive
+(struct tty_struct *tty, const unsigned char *cp, char *fp, int count)
+{
+	struct ld_phonet *ld_pn = tty->disc_data;
+	struct sk_buff *skb = ld_pn->skb;
+	unsigned long flags = 0;
+	unsigned int msglen = 0;
+	unsigned int i = 0;
+
+	struct phonethdr *ph = NULL;
+
+	PN_DATA_PRINTK("PHONET : Receive  Data From tty = %d\nPHONET :", count);
+	for (i = 1; i <= count; i++) {
+		PN_DATA_PRINTK(" %02x", cp[i-1]);
+		if ((i%8) == 0)
+			PN_DATA_PRINTK("\n");
+	}
+
+	if (ld_pn->link_up == false) {
+		/* data received from PC => can TX */
+		ld_pn->link_up = true;
+
+		ld_pn->nb_try_to_tx = 0;
+	}
+
+	spin_lock_irqsave(&ld_pn->lock, flags);
+
+	/*Whenever you receive a new USB frame Data Processed should be reset*/
+	ld_pn->n_Data_Processed = 0;
+
+	switch (ld_pn->ld_phonet_state) {
+	case LD_PHONET_NEW_ISI_MSG:
+		PN_PRINTK("ld_phonet : new_isi_msg\n");
+		ld_phonet_ldisc_initiate_transfer(ld_pn, cp, count);
+		break;
+
+	case LD_PHONET_ISI_MSG_LEN:
+		/* check if Remaining Data is complete */
+		PN_PRINTK("ld_phonet : isi_msg_len\n");
+		if (ld_pn->n_Remaining_Data > count) {
+			/* We dont receive complete data */
+			/* Copy the available data */
+			memcpy(skb_put(skb, count), cp + \
+			ld_pn->n_Data_Processed, count);
+			ld_pn->n_Data_Processed += count;
+			ld_pn->ld_phonet_state = LD_PHONET_ISI_MSG_LEN;
+			ld_pn->n_Remaining_Data -= count;
+		} else {
+			/* We have complete data available */
+			/* Copy remaining data */
+			memcpy(skb_put(skb, ld_pn->n_Remaining_Data), \
+			cp + ld_pn->n_Data_Processed, ld_pn->n_Remaining_Data);
+			/* Send to Phonet */
+			ld_pn->dev->stats.rx_packets++;
+			ld_pn->dev->stats.rx_bytes += skb->len;
+			__skb_pull(skb, sizeof(char));
+			netif_rx(skb);
+			ld_pn->n_Data_Sent += ld_pn->len;
+
+			/* TBD : Update pointers */
+			ld_pn->n_Data_Sent += ld_pn->n_Remaining_Data;
+			ld_pn->n_Data_Processed += ld_pn->n_Remaining_Data;
+
+			/* Initiate a new ISI transfer */
+			ld_phonet_ldisc_initiate_transfer(ld_pn, cp, count);
+			}
+			break;
+
+	case LD_PHONET_ISI_MSG_NO_LEN:
+		/*Check if we can extact length */
+		PN_PRINTK("ld_phonet : isi_msg_no_len\n");
+		if ((ld_pn->len + count) >= ISI_MSG_HEADER_SIZE) {
+
+			/* Copy remaining header to SKBuff to extract length */
+			memcpy(skb_put(skb, ISI_MSG_HEADER_SIZE - ld_pn->len),\
+			cp + ld_pn->n_Data_Processed, ISI_MSG_HEADER_SIZE - \
+			ld_pn->len);
+			ph = (struct phonethdr *) (skb->data + sizeof(char));
+			msglen = get_unaligned_be16(&ph->pn_length);
+
+			ld_pn->n_Data_Processed += \
+			ISI_MSG_HEADER_SIZE - ld_pn->len;
+
+			/* Check if we receive complete data */
+			if ((count + ld_pn->len) < \
+				(msglen + ISI_MSG_HEADER_SIZE)) {
+				/* We have not received complete data */
+				/* Copy available data */
+				memcpy(skb_put(skb, count - \
+				(ISI_MSG_HEADER_SIZE - ld_pn->len)), \
+				cp + ld_pn->n_Data_Processed, count - \
+				(ISI_MSG_HEADER_SIZE - ld_pn->len));
+				ld_pn->ld_phonet_state = LD_PHONET_ISI_MSG_LEN;
+				ld_pn->n_Remaining_Data = (msglen + \
+				ISI_MSG_HEADER_SIZE) - (count + ld_pn->len);
+				ld_pn->n_Data_Processed += count - \
+				(ISI_MSG_HEADER_SIZE - ld_pn->len);
+
+				/* Reset pointers */
+				ld_pn->len = msglen + ISI_MSG_HEADER_SIZE;
+
+				/*return;*/
+				break;
+			} else {
+				/* We receive complete data */
+				/* Copy remaining data */
+				/*memcpy(skb_put(skb, msglen), cp + \
+				ld_pn->n_Data_Processed \
+				+ (ISI_MSG_HEADER_SIZE - ld_pn->len), \
+				(msglen + ISI_MSG_HEADER_SIZE) - ld_pn->len);*/
+				memcpy( \
+				skb_put(skb, (msglen + ISI_MSG_HEADER_SIZE) - \
+				(ld_pn->len + ld_pn->n_Data_Processed)), \
+				cp + ld_pn->n_Data_Processed, \
+				(msglen + ISI_MSG_HEADER_SIZE) - \
+				(ld_pn->len + ld_pn->n_Data_Processed));
+
+				/* Send to Phonet */
+				ld_pn->dev->stats.rx_packets++;
+				ld_pn->dev->stats.rx_bytes += skb->len;
+				__skb_pull(skb, sizeof(char));
+				netif_rx(skb);
+
+				/* Update pointers */
+				/*ld_pn->n_Data_Sent += \
+				(msglen + ISI_MSG_HEADER_SIZE) - ld_pn->len;
+				ld_pn->n_Data_Processed += \
+				(msglen + ISI_MSG_HEADER_SIZE) - ld_pn->len;*/
+
+				ld_pn->n_Data_Sent += \
+				(msglen + ISI_MSG_HEADER_SIZE) - \
+				(ld_pn->len + ld_pn->n_Data_Processed);
+
+				ld_pn->n_Data_Processed += \
+				(msglen + ISI_MSG_HEADER_SIZE) - \
+				(ld_pn->len + ld_pn->n_Data_Processed);
+
+				/* Check if we still have data in cp */
+				/*if (count > ld_pn->n_Data_Sent) {*/
+				if (count > ld_pn->n_Data_Processed) {
+					/* We still have data in cp */
+					/* Initiate a new ISI transfer */
+					ld_phonet_ldisc_initiate_transfer\
+							(ld_pn, cp, count);
+				} else {
+					/* No more data in cp */
+					ld_pn->ld_phonet_state = \
+					LD_PHONET_NEW_ISI_MSG;
+
+					/* Reset pointers */
+					ld_pn->len = 0;
+					ld_pn->n_Data_Processed = 0;
+					ld_pn->n_Data_Sent = 0;
+					ld_pn->n_Remaining_Data = 0;
+				}
+			}
+		} else {
+				/* Cannot extract length */
+				/* Copy available data */
+				memcpy(skb_put(skb, count), cp + \
+				ld_pn->n_Data_Processed, count);
+				ld_pn->len += count;
+				ld_pn->ld_phonet_state = \
+				LD_PHONET_ISI_MSG_NO_LEN;
+				ld_pn->n_Data_Processed += count;
+			}
+
+		break;
+
+	default:
+		break;
+	}
+
+	spin_unlock_irqrestore(&ld_pn->lock, flags);
+
+}
+
+static void ld_phonet_ldisc_write_wakeup(struct tty_struct *tty)
+{
+
+	struct ld_phonet *ld_pn;
+	ld_pn = tty->disc_data;
+	BUG_ON(ld_pn == NULL);
+	BUG_ON(ld_pn->tty != tty);
+	ld_pn_handle_tx(ld_pn);
+}
+
+static struct tty_ldisc_ops ld_phonet_ldisc = {
+	.owner =	THIS_MODULE,
+	.name =		"phonet",
+	.open =		ld_phonet_ldisc_open,
+	.close =	ld_phonet_ldisc_close,
+	.receive_buf =	ld_phonet_ldisc_receive,
+	.write_wakeup =	ld_phonet_ldisc_write_wakeup
+};
+
+/*
+ * The functions for insering/removing us as a module.
+ */
+
+static int __init ld_phonet_init(void)
+{
+	int retval;
+	retval = tty_register_ldisc(N_PHONET, &ld_phonet_ldisc);
+
+	return  retval;
+}
+
+static void __exit ld_phonet_exit(void)
+{
+	tty_unregister_ldisc(N_PHONET);
+}
+
+
+
+
+module_init(ld_phonet_init);
+module_exit(ld_phonet_exit);