// CrossWorks Tasking Library.
//
// Copyright (c) 2004 Rowley Associates Limited.
//
// This file may be distributed under the terms of the License Agreement
// provided with this software.
//
// THIS FILE IS PROVIDED AS IS WITH NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.

#include "ctl_api.h"
#include <targets/ML674000.h>

#define INTERRUPT_SOURCE_COUNT 32

struct vector_rec_t
{
  CTL_ISR_FN_t isr;
  unsigned int priority;
};

static struct vector_rec_t vectors[INTERRUPT_SOURCE_COUNT];

void irq_handler(void) __attribute__((naked));
static unsigned int currentVector;

void
irq_handler(void)
{
  asm("stmfd sp!, {r0-r12, lr}"); 
  asm("mrs r0, spsr");
  asm("stmfd sp!, {r0}");
  ctl_interrupt_count++;
  currentVector = IRN;
  IRCL = currentVector;
  vectors[currentVector].isr();
  CILCL = 0;
  asm("mov r0, sp");
  asm("ldr r1, =ctl_exit_isr");
  asm("bx r1");
}

int
ctl_set_isr(unsigned int vector, unsigned int priority, CTL_ISR_TRIGGER_t trigger, CTL_ISR_FN_t isr, CTL_ISR_FN_t *oldisr)
{
  if (oldisr)
    *oldisr = vectors[vector].isr;
  vectors[vector].isr = isr;
  vectors[vector].priority = priority;
  if (vector >= 28)
    {
      /* Interrupt can setup trigger mode */
      unsigned int mode;
      switch (trigger)
        {
          case CTL_ISR_TRIGGER_LOW_LEVEL:
            mode = 0;
            break;
          case CTL_ISR_TRIGGER_HIGH_LEVEL:
            mode = 2;
            break;
          case CTL_ISR_TRIGGER_NEGATIVE_EDGE:
            mode = 1;
            break;
          case CTL_ISR_TRIGGER_POSITIVE_EDGE:
            mode = 3;
            break;
        }
      if (vector >= 30)
        IDM = (IDM & ~(3 << 14)) | (mode << 14);
      else
        IDM = (IDM & ~(3 << 12)) | (mode << 12);
    }
  return 1;
}

static void
setInterruptLevel(unsigned int vector, unsigned int priority)
{
  unsigned int offset;
  if (vector >= 16)
    {
      offset = ((vector - 16) & ~1) * 2;
      ILC = (ILC & ~(7 << offset)) | ((priority & 7) << offset);
    }
  else if (vector >= 8)
    {
      offset = (vector - 8) * 4;
      ILC1 = (ILC1 & ~(7 << offset)) | ((priority & 7) << offset);
    }
  else
    {
      if (vector >= 6)
        offset = 24;
      else if (vector >= 4)
        offset = 16;
      else if (vector >= 1)
        offset = 4;
      else
        offset = 0;
      ILC0 = (ILC0 & ~(7 << offset)) | ((priority & 7) << offset);
    }
}

int
ctl_unmask_isr(unsigned int vector)
{
  setInterruptLevel(vector, vectors[vector].priority);
  return 1;
}

int
ctl_mask_isr(unsigned int vector)
{
  setInterruptLevel(vector, 0);
  return 1;
}
