2 files changed, 37 insertions, 9 deletions

diff --git a/arch/mips/kernel/time.c b/arch/mips/kernel/time.c
index 11aab6d6bfe5..8aa544f73a5e 100644
--- a/arch/mips/kernel/time.c
+++ b/arch/mips/kernel/time.c
@@ -94,10 +94,8 @@ static void c0_timer_ack(void)
 {
         unsigned int count;
 
-#ifndef CONFIG_SOC_PNX8550      /* pnx8550 resets to zero */
         /* Ack this timer interrupt and set the next one.  */
         expirelo += cycles_per_jiffy;
-#endif
         write_c0_compare(expirelo);
 
         /* Check to see if we have missed any timer interrupts.  */
diff --git a/arch/mips/philips/pnx8550/common/time.c b/arch/mips/philips/pnx8550/common/time.c
index 65c440e8480b..f80acae07cee 100644
--- a/arch/mips/philips/pnx8550/common/time.c
+++ b/arch/mips/philips/pnx8550/common/time.c
@@ -33,7 +33,17 @@
 #include <int.h>
 #include <cm.h>
 
-extern unsigned int mips_hpt_frequency;
+static unsigned long cpj;
+
+static cycle_t hpt_read(void)
+{
+        return read_c0_count2();
+}
+
+static void timer_ack(void)
+{
+        write_c0_compare(cpj);
+}
 
 /*
  * pnx8550_time_init() - it does the following things:
@@ -68,27 +78,47 @@ void pnx8550_time_init(void)
          * HZ timer interrupts per second.
          */
         mips_hpt_frequency = 27UL * ((1000000UL * n)/(m * pow2p));
+        cpj = (mips_hpt_frequency + HZ / 2) / HZ;
+        timer_ack();
+
+        /* Setup Timer 2 */
+        write_c0_count2(0);
+        write_c0_compare2(0xffffffff);
+
+        clocksource_mips.read = hpt_read;
+        mips_timer_ack = timer_ack;
+}
+
+static irqreturn_t monotonic_interrupt(int irq, void *dev_id)
+{
+        /* Timer 2 clear interrupt */
+        write_c0_compare2(-1);
+        return IRQ_HANDLED;
 }
 
+static struct irqaction monotonic_irqaction = {
+        .handler = monotonic_interrupt,
+        .flags = IRQF_DISABLED,
+        .name = "Monotonic timer",
+};
+
 void __init plat_timer_setup(struct irqaction *irq)
 {
         int configPR;
 
         setup_irq(PNX8550_INT_TIMER1, irq);
+        setup_irq(PNX8550_INT_TIMER2, &monotonic_irqaction);
 
-        /* Start timer1 */
+        /* Timer 1 start */
         configPR = read_c0_config7();
         configPR &= ~0x00000008;
         write_c0_config7(configPR);
 
-        /* Timer 2 stop */
+        /* Timer 2 start */
         configPR = read_c0_config7();
-        configPR |= 0x00000010;
+        configPR &= ~0x00000010;
         write_c0_config7(configPR);
 
-        write_c0_count2(0);
-        write_c0_compare2(0xffffffff);
-
         /* Timer 3 stop */
         configPR = read_c0_config7();
         configPR |= 0x00000020;