1 files changed, 131 insertions, 211 deletions

diff --git a/drivers/mfd/twl4030-irq.c b/drivers/mfd/twl4030-irq.c
index 8a7ee3139b86..f062c8cc6c38 100644
--- a/drivers/mfd/twl4030-irq.c
+++ b/drivers/mfd/twl4030-irq.c
@@ -30,7 +30,6 @@
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
-#include <linux/kthread.h>
 #include <linux/slab.h>
 
 #include <linux/i2c/twl.h>
@@ -278,59 +277,6 @@ static const struct sih sih_modules_twl5031[8] = {
 
 static unsigned twl4030_irq_base;
 
-static struct completion irq_event;
-
-/*
- * This thread processes interrupts reported by the Primary Interrupt Handler.
- */
-static int twl4030_irq_thread(void *data)
-{
-        long irq = (long)data;
-        static unsigned i2c_errors;
-        static const unsigned max_i2c_errors = 100;
-
-
-        current->flags |= PF_NOFREEZE;
-
-        while (!kthread_should_stop()) {
-                int ret;
-                int module_irq;
-                u8 pih_isr;
-
-                /* Wait for IRQ, then read PIH irq status (also blocking) */
-                wait_for_completion_interruptible(&irq_event);
-
-                ret = twl_i2c_read_u8(TWL4030_MODULE_PIH, &pih_isr,
-                                          REG_PIH_ISR_P1);
-                if (ret) {
-                        pr_warning("twl4030: I2C error %d reading PIH ISR\n",
-                                        ret);
-                        if (++i2c_errors >= max_i2c_errors) {
-                                printk(KERN_ERR "Maximum I2C error count"
-                                                " exceeded.  Terminating %s.\n",
-                                                __func__);
-                                break;
-                        }
-                        complete(&irq_event);
-                        continue;
-                }
-
-                /* these handlers deal with the relevant SIH irq status */
-                local_irq_disable();
-                for (module_irq = twl4030_irq_base;
-                                pih_isr;
-                                pih_isr >>= 1, module_irq++) {
-                        if (pih_isr & 0x1)
-                                generic_handle_irq(module_irq);
-                }
-                local_irq_enable();
-
-                enable_irq(irq);
-        }
-
-        return 0;
-}
-
 /*
  * handle_twl4030_pih() is the desc->handle method for the twl4030 interrupt.
  * This is a chained interrupt, so there is no desc->action method for it.
@@ -342,9 +288,25 @@ static int twl4030_irq_thread(void *data)
  */
 static irqreturn_t handle_twl4030_pih(int irq, void *devid)
 {
-        /* Acknowledge, clear *AND* mask the interrupt... */
-        disable_irq_nosync(irq);
-        complete(devid);
+        int             module_irq;
+        irqreturn_t     ret;
+        u8              pih_isr;
+
+        ret = twl_i2c_read_u8(TWL4030_MODULE_PIH, &pih_isr,
+                        REG_PIH_ISR_P1);
+        if (ret) {
+                pr_warning("twl4030: I2C error %d reading PIH ISR\n", ret);
+                return IRQ_NONE;
+        }
+
+        /* these handlers deal with the relevant SIH irq status */
+        for (module_irq = twl4030_irq_base;
+                        pih_isr;
+                        pih_isr >>= 1, module_irq++) {
+                if (pih_isr & 0x1)
+                        handle_nested_irq(module_irq);
+        }
+
         return IRQ_HANDLED;
 }
 /*----------------------------------------------------------------------*/
@@ -460,113 +422,17 @@ static inline void activate_irq(int irq)
 
 /*----------------------------------------------------------------------*/
 
-static DEFINE_SPINLOCK(sih_agent_lock);
-
-static struct workqueue_struct *wq;
-
 struct sih_agent {
         int                     irq_base;
         const struct sih        *sih;
 
         u32                     imr;
         bool                    imr_change_pending;
-        struct work_struct      mask_work;
-
-        u32                     edge_change;
-        struct work_struct      edge_work;
-};
-
-static void twl4030_sih_do_mask(struct work_struct *work)
-{
-        struct sih_agent        *agent;
-        const struct sih        *sih;
-        union {
-                u8      bytes[4];
-                u32     word;
-        }                       imr;
-        int                     status;
 
-        agent = container_of(work, struct sih_agent, mask_work);
-
-        /* see what work we have */
-        spin_lock_irq(&sih_agent_lock);
-        if (agent->imr_change_pending) {
-                sih = agent->sih;
-                /* byte[0] gets overwritten as we write ... */
-                imr.word = cpu_to_le32(agent->imr << 8);
-                agent->imr_change_pending = false;
-        } else
-                sih = NULL;
-        spin_unlock_irq(&sih_agent_lock);
-        if (!sih)
-                return;
-
-        /* write the whole mask ... simpler than subsetting it */
-        status = twl_i2c_write(sih->module, imr.bytes,
-                        sih->mask[irq_line].imr_offset, sih->bytes_ixr);
-        if (status)
-                pr_err("twl4030: %s, %s --> %d\n", __func__,
-                                "write", status);
-}
-
-static void twl4030_sih_do_edge(struct work_struct *work)
-{
-        struct sih_agent        *agent;
-        const struct sih        *sih;
-        u8                      bytes[6];
         u32                     edge_change;
-        int                     status;
-
-        agent = container_of(work, struct sih_agent, edge_work);
-
-        /* see what work we have */
-        spin_lock_irq(&sih_agent_lock);
-        edge_change = agent->edge_change;
-        agent->edge_change = 0;
-        sih = edge_change ? agent->sih : NULL;
-        spin_unlock_irq(&sih_agent_lock);
-        if (!sih)
-                return;
-
-        /* Read, reserving first byte for write scratch.  Yes, this
-         * could be cached for some speedup ... but be careful about
-         * any processor on the other IRQ line, EDR registers are
-         * shared.
-         */
-        status = twl_i2c_read(sih->module, bytes + 1,
-                        sih->edr_offset, sih->bytes_edr);
-        if (status) {
-                pr_err("twl4030: %s, %s --> %d\n", __func__,
-                                "read", status);
-                return;
-        }
-
-        /* Modify only the bits we know must change */
-        while (edge_change) {
-                int             i = fls(edge_change) - 1;
-                struct irq_data *idata = irq_get_irq_data(i + agent->irq_base);
-                int             byte = 1 + (i >> 2);
-                int             off = (i & 0x3) * 2;
-                unsigned int    type;
-
-                bytes[byte] &= ~(0x03 << off);
 
-                type = irqd_get_trigger_type(idata);
-                if (type & IRQ_TYPE_EDGE_RISING)
-                        bytes[byte] |= BIT(off + 1);
-                if (type & IRQ_TYPE_EDGE_FALLING)
-                        bytes[byte] |= BIT(off + 0);
-
-                edge_change &= ~BIT(i);
-        }
-
-        /* Write */
-        status = twl_i2c_write(sih->module, bytes,
-                        sih->edr_offset, sih->bytes_edr);
-        if (status)
-                pr_err("twl4030: %s, %s --> %d\n", __func__,
-                                "write", status);
-}
+        struct mutex            irq_lock;
+};
 
 /*----------------------------------------------------------------------*/
 
@@ -579,50 +445,125 @@ static void twl4030_sih_do_edge(struct work_struct *work)
 
 static void twl4030_sih_mask(struct irq_data *data)
 {
-        struct sih_agent *sih = irq_data_get_irq_chip_data(data);
-        unsigned long flags;
-
-        spin_lock_irqsave(&sih_agent_lock, flags);
-        sih->imr |= BIT(data->irq - sih->irq_base);
-        sih->imr_change_pending = true;
-        queue_work(wq, &sih->mask_work);
-        spin_unlock_irqrestore(&sih_agent_lock, flags);
+        struct sih_agent *agent = irq_data_get_irq_chip_data(data);
+
+        agent->imr |= BIT(data->irq - agent->irq_base);
+        agent->imr_change_pending = true;
 }
 
 static void twl4030_sih_unmask(struct irq_data *data)
 {
-        struct sih_agent *sih = irq_data_get_irq_chip_data(data);
-        unsigned long flags;
-
-        spin_lock_irqsave(&sih_agent_lock, flags);
-        sih->imr &= ~BIT(data->irq - sih->irq_base);
-        sih->imr_change_pending = true;
-        queue_work(wq, &sih->mask_work);
-        spin_unlock_irqrestore(&sih_agent_lock, flags);
+        struct sih_agent *agent = irq_data_get_irq_chip_data(data);
+
+        agent->imr &= ~BIT(data->irq - agent->irq_base);
+        agent->imr_change_pending = true;
 }
 
 static int twl4030_sih_set_type(struct irq_data *data, unsigned trigger)
 {
-        struct sih_agent *sih = irq_data_get_irq_chip_data(data);
-        unsigned long flags;
+        struct sih_agent *agent = irq_data_get_irq_chip_data(data);
 
         if (trigger & ~(IRQ_TYPE_EDGE_FALLING | IRQ_TYPE_EDGE_RISING))
                 return -EINVAL;
 
-        spin_lock_irqsave(&sih_agent_lock, flags);
-        if (irqd_get_trigger_type(data) != trigger) {
-                sih->edge_change |= BIT(data->irq - sih->irq_base);
-                queue_work(wq, &sih->edge_work);
-        }
-        spin_unlock_irqrestore(&sih_agent_lock, flags);
+        if (irqd_get_trigger_type(data) != trigger)
+                agent->edge_change |= BIT(data->irq - agent->irq_base);
+
         return 0;
 }
 
+static void twl4030_sih_bus_lock(struct irq_data *data)
+{
+        struct sih_agent        *agent = irq_data_get_irq_chip_data(data);
+
+        mutex_lock(&agent->irq_lock);
+}
+
+static void twl4030_sih_bus_sync_unlock(struct irq_data *data)
+{
+        struct sih_agent        *agent = irq_data_get_irq_chip_data(data);
+        const struct sih        *sih = agent->sih;
+        int                     status;
+
+        if (agent->imr_change_pending) {
+                union {
+                        u32     word;
+                        u8      bytes[4];
+                } imr;
+
+                /* byte[0] gets overwriten as we write ... */
+                imr.word = cpu_to_le32(agent->imr << 8);
+                agent->imr_change_pending = false;
+
+                /* write the whole mask ... simpler than subsetting it */
+                status = twl_i2c_write(sih->module, imr.bytes,
+                                sih->mask[irq_line].imr_offset,
+                                sih->bytes_ixr);
+                if (status)
+                        pr_err("twl4030: %s, %s --> %d\n", __func__,
+                                        "write", status);
+        }
+
+        if (agent->edge_change) {
+                u32             edge_change;
+                u8              bytes[6];
+
+                edge_change = agent->edge_change;
+                agent->edge_change = 0;
+
+                /*
+                 * Read, reserving first byte for write scratch.  Yes, this
+                 * could be cached for some speedup ... but be careful about
+                 * any processor on the other IRQ line, EDR registers are
+                 * shared.
+                 */
+                status = twl_i2c_read(sih->module, bytes + 1,
+                                sih->edr_offset, sih->bytes_edr);
+                if (status) {
+                        pr_err("twl4030: %s, %s --> %d\n", __func__,
+                                        "read", status);
+                        return;
+                }
+
+                /* Modify only the bits we know must change */
+                while (edge_change) {
+                        int             i = fls(edge_change) - 1;
+                        struct irq_data *idata;
+                        int             byte = 1 + (i >> 2);
+                        int             off = (i & 0x3) * 2;
+                        unsigned int    type;
+
+                        idata = irq_get_irq_data(i + agent->irq_base);
+
+                        bytes[byte] &= ~(0x03 << off);
+
+                        type = irqd_get_trigger_type(idata);
+                        if (type & IRQ_TYPE_EDGE_RISING)
+                                bytes[byte] |= BIT(off + 1);
+                        if (type & IRQ_TYPE_EDGE_FALLING)
+                                bytes[byte] |= BIT(off + 0);
+
+                        edge_change &= ~BIT(i);
+                }
+
+                /* Write */
+                status = twl_i2c_write(sih->module, bytes,
+                                sih->edr_offset, sih->bytes_edr);
+                if (status)
+                        pr_err("twl4030: %s, %s --> %d\n", __func__,
+                                        "write", status);
+        }
+
+        mutex_unlock(&agent->irq_lock);
+}
+
 static struct irq_chip twl4030_sih_irq_chip = {
         .name           = "twl4030",
-        .irq_mask       = twl4030_sih_mask,
+        .irq_mask       = twl4030_sih_mask,
         .irq_unmask     = twl4030_sih_unmask,
         .irq_set_type   = twl4030_sih_set_type,
+        .irq_bus_lock   = twl4030_sih_bus_lock,
+        .irq_bus_sync_unlock = twl4030_sih_bus_sync_unlock,
 };
 
 /*----------------------------------------------------------------------*/
@@ -655,9 +596,7 @@ static void handle_twl4030_sih(unsigned irq, struct irq_desc *desc)
         int isr;
 
         /* reading ISR acks the IRQs, using clear-on-read mode */
-        local_irq_enable();
         isr = sih_read_isr(sih);
-        local_irq_disable();
 
         if (isr < 0) {
                 pr_err("twl4030: %s SIH, read ISR error %d\n",
@@ -672,7 +611,7 @@ static void handle_twl4030_sih(unsigned irq, struct irq_desc *desc)
                 isr &= ~BIT(irq);
 
                 if (irq < sih->bits)
-                        generic_handle_irq(agent->irq_base + irq);
+                        handle_nested_irq(agent->irq_base + irq);
                 else
                         pr_err("twl4030: %s SIH, invalid ISR bit %d\n",
                                 sih->name, irq);
@@ -718,15 +657,14 @@ int twl4030_sih_setup(int module)
         agent->irq_base = irq_base;
         agent->sih = sih;
         agent->imr = ~0;
-        INIT_WORK(&agent->mask_work, twl4030_sih_do_mask);
-        INIT_WORK(&agent->edge_work, twl4030_sih_do_edge);
+        mutex_init(&agent->irq_lock);
 
         for (i = 0; i < sih->bits; i++) {
                 irq = irq_base + i;
 
+                irq_set_chip_data(irq, agent);
                 irq_set_chip_and_handler(irq, &twl4030_sih_irq_chip,
                                          handle_edge_irq);
-                irq_set_chip_data(irq, agent);
                 activate_irq(irq);
         }
 
@@ -758,7 +696,6 @@ int twl4030_init_irq(int irq_num, unsigned irq_base, unsigned irq_end)
 
         int                     status;
         int                     i;
-        struct task_struct      *task;
 
         /*
          * Mask and clear all TWL4030 interrupts since initially we do
@@ -768,12 +705,6 @@ int twl4030_init_irq(int irq_num, unsigned irq_base, unsigned irq_end)
         if (status < 0)
                 return status;
 
-        wq = create_singlethread_workqueue("twl4030-irqchip");
-        if (!wq) {
-                pr_err("twl4030: workqueue FAIL\n");
-                return -ESRCH;
-        }
-
         twl4030_irq_base = irq_base;
 
         /* install an irq handler for each of the SIH modules;
@@ -787,6 +718,7 @@ int twl4030_init_irq(int irq_num, unsigned irq_base, unsigned irq_end)
         for (i = irq_base; i < irq_end; i++) {
                 irq_set_chip_and_handler(i, &twl4030_irq_chip,
                                          handle_simple_irq);
+                irq_set_nested_thread(i, 1);
                 activate_irq(i);
         }
         twl4030_irq_next = i;
@@ -801,34 +733,22 @@ int twl4030_init_irq(int irq_num, unsigned irq_base, unsigned irq_end)
         }
 
         /* install an irq handler to demultiplex the TWL4030 interrupt */
-
-
-        init_completion(&irq_event);
-
-        status = request_irq(irq_num, handle_twl4030_pih, IRQF_DISABLED,
-                                "TWL4030-PIH", &irq_event);
+        status = request_threaded_irq(irq_num, NULL, handle_twl4030_pih, 0,
+                                        "TWL4030-PIH", NULL);
         if (status < 0) {
                 pr_err("twl4030: could not claim irq%d: %d\n", irq_num, status);
                 goto fail_rqirq;
         }
 
-        task = kthread_run(twl4030_irq_thread, (void *)(long)irq_num,
-                                                                "twl4030-irq");
-        if (IS_ERR(task)) {
-                pr_err("twl4030: could not create irq %d thread!\n", irq_num);
-                status = PTR_ERR(task);
-                goto fail_kthread;
-        }
         return status;
-fail_kthread:
-        free_irq(irq_num, &irq_event);
 fail_rqirq:
         /* clean up twl4030_sih_setup */
 fail:
-        for (i = irq_base; i < irq_end; i++)
+        for (i = irq_base; i < irq_end; i++) {
+                irq_set_nested_thread(i, 0);
                 irq_set_chip_and_handler(i, NULL, NULL);
-        destroy_workqueue(wq);
-        wq = NULL;
+        }
+
         return status;
 }