git.oblomov.eu Git - linux-2.6/blob - arch/avr32/kernel/entry-avr32b.S

   1 /*
   2  * Copyright (C) 2004-2006 Atmel Corporation
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License version 2 as
   6  * published by the Free Software Foundation.
   7  */
   8
   9 /*
  10  * This file contains the low-level entry-points into the kernel, that is,
  11  * exception handlers, debug trap handlers, interrupt handlers and the
  12  * system call handler.
  13  */
  14 #include <linux/errno.h>
  15
  16 #include <asm/asm.h>
  17 #include <asm/hardirq.h>
  18 #include <asm/irq.h>
  19 #include <asm/ocd.h>
  20 #include <asm/page.h>
  21 #include <asm/pgtable.h>
  22 #include <asm/ptrace.h>
  23 #include <asm/sysreg.h>
  24 #include <asm/thread_info.h>
  25 #include <asm/unistd.h>
  26
  27 #ifdef CONFIG_PREEMPT
  28 # define preempt_stop           mask_interrupts
  29 #else
  30 # define preempt_stop
  31 # define fault_resume_kernel    fault_restore_all
  32 #endif
  33
  34 #define __MASK(x)       ((1 << (x)) - 1)
  35 #define IRQ_MASK        ((__MASK(SOFTIRQ_BITS) << SOFTIRQ_SHIFT) | \
  36                          (__MASK(HARDIRQ_BITS) << HARDIRQ_SHIFT))
  37
  38         .section .ex.text,"ax",@progbits
  39         .align  2
  40 exception_vectors:
  41         bral    handle_critical
  42         .align  2
  43         bral    handle_critical
  44         .align  2
  45         bral    do_bus_error_write
  46         .align  2
  47         bral    do_bus_error_read
  48         .align  2
  49         bral    do_nmi_ll
  50         .align  2
  51         bral    handle_address_fault
  52         .align  2
  53         bral    handle_protection_fault
  54         .align  2
  55         bral    handle_debug
  56         .align  2
  57         bral    do_illegal_opcode_ll
  58         .align  2
  59         bral    do_illegal_opcode_ll
  60         .align  2
  61         bral    do_illegal_opcode_ll
  62         .align  2
  63         bral    do_fpe_ll
  64         .align  2
  65         bral    do_illegal_opcode_ll
  66         .align  2
  67         bral    handle_address_fault
  68         .align  2
  69         bral    handle_address_fault
  70         .align  2
  71         bral    handle_protection_fault
  72         .align  2
  73         bral    handle_protection_fault
  74         .align  2
  75         bral    do_dtlb_modified
  76
  77         /*
  78          * r0 : PGD/PT/PTE
  79          * r1 : Offending address
  80          * r2 : Scratch register
  81          * r3 : Cause (5, 12 or 13)
  82          */
  83 #define tlbmiss_save    pushm   r0-r3
  84 #define tlbmiss_restore popm    r0-r3
  85
  86         .org    0x50
  87         .global itlb_miss
  88 itlb_miss:
  89         tlbmiss_save
  90         rjmp    tlb_miss_common
  91
  92         .org    0x60
  93 dtlb_miss_read:
  94         tlbmiss_save
  95         rjmp    tlb_miss_common
  96
  97         .org    0x70
  98 dtlb_miss_write:
  99         tlbmiss_save
 100
 101         .global tlb_miss_common
 102         .align  2
 103 tlb_miss_common:
 104         mfsr    r0, SYSREG_TLBEAR
 105         mfsr    r1, SYSREG_PTBR
 106
 107         /* Is it the vmalloc space? */
 108         bld     r0, 31
 109         brcs    handle_vmalloc_miss
 110
 111         /* First level lookup */
 112 pgtbl_lookup:
 113         lsr     r2, r0, PGDIR_SHIFT
 114         ld.w    r3, r1[r2 << 2]
 115         bfextu  r1, r0, PAGE_SHIFT, PGDIR_SHIFT - PAGE_SHIFT
 116         bld     r3, _PAGE_BIT_PRESENT
 117         brcc    page_table_not_present
 118
 119         /* Translate to virtual address in P1. */
 120         andl    r3, 0xf000
 121         sbr     r3, 31
 122
 123         /* Second level lookup */
 124         ld.w    r2, r3[r1 << 2]
 125         mfsr    r0, SYSREG_TLBARLO
 126         bld     r2, _PAGE_BIT_PRESENT
 127         brcc    page_not_present
 128
 129         /* Mark the page as accessed */
 130         sbr     r2, _PAGE_BIT_ACCESSED
 131         st.w    r3[r1 << 2], r2
 132
 133         /* Drop software flags */
 134         andl    r2, _PAGE_FLAGS_HARDWARE_MASK & 0xffff
 135         mtsr    SYSREG_TLBELO, r2
 136
 137         /* Figure out which entry we want to replace */
 138         mfsr    r1, SYSREG_MMUCR
 139         clz     r2, r0
 140         brcc    1f
 141         mov     r3, -1                  /* All entries have been accessed, */
 142         mov     r2, 0                   /* so start at 0 */
 143         mtsr    SYSREG_TLBARLO, r3      /* and reset TLBAR */
 144
 145 1:      bfins   r1, r2, SYSREG_DRP_OFFSET, SYSREG_DRP_SIZE
 146         mtsr    SYSREG_MMUCR, r1
 147         tlbw
 148
 149         tlbmiss_restore
 150         rete
 151
 152 handle_vmalloc_miss:
 153         /* Simply do the lookup in init's page table */
 154         mov     r1, lo(swapper_pg_dir)
 155         orh     r1, hi(swapper_pg_dir)
 156         rjmp    pgtbl_lookup
 157
 158
 159         /* ---                    System Call                    --- */
 160
 161         .org    0x100
 162 system_call:
 163 #ifdef CONFIG_PREEMPT
 164         mask_interrupts
 165 #endif
 166         pushm   r12             /* r12_orig */
 167         stmts   --sp, r0-lr
 168
 169         mfsr    r0, SYSREG_RAR_SUP
 170         mfsr    r1, SYSREG_RSR_SUP
 171 #ifdef CONFIG_PREEMPT
 172         unmask_interrupts
 173 #endif
 174         zero_fp
 175         stm     --sp, r0-r1
 176
 177         /* check for syscall tracing */
 178         get_thread_info r0
 179         ld.w    r1, r0[TI_flags]
 180         bld     r1, TIF_SYSCALL_TRACE
 181         brcs    syscall_trace_enter
 182
 183 syscall_trace_cont:
 184         cp.w    r8, NR_syscalls
 185         brhs    syscall_badsys
 186
 187         lddpc   lr, syscall_table_addr
 188         ld.w    lr, lr[r8 << 2]
 189         mov     r8, r5          /* 5th argument (6th is pushed by stub) */
 190         icall   lr
 191
 192         .global syscall_return
 193 syscall_return:
 194         get_thread_info r0
 195         mask_interrupts         /* make sure we don't miss an interrupt
 196                                    setting need_resched or sigpending
 197                                    between sampling and the rets */
 198
 199         /* Store the return value so that the correct value is loaded below */
 200         stdsp   sp[REG_R12], r12
 201
 202         ld.w    r1, r0[TI_flags]
 203         andl    r1, _TIF_ALLWORK_MASK, COH
 204         brne    syscall_exit_work
 205
 206 syscall_exit_cont:
 207         popm    r8-r9
 208         mtsr    SYSREG_RAR_SUP, r8
 209         mtsr    SYSREG_RSR_SUP, r9
 210         ldmts   sp++, r0-lr
 211         sub     sp, -4          /* r12_orig */
 212         rets
 213
 214         .align  2
 215 syscall_table_addr:
 216         .long   sys_call_table
 217
 218 syscall_badsys:
 219         mov     r12, -ENOSYS
 220         rjmp    syscall_return
 221
 222         .global ret_from_fork
 223 ret_from_fork:
 224         rcall   schedule_tail
 225
 226         /* check for syscall tracing */
 227         get_thread_info r0
 228         ld.w    r1, r0[TI_flags]
 229         andl    r1, _TIF_ALLWORK_MASK, COH
 230         brne    syscall_exit_work
 231         rjmp    syscall_exit_cont
 232
 233 syscall_trace_enter:
 234         pushm   r8-r12
 235         rcall   syscall_trace
 236         popm    r8-r12
 237         rjmp    syscall_trace_cont
 238
 239 syscall_exit_work:
 240         bld     r1, TIF_SYSCALL_TRACE
 241         brcc    1f
 242         unmask_interrupts
 243         rcall   syscall_trace
 244         mask_interrupts
 245         ld.w    r1, r0[TI_flags]
 246
 247 1:      bld     r1, TIF_NEED_RESCHED
 248         brcc    2f
 249         unmask_interrupts
 250         rcall   schedule
 251         mask_interrupts
 252         ld.w    r1, r0[TI_flags]
 253         rjmp    1b
 254
 255 2:      mov     r2, _TIF_SIGPENDING | _TIF_RESTORE_SIGMASK
 256         tst     r1, r2
 257         breq    3f
 258         unmask_interrupts
 259         mov     r12, sp
 260         mov     r11, r0
 261         rcall   do_notify_resume
 262         mask_interrupts
 263         ld.w    r1, r0[TI_flags]
 264         rjmp    1b
 265
 266 3:      bld     r1, TIF_BREAKPOINT
 267         brcc    syscall_exit_cont
 268         rjmp    enter_monitor_mode
 269
 270         /* The slow path of the TLB miss handler */
 271 page_table_not_present:
 272 page_not_present:
 273         tlbmiss_restore
 274         sub     sp, 4
 275         stmts   --sp, r0-lr
 276         rcall   save_full_context_ex
 277         mfsr    r12, SYSREG_ECR
 278         mov     r11, sp
 279         rcall   do_page_fault
 280         rjmp    ret_from_exception
 281
 282         /* This function expects to find offending PC in SYSREG_RAR_EX */
 283         .type   save_full_context_ex, @function
 284         .align  2
 285 save_full_context_ex:
 286         mfsr    r11, SYSREG_RAR_EX
 287         sub     r9, pc, . - debug_trampoline
 288         mfsr    r8, SYSREG_RSR_EX
 289         cp.w    r9, r11
 290         breq    3f
 291         mov     r12, r8
 292         andh    r8, (MODE_MASK >> 16), COH
 293         brne    2f
 294
 295 1:      pushm   r11, r12        /* PC and SR */
 296         unmask_exceptions
 297         ret     r12
 298
 299 2:      sub     r10, sp, -(FRAME_SIZE_FULL - REG_LR)
 300         stdsp   sp[4], r10      /* replace saved SP */
 301         rjmp    1b
 302
 303         /*
 304          * The debug handler set up a trampoline to make us
 305          * automatically enter monitor mode upon return, but since
 306          * we're saving the full context, we must assume that the
 307          * exception handler might want to alter the return address
 308          * and/or status register. So we need to restore the original
 309          * context and enter monitor mode manually after the exception
 310          * has been handled.
 311          */
 312 3:      get_thread_info r8
 313         ld.w    r11, r8[TI_rar_saved]
 314         ld.w    r12, r8[TI_rsr_saved]
 315         rjmp    1b
 316         .size   save_full_context_ex, . - save_full_context_ex
 317
 318         /* Low-level exception handlers */
 319 handle_critical:
 320         sub     sp, 4
 321         stmts   --sp, r0-lr
 322         rcall   save_full_context_ex
 323         mfsr    r12, SYSREG_ECR
 324         mov     r11, sp
 325         rcall   do_critical_exception
 326
 327         /* We should never get here... */
 328 bad_return:
 329         sub     r12, pc, (. - 1f)
 330         bral    panic
 331         .align  2
 332 1:      .asciz  "Return from critical exception!"
 333
 334         .align  1
 335 do_bus_error_write:
 336         sub     sp, 4
 337         stmts   --sp, r0-lr
 338         rcall   save_full_context_ex
 339         mov     r11, 1
 340         rjmp    1f
 341
 342 do_bus_error_read:
 343         sub     sp, 4
 344         stmts   --sp, r0-lr
 345         rcall   save_full_context_ex
 346         mov     r11, 0
 347 1:      mfsr    r12, SYSREG_BEAR
 348         mov     r10, sp
 349         rcall   do_bus_error
 350         rjmp    ret_from_exception
 351
 352         .align  1
 353 do_nmi_ll:
 354         sub     sp, 4
 355         stmts   --sp, r0-lr
 356         mfsr    r9, SYSREG_RSR_NMI
 357         mfsr    r8, SYSREG_RAR_NMI
 358         bfextu  r0, r9, MODE_SHIFT, 3
 359         brne    2f
 360
 361 1:      pushm   r8, r9  /* PC and SR */
 362         mfsr    r12, SYSREG_ECR
 363         mov     r11, sp
 364         rcall   do_nmi
 365         popm    r8-r9
 366         mtsr    SYSREG_RAR_NMI, r8
 367         tst     r0, r0
 368         mtsr    SYSREG_RSR_NMI, r9
 369         brne    3f
 370
 371         ldmts   sp++, r0-lr
 372         sub     sp, -4          /* skip r12_orig */
 373         rete
 374
 375 2:      sub     r10, sp, -(FRAME_SIZE_FULL - REG_LR)
 376         stdsp   sp[4], r10      /* replace saved SP */
 377         rjmp    1b
 378
 379 3:      popm    lr
 380         sub     sp, -4          /* skip sp */
 381         popm    r0-r12
 382         sub     sp, -4          /* skip r12_orig */
 383         rete
 384
 385 handle_address_fault:
 386         sub     sp, 4
 387         stmts   --sp, r0-lr
 388         rcall   save_full_context_ex
 389         mfsr    r12, SYSREG_ECR
 390         mov     r11, sp
 391         rcall   do_address_exception
 392         rjmp    ret_from_exception
 393
 394 handle_protection_fault:
 395         sub     sp, 4
 396         stmts   --sp, r0-lr
 397         rcall   save_full_context_ex
 398         mfsr    r12, SYSREG_ECR
 399         mov     r11, sp
 400         rcall   do_page_fault
 401         rjmp    ret_from_exception
 402
 403         .align  1
 404 do_illegal_opcode_ll:
 405         sub     sp, 4
 406         stmts   --sp, r0-lr
 407         rcall   save_full_context_ex
 408         mfsr    r12, SYSREG_ECR
 409         mov     r11, sp
 410         rcall   do_illegal_opcode
 411         rjmp    ret_from_exception
 412
 413 do_dtlb_modified:
 414         pushm   r0-r3
 415         mfsr    r1, SYSREG_TLBEAR
 416         mfsr    r0, SYSREG_PTBR
 417         lsr     r2, r1, PGDIR_SHIFT
 418         ld.w    r0, r0[r2 << 2]
 419         lsl     r1, (32 - PGDIR_SHIFT)
 420         lsr     r1, (32 - PGDIR_SHIFT) + PAGE_SHIFT
 421
 422         /* Translate to virtual address in P1 */
 423         andl    r0, 0xf000
 424         sbr     r0, 31
 425         add     r2, r0, r1 << 2
 426         ld.w    r3, r2[0]
 427         sbr     r3, _PAGE_BIT_DIRTY
 428         mov     r0, r3
 429         st.w    r2[0], r3
 430
 431         /* The page table is up-to-date. Update the TLB entry as well */
 432         andl    r0, lo(_PAGE_FLAGS_HARDWARE_MASK)
 433         mtsr    SYSREG_TLBELO, r0
 434
 435         /* MMUCR[DRP] is updated automatically, so let's go... */
 436         tlbw
 437
 438         popm    r0-r3
 439         rete
 440
 441 do_fpe_ll:
 442         sub     sp, 4
 443         stmts   --sp, r0-lr
 444         rcall   save_full_context_ex
 445         unmask_interrupts
 446         mov     r12, 26
 447         mov     r11, sp
 448         rcall   do_fpe
 449         rjmp    ret_from_exception
 450
 451 ret_from_exception:
 452         mask_interrupts
 453         lddsp   r4, sp[REG_SR]
 454
 455         andh    r4, (MODE_MASK >> 16), COH
 456         brne    fault_resume_kernel
 457
 458         get_thread_info r0
 459         ld.w    r1, r0[TI_flags]
 460         andl    r1, _TIF_WORK_MASK, COH
 461         brne    fault_exit_work
 462
 463 fault_resume_user:
 464         popm    r8-r9
 465         mask_exceptions
 466         mtsr    SYSREG_RAR_EX, r8
 467         mtsr    SYSREG_RSR_EX, r9
 468         ldmts   sp++, r0-lr
 469         sub     sp, -4
 470         rete
 471
 472 fault_resume_kernel:
 473 #ifdef CONFIG_PREEMPT
 474         get_thread_info r0
 475         ld.w    r2, r0[TI_preempt_count]
 476         cp.w    r2, 0
 477         brne    1f
 478         ld.w    r1, r0[TI_flags]
 479         bld     r1, TIF_NEED_RESCHED
 480         brcc    1f
 481         lddsp   r4, sp[REG_SR]
 482         bld     r4, SYSREG_GM_OFFSET
 483         brcs    1f
 484         rcall   preempt_schedule_irq
 485 1:
 486 #endif
 487
 488         popm    r8-r9
 489         mask_exceptions
 490         mfsr    r1, SYSREG_SR
 491         mtsr    SYSREG_RAR_EX, r8
 492         mtsr    SYSREG_RSR_EX, r9
 493         popm    lr
 494         sub     sp, -4          /* ignore SP */
 495         popm    r0-r12
 496         sub     sp, -4          /* ignore r12_orig */
 497         rete
 498
 499 irq_exit_work:
 500         /* Switch to exception mode so that we can share the same code. */
 501         mfsr    r8, SYSREG_SR
 502         cbr     r8, SYSREG_M0_OFFSET
 503         orh     r8, hi(SYSREG_BIT(M1) | SYSREG_BIT(M2))
 504         mtsr    SYSREG_SR, r8
 505         sub     pc, -2
 506         get_thread_info r0
 507         ld.w    r1, r0[TI_flags]
 508
 509 fault_exit_work:
 510         bld     r1, TIF_NEED_RESCHED
 511         brcc    1f
 512         unmask_interrupts
 513         rcall   schedule
 514         mask_interrupts
 515         ld.w    r1, r0[TI_flags]
 516         rjmp    fault_exit_work
 517
 518 1:      mov     r2, _TIF_SIGPENDING | _TIF_RESTORE_SIGMASK
 519         tst     r1, r2
 520         breq    2f
 521         unmask_interrupts
 522         mov     r12, sp
 523         mov     r11, r0
 524         rcall   do_notify_resume
 525         mask_interrupts
 526         ld.w    r1, r0[TI_flags]
 527         rjmp    fault_exit_work
 528
 529 2:      bld     r1, TIF_BREAKPOINT
 530         brcc    fault_resume_user
 531         rjmp    enter_monitor_mode
 532
 533         .section .kprobes.text, "ax", @progbits
 534         .type   handle_debug, @function
 535 handle_debug:
 536         sub     sp, 4           /* r12_orig */
 537         stmts   --sp, r0-lr
 538         mfsr    r8, SYSREG_RAR_DBG
 539         mfsr    r9, SYSREG_RSR_DBG
 540         unmask_exceptions
 541         pushm   r8-r9
 542         bfextu  r9, r9, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
 543         brne    debug_fixup_regs
 544
 545 .Ldebug_fixup_cont:
 546 #ifdef CONFIG_TRACE_IRQFLAGS
 547         rcall   trace_hardirqs_off
 548 #endif
 549         mov     r12, sp
 550         rcall   do_debug
 551         mov     sp, r12
 552
 553         lddsp   r2, sp[REG_SR]
 554         bfextu  r3, r2, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
 555         brne    debug_resume_kernel
 556
 557         get_thread_info r0
 558         ld.w    r1, r0[TI_flags]
 559         mov     r2, _TIF_DBGWORK_MASK
 560         tst     r1, r2
 561         brne    debug_exit_work
 562
 563         bld     r1, TIF_SINGLE_STEP
 564         brcc    1f
 565         mfdr    r4, OCD_DC
 566         sbr     r4, OCD_DC_SS_BIT
 567         mtdr    OCD_DC, r4
 568
 569 1:      popm    r10,r11
 570         mask_exceptions
 571         mtsr    SYSREG_RSR_DBG, r11
 572         mtsr    SYSREG_RAR_DBG, r10
 573 #ifdef CONFIG_TRACE_IRQFLAGS
 574         rcall   trace_hardirqs_on
 575 1:
 576 #endif
 577         ldmts   sp++, r0-lr
 578         sub     sp, -4
 579         retd
 580         .size   handle_debug, . - handle_debug
 581
 582         /* Mode of the trapped context is in r9 */
 583         .type   debug_fixup_regs, @function
 584 debug_fixup_regs:
 585         mfsr    r8, SYSREG_SR
 586         mov     r10, r8
 587         bfins   r8, r9, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
 588         mtsr    SYSREG_SR, r8
 589         sub     pc, -2
 590         stdsp   sp[REG_LR], lr
 591         mtsr    SYSREG_SR, r10
 592         sub     pc, -2
 593         sub     r8, sp, -FRAME_SIZE_FULL
 594         stdsp   sp[REG_SP], r8
 595         rjmp    .Ldebug_fixup_cont
 596         .size   debug_fixup_regs, . - debug_fixup_regs
 597
 598         .type   debug_resume_kernel, @function
 599 debug_resume_kernel:
 600         mask_exceptions
 601         popm    r10, r11
 602         mtsr    SYSREG_RAR_DBG, r10
 603         mtsr    SYSREG_RSR_DBG, r11
 604 #ifdef CONFIG_TRACE_IRQFLAGS
 605         bld     r11, SYSREG_GM_OFFSET
 606         brcc    1f
 607         rcall   trace_hardirqs_on
 608 1:
 609 #endif
 610         mfsr    r2, SYSREG_SR
 611         mov     r1, r2
 612         bfins   r2, r3, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
 613         mtsr    SYSREG_SR, r2
 614         sub     pc, -2
 615         popm    lr
 616         mtsr    SYSREG_SR, r1
 617         sub     pc, -2
 618         sub     sp, -4          /* skip SP */
 619         popm    r0-r12
 620         sub     sp, -4
 621         retd
 622         .size   debug_resume_kernel, . - debug_resume_kernel
 623
 624         .type   debug_exit_work, @function
 625 debug_exit_work:
 626         /*
 627          * We must return from Monitor Mode using a retd, and we must
 628          * not schedule since that involves the D bit in SR getting
 629          * cleared by something other than the debug hardware. This
 630          * may cause undefined behaviour according to the Architecture
 631          * manual.
 632          *
 633          * So we fix up the return address and status and return to a
 634          * stub below in Exception mode. From there, we can follow the
 635          * normal exception return path.
 636          *
 637          * The real return address and status registers are stored on
 638          * the stack in the way the exception return path understands,
 639          * so no need to fix anything up there.
 640          */
 641         sub     r8, pc, . - fault_exit_work
 642         mtsr    SYSREG_RAR_DBG, r8
 643         mov     r9, 0
 644         orh     r9, hi(SR_EM | SR_GM | MODE_EXCEPTION)
 645         mtsr    SYSREG_RSR_DBG, r9
 646         sub     pc, -2
 647         retd
 648         .size   debug_exit_work, . - debug_exit_work
 649
 650         .set    rsr_int0,       SYSREG_RSR_INT0
 651         .set    rsr_int1,       SYSREG_RSR_INT1
 652         .set    rsr_int2,       SYSREG_RSR_INT2
 653         .set    rsr_int3,       SYSREG_RSR_INT3
 654         .set    rar_int0,       SYSREG_RAR_INT0
 655         .set    rar_int1,       SYSREG_RAR_INT1
 656         .set    rar_int2,       SYSREG_RAR_INT2
 657         .set    rar_int3,       SYSREG_RAR_INT3
 658
 659         .macro  IRQ_LEVEL level
 660         .type   irq_level\level, @function
 661 irq_level\level:
 662         sub     sp, 4           /* r12_orig */
 663         stmts   --sp,r0-lr
 664         mfsr    r8, rar_int\level
 665         mfsr    r9, rsr_int\level
 666
 667 #ifdef CONFIG_PREEMPT
 668         sub     r11, pc, (. - system_call)
 669         cp.w    r11, r8
 670         breq    4f
 671 #endif
 672
 673         pushm   r8-r9
 674
 675         mov     r11, sp
 676         mov     r12, \level
 677
 678         rcall   do_IRQ
 679
 680         lddsp   r4, sp[REG_SR]
 681         bfextu  r4, r4, SYSREG_M0_OFFSET, 3
 682         cp.w    r4, MODE_SUPERVISOR >> SYSREG_M0_OFFSET
 683         breq    2f
 684         cp.w    r4, MODE_USER >> SYSREG_M0_OFFSET
 685 #ifdef CONFIG_PREEMPT
 686         brne    3f
 687 #else
 688         brne    1f
 689 #endif
 690
 691         get_thread_info r0
 692         ld.w    r1, r0[TI_flags]
 693         andl    r1, _TIF_WORK_MASK, COH
 694         brne    irq_exit_work
 695
 696 1:
 697 #ifdef CONFIG_TRACE_IRQFLAGS
 698         rcall   trace_hardirqs_on
 699 #endif
 700         popm    r8-r9
 701         mtsr    rar_int\level, r8
 702         mtsr    rsr_int\level, r9
 703         ldmts   sp++,r0-lr
 704         sub     sp, -4          /* ignore r12_orig */
 705         rete
 706
 707 #ifdef CONFIG_PREEMPT
 708 4:      mask_interrupts
 709         mfsr    r8, rsr_int\level
 710         sbr     r8, 16
 711         mtsr    rsr_int\level, r8
 712         ldmts   sp++, r0-lr
 713         sub     sp, -4          /* ignore r12_orig */
 714         rete
 715 #endif
 716
 717 2:      get_thread_info r0
 718         ld.w    r1, r0[TI_flags]
 719         bld     r1, TIF_CPU_GOING_TO_SLEEP
 720 #ifdef CONFIG_PREEMPT
 721         brcc    3f
 722 #else
 723         brcc    1b
 724 #endif
 725         sub     r1, pc, . - cpu_idle_skip_sleep
 726         stdsp   sp[REG_PC], r1
 727 #ifdef CONFIG_PREEMPT
 728 3:      get_thread_info r0
 729         ld.w    r2, r0[TI_preempt_count]
 730         cp.w    r2, 0
 731         brne    1b
 732         ld.w    r1, r0[TI_flags]
 733         bld     r1, TIF_NEED_RESCHED
 734         brcc    1b
 735         lddsp   r4, sp[REG_SR]
 736         bld     r4, SYSREG_GM_OFFSET
 737         brcs    1b
 738         rcall   preempt_schedule_irq
 739 #endif
 740         rjmp    1b
 741         .endm
 742
 743         .section .irq.text,"ax",@progbits
 744
 745         .global irq_level0
 746         .global irq_level1
 747         .global irq_level2
 748         .global irq_level3
 749         IRQ_LEVEL 0
 750         IRQ_LEVEL 1
 751         IRQ_LEVEL 2
 752         IRQ_LEVEL 3
 753
 754         .section .kprobes.text, "ax", @progbits
 755         .type   enter_monitor_mode, @function
 756 enter_monitor_mode:
 757         /*
 758          * We need to enter monitor mode to do a single step. The
 759          * monitor code will alter the return address so that we
 760          * return directly to the user instead of returning here.
 761          */
 762         breakpoint
 763         rjmp    breakpoint_failed
 764
 765         .size   enter_monitor_mode, . - enter_monitor_mode
 766
 767         .type   debug_trampoline, @function
 768         .global debug_trampoline
 769 debug_trampoline:
 770         /*
 771          * Save the registers on the stack so that the monitor code
 772          * can find them easily.
 773          */
 774         sub     sp, 4           /* r12_orig */
 775         stmts   --sp, r0-lr
 776         get_thread_info r0
 777         ld.w    r8, r0[TI_rar_saved]
 778         ld.w    r9, r0[TI_rsr_saved]
 779         pushm   r8-r9
 780
 781         /*
 782          * The monitor code will alter the return address so we don't
 783          * return here.
 784          */
 785         breakpoint
 786         rjmp    breakpoint_failed
 787         .size   debug_trampoline, . - debug_trampoline
 788
 789         .type breakpoint_failed, @function
 790 breakpoint_failed:
 791         /*
 792          * Something went wrong. Perhaps the debug hardware isn't
 793          * enabled?
 794          */
 795         lda.w   r12, msg_breakpoint_failed
 796         mov     r11, sp
 797         mov     r10, 9          /* SIGKILL */
 798         call    die
 799 1:      rjmp    1b
 800
 801 msg_breakpoint_failed:
 802         .asciz  "Failed to enter Debug Mode"