1 files changed, 70 insertions, 0 deletions

diff --git a/arch/m68k/atari/ataints.c b/arch/m68k/atari/ataints.c
index 3f41092d1b70..f699c829e594 100644
--- a/arch/m68k/atari/ataints.c
+++ b/arch/m68k/atari/ataints.c
@@ -122,6 +122,62 @@ static struct irq_chip atari_irq_chip = {
 };
 
 /*
+ * ST-MFP timer D chained interrupts - each driver gets its own timer
+ * interrupt instance.
+ */
+
+struct mfptimerbase {
+        volatile struct MFP *mfp;
+        unsigned char mfp_mask, mfp_data;
+        unsigned short int_mask;
+        int handler_irq, mfptimer_irq, server_irq;
+        char *name;
+} stmfp_base = {
+        .mfp            = &st_mfp,
+        .int_mask       = 0x0,
+        .handler_irq    = IRQ_MFP_TIMD,
+        .mfptimer_irq   = IRQ_MFP_TIMER1,
+        .name           = "MFP Timer D"
+};
+
+static irqreturn_t mfptimer_handler(int irq, void *dev_id)
+{
+        struct mfptimerbase *base = dev_id;
+        int mach_irq;
+        unsigned char ints;
+
+        mach_irq = base->mfptimer_irq;
+        ints = base->int_mask;
+        for (; ints; mach_irq++, ints >>= 1) {
+                if (ints & 1)
+                        generic_handle_irq(mach_irq);
+        }
+        return IRQ_HANDLED;
+}
+
+
+static void atari_mfptimer_enable(struct irq_data *data)
+{
+        int mfp_num = data->irq - IRQ_MFP_TIMER1;
+        stmfp_base.int_mask |= 1 << mfp_num;
+        atari_enable_irq(IRQ_MFP_TIMD);
+}
+
+static void atari_mfptimer_disable(struct irq_data *data)
+{
+        int mfp_num = data->irq - IRQ_MFP_TIMER1;
+        stmfp_base.int_mask &= ~(1 << mfp_num);
+        if (!stmfp_base.int_mask)
+                atari_disable_irq(IRQ_MFP_TIMD);
+}
+
+static struct irq_chip atari_mfptimer_chip = {
+        .name           = "timer_d",
+        .irq_enable     = atari_mfptimer_enable,
+        .irq_disable    = atari_mfptimer_disable,
+};
+
+/*
  * void atari_init_IRQ (void)
  *
  * Parameters:  None
@@ -198,6 +254,20 @@ void __init atari_init_IRQ(void)
         /* Initialize the PSG: all sounds off, both ports output */
         sound_ym.rd_data_reg_sel = 7;
         sound_ym.wd_data = 0xff;
+
+        m68k_setup_irq_controller(&atari_mfptimer_chip, handle_simple_irq,
+                                  IRQ_MFP_TIMER1, 8);
+
+        /* prepare timer D data for use as poll interrupt */
+        /* set Timer D data Register - needs to be > 0 */
+        st_mfp.tim_dt_d = 254;  /* < 100 Hz */
+        /* start timer D, div = 1:100 */
+        st_mfp.tim_ct_cd = (st_mfp.tim_ct_cd & 0xf0) | 0x6;
+
+        /* request timer D dispatch handler */
+        if (request_irq(IRQ_MFP_TIMD, mfptimer_handler, IRQF_SHARED,
+                        stmfp_base.name, &stmfp_base))
+                pr_err("Couldn't register %s interrupt\n", stmfp_base.name);
 }