79 lines
1.4 KiB
C
79 lines
1.4 KiB
C
/*
|
|
* Interrupts
|
|
*
|
|
* Handle interrupts, the levels.
|
|
*
|
|
*/
|
|
|
|
#include <stdint.h>
|
|
#include "bas_utils.h"
|
|
#include "interrupts.h"
|
|
#include "MCF5475.h"
|
|
|
|
extern uint8_t _rtl_vbr[];
|
|
#define VBR ((uint32_t **) &_rtl_vbr[0])
|
|
|
|
/*
|
|
* register an interrupt handler at the Coldfire interrupt controller and add the handler to the interrupt vector table
|
|
*/
|
|
int register_handler(uint8_t priority, uint8_t intr, void (*func)())
|
|
{
|
|
int i;
|
|
uint8_t level = 0b01111111;
|
|
uint32_t **adr = VBR;
|
|
|
|
intr &= 63;
|
|
priority &= 7;
|
|
|
|
if (intr <= 0)
|
|
return -1;
|
|
|
|
for (i = 1; i < 64; i++)
|
|
if (i != intr)
|
|
{
|
|
if ((MCF_INTC_ICR(i) & 7) == priority)
|
|
CLEAR_BIT_NO(level, (MCF_INTC_ICR(i) >> 3) & 7);
|
|
}
|
|
|
|
for (i = 0; 1 < 7; i++)
|
|
if (level & (1 << i))
|
|
break;
|
|
|
|
if (i >= 7)
|
|
return -1;
|
|
|
|
/*
|
|
* Make sure priority level is high, before changing registers
|
|
*/
|
|
__asm__ volatile (
|
|
"move.w sr,d0\n\t"
|
|
"move.w d0,-(sp) \n\t"
|
|
"move.w #0x2700,sr\n\t"
|
|
:
|
|
:
|
|
: "sp","d0","memory"
|
|
);
|
|
|
|
if (intr < 32)
|
|
CLEAR_BIT(MCF_INTC_IMRL, (1 << intr));
|
|
else
|
|
CLEAR_BIT(MCF_INTC_IMRH, (1 << (intr - 32)));
|
|
|
|
MCF_INTC_ICR(intr) = MCF_INTC_ICR_IP(priority) | MCF_INTC_ICR_IL(i);
|
|
|
|
adr[64 + intr] = (uint32_t *) func; /* first 64 vectors are system exceptions */
|
|
|
|
/*
|
|
* Return the saved priority level
|
|
*/
|
|
__asm__ volatile (
|
|
"move.w (sp)+,d2\n\t"
|
|
"move.w d2,sr\n\t"
|
|
:
|
|
:
|
|
: "sp","d2","memory"
|
|
);
|
|
|
|
return 0;
|
|
}
|