diff --git a/BaS_gcc/sys/interrupts.c b/BaS_gcc/sys/interrupts.c index eb5cbeb..991ad52 100644 --- a/BaS_gcc/sys/interrupts.c +++ b/BaS_gcc/sys/interrupts.c @@ -486,3 +486,26 @@ bool gpt0_interrupt_handler(void *arg0, void *arg1) return true; } #endif /* MACHINE_FIREBEE */ + + +uint32_t set_ipl(uint32_t ipl) +{ + uint32_t ret; + + __asm__ __volatile__( + " move.w sr,%[ret]\r\n" /* retrieve status register */ + " andi.l #0x07,%[ipl]\n\t" /* mask out ipl bits on new value */ + " lsl.l #8,%[ipl]\n\t" /* shift them to position */ + " move.l %[ret],d0\n\t" /* retrieve original value */ + " andi.l #0x0000f8ff,d0\n\t" /* clear ipl part */ + " or.l %[ipl],d0\n\t" /* or in new value */ + " move.w d0,sr\n\t" /* put it in place */ + " andi.l #0x0700,%[ret]\r\n" /* mask out ipl bits */ + " lsr.l #8,%[ret]\r\n" /* shift them to position */ + : [ret] "=&d" (ret) /* output */ + : [ipl] "d" (ipl) /* input */ + : "cc" /* clobber */ + ); + + return ret; +}