tty: n_gsm: Fix for modems with brk in modem status control

[pandora-kernel.git] / drivers / tty / n_gsm.c

diff --git a/drivers/tty/n_gsm.c b/drivers/tty/n_gsm.c
index 1460ab3..643a0a0 100644 (file)
--- a/drivers/tty/n_gsm.c
+++ b/drivers/tty/n_gsm.c
@@ -108,7 +108,7 @@ struct gsm_mux_net {
  */
 
 struct gsm_msg {
-       struct gsm_msg *next;
+       struct list_head list;
        u8 addr;                /* DLCI address + flags */
        u8 ctrl;                /* Control byte + flags */
        unsigned int len;       /* Length of data block (can be zero) */
@@ -245,8 +245,7 @@ struct gsm_mux {
        unsigned int tx_bytes;          /* TX data outstanding */
 #define TX_THRESH_HI           8192
 #define TX_THRESH_LO           2048
-       struct gsm_msg *tx_head;        /* Pending data packets */
-       struct gsm_msg *tx_tail;
+       struct list_head tx_list;       /* Pending data packets */
 
        /* Control messages */
        struct timer_list t2_timer;     /* Retransmit timer for commands */
@@ -663,7 +662,7 @@ static struct gsm_msg *gsm_data_alloc(struct gsm_mux *gsm, u8 addr, int len,
        m->len = len;
        m->addr = addr;
        m->ctrl = ctrl;
-       m->next = NULL;
+       INIT_LIST_HEAD(&m->list);
        return m;
 }
 
@@ -681,16 +680,13 @@ static struct gsm_msg *gsm_data_alloc(struct gsm_mux *gsm, u8 addr, int len,
 
 static void gsm_data_kick(struct gsm_mux *gsm)
 {
-       struct gsm_msg *msg = gsm->tx_head;
-       struct gsm_msg *free_msg;
+       struct gsm_msg *msg, *nmsg;
        int len;
        int skip_sof = 0;
 
-       while (msg) {
-               if (gsm->constipated && msg->addr) {
-                       msg = msg->next;
+       list_for_each_entry_safe(msg, nmsg, &gsm->tx_list, list) {
+               if (gsm->constipated && msg->addr)
                        continue;
-               }
                if (gsm->encoding != 0) {
                        gsm->txframe[0] = GSM1_SOF;
                        len = gsm_stuff_frame(msg->data,
@@ -718,14 +714,9 @@ static void gsm_data_kick(struct gsm_mux *gsm)
                   burst */
                skip_sof = 1;
 
-               if (gsm->tx_head == msg)
-                       gsm->tx_head = msg->next;
-               free_msg = msg;
-               msg = msg->next;
-               kfree(free_msg);
+               list_del(&msg->list);
+               kfree(msg);
        }
-       if (!gsm->tx_head)
-               gsm->tx_tail = NULL;
 }
 
 /**
@@ -774,11 +765,7 @@ static void __gsm_data_queue(struct gsm_dlci *dlci, struct gsm_msg *msg)
        msg->data = dp;
 
        /* Add to the actual output queue */
-       if (gsm->tx_tail)
-               gsm->tx_tail->next = msg;
-       else
-               gsm->tx_head = msg;
-       gsm->tx_tail = msg;
+       list_add_tail(&msg->list, &gsm->tx_list);
        gsm->tx_bytes += msg->len;
        gsm_data_kick(gsm);
 }
@@ -881,7 +868,7 @@ static int gsm_dlci_data_output_framed(struct gsm_mux *gsm,
 
        /* dlci->skb is locked by tx_lock */
        if (dlci->skb == NULL) {
-               dlci->skb = skb_dequeue(&dlci->skb_list);
+               dlci->skb = skb_dequeue_tail(&dlci->skb_list);
                if (dlci->skb == NULL)
                        return 0;
                first = 1;
@@ -892,7 +879,7 @@ static int gsm_dlci_data_output_framed(struct gsm_mux *gsm,
        if (len > gsm->mtu) {
                if (dlci->adaption == 3) {
                        /* Over long frame, bin it */
-                       kfree_skb(dlci->skb);
+                       dev_kfree_skb_any(dlci->skb);
                        dlci->skb = NULL;
                        return 0;
                }
@@ -905,8 +892,11 @@ static int gsm_dlci_data_output_framed(struct gsm_mux *gsm,
 
        /* FIXME: need a timer or something to kick this so it can't
           get stuck with no work outstanding and no buffer free */
-       if (msg == NULL)
+       if (msg == NULL) {
+               skb_queue_tail(&dlci->skb_list, dlci->skb);
+               dlci->skb = NULL;
                return -ENOMEM;
+       }
        dp = msg->data;
 
        if (dlci->adaption == 4) { /* Interruptible framed (Packetised Data) */
@@ -918,7 +908,7 @@ static int gsm_dlci_data_output_framed(struct gsm_mux *gsm,
        skb_pull(dlci->skb, len);
        __gsm_data_queue(dlci, msg);
        if (last) {
-               kfree_skb(dlci->skb);
+               dev_kfree_skb_any(dlci->skb);
                dlci->skb = NULL;
        }
        return size;
@@ -1100,6 +1090,7 @@ static void gsm_control_modem(struct gsm_mux *gsm, u8 *data, int clen)
 {
        unsigned int addr = 0;
        unsigned int modem = 0;
+       unsigned int brk = 0;
        struct gsm_dlci *dlci;
        int len = clen;
        u8 *dp = data;
@@ -1126,6 +1117,16 @@ static void gsm_control_modem(struct gsm_mux *gsm, u8 *data, int clen)
                if (len == 0)
                        return;
        }
+       len--;
+       if (len > 0) {
+               while (gsm_read_ea(&brk, *dp++) == 0) {
+                       len--;
+                       if (len == 0)
+                               return;
+               }
+               modem <<= 7;
+               modem |= (brk & 0x7f);
+       }
        tty = tty_port_tty_get(&dlci->port);
        gsm_process_modem(tty, dlci, modem, clen);
        if (tty) {
@@ -1205,6 +1206,8 @@ static void gsm_control_message(struct gsm_mux *gsm, unsigned int command,
                                                        u8 *data, int clen)
 {
        u8 buf[1];
+       unsigned long flags;
+
        switch (command) {
        case CMD_CLD: {
                struct gsm_dlci *dlci = gsm->dlci[0];
@@ -1225,7 +1228,9 @@ static void gsm_control_message(struct gsm_mux *gsm, unsigned int command,
                gsm->constipated = 0;
                gsm_control_reply(gsm, CMD_FCON, NULL, 0);
                /* Kick the link in case it is idling */
+               spin_lock_irqsave(&gsm->tx_lock, flags);
                gsm_data_kick(gsm);
+               spin_unlock_irqrestore(&gsm->tx_lock, flags);
                break;
        case CMD_FCOFF:
                /* Modem wants us to STFU */
@@ -1683,7 +1688,7 @@ static void gsm_dlci_free(struct kref *ref)
        dlci->gsm->dlci[dlci->addr] = NULL;
        kfifo_free(dlci->fifo);
        while ((dlci->skb = skb_dequeue(&dlci->skb_list)))
-               kfree_skb(dlci->skb);
+               dev_kfree_skb(dlci->skb);
        kfree(dlci);
 }
 
@@ -2022,7 +2027,7 @@ void gsm_cleanup_mux(struct gsm_mux *gsm)
 {
        int i;
        struct gsm_dlci *dlci = gsm->dlci[0];
-       struct gsm_msg *txq;
+       struct gsm_msg *txq, *ntxq;
        struct gsm_control *gc;
 
        gsm->dead = 1;
@@ -2057,11 +2062,9 @@ void gsm_cleanup_mux(struct gsm_mux *gsm)
                if (gsm->dlci[i])
                        gsm_dlci_release(gsm->dlci[i]);
        /* Now wipe the queues */
-       for (txq = gsm->tx_head; txq != NULL; txq = gsm->tx_head) {
-               gsm->tx_head = txq->next;
+       list_for_each_entry_safe(txq, ntxq, &gsm->tx_list, list)
                kfree(txq);
-       }
-       gsm->tx_tail = NULL;
+       INIT_LIST_HEAD(&gsm->tx_list);
 }
 EXPORT_SYMBOL_GPL(gsm_cleanup_mux);
 
@@ -2172,6 +2175,7 @@ struct gsm_mux *gsm_alloc_mux(void)
        }
        spin_lock_init(&gsm->lock);
        kref_init(&gsm->ref);
+       INIT_LIST_HEAD(&gsm->tx_list);
 
        gsm->t1 = T1;
        gsm->t2 = T2;
@@ -2392,12 +2396,12 @@ static void gsmld_write_wakeup(struct tty_struct *tty)
 
        /* Queue poll */
        clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
+       spin_lock_irqsave(&gsm->tx_lock, flags);
        gsm_data_kick(gsm);
        if (gsm->tx_bytes < TX_THRESH_LO) {
-               spin_lock_irqsave(&gsm->tx_lock, flags);
                gsm_dlci_data_sweep(gsm);
-               spin_unlock_irqrestore(&gsm->tx_lock, flags);
        }
+       spin_unlock_irqrestore(&gsm->tx_lock, flags);
 }
 
 /**