usb: gadget: phonet: Add Phonet over ACM for RMC
BH Hsieh [Thu, 1 Nov 2012 10:21:57 +0000 (18:21 +0800)]
* 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>

drivers/usb/gadget/android.c
include/net/phonet/phonet.h
include/uapi/linux/tty.h
net/phonet/Kconfig
net/phonet/Makefile
net/phonet/af_phonet.c
net/phonet/ld_phonet.c [new file with mode: 0644]

index 50bd12d..173e95a 100644 (file)
@@ -359,7 +359,7 @@ static void functionfs_release_dev_callback(struct ffs_data *ffs_data)
 {
 }
 
-#define MAX_ACM_INSTANCES 4
+#define MAX_ACM_INSTANCES 2
 struct acm_function_config {
        int instances;
        int instances_on;
index f23fcf1..b151b14 100644 (file)
@@ -120,4 +120,27 @@ void phonet_sysctl_exit(void);
 int isi_register(void);
 void isi_unregister(void);
 
+#ifdef CONFIG_PHONET_DEBUG
+#define ACTIVATE_PHONET_DEBUG
+#else
+#undef ACTIVATE_PHONET_DEBUG
+#endif
+
+#ifdef ACTIVATE_PHONET_DEBUG
+extern enum phonet_debug_state {
+       OFF = 0,
+       ON,
+       DATA,
+} phonet_dbg_state;
+
+#define PN_PRINTK(...) \
+       do { if (OFF != phonet_dbg_state) \
+               printk(KERN_DEBUG "PHONET: " __VA_ARGS__); } while (0)
+#define PN_DATA_PRINTK(...) \
+       do { if (DATA == phonet_dbg_state) printk(__VA_ARGS__); } while (0)
+#else
+#define PN_PRINTK(...)
+#define PN_DATA_PRINTK(...)
+#endif
+
 #endif
index dac199a..63f2580 100644 (file)
@@ -34,5 +34,6 @@
 #define N_TI_WL                22      /* for TI's WL BT, FM, GPS combo chips */
 #define N_TRACESINK    23      /* Trace data routing for MIPI P1149.7 */
 #define N_TRACEROUTER  24      /* Trace data routing for MIPI P1149.7 */
+#define N_PHONET   25  /* PHONET */
 
 #endif /* _UAPI_LINUX_TTY_H */
index 6ec7d55..dba14b0 100644 (file)
@@ -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
index e10b1b1..f219b2d 100644 (file)
@@ -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
index a6ecd47..da25b8a 100644 (file)
 #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 (file)
index 0000000..10af8c0
--- /dev/null
@@ -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);