| @@ -585,51 +585,59 @@ enable_interrupts(uint32_t mask) | | | @@ -585,51 +585,59 @@ enable_interrupts(uint32_t mask) |
585 | | | 585 | |
586 | return ret; | | 586 | return ret; |
587 | } | | 587 | } |
588 | | | 588 | |
589 | #define restore_interrupts(old_cpsr) \ | | 589 | #define restore_interrupts(old_cpsr) \ |
590 | (__set_cpsr_c((I32_bit | F32_bit), (old_cpsr) & (I32_bit | F32_bit))) | | 590 | (__set_cpsr_c((I32_bit | F32_bit), (old_cpsr) & (I32_bit | F32_bit))) |
591 | | | 591 | |
592 | static inline void cpsie(register_t psw) __attribute__((__unused__)); | | 592 | static inline void cpsie(register_t psw) __attribute__((__unused__)); |
593 | static inline register_t cpsid(register_t psw) __attribute__((__unused__)); | | 593 | static inline register_t cpsid(register_t psw) __attribute__((__unused__)); |
594 | | | 594 | |
595 | static inline void | | 595 | static inline void |
596 | cpsie(register_t psw) | | 596 | cpsie(register_t psw) |
597 | { | | 597 | { |
| | | 598 | #if _ARM_ARCH_6 |
598 | if (!__builtin_constant_p(psw)) { | | 599 | if (!__builtin_constant_p(psw)) { |
599 | enable_interrupts(psw); | | 600 | enable_interrupts(psw); |
600 | return; | | 601 | return; |
601 | } | | 602 | } |
602 | switch (psw & (I32_bit|F32_bit)) { | | 603 | switch (psw & (I32_bit|F32_bit)) { |
603 | case I32_bit: __asm("cpsie\ti"); break; | | 604 | case I32_bit: __asm("cpsie\ti"); break; |
604 | case F32_bit: __asm("cpsie\tf"); break; | | 605 | case F32_bit: __asm("cpsie\tf"); break; |
605 | case I32_bit|F32_bit: __asm("cpsie\tif"); break; | | 606 | case I32_bit|F32_bit: __asm("cpsie\tif"); break; |
606 | } | | 607 | } |
| | | 608 | #else |
| | | 609 | enable_interrupts(psw); |
| | | 610 | #endif |
607 | } | | 611 | } |
608 | | | 612 | |
609 | static inline register_t | | 613 | static inline register_t |
610 | cpsid(register_t psw) | | 614 | cpsid(register_t psw) |
611 | { | | 615 | { |
| | | 616 | #if _ARM_ARCH_6 |
612 | register_t oldpsw; | | 617 | register_t oldpsw; |
613 | if (!__builtin_constant_p(psw)) | | 618 | if (!__builtin_constant_p(psw)) |
614 | return disable_interrupts(psw); | | 619 | return disable_interrupts(psw); |
615 | | | 620 | |
616 | __asm("mrs %0, cpsr" : "=r"(oldpsw)); | | 621 | __asm("mrs %0, cpsr" : "=r"(oldpsw)); |
617 | switch (psw & (I32_bit|F32_bit)) { | | 622 | switch (psw & (I32_bit|F32_bit)) { |
618 | case I32_bit: __asm("cpsid\ti"); break; | | 623 | case I32_bit: __asm("cpsid\ti"); break; |
619 | case F32_bit: __asm("cpsid\tf"); break; | | 624 | case F32_bit: __asm("cpsid\tf"); break; |
620 | case I32_bit|F32_bit: __asm("cpsid\tif"); break; | | 625 | case I32_bit|F32_bit: __asm("cpsid\tif"); break; |
621 | } | | 626 | } |
622 | return oldpsw; | | 627 | return oldpsw; |
| | | 628 | #else |
| | | 629 | return disable_interrupts(psw); |
| | | 630 | #endif |
623 | } | | 631 | } |
624 | | | 632 | |
625 | #else /* ! __PROG32 */ | | 633 | #else /* ! __PROG32 */ |
626 | #define disable_interrupts(mask) \ | | 634 | #define disable_interrupts(mask) \ |
627 | (set_r15((mask) & (R15_IRQ_DISABLE | R15_FIQ_DISABLE), \ | | 635 | (set_r15((mask) & (R15_IRQ_DISABLE | R15_FIQ_DISABLE), \ |
628 | (mask) & (R15_IRQ_DISABLE | R15_FIQ_DISABLE))) | | 636 | (mask) & (R15_IRQ_DISABLE | R15_FIQ_DISABLE))) |
629 | | | 637 | |
630 | #define enable_interrupts(mask) \ | | 638 | #define enable_interrupts(mask) \ |
631 | (set_r15((mask) & (R15_IRQ_DISABLE | R15_FIQ_DISABLE), 0)) | | 639 | (set_r15((mask) & (R15_IRQ_DISABLE | R15_FIQ_DISABLE), 0)) |
632 | | | 640 | |
633 | #define restore_interrupts(old_r15) \ | | 641 | #define restore_interrupts(old_r15) \ |
634 | (set_r15((R15_IRQ_DISABLE | R15_FIQ_DISABLE), \ | | 642 | (set_r15((R15_IRQ_DISABLE | R15_FIQ_DISABLE), \ |
635 | (old_r15) & (R15_IRQ_DISABLE | R15_FIQ_DISABLE))) | | 643 | (old_r15) & (R15_IRQ_DISABLE | R15_FIQ_DISABLE))) |