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