147 lines
3 KiB
C
147 lines
3 KiB
C
#include "serial.h"
|
|
#include "printf.h"
|
|
#include "irq.h"
|
|
#include "memio.h"
|
|
|
|
static struct {
|
|
void (*handler)(enum irq_number irq_num, void *opaque);
|
|
void *opaque;
|
|
} handlers[__irq_max__];
|
|
|
|
int irq_init(void)
|
|
{
|
|
register int var;
|
|
|
|
/* Acknowledge all interrupts */
|
|
writel(0xffffffff, IRQ_BASE + IRQ_MASK_OFF + IRQ_NUM_ADJ(0));
|
|
writel(0xffffffff, IRQ_BASE + IRQ_MASK_OFF + IRQ_NUM_ADJ(32));
|
|
|
|
asm volatile ("mrs %0, cpsr":"=r" (var));
|
|
if (!(var & 0x80)) {
|
|
serial_puts("Interrupts already enabled\n");
|
|
return -1;
|
|
}
|
|
|
|
serial_puts("Interrupts were disabled. Re-enabling...\n");
|
|
var &= ~0x80;
|
|
var |= 0x40;
|
|
var &= ~0x1f;
|
|
var |= 0x10;
|
|
asm volatile ("msr cpsr, %0":"=r" (var));
|
|
|
|
return 0;
|
|
}
|
|
|
|
int fiq_init(void)
|
|
{
|
|
serial_puts("FIQs compiled out\n");
|
|
return -1;
|
|
/*
|
|
register int var;
|
|
asm volatile ("mrs %0, cpsr":"=r" (var));
|
|
if (!(var & 0x40)) {
|
|
serial_puts("FIQ already enabled\n");
|
|
return -1;
|
|
}
|
|
|
|
serial_puts("FIQ was disabled. Re-enabling...\n");
|
|
var &= ~0x40;
|
|
asm volatile ("msr cpsr, %0":"=r" (var));
|
|
|
|
return 0;
|
|
*/
|
|
}
|
|
|
|
int irq_enable(enum irq_number irq_num)
|
|
{
|
|
uint32_t reg = IRQ_BASE + IRQ_MASK_OFF + IRQ_CLR + IRQ_NUM_ADJ(irq_num);
|
|
if (irq_num >= __irq_max__)
|
|
return -1;
|
|
|
|
writel(1 << (irq_num & 31), reg);
|
|
return 0;
|
|
}
|
|
|
|
int irq_disable(enum irq_number irq_num)
|
|
{
|
|
uint32_t reg = IRQ_BASE + IRQ_MASK_OFF + IRQ_SET + IRQ_NUM_ADJ(irq_num);
|
|
|
|
if (irq_num >= __irq_max__)
|
|
return -1;
|
|
|
|
writel(1 << (irq_num & 31), reg);
|
|
return 0;
|
|
}
|
|
|
|
void irq_stimulate(enum irq_number irq_num)
|
|
{
|
|
uint32_t reg = IRQ_BASE + IRQ_STIM_OFF + IRQ_SET + IRQ_NUM_ADJ(irq_num);
|
|
writel(1 << (irq_num & 31), reg);
|
|
}
|
|
|
|
void irq_stimulate_reset(enum irq_number irq_num)
|
|
{
|
|
uint32_t reg = IRQ_BASE + IRQ_STIM_OFF + IRQ_CLR + IRQ_NUM_ADJ(irq_num);
|
|
writel(1 << (irq_num & 31), reg);
|
|
}
|
|
|
|
void irq_acknowledge(enum irq_number irq_num)
|
|
{
|
|
uint32_t reg = IRQ_BASE + IRQ_ACK_OFF + IRQ_NUM_ADJ(irq_num);
|
|
|
|
if (irq_num >= __irq_max__)
|
|
return;
|
|
|
|
writel(1 << (irq_num & 31), reg);
|
|
return;
|
|
}
|
|
|
|
void irq_mask_acknowledge(enum irq_number irq_num)
|
|
{
|
|
irq_disable(irq_num);
|
|
irq_acknowledge(irq_num);
|
|
}
|
|
|
|
void irq_register_handler(enum irq_number irq_num,
|
|
void (*handler)(enum irq_number irq_num, void *opaque),
|
|
void *opaque)
|
|
{
|
|
if (irq_num >= __irq_max__)
|
|
return;
|
|
handlers[irq_num].handler = handler;
|
|
handlers[irq_num].opaque = opaque;
|
|
}
|
|
|
|
static void irq_dispatch_one(enum irq_number irq_num)
|
|
{
|
|
if (handlers[irq_num].handler)
|
|
handlers[irq_num].handler(irq_num, handlers[irq_num].opaque);
|
|
else
|
|
printf("Unhandled IRQ: %d\n", irq_num);
|
|
irq_acknowledge(irq_num);
|
|
}
|
|
|
|
void irq_dispatch(void)
|
|
{
|
|
uint32_t reg;
|
|
uint32_t val;
|
|
int i;
|
|
|
|
printf("Dispatching IRQs...\n");
|
|
reg = IRQ_BASE + IRQ_STATUS_OFF;
|
|
val = readl(reg);
|
|
printf("Lower Mask: 0x%08x\n", val);
|
|
|
|
for (i = 0; i < 32; i++)
|
|
if (val & (1 << i))
|
|
irq_dispatch_one(i);
|
|
|
|
reg += IRQ_BASE + IRQ_STATUS_OFF + 4;
|
|
val = readl(reg);
|
|
printf("Upper Mask: 0x%08x\n", val);
|
|
for (i = 0; i < (__irq_max__ - 32); i++)
|
|
if (val & (1 << i))
|
|
irq_dispatch_one(32 + i);
|
|
|
|
printf("Done dispatch\n");
|
|
}
|