1 files changed, 72 insertions, 19 deletions

diff --git a/arch/powerpc/kernel/traps.c b/arch/powerpc/kernel/traps.c
index 4e293b75f951..987437e04e61 100644
--- a/arch/powerpc/kernel/traps.c
+++ b/arch/powerpc/kernel/traps.c
@@ -1034,9 +1034,68 @@ void SoftwareEmulation(struct pt_regs *regs)
 #endif /* CONFIG_8xx */
 
 #ifdef CONFIG_PPC_ADV_DEBUG_REGS
+static void handle_debug(struct pt_regs *regs, unsigned long debug_status)
+{
+        int changed = 0;
+        /*
+         * Determine the cause of the debug event, clear the
+         * event flags and send a trap to the handler. Torez
+         */
+        if (debug_status & (DBSR_DAC1R | DBSR_DAC1W)) {
+                dbcr_dac(current) &= ~(DBCR_DAC1R | DBCR_DAC1W);
+#ifdef CONFIG_PPC_ADV_DEBUG_DAC_RANGE
+                current->thread.dbcr2 &= ~DBCR2_DAC12MODE;
+#endif
+                do_send_trap(regs, mfspr(SPRN_DAC1), debug_status, TRAP_HWBKPT,
+                             5);
+                changed |= 0x01;
+        }  else if (debug_status & (DBSR_DAC2R | DBSR_DAC2W)) {
+                dbcr_dac(current) &= ~(DBCR_DAC2R | DBCR_DAC2W);
+                do_send_trap(regs, mfspr(SPRN_DAC2), debug_status, TRAP_HWBKPT,
+                             6);
+                changed |= 0x01;
+        }  else if (debug_status & DBSR_IAC1) {
+                current->thread.dbcr0 &= ~DBCR0_IAC1;
+                dbcr_iac_range(current) &= ~DBCR_IAC12MODE;
+                do_send_trap(regs, mfspr(SPRN_IAC1), debug_status, TRAP_HWBKPT,
+                             1);
+                changed |= 0x01;
+        }  else if (debug_status & DBSR_IAC2) {
+                current->thread.dbcr0 &= ~DBCR0_IAC2;
+                do_send_trap(regs, mfspr(SPRN_IAC2), debug_status, TRAP_HWBKPT,
+                             2);
+                changed |= 0x01;
+        }  else if (debug_status & DBSR_IAC3) {
+                current->thread.dbcr0 &= ~DBCR0_IAC3;
+                dbcr_iac_range(current) &= ~DBCR_IAC34MODE;
+                do_send_trap(regs, mfspr(SPRN_IAC3), debug_status, TRAP_HWBKPT,
+                             3);
+                changed |= 0x01;
+        }  else if (debug_status & DBSR_IAC4) {
+                current->thread.dbcr0 &= ~DBCR0_IAC4;
+                do_send_trap(regs, mfspr(SPRN_IAC4), debug_status, TRAP_HWBKPT,
+                             4);
+                changed |= 0x01;
+        }
+        /*
+         * At the point this routine was called, the MSR(DE) was turned off.
+         * Check all other debug flags and see if that bit needs to be turned
+         * back on or not.
+         */
+        if (DBCR_ACTIVE_EVENTS(current->thread.dbcr0, current->thread.dbcr1))
+                regs->msr |= MSR_DE;
+        else
+                /* Make sure the IDM flag is off */
+                current->thread.dbcr0 &= ~DBCR0_IDM;
+
+        if (changed & 0x01)
+                mtspr(SPRN_DBCR0, current->thread.dbcr0);
+}
 
 void __kprobes DebugException(struct pt_regs *regs, unsigned long debug_status)
 {
+        current->thread.dbsr = debug_status;
+
         /* Hack alert: On BookE, Branch Taken stops on the branch itself, while
          * on server, it stops on the target of the branch. In order to simulate
          * the server behaviour, we thus restart right away with a single step
@@ -1080,27 +1139,21 @@ void __kprobes DebugException(struct pt_regs *regs, unsigned long debug_status)
                 if (debugger_sstep(regs))
                         return;
 
-                if (user_mode(regs))
-                        current->thread.dbcr0 &= ~(DBCR0_IC);
-
-                _exception(SIGTRAP, regs, TRAP_TRACE, regs->nip);
-        } else if (debug_status & (DBSR_DAC1R | DBSR_DAC1W)) {
-                regs->msr &= ~MSR_DE;
-
                 if (user_mode(regs)) {
-                        current->thread.dbcr0 &= ~(DBSR_DAC1R | DBSR_DAC1W |
-                                                                DBCR0_IDM);
-                } else {
-                        /* Disable DAC interupts */
-                        mtspr(SPRN_DBCR0, mfspr(SPRN_DBCR0) & ~(DBSR_DAC1R |
-                                                DBSR_DAC1W | DBCR0_IDM));
-
-                        /* Clear the DAC event */
-                        mtspr(SPRN_DBSR, (DBSR_DAC1R | DBSR_DAC1W));
+                        current->thread.dbcr0 &= ~DBCR0_IC;
+#ifdef CONFIG_PPC_ADV_DEBUG_REGS
+                        if (DBCR_ACTIVE_EVENTS(current->thread.dbcr0,
+                                               current->thread.dbcr1))
+                                regs->msr |= MSR_DE;
+                        else
+                                /* Make sure the IDM bit is off */
+                                current->thread.dbcr0 &= ~DBCR0_IDM;
+#endif
                 }
-                /* Setup and send the trap to the handler */
-                do_dabr(regs, mfspr(SPRN_DAC1), debug_status);
-        }
+
+                _exception(SIGTRAP, regs, TRAP_TRACE, regs->nip);
+        } else
+                handle_debug(regs, debug_status);
 }
 #endif /* CONFIG_PPC_ADV_DEBUG_REGS */