diff --git a/xen/arch/arm/arm32/traps.c b/xen/arch/arm/arm32/traps.c
index ff0b945..f8cf864 100644
--- a/xen/arch/arm/arm32/traps.c
+++ b/xen/arch/arm/arm32/traps.c
@@ -18,6 +18,7 @@
 
 #include <xen/config.h>
 #include <xen/lib.h>
+#include <xen/kernel.h>
 
 #include <public/xen.h>
 
@@ -25,6 +26,28 @@
 
 asmlinkage void do_trap_undefined_instruction(struct cpu_user_regs *regs)
 {
+    uint32_t pc = regs->pc;
+    uint32_t instr;
+
+    if ( !is_kernel_text(pc) &&
+         (system_state >= SYS_STATE_active || !is_kernel_inittext(pc)) )
+        goto die;
+
+    /* PC should be always a multiple of 4, as Xen is using ARM instruction set */
+    if ( regs->pc & 0x3 )
+        goto die;
+
+    instr = *((uint32_t *)pc);
+    if ( instr != BUG_OPCODE )
+        goto die;
+
+    if ( do_bug_frame(regs, pc) )
+        goto die;
+
+    regs->pc += 4;
+    return;
+
+die:
     do_unexpected_trap("Undefined Instruction", regs);
 }
 
diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
index 019991f..0022187 100644
--- a/xen/arch/arm/traps.c
+++ b/xen/arch/arm/traps.c
@@ -32,6 +32,7 @@
 #include <xen/domain_page.h>
 #include <public/sched.h>
 #include <public/xen.h>
+#include <asm/debugger.h>
 #include <asm/event.h>
 #include <asm/regs.h>
 #include <asm/cpregs.h>
@@ -1047,6 +1048,105 @@ void do_unexpected_trap(const char *msg, struct cpu_user_regs *regs)
     panic("CPU%d: Unexpected Trap: %s\n", smp_processor_id(), msg);
 }
 
+int do_bug_frame(struct cpu_user_regs *regs, vaddr_t pc)
+{
+    const struct bug_frame *bug;
+    const char *prefix = "", *filename, *predicate;
+    unsigned long fixup;
+    int id, lineno;
+    static const struct bug_frame *const stop_frames[] = {
+        __stop_bug_frames_0,
+        __stop_bug_frames_1,
+        __stop_bug_frames_2,
+        NULL
+    };
+
+    for ( bug = __start_bug_frames, id = 0; stop_frames[id]; ++bug )
+    {
+        while ( unlikely(bug == stop_frames[id]) )
+            ++id;
+
+        if ( ((vaddr_t)bug_loc(bug)) == pc )
+            break;
+    }
+
+    if ( !stop_frames[id] )
+        return -ENOENT;
+
+    /* WARN, BUG or ASSERT: decode the filename pointer and line number. */
+    filename = bug_file(bug);
+    if ( !is_kernel(filename) )
+        return -EINVAL;
+    fixup = strlen(filename);
+    if ( fixup > 50 )
+    {
+        filename += fixup - 47;
+        prefix = "...";
+    }
+    lineno = bug_line(bug);
+
+    switch ( id )
+    {
+    case BUGFRAME_warn:
+        printk("Xen WARN at %s%s:%d\n", prefix, filename, lineno);
+        show_execution_state(regs);
+        return 0;
+
+    case BUGFRAME_bug:
+        printk("Xen BUG at %s%s:%d\n", prefix, filename, lineno);
+
+        if ( debugger_trap_fatal(TRAP_invalid_op, regs) )
+            return 0;
+
+        show_execution_state(regs);
+        panic("Xen BUG at %s%s:%d", prefix, filename, lineno);
+
+    case BUGFRAME_assert:
+        /* ASSERT: decode the predicate string pointer. */
+        predicate = bug_msg(bug);
+        if ( !is_kernel(predicate) )
+            predicate = "<unknown>";
+
+        printk("Assertion '%s' failed at %s%s:%d\n",
+               predicate, prefix, filename, lineno);
+        if ( debugger_trap_fatal(TRAP_invalid_op, regs) )
+            return 0;
+        show_execution_state(regs);
+        panic("Assertion '%s' failed at %s%s:%d",
+              predicate, prefix, filename, lineno);
+    }
+
+    return -EINVAL;
+}
+
+#ifdef CONFIG_ARM_64
+static void do_trap_brk(struct cpu_user_regs *regs, union hsr hsr)
+{
+    /* HCR_EL2.TGE and MDCR_EL2.TDE are not set so we never receive
+     * software breakpoint exception for EL1 and EL0 here
+     */
+    /* It's not possible to use BUG_ON here, because we would recurse */
+    if ( unlikely(READ_SYSREG(HCR_EL2) & HCR_TGE) ||
+         unlikely(READ_SYSREG(MDCR_EL2) & HDCR_TDE) )
+        panic("Unable to handle brk exception from EL1/EL0");
+
+    switch (hsr.brk.comment)
+    {
+    case BRK_BUG_FRAME:
+        if ( do_bug_frame(regs, regs->pc) )
+            goto die;
+
+        regs->pc += 4;
+
+        break;
+
+    default:
+die:
+        do_unexpected_trap("Undefined Breakpoint Value", regs);
+    }
+}
+#endif
+
 typedef register_t (*arm_hypercall_fn_t)(
     register_t, register_t, register_t, register_t, register_t);
 
@@ -1904,7 +2004,8 @@ asmlinkage void do_trap_hypervisor(struct cpu_user_regs *regs)
      * correctly (See XSA-102). Until that is resolved we treat any
      * trap from 32-bit userspace on 64-bit kernel as undefined.
      */
-    if ( is_64bit_domain(current->domain) && psr_mode_is_32bit(regs->cpsr) )
+    if ( !hyp_mode(regs) && is_64bit_domain(current->domain) &&
+         psr_mode_is_32bit(regs->cpsr) )
     {
         inject_undef_exception(regs, hsr.len);
         return;
@@ -1989,6 +2090,13 @@ asmlinkage void do_trap_hypervisor(struct cpu_user_regs *regs)
     case HSR_EC_DATA_ABORT_LOWER_EL:
         do_trap_data_abort_guest(regs, hsr);
         break;
+
+#ifdef CONFIG_ARM_64
+    case HSR_EC_BRK:
+        do_trap_brk(regs, hsr);
+        break;
+#endif
+
     default:
  bad_trap:
         printk("Hypervisor Trap. HSR=0x%x EC=0x%x IL=%x Syndrome=0x%"PRIx32"\n",
diff --git a/xen/arch/arm/xen.lds.S b/xen/arch/arm/xen.lds.S
index 079e085..cca1d8c 100644
--- a/xen/arch/arm/xen.lds.S
+++ b/xen/arch/arm/xen.lds.S
@@ -40,6 +40,14 @@ SECTIONS
   . = ALIGN(PAGE_SIZE);
   .rodata : {
         _srodata = .;          /* Read-only data */
+        /* Bug frames table */
+       __start_bug_frames = .;
+       *(.bug_frames.0)
+       __stop_bug_frames_0 = .;
+       *(.bug_frames.1)
+       __stop_bug_frames_1 = .;
+       *(.bug_frames.2)
+       __stop_bug_frames_2 = .;
        *(.rodata)
        *(.rodata.*)
         _erodata = .;          /* End of read-only data */
diff --git a/xen/include/asm-arm/arm32/bug.h b/xen/include/asm-arm/arm32/bug.h
new file mode 100644
index 0000000..155b420
--- /dev/null
+++ b/xen/include/asm-arm/arm32/bug.h
@@ -0,0 +1,13 @@
+#ifndef __ARM_ARM32_BUG_H__
+#define __ARM_ARM32_BUG_H__
+
+#include <xen/stringify.h>
+
+/* ARMv7 provides a list of undefined opcode (see A8.8.247 DDI 0406C.b)
+ * Use one them encoding A1 to go in exception mode
+ */
+#define BUG_OPCODE  0xe7f00f0
+
+#define BUG_INSTR ".word " __stringify(BUG_OPCODE)
+
+#endif /* __ARM_ARM32_BUG_H__ */
diff --git a/xen/include/asm-arm/arm64/bug.h b/xen/include/asm-arm/arm64/bug.h
new file mode 100644
index 0000000..42b0e4f
--- /dev/null
+++ b/xen/include/asm-arm/arm64/bug.h
@@ -0,0 +1,10 @@
+#ifndef __ARM_ARM64_BUG_H__
+#define __ARM_ARM64_BUG_H__
+
+#include <xen/stringify.h>
+
+#define BRK_BUG_FRAME 1
+
+#define BUG_INSTR "brk " __stringify(BRK_BUG_FRAME)
+
+#endif /* __ARM_ARM64_BUG_H__ */
diff --git a/xen/include/asm-arm/bug.h b/xen/include/asm-arm/bug.h
index 458c818..ab9e811 100644
--- a/xen/include/asm-arm/bug.h
+++ b/xen/include/asm-arm/bug.h
@@ -1,10 +1,81 @@
 #ifndef __ARM_BUG_H__
 #define __ARM_BUG_H__
 
-#define BUG() __bug(__FILE__, __LINE__)
-#define WARN() __warn(__FILE__, __LINE__)
+#include <asm/processor.h>
 
-#endif /* __X86_BUG_H__ */
+#if defined(CONFIG_ARM_32)
+# include <asm/arm32/bug.h>
+#elif defined(CONFIG_ARM_64)
+# include <asm/arm64/bug.h>
+#else
+# error "unknown ARM variant"
+#endif
+
+#define BUG_DISP_WIDTH    24
+#define BUG_LINE_LO_WIDTH (31 - BUG_DISP_WIDTH)
+#define BUG_LINE_HI_WIDTH (31 - BUG_DISP_WIDTH)
+
+struct bug_frame {
+    signed int loc_disp;    /* Relative address to the bug address */
+    signed int file_disp;   /* Relative address to the filename */
+    signed int msg_disp;    /* Relative address to the predicate (for ASSERT) */
+    uint16_t line;          /* Line number */
+    uint32_t pad0:16;       /* Padding for 8-bytes align */
+};
+
+#define bug_loc(b) ((const void *)(b) + (b)->loc_disp)
+#define bug_file(b) ((const void *)(b) + (b)->file_disp);
+#define bug_line(b) ((b)->line)
+#define bug_msg(b) ((const char *)(b) + (b)->msg_disp)
+
+#define BUGFRAME_warn   0
+#define BUGFRAME_bug    1
+#define BUGFRAME_assert 2
+
+/* Many versions of GCC doesn't support the asm %c parameter which would
+ * be preferable to this unpleasantness. We use mergeable string
+ * sections to avoid multiple copies of the string appearing in the
+ * Xen image.
+ */
+#define BUG_FRAME(type, line, file, has_msg, msg) do {                      \
+    BUILD_BUG_ON((line) >> 16);                                             \
+    asm ("1:"BUG_INSTR"\n"                                                  \
+         ".pushsection .rodata.str, \"aMS\", %progbits, 1\n"                \
+         "2:\t.asciz " __stringify(file) "\n"                               \
+         "3:\n"                                                             \
+         ".if " #has_msg "\n"                                               \
+         "\t.asciz " #msg "\n"                                              \
+         ".endif\n"                                                         \
+         ".popsection\n"                                                    \
+         ".pushsection .bug_frames." __stringify(type) ", \"a\", %progbits\n"\
+         "4:\n"                                                             \
+         ".long (1b - 4b)\n"                                                \
+         ".long (2b - 4b)\n"                                                \
+         ".long (3b - 4b)\n"                                                \
+         ".hword " __stringify(line) ", 0\n"                                \
+         ".popsection");                                                    \
+} while (0)
+
+#define WARN() BUG_FRAME(BUGFRAME_warn, __LINE__, __FILE__, 0, "")
+
+#define BUG() do {                                              \
+    BUG_FRAME(BUGFRAME_bug,  __LINE__, __FILE__, 0, "");        \
+    unreachable();                                              \
+} while (0)
+
+#define assert_failed(msg) do {                                 \
+    BUG_FRAME(BUGFRAME_assert, __LINE__, __FILE__, 1, msg);     \
+    unreachable();                                              \
+} while (0)
+
+extern const struct bug_frame __start_bug_frames[],
+                              __stop_bug_frames_0[],
+                              __stop_bug_frames_1[],
+                              __stop_bug_frames_2[];
+
+int do_bug_frame(struct cpu_user_regs *regs, vaddr_t pc);
+
+#endif /* __ARM_BUG_H__ */
 /*
  * Local variables:
  * mode: C
diff --git a/xen/include/asm-arm/debugger.h b/xen/include/asm-arm/debugger.h
index 916860b..ac776ef 100644
--- a/xen/include/asm-arm/debugger.h
+++ b/xen/include/asm-arm/debugger.h
@@ -1,7 +1,7 @@
 #ifndef __ARM_DEBUGGER_H__
 #define __ARM_DEBUGGER_H__
 
-#define debugger_trap_fatal(v, r) ((void) 0)
+#define debugger_trap_fatal(v, r) (0)
 #define debugger_trap_immediate() ((void) 0)
 
 #endif /* __ARM_DEBUGGER_H__ */
diff --git a/xen/include/asm-arm/processor.h b/xen/include/asm-arm/processor.h
index 0cc5b6d..a412270 100644
--- a/xen/include/asm-arm/processor.h
+++ b/xen/include/asm-arm/processor.h
@@ -3,6 +3,7 @@
 
 #include <asm/cpregs.h>
 #include <asm/sysregs.h>
+#include <public/arch-arm.h>
 
 /* MIDR Main ID Register */
 #define MIDR_MASK    0xff0ffff0
@@ -99,6 +100,7 @@
 #define HDCR_TDRA       (_AC(1,U)<<11)          /* Trap Debug ROM access */
 #define HDCR_TDOSA      (_AC(1,U)<<10)          /* Trap Debug-OS-related register access */
 #define HDCR_TDA        (_AC(1,U)<<9)           /* Trap Debug Access */
+#define HDCR_TDE        (_AC(1,U)<<8)           /* Route Soft Debug exceptions from EL1/EL1 to EL2 */
 #define HDCR_TPM        (_AC(1,U)<<6)           /* Trap Performance Monitors accesses */
 #define HDCR_TPMCR      (_AC(1,U)<<5)           /* Trap PMCR accesses */
 
@@ -125,6 +127,9 @@
 #define HSR_EC_INSTR_ABORT_CURR_EL  0x21
 #define HSR_EC_DATA_ABORT_LOWER_EL  0x24
 #define HSR_EC_DATA_ABORT_CURR_EL   0x25
+#ifdef CONFIG_ARM_64
+#define HSR_EC_BRK                  0x3c
+#endif
 
 /* FSR format, common */
 #define FSR_LPAE                (_AC(1,UL)<<9)
@@ -361,6 +366,17 @@ union hsr {
         unsigned long len:1;   /* Instruction length */
         unsigned long ec:6;    /* Exception Class */
     } dabt; /* HSR_EC_DATA_ABORT_* */
+
+#ifdef CONFIG_ARM_64
+    struct hsr_brk {
+        unsigned long comment:16;   /* Comment */
+        unsigned long res0:9;
+        unsigned long len:1;        /* Instruction length */
+        unsigned long ec:6;         /* Exception Class */
+    } brk;
+#endif
+
+
 };
 #endif
 
@@ -492,7 +508,7 @@ void panic_PAR(uint64_t par);
 void show_execution_state(struct cpu_user_regs *regs);
 void show_registers(struct cpu_user_regs *regs);
 //#define dump_execution_state() run_in_exception_handler(show_execution_state)
-#define dump_execution_state() asm volatile (".word 0xe7f000f0\n"); /* XXX */
+#define dump_execution_state() WARN()
 
 #define cpu_relax() barrier() /* Could yield? */
 
