/*
 * Stand-Alone RTLinux IRQ Managment
 *
 * Written by Vicente Esteve LLoret
 * Released under the terms of the GPL Version 2
 *
 */

#include <arch/rtl_io.h>
#include <arch/hw_irq.h>
#include <arch/linkage.h>
#include <arch/segment.h>
#include <arch/ptrace.h>
#include <arch/timer.h>
#include <arch/mprot.h>
#include <arch/system.h>
#include <deblin/vhal.h>
#include <rtl_core.h>
#include <rtl_conf.h>

#ifdef CONFIG_ARM_SA1100
#include <asm/arch/SA-1100.h>
#endif

#ifdef CONFIG_ARM_PXA
//#include <asm/arch/pxa-regs.h>
#include <asm/arch/hardware.h>
#endif





unsigned long mimodo;
extern char _end;
unsigned long variable;

asmlinkage unsigned int do_IRQ(struct pt_regs regs);
asmlinkage unsigned int do_FIRQ(struct pt_regs regs);
asmlinkage unsigned int do_DATA(struct pt_regs regs);
asmlinkage unsigned int do_RESERVED(struct pt_regs regs);
asmlinkage unsigned int do_PRE(struct pt_regs regs);
asmlinkage unsigned int do_SWI(struct pt_regs regs);

BUILD_IRQ_HANDLER();
BUILD_FIRQ_HANDLER();
BUILD_RESERVED_HANDLER();
BUILD_DATA_HANDLER();
BUILD_PRE_HANDLER();
BUILD_SWI_HANDLER();

BUILD_IRQ_BRANCH();

asmlinkage unsigned int do_SWI(struct pt_regs regs)
{
  DebugString("Estoy en DO_SWI");
  return 0;
};


asmlinkage unsigned int do_PRE(struct pt_regs regs)
{
  DebugString("Estoy en DO_PRE");
  return 0;
  
};


asmlinkage unsigned int do_RESERVED(struct pt_regs regs)
{
  DebugString("Estoy en DO_RESERVED");
  return 0;
};


asmlinkage unsigned int do_DATA(struct pt_regs regs)
{
  DebugString("Estoy en DO_DATA");
  return 0;
};



asmlinkage unsigned int do_FIRQ(struct pt_regs regs)
{
  DebugString("Estoy en DO_FIRQ");
  return 0;
};

asmlinkage unsigned int do_IRQ(struct pt_regs regs)
{
  unsigned int irq;
#if CONFIG_KERNEL_MEMORYPROT  
  mprot_t mprot; 
#endif  

  STARTKERNELCODE(mprot);
//  __asm__ __volatile("str r0,%0":"=m"(variable));
//  DebugString("do_IRQin");
  
//  if (ICIP&IC_OST0) {
     irq=IRQ_OST0;
//  };
//  if (ICIP&IC_OST1) {
//     irq=IRQ_OST1;
//  };
//  DebugValue(OSSR);  
  dispatch_rtl_handler(irq,&regs);

//  DebugString("do_IRQout");
//infinito:
//  goto infinito;
  ENDKERNELCODE(mprot);

  return 1; 
}


void patch_vector_table(void)
{
  unsigned long long *from = &irq_branch; 
  
#ifdef CONFIG_ARM_SA1100
  unsigned long long *to   = (unsigned long long *) 0xc0008000;
#endif
#ifdef CONFIG_ARM_PXA
  unsigned long long *to   = (unsigned long long *) 0xa0008000;
#endif
  
//  unsigned long *ptr = (unsigned long *) 0xc0008000;
  *to++ = *from++;
  *to++ = *from++;
  *to++ = *from++;
  *to++ = *from++;
  *to++ = *from++;
  *to++ = *from++;
  *to++ = *from++;
  *to = *from;
};

#define GET_MODE(x) __asm__ __volatile__("mrs %0,cpsr \n":"=r" (x));
 
void show_mode(void)
{
  GET_MODE(mimodo);
  DebugString("MIMODO");
  DebugValue(mimodo);
};

void setup_arch(void)
{
// show_mode();
  patch_vector_table(); 
    
    /* disable all IRQs */
  ICMR = 0;
    /* all IRQs are IRQ, not FIQ */
  ICLR = 0;
    /* I don't like idle mode */
//  ICCR = ICCR_IdleMskInt; 
    /* clear all GPIO edge detects */
//  GFER = 0;
//  GRER = 0;
//  GEDR = -1;
  DebugString("FIN Setup_ARCH");
}