diff options
Diffstat (limited to 'arch/avr32/kernel/entry-avr32b.S')
-rw-r--r-- | arch/avr32/kernel/entry-avr32b.S | 42 |
1 files changed, 37 insertions, 5 deletions
diff --git a/arch/avr32/kernel/entry-avr32b.S b/arch/avr32/kernel/entry-avr32b.S index eeb66792bc37..5f5f7e42f51b 100644 --- a/arch/avr32/kernel/entry-avr32b.S +++ b/arch/avr32/kernel/entry-avr32b.S | |||
@@ -630,9 +630,12 @@ irq_level\level: | |||
630 | rcall do_IRQ | 630 | rcall do_IRQ |
631 | 631 | ||
632 | lddsp r4, sp[REG_SR] | 632 | lddsp r4, sp[REG_SR] |
633 | andh r4, (MODE_MASK >> 16), COH | 633 | bfextu r4, r4, SYSREG_M0_OFFSET, 3 |
634 | cp.w r4, MODE_SUPERVISOR >> SYSREG_M0_OFFSET | ||
635 | breq 2f | ||
636 | cp.w r4, MODE_USER >> SYSREG_M0_OFFSET | ||
634 | #ifdef CONFIG_PREEMPT | 637 | #ifdef CONFIG_PREEMPT |
635 | brne 2f | 638 | brne 3f |
636 | #else | 639 | #else |
637 | brne 1f | 640 | brne 1f |
638 | #endif | 641 | #endif |
@@ -649,9 +652,18 @@ irq_level\level: | |||
649 | sub sp, -4 /* ignore r12_orig */ | 652 | sub sp, -4 /* ignore r12_orig */ |
650 | rete | 653 | rete |
651 | 654 | ||
655 | 2: get_thread_info r0 | ||
656 | ld.w r1, r0[TI_flags] | ||
657 | bld r1, TIF_CPU_GOING_TO_SLEEP | ||
652 | #ifdef CONFIG_PREEMPT | 658 | #ifdef CONFIG_PREEMPT |
653 | 2: | 659 | brcc 3f |
654 | get_thread_info r0 | 660 | #else |
661 | brcc 1b | ||
662 | #endif | ||
663 | sub r1, pc, . - cpu_idle_skip_sleep | ||
664 | stdsp sp[REG_PC], r1 | ||
665 | #ifdef CONFIG_PREEMPT | ||
666 | 3: get_thread_info r0 | ||
655 | ld.w r2, r0[TI_preempt_count] | 667 | ld.w r2, r0[TI_preempt_count] |
656 | cp.w r2, 0 | 668 | cp.w r2, 0 |
657 | brne 1b | 669 | brne 1b |
@@ -662,12 +674,32 @@ irq_level\level: | |||
662 | bld r4, SYSREG_GM_OFFSET | 674 | bld r4, SYSREG_GM_OFFSET |
663 | brcs 1b | 675 | brcs 1b |
664 | rcall preempt_schedule_irq | 676 | rcall preempt_schedule_irq |
665 | rjmp 1b | ||
666 | #endif | 677 | #endif |
678 | rjmp 1b | ||
667 | .endm | 679 | .endm |
668 | 680 | ||
669 | .section .irq.text,"ax",@progbits | 681 | .section .irq.text,"ax",@progbits |
670 | 682 | ||
683 | .global cpu_idle_sleep | ||
684 | cpu_idle_sleep: | ||
685 | mask_interrupts | ||
686 | get_thread_info r8 | ||
687 | ld.w r9, r8[TI_flags] | ||
688 | bld r9, TIF_NEED_RESCHED | ||
689 | brcs cpu_idle_enable_int_and_exit | ||
690 | sbr r9, TIF_CPU_GOING_TO_SLEEP | ||
691 | st.w r8[TI_flags], r9 | ||
692 | unmask_interrupts | ||
693 | sleep 0 | ||
694 | cpu_idle_skip_sleep: | ||
695 | mask_interrupts | ||
696 | ld.w r9, r8[TI_flags] | ||
697 | cbr r9, TIF_CPU_GOING_TO_SLEEP | ||
698 | st.w r8[TI_flags], r9 | ||
699 | cpu_idle_enable_int_and_exit: | ||
700 | unmask_interrupts | ||
701 | retal r12 | ||
702 | |||
671 | .global irq_level0 | 703 | .global irq_level0 |
672 | .global irq_level1 | 704 | .global irq_level1 |
673 | .global irq_level2 | 705 | .global irq_level2 |