/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*                                                Logic PD                                                                              */
/*                                         Board Support Package                                                                        */
/*                                         PXA 270-520-10 Development Board                                                             */
/*                                                                                                                                      */
/*                             (c) Copyright 2004, Micrium, Inc., Weston, FL                                                            */
/*                                          All Rights Reserved                                                                         */
/*                                                                                                                                      */
/* File         : BSP.C                                                                                                                 */
/* Originally by: Jean J. Labrosse                                                                                                      */
/* Modified by:   Lance Simms                                                                                                           */
/*                Lawrence Livermore National Labs                                                                                      */
/*                                                                                                                                      */              
/*                Modifications have been made for PXA270 specific registers.  Interrupt handling is                                    */
/*                still a work in progress.                                                                                             */
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
#include <includes.h>
#include <fs_app.h>
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*                                           BSP INITIALIZATION                                                                         */
/*                                                                                                                                      */
/* Description : This function should be called by your application code before you make use of any of the                              */
/*               functions found in this module.                                                                                        */
/*                                                                                                                                      */
/* Arguments   : none                                                                                                                   */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/

void  BSP_Init (void)
{

    BSP_InitIntCtrl();                            /* Initialize the interrupt controller, timers, etc. */

}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*                                    INITIALIZE INTERRUPT CONTROLLER                                                                   */
/*                                                                                                                                      */
/* FUNCION   : BSP_InitIntCtrl                                                                                                          */
/*                                                                                                                                      */ 
/* PURPOSE   : This function should be called by your application code before you make use of any of the                                */
/*               functions found in this module.                                                                                        */
/*                                                                                                                                      */
/* INPUTS    : none                                                                                                                     */
/*                                                                                                                                      */
/* RETURNS   : none                                                                                                                     */
/*                                                                                                                                      */
/* Note(s)     : 1) 0xE59FF020 is opcode of:  ldr pc,[pc,#+0x20]                                                                        */
/*               2) By default, OS Timer 1 produces an interrupt that halts the system if handling is not                               */
/*                  done (INT = 0x00000080)                                                                                             */
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/

static  void  BSP_InitIntCtrl (void)
{
  
#ifdef DISABLE_THE_CACHE
  //Disable the cache.  This works!!
  DISABLE_CACHE();
#endif    
  
  //Declare a counter for printing to the serial port at a rate it can handle
  INT32U counter;
  INT32U i=0;
  INT32U j=0;
  
  //Result of register write and reads for SPI interface  
  INT32S RegResult = 0;

  //Declare variables for writing and Reading SSP1 Registers
  INT8U  This8RegValue  = 0;
  INT16U This16RegValue = 0;

  //Values for the upper and lower byte of the divisor latches
  INT8U a,b;
 
  //SET INTERRUPT VECTORS (mapped from 0x00000000 to 0xA0000000)
  BSP_IRQ_VECTOR_ADDR               = 0xE59FF018;                  /* LDR PC,[PC,#0x18] instruction                                     */
  BSP_IRQ_ISR_ADDR                  = (INT32U)OS_CPU_IRQ_ISR;      /* IRQ exception vector address                                      */

  BSP_FIQ_VECTOR_ADDR               = 0xE59FF018;                  /* LDR PC,[PC,#0x18] instruction                                     */
  BSP_FIQ_ISR_ADDR                  = (INT32U)OS_CPU_FIQ_ISR;      /* FIQ exception vector address                                      */

  //SET INSTRUCTIONS for other vectors to jump to themselves if exception occurs    
  BSP_UNDEF_INSTRUCTION_VECTOR_ADDR = 0xEAFFFFFE;                  /* Jump to itself                                                    */
  BSP_SWI_VECTOR_ADDR               = 0xEAFFFFFE;
  BSP_PREFETCH_ABORT_VECTOR_ADDR    = 0xEAFFFFFE;
  BSP_DATA_ABORT_VECTOR_ADDR        = 0xEAFFFFFE;
  
  //MASK INTERRUPTS to begin with
  INT_MASK_ADDR                     = 0x00000000;                  /* MASK ALL INTERRUPTS to start with.  Reenable in 1st task          */ 
 
  //Enable the 32.768 kHz clock for timekeeping purposes
  OS_OSCILLATOR_CONFIG_REG_ADDR     = 0x00000002;                  /* Enable 32.768 kHz oscillator; Disable 13 MHz oscillaotr           */
  
#ifdef PRINTINITIALIZATION
  printf("Waiting for 32.768 kHz oscillator to be enabled\n\r");   /* We must wait for OSCC[OOK] (bit0) to go high before               */     
#endif
  while((OS_OSCILLATOR_CONFIG_REG_ADDR & BIT0)==0){uart_pause();}  /* the oscillator is actually available                              */
#ifdef PRINTINITIALIZATION
  printf("32.768 kHz oscillator is enabled\n\r");
#endif
  
  //OSTIMERS ==================================== Enable Timer 4 to set interrupts and set time
  OS_TIMER_INT_ENABLE_REG_ADDR      = 0x00000010;                  /* Enable Timer 4 as the IRQ ISR timer                               */
  OS_TIMER_OMCR4_REG_ADDR           = 0x000000CA;                  /* Set timer 4 to 1 ms tick                                          */
  OS_TIMER_MATCH4_REG_ADDR          = 0x000000FA;                  /* Set timer 4 to match at 250 ms                                    */

  //Enable Timers 8 and 9 to generate time-stamps
  OS_TIMER_OMCR8_REG_ADDR           = 0x000000CA;                  /* Enable Timer 8 for millisecond counter, periodic at 1000 ms       */
  OS_TIMER_MATCH8_REG_ADDR          = 0x000003E8;                  /* Set match for 1000 ticks                                          */
  OS_TIMER_OSCR8_REG_ADDR           = 0x00000000;                  /* RESET COUNTER 8                                                   */

  OS_TIMER_OMCR9_REG_ADDR           = 0x000002CB;                  /* Enable Timer 9 for 1 second, not periodic, snapshot mode          */
  OS_TIMER_MATCH9_REG_ADDR          = 0xFFFFFFFF;                  /* Set match for infinity                                            */
  OS_TIMER_OSCR9_REG_ADDR           = 0x00000000;                  /* RESET COUNTER 9                                                   */

  OS_TIMER_OMCR10_REG_ADDR          = 0x00000084;                  /* Enable Timer 10 for 1 microsecond, not periodic                   */
  OS_TIMER_MATCH10_REG_ADDR         = 0xFFFFFFFF;                  /* Set match for infinity                                            */
  
  //UARTA ======================================= Enable serial port interrupts and configure flow control on FFUARTA
  FFUARTA_INT_CONTROL_REG_ADDR      = 0x00000041;                  /* Enable UART Unit and Receiver Data Available interrupt            */
  FFUARTA_FCR_REG_ADDR              = 0x00000001;                  /* Enable Receive FIFO interrupts with 1 byte in RBR                 */
  FFUARTA_MCR_REG_ADDR              = 0x0000002A;                  /* Enable interrupts and auto-flow control on the modem control line */
  FFUARTA_LCR_REG_ADDR              = 0x00000003;                  /* Set the number of bits per character to 8                         */
  GPIO_ALT_DEF_32_47_REG_ADDR      &= 0xFFF33F0F;                  /* Zero alt def for GPIO 34 (RXD), 35 (CTS), 39 (TXD), 41 (RTS)      */
  GPIO_ALT_DEF_32_47_REG_ADDR      |= 0x00088050;                  /* Function 1-34 (RXD), 1-35 (CTS), 2-39 (TXD), 2-41 (RTS)           */
  GPIO_PIN_DIRECTION1_REG_ADDR     |= 0x00000280;                  /* Set Output 39 (TXD), 41 (RTS)                                     */

  //SSP1 ======================================== Enable SPI1 to communicate with GPS receiver (write SSCR0[7] last) (20 MHz max on SC)
  SPI_SSCR1_1_REG_ADDR              = 0x00000000;                  /* Set TX, RX Threshold at 1 byte                                    */
  SPI_SSCR0_1_REG_ADDR              = 0x00C02087;                  /* Set 400 kb/s, disable TX/RX FIFO IRQ, 8 bit data, Enable SPI      */
  GPIO_PIN_DIRECTION0_REG_ADDR     |= 0x03800000;                  /* Set SSP1 TXD (GPIO25), SSP1 CLK (GPIO23), SSP1 FRM (GPIO24)       */
  GPIO_PIN_DIRECTION1_REG_ADDR     &= 0xFBFFFFFF;                  /* Set GPIO58 as input (when real IRQ is available !)                */
  GPIO_PIN_DIRECTION1_REG_ADDR     |= 0x08000000;                  /* Set GPIO59 as output (EVENT1 on GPS)                              */
  GPIO_ALT_DEF_16_31_REG_ADDR      &= 0xFFC03FFF;                  /* Zero relevant alternate function definitions                      */
  GPIO_ALT_DEF_16_31_REG_ADDR      |= 0x001A8000;                  /* Functions SSP1 TXD, RXD (GPIO26), CLK, FRM, keep RDY high at 0x10 */
  GPIO_PIN_OUTPUT_SET1_REG_ADDR    |= MARK1_GPS_GPIO_PIN;          /* Make sure MARKTIME and MARKPOS Trigger is high                    */
  GPIO_PIN_FALLING_EDGE1_REG_ADDR   = 0x04000000;                  /* Enable Falling Edge detection on GPIO 58                          */
  GPIO_PIN_RISING_EDGE1_REG_ADDR   &= 0xFBFFFFFF;                  /* Disable Rising Edge detection on GPIO 58                          */
#ifdef NO_RX_INTERRUPT
  GPIO_PIN_DIRECTION1_REG_ADDR     |= 0x04000000;                  /* Set GPIO58 as output (temporary solution for lack of IRQ)         */
#endif
  
  //SSP2 ======================================== Enable SPI2 to communicate with IBIS5 B (2.5 MHz max on Cypress serial register)      
  SPI_SSCR1_2_REG_ADDR              = 0x00000000;                  /* Set TX, RX Threshold at 1 byte                                    */
  SPI_SSCR0_2_REG_ADDR              = 0x00C00A8F;                  /* Set 1.3 Mb/s, disable TX/RX FIFO IRQ, 16 bit data, Enable SPI     */
  GPIO_PIN_DIRECTION0_REG_ADDR     |= 0x00084000;                  /* Set SSP2 CLK (GPIO19) and FRM (GPIO14)                            */
  GPIO_PIN_DIRECTION2_REG_ADDR     |= 0x00800000;                  /* Set SSP2 TXD (GPIO87)                                             */
  GPIO_ALT_DEF_00_15_REG_ADDR      &= 0xCFFFFFFF;                  /* Zero alternate definition for GPIO14 SSP2 FRM                     */
  GPIO_ALT_DEF_00_15_REG_ADDR      |= 0x20000000;                  /* Function 1 for SSP2 FRM (SSPSFRM2)                                */
  GPIO_ALT_DEF_16_31_REG_ADDR      &= 0xFFFFFF3F;                  /* Zero alternate definition for GPIO19 SSP2 CLK                     */
  GPIO_ALT_DEF_16_31_REG_ADDR      |= 0x00000040;                  /* Function 1 for SSP2 CLK (SSPSCLK2)                                */
  GPIO_ALT_DEF_80_95_REG_ADDR      &= 0xFFFF3FFF;                  /* Zero alternate definition for GPIO87 SSP2 TXD                     */
  GPIO_ALT_DEF_80_95_REG_ADDR      |= 0x00004000;                  /* Function 1 for SSP2 TXD (SSPTXD2)                                 */

  //I2C ========================================= Enable I2C Interface to talk with MAX1239 A/D CONVERTER       
#ifdef USE_I2C_INTERFACE
  GPIO_ALT_DEF_112_120_REG_ADDR    &= 0x0003C3FF;                  /* Zero alternate function definition for GPIO117 and 118 (SCL, SDA) */
  GPIO_ALT_DEF_112_120_REG_ADDR    |= 0x00001400;                  /* Set GPIO117 and 118 for Function 1                                */
  I2C_ISAR_REG_ADDR                 = 0x000000FF;                  /* Set the PXA270 I2C Interface Address                              */
  I2C_ICR_REG_ADDR                 |= 0x000000E0;                  /* Enable I2C as master in slow mode (100kbps), reset it             */  
  //RegResult = WriteI2C(I2C_MAX1239_SLAVE_ADDR, 0x00);              /* Write the Setup byte for internal clock, unipolar, and reset cfg  */
  //RegResult = WriteI2C(I2C_MAX1239_SLAVE_ADDR, 0x8D);              /* Write the Config byte for AIN6, CAMERA0_TEMP, single-ended        */ 
#endif
  
  //COMPACT FLASH =============================== Configure Compact Flash card for reading and writing images without image acquisition
#ifdef USECOMPACTFLASHCARD
  //Compact Flash card (see older version of bsp.c for general GPIO pins used for Compact Flash; i.e. not memory mapped) 
  cf_wait();
  CF_DEVICE_CONTROL_REG_ADDR       |= 0x00000002;                  /* Disable interrupts on flash card                                  */
#endif
  
  //GPS ========================================= Configure GPS over either UARTB or SPI1 (the latter is the actual implementation) 
#ifdef USE_UARTB
  //UARTB ======================================= Enable serial port interrupts and configure flow control on UARTB
  UARTB_LCR_REG_ADDR               |= 0x00000080;                  /* Set DLAB bit to change baud rate                                  */
  UARTB_DIVISOR_LBYTE_REG_ADDR      = 0x00000008;                  /* Set Lower byte to 8 for baud of 115200 (0x60 for 9600)            */
  UARTB_DIVISOR_UBYTE_REG_ADDR      = 0x00000000;                  /* Set Upper byte to 0                                               */
  UARTB_LCR_REG_ADDR               &= 0x7F000000;                  /* Unset DLAB bit so we can use TX and RX                            */  
  UARTB_INT_CONTROL_REG_ADDR        = 0x00000041;                  /* Enable UART Unit and Receiver Data Available interrupt            */
  UARTB_FCR_REG_ADDR                = 0x00000001;                  /* Enable Receive FIFO interrupts with 1 byte in RBR                 */
  UARTB_MCR_REG_ADDR                = 0x00000008;                  /* Enable interrupts on the modem control line                       */
  UARTB_LCR_REG_ADDR                = 0x00000003;                  /* Set the number of bits per characters to 8                        */  
  GPIO_ALT_DEF_32_47_REG_ADDR      &= 0xFF0FFFFF;                  /* Zero alt def for GPIO 42 UARTB (RXD), 43 (TXD)                    */
  GPIO_ALT_DEF_32_47_REG_ADDR      |= 0x00900000;                  /* Function 1-42 (RXD), 2-43 (TXD)                                   */
  GPIO_PIN_DIRECTION1_REG_ADDR     |= 0x00001800;                  /* Set 43 (TXD), 44 (EVENT1 for GPS)                                 */

  #ifdef RESTORE_GPS_FROM_DEFAULTS
  //THE OMEV-1G is set at 9600 baud .  This will set it to 115200 baud and disable the prompt characters

  //Enable serial port interrupts and configure flow control on UARTB at 9600 Baud (Factory default for OEMV-1G)
  UARTB_LCR_REG_ADDR               |= 0x80;                        /* Set DLAB bit to change baud rate                                  */
  UARTB_DIVISOR_LBYTE_REG_ADDR      = 0x60;                        /* Set Lower byte to 0x60 for 9600                                   */
  UARTB_DIVISOR_UBYTE_REG_ADDR      = 0x00;                        /* Set Upper byte to 0                                               */
  UARTB_LCR_REG_ADDR               &= 0x7F;                        /* Unset DLAB bit so we can use TX and RX                            */  
  uart_pause();

  //Change the baud on the receiver to 115200
  sprintf(&GPSCmndBuf[0], "com com1 115200 n 8 1 n off on");
  SendGPSCommand(GPSCmndBuf, Str_Len(GPSCmndBuf));
  for (i=0;i<10;i++){uart_pause();}
    
  //Enable serial port interrupts and configure flow control on UARTB at 115200 Baud (Factory default for OEMV-1G)
  UARTB_LCR_REG_ADDR               |= 0x80;                        /* Set DLAB bit to change baud rate                                  */
  UARTB_DIVISOR_LBYTE_REG_ADDR      = 0x08;                        /* Set Lower byte to 8 for baud of 115200 (0x60 for 9600)            */
  UARTB_DIVISOR_UBYTE_REG_ADDR      = 0x00;                        /* Set Upper byte to 0                                               */
  UARTB_LCR_REG_ADDR               &= 0x7F;                        /* Unset DLAB bit so we can use TX and RX                            */  

  //Turn off command prompt
  sprintf(&GPSCmndBuf[0], "interfacemode com1 novatel novatel off");
  SendGPSCommand(GPSCmndBuf, Str_Len(GPSCmndBuf)); 
  for (i=0;i<10;i++){uart_pause();}
  //Save the changes
  sprintf(&GPSCmndBuf[0], "saveconfig");
  SendGPSCommand(GPSCmndBuf, Str_Len(GPSCmndBuf));  
  #endif
#else

  //Set up the FIFO
  RegResult = WriteSC16Register(SC16_IER_ADDR, 0x01);              /* Enable FIFO Receive Interrupts                                    */
  RegResult = WriteSC16Register(SC16_FCR_ADDR, 0x87);              /* Enable the TX and RX FIFOs on the SPI to UART chip and clear them */
  RegResult = WriteSC16Register(SC16_LCR_ADDR, 0x03);              /* No Parity, 1 Stop bit, 8 character word length                    */  

  //First set up baud rate
  RegResult = WriteSC16Register(SC16_LCR_ADDR, 0x80);              /* Enable the Divisor Latch to change the baud rate                  */
  RegResult = WriteSC16Register(SC16_DLL_ADDR, 0x08);              /* Write 8 to DLL to divide 14.756 MHz clock to 115,200 baud         */
  RegResult = WriteSC16Register(SC16_DLH_ADDR, 0x00);              /* Write 0 to DLH                                                    */
  RegResult = WriteSC16Register(SC16_LCR_ADDR, 0x03);              /* Disable the Divisor Latch                                         */
  
  #ifdef RESTORE_GPS_FROM_DEFAULTS
  //THE OMEV-1G is set at 9600 baud .  This will set it to 115200 baud and disable the prompt characters

  //First set up baud rate
  RegResult = WriteSC16Register(SC16_LCR_ADDR, 0x80);              /* Enable the Divisor Latch to change the baud rate                  */
  RegResult = WriteSC16Register(SC16_DLL_ADDR, 0x60);              /* Write 8 to DLL to divide 14.756 MHz clock to 115,200 baud         */
  RegResult = WriteSC16Register(SC16_DLH_ADDR, 0x00);              /* Write 0 to DLH                                                    */
  RegResult = WriteSC16Register(SC16_LCR_ADDR, 0x03);              /* Disable the Divisor Latch                                         */
  
  //Change the baud on the receiver to 115200
  sprintf(&GPSCmndBuf[0], "com com1 115200 n 8 1 n off on");
  SendGPSCommand(GPSCmndBuf, Str_Len(GPSCmndBuf));
  for (i=0;i<10;i++){uart_pause();}

  //First set up baud rate
  RegResult = WriteSC16Register(SC16_LCR_ADDR, 0x80);              /* Enable the Divisor Latch to change the baud rate                  */
  RegResult = WriteSC16Register(SC16_DLL_ADDR, 0x08);              /* Write 8 to DLL to divide 14.756 MHz clock to 115,200 baud         */
  RegResult = WriteSC16Register(SC16_DLH_ADDR, 0x00);              /* Write 0 to DLH                                                    */
  RegResult = WriteSC16Register(SC16_LCR_ADDR, 0x03);              /* Disable the Divisor Latch                                         */

  //Turn off command prompt
  sprintf(&GPSCmndBuf[0], "interfacemode com1 novatel novatel off");
  SendGPSCommand(GPSCmndBuf, Str_Len(GPSCmndBuf)); 
  for (i=0;i<10;i++){uart_pause();}
  //Save the changes
  sprintf(&GPSCmndBuf[0], "saveconfig");
  SendGPSCommand(GPSCmndBuf, Str_Len(GPSCmndBuf));  
  #endif
  
#endif

#ifdef USE_QC_INTERFACE
  
  //SERIAL REGISTER ON CYPRESS IBIS5
  RegResult = WriteIBIS5Register(IBIS5_AMPLIFIER_ADDR, 0x0046);    /* Set gain to 0110, DC = 4.25, Amplifier is out of standby mode     */
  RegResult = WriteIBIS5Register(IBIS5_ADC_REGISTER,   0x0003);    /* Output bits in normal mode (TRISTATE=1), Gamma correction off     */

  //DMA CONTROLLER ============================== Channel 0 is used for Quick Capture Interface
  //First load the descriptor chain     
  for (j=0;j<MAXEXPOSURES;j++){
    for (i=0;i<NAXIS2;i++){
      // Set descriptor to point at next descriptor address       
      QCI_DMA_DesArr[4*(NAXIS2*j+i)+0] = ((INT32U)&QCI_DMA_DesArr[4*(NAXIS2*j+(i+1))]);
      // Set source address for Quick Capture Receive Buffer 0             
      QCI_DMA_DesArr[4*(NAXIS2*j+i)+1] = 0x50000028;
      // Set target address for Image buffer                               
      QCI_DMA_DesArr[4*(NAXIS2*j+i)+2] = (INT32U)&QCI_ImBuf[(INT32U)NAXIS2*NAXIS1*j+NAXIS1*i];
      // Set width (00), Burst Size (32 bytes), length, and flow control   
      QCI_DMA_DesArr[4*(NAXIS2*j+i)+3] = 0x60030000 | NAXIS1;
      // Set Stop bit for last DDADR so transfer stops at NAXIS2           
      if (i==(NAXIS2-1)){ 
        QCI_DMA_DesArr[4*(NAXIS2*j+i)+0] = 0x00000001;                          
      }
    }
  }
  
  //Load the other DMA Registers, mapping channel 0 to QCI
  DMA_QCI_DRCMR_68_REG_ADDR         = 0x00000080;                  /* Map the Quick Capture Interface to Channel 0                      */
  DMA_DSCR_0_REG_ADDR               = 0x00000000;
  DMA_DDADR_0_REG_ADDR              = (INT32U)QCI_DMA_DesArr;      /* Set up the descriptor chain for the Quick Capture Interface       */
   
  //QUICK CAPTURE INTERFACE ===================== Configure Quick Capture Interface
  GPIO_ALT_DEF_16_31_REG_ADDR      &= 0xF3FFFCFF;                  /* Zero alt def for GPIO20 SS_START, GPIO29 X_LOAD                   */
  GPIO_ALT_DEF_48_63_REG_ADDR      &= 0xFFFFF3CF;                  /* Zero alt def for GPIO50 SYS_RESET, GPIO53 CIF_MCLK                */
  GPIO_ALT_DEF_48_63_REG_ADDR      |= 0x00000800;                  /* Function 2 for GPIO53 CIF_MCLK, 0 for GPIO50                      */  
  GPIO_ALT_DEF_80_95_REG_ADDR      &= 0xFFFFF0FC;                  /* Zero alt def for GPIO80 SS_STOP, 84 CIF_FV, 85 CIF_LV             */
  GPIO_ALT_DEF_80_95_REG_ADDR      |= 0x00000F00;                  /* Function 3-84 CIF_FV, 3-85 CIF_LV                                 */   
  GPIO_PIN_DIRECTION0_REG_ADDR     |= 0x00100000;                  /* Set GPIO 20 as output for SS_START, 29 for X_LOAD                 */
  GPIO_PIN_DIRECTION1_REG_ADDR     |= 0x00240000;                  /* Set GPIO 50 as output for SYS_RESET, 53 as for CIF_MCLK           */
  GPIO_PIN_DIRECTION2_REG_ADDR     |= 0x00310000;                  /* Set GPIO 80,84,85 as output for SS_STOP, CIF_FV, CIF_LV           */
  GPIO_PIN_OUTPUT_CLR0_REG_ADDR    |= 0x00100000;                  /* Clear GPIO 20, since SS_START needs to rise for an exposure start */
  GPIO_PIN_OUTPUT_CLR1_REG_ADDR    |= 0x00040000;                  /* Clear CPIO 50, since SS_STOP needs to rise for an exposure stop   */       
  QCI_CICR0_REG_ADDR               &= 0xEFFFFFFF;                  /* Disable QCI interface before programming values                   */
  QCI_FIFO_CONTROL_REG_ADDR         = 0x00000000;                  /* Enable QCI FIFO 0, and set the value at 96 bytes for a DMA req.   */ 
  QCI_CICR1_REG_ADDR                = ((NAXIS1-1) << 15);          /* Set Pix Per Line as NAXIS1 - 1 (PPL+1=Real#)                      */
  QCI_CICR1_REG_ADDR               |= 0x00000002;                  /* Set Raw pix, 8 bits wide                                          */
  QCI_CICR2_REG_ADDR                = 0x03030000;                  /* BLW = 3, ELW = 3 (Other waits need TESTING!)                      */
  QCI_CICR3_REG_ADDR                = (NAXIS2-1);                  /* Set Lines Per Frame = 1023 (LPF+1 = Real#) Other waits needed     */
  QCI_CICR4_REG_ADDR                = 0x00080008;                  /* Set MCLK from QCI and Divisor = 8 (100 nS/pixel)                  */
  QCI_CICR0_REG_ADDR                = 0xA10003FD;                  /* Set up Slave parallel mode, DMA enable, Enable QCI                */  
#endif
 
#ifdef PRINTINITIALIZATION    
  
  //Print out the status of some registers   
  printf("GPIO Registers -------------------------------------------------------------------\n\r");
  printf("GPIO_PIN_DIRECTION0_REG_ADDR   (0-31)    %08X\n\r",  GPIO_PIN_DIRECTION0_REG_ADDR);
  printf("GPIO_PIN_DIRECTION1_REG_ADDR   (32-63)   %08X\n\r",  GPIO_PIN_DIRECTION1_REG_ADDR);
  printf("GPIO_PIN_DIRECTION2_REG_ADDR   (64-95)   %08X\n\r",  GPIO_PIN_DIRECTION2_REG_ADDR);
  printf("GPIO_PIN_DIRECTION3_REG_ADDR   (96-120)  %08X\n\r",  GPIO_PIN_DIRECTION3_REG_ADDR);
  printf("GPIO_PIN_OUTPUT_SET0_REG_ADDR  (0-31)    %08X\n\r",  GPIO_PIN_OUTPUT_SET0_REG_ADDR);
  printf("GPIO_PIN_OUTPUT_SET1_REG_ADDR  (32-63)   %08X\n\r",  GPIO_PIN_OUTPUT_SET1_REG_ADDR);
  printf("GPIO_PIN_OUTPUT_SET2_REG_ADDR  (64-95)   %08X\n\r",  GPIO_PIN_OUTPUT_SET2_REG_ADDR);
  printf("GPIO_PIN_OUTPUT_SET3_REG_ADDR  (96-120)  %08X\n\r",  GPIO_PIN_OUTPUT_SET3_REG_ADDR);  
  printf("GPIO_PIN_OUTPUT_CLR0_REG_ADDR  (0-31)    %08X\n\r",  GPIO_PIN_OUTPUT_CLR0_REG_ADDR);
  printf("GPIO_PIN_OUTPUT_CLR1_REG_ADDR  (32-63)   %08X\n\r",  GPIO_PIN_OUTPUT_CLR1_REG_ADDR);
  printf("GPIO_PIN_OUTPUT_CLR2_REG_ADDR  (64-95)   %08X\n\r",  GPIO_PIN_OUTPUT_CLR2_REG_ADDR);
  printf("GPIO_PIN_OUTPUT_CLR3_REG_ADDR  (96-120)  %08X\n\r",  GPIO_PIN_OUTPUT_CLR3_REG_ADDR);  
  printf("GPIO_ALT_DEF_16_31_REG_ADDR              %08X\n\r",  GPIO_ALT_DEF_16_31_REG_ADDR);
  printf("GPIO_ALT_DEF_32_47_REG_ADDR              %08X\n\r",  GPIO_ALT_DEF_32_47_REG_ADDR);
  printf("GPIO_ALT_DEF_48_63_REG_ADDR              %08X\n\r",  GPIO_ALT_DEF_48_63_REG_ADDR);
  printf("GPIO_ALT_DEF_64_79_REG_ADDR              %08X\n\r",  GPIO_ALT_DEF_64_79_REG_ADDR);
  printf("GPIO_ALT_DEF_80_95_REG_ADDR              %08X\n\r",  GPIO_ALT_DEF_80_95_REG_ADDR);
  printf("GPIO_ALT_DEF_96_111_REG_ADDR             %08X\n\r",  GPIO_ALT_DEF_96_111_REG_ADDR);
  printf("GPIO_ALT_DEF_112_120_REG_ADDR            %08X\n\r",  GPIO_ALT_DEF_112_120_REG_ADDR);    
  
  //Interrupt Registers
  printf("\n\r");
  printf("Interrupt Registers --------------------------------------------------------------\n\r");
  printf("Interrupt Mask Register:                 %08X\n\r",  INT_MASK_ADDR);    
  printf("Interrupt Level Register:                %08X\n\r",  INT_LEVEL_REG_ADDR);   

  //Print out all the FFUARTA serial port registers to see what their initial values are set at
  printf("\n\r");
  printf("Serial Port Registers FFUARTA ----------------------------------------------------\n\r");
  FFUARTA_LCR_REG_ADDR              |= 0x80;        /* Set DLAB bit to change baud rate         */
  a = FFUARTA_DIVISOR_LBYTE_REG_ADDR;
  b = FFUARTA_DIVISOR_UBYTE_REG_ADDR;
  FFUARTA_LCR_REG_ADDR              &= 0x7F;        /* Unset DLAB bit so we can use TX and RX   */
  printf("FFUARTA_DIVISOR_LBYTE_REG_ADDR           %08X\n\r",  a);
  printf("FFUARTA_DIVISOR_HBYTE_REG_ADDR           %08X\n\r",  b);
  printf("FFUARTA_INT_CONTROL_REG_ADDR             %08X\n\r",  FFUARTA_INT_CONTROL_REG_ADDR);
  printf("FFUARTA_INT_ID_REG_ADDR                  %08X\n\r",  FFUARTA_INT_ID_REG_ADDR);
  printf("FFUARTA_FCR_REG_ADDR                     %08X\n\r",  FFUARTA_FCR_REG_ADDR);
  printf("FFUARTA_LCR_REG_ADDR                     %08X\n\r",  FFUARTA_LCR_REG_ADDR );
  printf("FFUARTA_MCR_REG_ADDR                     %08X\n\r",  FFUARTA_MCR_REG_ADDR);
  printf("FFUARTA_LINE_STATUS_REG_ADDR             %08X\n\r",  FFUARTA_LINE_STATUS_REG_ADDR);
  printf("FFUARTA_MSR_REG_ADDR                     %08X\n\r",  FFUARTA_MSR_REG_ADDR);
  printf("FFUARTA_MSR_REG_ADDR                     %08X\n\r",  FFUARTA_MSR_REG_ADDR);
  printf("FFUARTA_MSR_REG_ADDR                     %08X\n\r",  FFUARTA_MSR_REG_ADDR);
  printf("FFUARTA_SCRATCH_PAD_REG_ADDR             %08X\n\r",  FFUARTA_SCRATCH_PAD_REG_ADDR);
  printf("FFUARTA_INFRARED_SELELCT_REG_ADDR        %08X\n\r",  FFUARTA_INFRARED_SELELCT_REG_ADDR);
  printf("FFUARTA_RX_FIFO_OCCUPANCY_REG_ADDR       %08X\n\r",  FFUARTA_RX_FIFO_OCCUPANCY_REG_ADDR);
  printf("FFUARTA_ABR_REG_ADDR                     %08X\n\r",  FFUARTA_ABR_REG_ADDR);
  printf("FFUARTA_ABC_REG_ADDR                     %08X\n\r",  FFUARTA_ABC_REG_ADDR);

  //PORT ASSIGNED TO GPS (UARTB or SSP1)
  #ifdef USE_UARTB
  //Print out all the UARTB serial port registers to see what their initial values are set at
  printf("\n\r");
  printf("Serial Port Registers UARTB ----------------------------------------------------\n\r");
  UARTB_LCR_REG_ADDR              |= 0x80;         /* Set DLAB bit to change baud rate         */
  a = UARTB_DIVISOR_LBYTE_REG_ADDR;
  b = UARTB_DIVISOR_UBYTE_REG_ADDR;
  UARTB_LCR_REG_ADDR              &= 0x7F;         /* Unset DLAB bit so we can use TX and RX   */
  printf("UARTB_DIVISOR_LBYTE_REG_ADDR             %08X\n\r",  a);
  printf("UARTB_DIVISOR_HBYTE_REG_ADDR             %08X\n\r",  b);
  printf("UARTB_INT_CONTROL_REG_ADDR               %08X\n\r",  UARTB_INT_CONTROL_REG_ADDR);
  printf("UARTB_INT_ID_REG_ADDR                    %08X\n\r",  UARTB_INT_ID_REG_ADDR);
  printf("UARTB_FCR_REG_ADDR                       %08X\n\r",  UARTB_FCR_REG_ADDR);
  printf("UARTB_LCR_REG_ADDR                       %08X\n\r",  UARTB_LCR_REG_ADDR );
  printf("UARTB_MCR_REG_ADDR                       %08X\n\r",  UARTB_MCR_REG_ADDR);
  printf("UARTB_LINE_STATUS_REG_ADDR               %08X\n\r",  UARTB_LINE_STATUS_REG_ADDR);
  printf("UARTB_MSR_REG_ADDR                       %08X\n\r",  UARTB_MSR_REG_ADDR);
  printf("UARTB_MSR_REG_ADDR                       %08X\n\r",  UARTB_MSR_REG_ADDR);
  printf("UARTB_MSR_REG_ADDR                       %08X\n\r",  UARTB_MSR_REG_ADDR);
  printf("UARTB_SCRATCH_PAD_REG_ADDR               %08X\n\r",  UARTB_SCRATCH_PAD_REG_ADDR);
  printf("UARTB_INFRARED_SELELCT_REG_ADDR          %08X\n\r",  UARTB_INFRARED_SELELCT_REG_ADDR);
  printf("UARTB_RX_FIFO_OCCUPANCY_REG_ADDR         %08X\n\r",  UARTB_RX_FIFO_OCCUPANCY_REG_ADDR);
  printf("UARTB_ABR_REG_ADDR                       %08X\n\r",  UARTB_ABR_REG_ADDR);
  printf("UARTB_ABC_REG_ADDR                       %08X\n\r",  UARTB_ABC_REG_ADDR);
  #else
  //Print out SSP1 Registers on PXA270 and SC16740 Internal Registers (TX Level should be 0x40)  
  printf("\n\r");
  printf("Syncrhonous Serial Port 1 Registers (Expected value) -----------------------\n\r");  
  printf("SSP1 Control Reg 0                       %08X\n\r",  SPI_SSCR0_1_REG_ADDR);
  printf("SSP1 Control Reg 1                       %08X\n\r",  SPI_SSCR1_1_REG_ADDR);  
  printf("SSP1 Status Reg                          %08X\n\r",  SPI_SSSR_1_REG_ADDR);

  RegResult = WriteSC16Register(SC16_FCR_ADDR, 0x07);              /* Enable the TX and RX FIFOs on the SPI to UART chip and clear them */
  RegResult = ReadSC16Register(SC16_IER_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 IER Address (0x02)        %08X\n\r", This8RegValue);   
  RegResult = ReadSC16Register(SC16_IIR_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 IIR Address (0xC1)        %08X\n\r", This8RegValue);   
  RegResult = ReadSC16Register(SC16_LCR_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 LCR Address (0x03)        %08X\n\r", This8RegValue);
  RegResult = ReadSC16Register(SC16_MCR_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 MCR (0x03)                %08X\n\r", This8RegValue);
  RegResult = ReadSC16Register(SC16_LSR_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 LSR (0x60)                %08X\n\r", This8RegValue);
  RegResult = ReadSC16Register(SC16_MSR_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 MSR (0x00)                %08X\n\r", This8RegValue);  
  RegResult = ReadSC16Register(SC16_TXLVL_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 TXLVL (0x40)              %08X\n\r", This8RegValue);
  RegResult = ReadSC16Register(SC16_RXLVL_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 RXLVL (0x00)              %08X\n\r", This8RegValue);  
  RegResult = ReadSC16Register(SC16_IO_STATE_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 IO_STATE (0xFF)           %08X\n\r", This8RegValue);
  RegResult = ReadSC16Register(SC16_IO_INT_EN_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 IO_INT_EN (0x00)          %08X\n\r", This8RegValue);
  RegResult = ReadSC16Register(SC16_IO_CTRL_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 IO_CTRL (0x00)            %08X\n\r", This8RegValue);
  RegResult = ReadSC16Register(SC16_EFCR_ADDR, &This8RegValue);
  printf("SSP1-SC16IS740 EFCR (0x00)               %08X\n\r", This8RegValue);
  #endif   
  
  //DMA Registers
  printf("\n\r");
  printf("DMA Registers ------------------------------------------------------------------\n\r");  
  printf("Image Buffer Address :                   %08X\n\r", QCI_ImBuf);
  printf("DMA DDADR Register :                     %08X\n\r", DMA_DDADR_0_REG_ADDR);
  printf("DMA Source Address :                     %08X\n\r", DMA_DSADR_0_REG_ADDR);
  printf("DMA Target Address :                     %08X\n\r", DMA_DTADR_0_REG_ADDR);
  printf("DMA DCMD  Register :                     %08X\n\r", DMA_DCMD_0_REG_ADDR);

  #ifdef PRINT_QCI_DMA_DESCRIPTOR_CHAIN
  for (i=0;i<(NAXIS2+1);i++){
    printf("Descriptor Arr %d : %08X\t%08X\t%08X\t%08X\n\r", i, QCI_DMA_DesArr[4*i+0], QCI_DMA_DesArr[4*i+1], QCI_DMA_DesArr[4*i+2], QCI_DMA_DesArr[4*i+3]);    
  }
  #endif
  //Timer Registers
  printf("\n\r");
  printf("Timer Registers ------------------------------------------------------------------\n\r");
  printf("OMCR4 control register bits:             %08X\n\r",  OS_TIMER_OMCR4_REG_ADDR);
  printf("OMCR9 control register bits:             %08X\n\r",  OS_TIMER_OMCR9_REG_ADDR);
  printf("Oscillator config register bits:         %08X\n\r",  OS_OSCILLATOR_CONFIG_REG_ADDR);
  printf("Clock Enable register bits:              %08X\n\r",  CLOCK_ENABLE_REG_ADDR);  
#endif

  //IMPORTANT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  //To minimize power consumption we should set all unused GPIOs to be outputs
  //Also we should disable clocks that are not going to be used
  
}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*                                           IRQ ISR HANDLER                                                                            */
/*                                                                                                                                      */
/* FUNCTION: OS_CPU_IRQ_ISR_Handler                                                                                                     */
/*                                                                                                                                      */
/* PURPOSE : This function is called by OS_CPU_IRQ_ISR() to determine the source of the interrupt                                       */
/*               and process it accordingly.                                                                                            */
/*                                                                                                                                      */
/* INPUTS  : none                                                                                                                       */
/*                                                                                                                                      */
/* RETURNS : none                                                                                                                       */
/*                                                                                                                                      */
/* NOTES:  1) If you spend too long in this ISR relative to the period at which it's issued (for instance, if                           */
/*         OS Timer 4 interrupts every 100 ms and it takes 105 ms to do printf statements) and interrupt                                */
/*         nesting is disabled, the interrupts will nest themselves and you'll be stuck with an overflowed stack.                       */
/*         You can see this because the value of OSIntNesting increments until there's no more room in stack land !!!                   */
/*                                                                                                                                      */
/*         2) TO DISABLE INTERRUPT NESTING:  Go to os_cpu_a.s and change the following line:                                            */
/*               MSR   CPSR_c, #0x1F             ;Switch to SYS mode with interrupts enabled                                            */
/*         to                                                                                                                           */
/*               MSR   CPSR_c, #0xDF             ;Switch to SYS mode with interrupts disabled                                           */
/*                                                                                                                                      */
/*                                                                                                                                      */  
/*         3) THE QCI INTERRUPT IS DISABLED RIGHT NOW.  TO ENABLE IT SET                                                                */
/*               INT_MASK2_ADDR = 0x00000002;                                                                                           */                            
/*            POLLING THE QCI FOR THE END OF FRAME BIT IS HOW WE ARE PROCEEDING NOW                                                     */
/*                                                                                                                                      */  
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/

void  OS_CPU_IRQ_ISR_Handler (void)
{

  //Need one incrementer
  INT8U  BytesInRXFIFO  = 100;
  INT8U  DummyRead      = 0;
  INT8U  i              = 0;
  INT8U  EndFound       = 0;
  INT32U TimeOutCounter = 0;
  
  //HANDLE THE INTERRUPTS ONE BY ONE------------------------------------------------------------------------------------------------------
 
  //###################################################################################################################################### 
  //1) IRQ FOR COUNTERS 4-11 
  if (INT_IRQ_CONTROLLER_ADDR & BIT7){             
     OS_TIMER_OSCR4_REG_ADDR  = 0x00000000L;                  //RESET COUNTER 4
     OSTimeTick();                                            //Call OSTimeTick;
     OS_TIMER_STATUS_REG_ADDR &= OS_TIMER_BIT_CLEAR;          //Clear the Interrupt
     return;
  }

  //###################################################################################################################################### 
  //2) IRQ FOR GPIO58 - Interrupt for SPI to UART Board connected to GPS -- STORE CHARACTER IN GPS BUFFER
  if (INT_IRQ_CONTROLLER_ADDR & BIT10){

    //First mask the interrupt to prevent nesting of these interrupts
    INT_MASK_ADDR &= 0xFFFFBFF;     

    //Check to see if it is GPIO58
    if ((GPIO_EDGE_DETECT_STATUS2_REG_ADDR | 0x04000000) != 0){
           
      //Clear this bit to remove the interrupt (must write a 1 to the bit)
      GPIO_EDGE_DETECT_STATUS2_REG_ADDR &= 0x04000000;

      //Now loop through characters as they come in out how many characters are in buffer
      while ((EndFound == 0)){

        //Increment the TimeOutCounter
        TimeOutCounter++;

        if (TimeOutCounter > 50000){
          printf("TimeOut in GPS Wait\n\r");
          goto TimeOutGPSWait;
        }
        
        //WRITE- Figure out how many bytes in FIFO by reading RXLVL (FIFO Occupancy Register) Don't use function because we're in IRQ
        while (((SPI_SSSR_1_REG_ADDR & 0x04) == 0) || ((SPI_SSSR_1_REG_ADDR & 0x10) != 0)){};
        SPI_SSDR_1_REG_ADDR = 0xC8;           //128 [R/W=1] + (0x09) << 3
        while (((SPI_SSSR_1_REG_ADDR & 0x04) == 0) || ((SPI_SSSR_1_REG_ADDR & 0x10) != 0)){};
        SPI_SSDR_1_REG_ADDR = 0xFF;           //Dummy Value for Register Contents

        //READ - Wait until the status bit is set and read the value of chars in the FIFO 
        while (((SPI_SSSR_1_REG_ADDR & 0x08) == 0) || ((SPI_SSSR_1_REG_ADDR & 0x10) != 0)){};
        DummyRead     = SPI_SSDR_1_REG_ADDR;  // Dummy value for first word
        BytesInRXFIFO = SPI_SSDR_1_REG_ADDR;  // BytesInRXFIFO contains the amount of bytes in the FIFO

        for (i=0;i<BytesInRXFIFO;i++){
          
          //Bytes were found, so reset TimeOutCounter
          TimeOutCounter=0;
          
          //Write the precharacter out once there is space in the TX FIFO
          while (((SPI_SSSR_1_REG_ADDR & 0x04) == 0) || ((SPI_SSSR_1_REG_ADDR & 0x10) != 0)){};
          SPI_SSDR_1_REG_ADDR = 0x80;
          SPI_SSDR_1_REG_ADDR = 0xFF;

          //Wait until the status bit is set and read the value
          while (((SPI_SSSR_1_REG_ADDR & 0x08) == 0) || ((SPI_SSSR_1_REG_ADDR & 0x10) != 0)){};
          DummyRead        = SPI_SSDR_1_REG_ADDR;
          *GPSLogBufWrtPtr = SPI_SSDR_1_REG_ADDR;

          if (*GPSLogBufWrtPtr == ASCII_CHAR_LINE_FEED) EndFound = 1;
          GPSLogBufWrtPtr++;
          GPSLogBytesUnread++;
          if (GPSLogBufWrtPtr == GPSLogBufEndPtr){
            GPSLogBufWrtPtr = GPSLogBuf; 
          }         
        }
      }
    }
    
    //Jump for case of timeout
    TimeOutGPSWait:
    
    //Other possible GPIO PINS
    //------------------------
    //Unmask the interrupt to get data in the future and clear this bit to remove the interrupt (must write a 1 to the bit)
    GPIO_EDGE_DETECT_STATUS2_REG_ADDR &= 0x04000000;      
    INT_MASK_ADDR |= 0x00000400;
    return;
  }  

  //###################################################################################################################################### 
  //3) IRQ FOR UARTB Serial Port connected to GPS -- STORE CHARACTER IN GPS BUFFER
  if (INT_IRQ_CONTROLLER_ADDR & BIT21){
    *GPSLogBufWrtPtr = UARTB_RXD_REG_ADDR;
    GPSLogBufWrtPtr++;
    GPSLogBytesUnread++;
    if (GPSLogBufWrtPtr == GPSLogBufEndPtr){
      GPSLogBufWrtPtr = GPSLogBuf; 
    }
    return;
  }
  
  //###################################################################################################################################### 
  //4) IRQ FOR FFUARTA Serial Port -- STORE CHARACTER IN COMMAND BUFFER!!
  if (INT_IRQ_CONTROLLER_ADDR & BIT22){
    if (CmndBufLLWrtPtr != CmndBufLLOverWrtPtr){
      *CmndBufLLWrtPtr = FFUARTA_RXD_REG_ADDR;                  //Read character into the command buffer                          
      CmndBufLLWrtPtr++;                                        //Increment the write ptr to point to the next spot in the buffer
      CmndBufLLBytesUnread++;                                   //Increment the number of bytes that have not been read
      if (CmndBufLLWrtPtr == CmndBufLLEndPtr){                  //If the buffer's full, roll back to the beginning for FIFO behavior 
        CmndBufLLWrtPtr = CmndBufLL;                           
      }
    } else {
      printf("CmndBufLLOverWrtPtr %X\n\r",  CmndBufLLOverWrtPtr);
      printf("CmndBufLLWrtPtr     %X\n\r",  CmndBufLLWrtPtr);
    }
    return;
  }
  //###################################################################################################################################### 
  //5) END OF FRAME FOR QUICK CAPTURE INTERFACE!!
  if (INT_IRQ2_CONTROLLER_ADDR & BIT1){
    //printf("Interrupt from QCI\n\r");
    if ((QCI_STATUS_REG_ADDR & 0x0008) != 0){ 
      QCI_CICR0_REG_ADDR  |= 0x08000000;
      QCI_STATUS_REG_ADDR |= 0x00000008;
    }
  }
  return;
}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*                                           FIQ ISR HANDLER                                                                            */
/*                                                                                                                                      */
/* FUNCTION    : OS_CPU_FIQ_ISR_Handler                                                                                                 */
/*                                                                                                                                      */
/* PURPOSE     : This function is called by OS_CPU_FIQ_ISR() to determine the source of the interrupt                                   */
/*               and process it accordingly.                                                                                            */
/*                                                                                                                                      */
/* INPUTS      : none                                                                                                                   */
/*                                                                                                                                      */
/* RETURNS     : none                                                                                                                   */
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/

void  OS_CPU_FIQ_ISR_Handler (void)
{
    printf("Interrupting this ass\n\r"); 
    REG32(OS_TIMER_STATUS_REG_ADDR) &= (0x00000000L) ;
    OS_TIMER_STATUS_REG_ADDR         = 0x00000000L;

}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*  FUNCTION: putchar                                                                                                                   */
/*    This is the definition for the function that will be used by                                                                      */ 
/*    printf to output text to the serial port program in use                                                                           */
/*                                                                                                                                      */
/*  INPUTS: INT32S                                                                                                                      */
/*    c : The character to be printed.  For some reason it is INT32...if I try to compile with INT8, I                                  */ 
/*        get the following error:                                                                                                      */
/*        "declaration is incompatible with "__interwork__Softfp int putchar(int)"                                                      */
/*                                                                                                                                      */ 
/*  RETURNS: INT32S                                                                                                                     */
/*    c : The same character that was read in.                                                                                          */
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/

INT32S putchar(INT32S c)
{
  //Make counter to prevent overflow of buffer
  volatile INT32U counter; 
  
  //Poll the transmit data reguest and transmit hold empty bit in the line status register to ensure FIFO can hold more characters
  while (((FFUARTA_LINE_STATUS_REG_ADDR & 0x20) == 0) || ((FFUARTA_LINE_STATUS_REG_ADDR & 0x40) == 0) || ((FFUARTA_MCR_REG_ADDR & 0x10) != 0)){}

  //With space, we can put the character there
  FFUARTA_TXD_REG_ADDR = c;

  //Poll the transmit data reguest and transmit hold empty bit in the line status register to ensure FIFO can hold more characters
  while (((FFUARTA_LINE_STATUS_REG_ADDR & 0x20) == 0) || ((FFUARTA_LINE_STATUS_REG_ADDR & 0x40) == 0) || ((FFUARTA_MCR_REG_ADDR & 0x10) != 0)){}
  
  return c; 

}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*                                                                                                                                      */
/* NAME: ReadSC16Register                                                                                                               */
/*                                                                                                                                      */
/* PURPOSE:                                                                                                                             */
/*   This function will read an internal register on the SC16IS740 SPI to UART chip and return the value in the variable RegValue       */
/*                                                                                                                                      */
/* INPUTS: NONE                                                                                                                         */
/*   RegAddr: INT8U                                                                                                                     */
/*     The Address of the register on the SC16IS740 to read from (16 in all)                                                            */
/*   RegValue: INT8U*                                                                                                                   */
/*     A pointer to the variable that will containt the result of the read                                                              */
/*                                                                                                                                      */
/* RETURNS: NONE                                                                                                                        */
/*   Success: 0                                                                                                                         */
/*   Error: -1 (if the address range is outside 0-15)                                                                                   */
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
INT32S ReadSC16Register(INT8U RegAddr, INT8U* RegValue){

  //Declare variables
  INT8U PreChar = 0;
  INT8U Dummy   = 0;

  //Return immediately if the address is too large
  if (RegAddr > 15) return -1;

  //Now bitshift the address by 1 (Bit for pre-character are [7]=R/W=1, [6-3]=Addr[0:3], [2]=Ch0=0, [1]=Ch1=0, [0]=0)
  PreChar = 128+(RegAddr << 3);

  //Write the precharacter out once there is space in the TX FIFO
  while (((SPI_SSSR_1_REG_ADDR & 0x04) == 0) || ((SPI_SSSR_1_REG_ADDR & 0x10) != 0)){};
  SPI_SSDR_1_REG_ADDR = PreChar;
  while (((SPI_SSSR_1_REG_ADDR & 0x04) == 0) || ((SPI_SSSR_1_REG_ADDR & 0x10) != 0)){};
  SPI_SSDR_1_REG_ADDR = Dummy;

  //Wait until the status bit is set and read the value
  while (((SPI_SSSR_1_REG_ADDR & 0x08) == 0) || ((SPI_SSSR_1_REG_ADDR & 0x10) != 0)){};
  Dummy      = SPI_SSDR_1_REG_ADDR;
  *RegValue  = SPI_SSDR_1_REG_ADDR & 0xFF;
  
  return 1;

}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*                                                                                                                                      */
/* NAME: WriteSC16Register                                                                                                              */
/*                                                                                                                                      */
/* PURPOSE:                                                                                                                             */
/*   This function will write an internal register on the SC16IS740 SPI to UART chip and return the value in the variable RegValue      */
/*                                                                                                                                      */
/* INPUTS: NONE                                                                                                                         */
/*   RegAddr: INT8U                                                                                                                     */
/*     The Address of the register on the SC16IS740 to read from (16 in all)                                                            */
/*   RegValue: INT8U                                                                                                                    */
/*     The value that will be written to RegAddr                                                                                        */
/*                                                                                                                                      */
/* RETURNS: NONE                                                                                                                        */
/*   Success: 0                                                                                                                         */
/*   Error: -1 (if the address range is outside 0-15)                                                                                   */
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
INT32S WriteSC16Register(INT8U RegAddr, INT8U RegValue){

  //Declare variables
  INT8U PreChar      = 0;
  INT8U Dummy        = 0;

  //Return immediately if the address is too large
  if (RegAddr > 15) return -1;

  //Now bitshift the address by 1 (Bit for pre-character are [7]=R/W=0, [6-3]=Addr[0:3], [2]=Ch0=0, [1]=Ch1=0, [0]=0)
  PreChar = RegAddr << 3;

  //Write the precharacter if there is space in the TX FIFO
  while (((SPI_SSSR_1_REG_ADDR & 0x04) == 0) || ((SPI_SSSR_1_REG_ADDR & 0x10) != 0)){};
  SPI_SSDR_1_REG_ADDR = PreChar;
  SPI_SSDR_1_REG_ADDR = RegValue;

  //Wait until the status bit is set and read the value
  while (((SPI_SSSR_1_REG_ADDR & 0x08) == 0) || ((SPI_SSSR_1_REG_ADDR & 0x10) != 0)){};
  Dummy = SPI_SSDR_1_REG_ADDR;
  while (((SPI_SSSR_1_REG_ADDR & 0x08) == 0) || ((SPI_SSSR_1_REG_ADDR & 0x10) != 0)){};
  Dummy = SPI_SSDR_1_REG_ADDR;

  return 1;
  
}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*  FUNCTION: WriteIBIS5Register()                                                                                                      */
/*                                                                                                                                      */
/*  PURPOSE: To write one of the 16 internal registers on the Cypress IBIS5-B-1300 CMOS sensor.  Writes are done using the 3 wire       */
/*           serial interface (IF_MODE is GND and SER_MODE is VDD on the IBIS5_Header.pdf schematic).  Reads are not possible.          */
/*           The writes are done with 16 bit words.                                                                                     */
/* INPUTS: NONE                                                                                                                         */
/*   RegAddr: INT8U                                                                                                                     */
/*     The Address of the register on the SC16IS740 to read from (16 in all)                                                            */
/*   RegValue: INT8U                                                                                                                    */
/*     The value that will be written to RegAddr                                                                                        */
/*                                                                                                                                      */ 
/*  RETURNS: NONE                                                                                                                       */
/*                                                                                                                                      */
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
INT32S WriteIBIS5Register(INT8U RegAddr, INT16U RegValue){

  //Declare variables
  INT16U PreChar      = 0;
  INT16U Dummy        = 0;

  //Return immediately if the address is too large
  if (RegAddr > 15) return -1;

  //Now bitshift the address by 1 (Bit for pre-character are [7]=R/W=0, [6-3]=Addr[0:3], [2]=Ch0=0, [1]=Ch1=0, [0]=0)
  PreChar = RegAddr << 12;

  //Write the precharacter if there is space in the TX FIFO
  while (((SPI_SSSR_2_REG_ADDR & 0x04) == 0) || ((SPI_SSSR_2_REG_ADDR & 0x10) != 0)){};
  SPI_SSDR_2_REG_ADDR = RegValue;

  //Wait until the status bit is set and read the value
  while (((SPI_SSSR_2_REG_ADDR & 0x08) == 0) || ((SPI_SSSR_2_REG_ADDR & 0x10) != 0)){};
  Dummy = SPI_SSDR_2_REG_ADDR;

  return 1;

}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*  FUNCTION: WriteI2C()                                                                                                                */
/*                                                                                                                                      */
/*  PURPOSE: To write a data byte to the MAX1239EEE via the I2C interface.                                                              */
/*           See page 579 of PXA Manual for reference.                                                                                  */
/*                                                                                                                                      */
/*  INPUTS: NONE                                                                                                                        */
/*   SlaveAddr: INT8U                                                                                                                   */
/*     The Address of the I2C device.  For now, it should always be 0x35                                                                */
/*   Value: INT8U                                                                                                                       */
/*     The value that will be written to the I2C Device                                                                                 */
/*   WriteType: INT8U                                                                                                                   */ 
/*     The type of write to be issued. Possible values are                                                                              */
/*       0 : Config                                                                                                                     */
/*       1 : Setup                                                                                                                      */
/*  RETURNS: NONE                                                                                                                       */
/*                                                                                                                                      */
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
INT32S WriteI2C(INT8U SlaveAddr, INT8U Value){

  INT8U AddrWriteVal;
  INT32U i;
  
  AddrWriteVal = (SlaveAddr<<1)+0;  //The LSB is a 0 for the write

  //Load the slave address and R/nW bit
  I2C_IDBR_REG_ADDR = AddrWriteVal;

  //Set ICR[START], clear ICR[STOP], clear ICR[ALDIE], set ICR[TB]
  I2C_ICR_REG_ADDR |= 0x00000001;  //Set START
  I2C_ICR_REG_ADDR &= 0xFFFFFFFD;  //Clear STOP
  I2C_ICR_REG_ADDR &= 0xFFFFEFFF;  //Clear Arbitration Loss Detected Interrupt Enable (ALDIE)
  I2C_ICR_REG_ADDR |= 0x00000008;  //Set Transmit Byte (TB)
  
  //Wait until IDBR Transmit Empty is set
  //while (((I2C_ISR_REG_ADDR & 0x00000040) == 0) || ((I2C_ICR_REG_ADDR & 0x00000008) != 0)) {}
    
  //Write the ISR[ITE] bit to clear the interrupt
  I2C_ISR_REG_ADDR |= 0x00000040;

  //Load the data byte
  I2C_IDBR_REG_ADDR = Value;
  
  //Initiate the write 
  I2C_ICR_REG_ADDR &= 0xFFFFFFFE;  //Clear START
  I2C_ICR_REG_ADDR |= 0x00000002;  //Set STOP
  I2C_ICR_REG_ADDR |= 0x00001000;  //Set ALDIE
  I2C_ICR_REG_ADDR |= 0x00000008;  //Set Transmit Byte (TB)
   
  //Wait until IDBR Transmit Empty is set
  //while ((I2C_ISR_REG_ADDR & 0x00000040) == 0){}

  //Write the ISR[ITE] bit to clear the interrupt
  I2C_ISR_REG_ADDR |= 0x00000040;

  //Clear ICR[STOP]
  I2C_ICR_REG_ADDR &= 0xFFFFFFFD;  //Clear STOP

  return 1;
  
}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*  FUNCTION: ReadI2C()                                                                                                                 */
/*                                                                                                                                      */
/*  PURPOSE: To read an A/D conversion from the MAX1239EEE via the I2C interface.                                                       */
/*           See page 579 of PXA Manual for reference.                                                                                  */
/*                                                                                                                                      */
/*  INPUTS: NONE                                                                                                                        */
/*   SlaveAddr: INT8U                                                                                                                   */
/*     The Address of the I2C device.  For now, it should always be 0x35                                                                */
/*   Value: INT8U                                                                                                                       */
/*     The value that will be written to the I2C Device                                                                                 */
/*   WriteType: INT8U                                                                                                                   */ 
/*     The type of write to be issued. Possible values are                                                                              */
/*       0 : Config                                                                                                                     */
/*       1 : Setup                                                                                                                      */
/*  RETURNS: NONE                                                                                                                       */
/*                                                                                                                                      */
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*****************************************************************************/
INT32S ReadI2C(INT8U SlaveAddr, INT16U* Result){

  //Variable to hold value that will be written
  INT8U WriteVal;

  //Variables to hold the result
  INT8U ReadValMSB;
  INT8U ReadValLSB;
  INT16U ReadVal;

  WriteVal = (SlaveAddr<<1)+1;  //The LSB is a 1 for the write

  //Load the slave address and R/nW bit
  I2C_IDBR_REG_ADDR = WriteVal;

  //Set ICR[START], clear ICR[STOP], clear ICR[ALDIE], set ICR[TB]
  I2C_ICR_REG_ADDR |= 0x00000001;  //Set START
  I2C_ICR_REG_ADDR &= 0xFFFFFFFD;  //Clear STOP
  I2C_ICR_REG_ADDR &= 0xFFFFEFFF;  //Clear Arbitration Loss Detected Interrupt Enable (ALDIE)
  I2C_ICR_REG_ADDR |= 0x00000008;  //Set Transmit Byte (TB)

  //Wait until IDBR Transmit Empty is set
  while ((I2C_ISR_REG_ADDR & 0x00000040) == 0){}

  //Write the ISR[ITE] bit to clear the interrupt
  I2C_ISR_REG_ADDR |= 0x00000040;

  //Clear ICR[START], clear ICR[STOP], clear ICR[ALDIE], set ICR[TB]
  I2C_ICR_REG_ADDR &= 0xFFFFFFFE;  //Clear START
  I2C_ICR_REG_ADDR &= 0xFFFFFFFE;  //Clear STOP
  I2C_ICR_REG_ADDR |= 0x00000004;  //Set ACKNAK (Negative Acknowledge bit)
  I2C_ICR_REG_ADDR |= 0x00001000;  //Set ALDIE
  I2C_ICR_REG_ADDR |= 0x00000008;  //Set Transmit Byte (TB)

  //Wait for a byte to be placed in the IDBR
  while ((I2C_ISR_REG_ADDR & 0x00000080) == 0){}

  //Write the ISR[IRF] interrupt
  I2C_ISR_REG_ADDR |= 0x00000080;

  //Get the MSB of the value
  ReadValMSB = I2C_IDBR_REG_ADDR;

  //Clear ICR[START], clear ICR[STOP], clear ICR[ALDIE], set ICR[TB]
  I2C_ICR_REG_ADDR &= 0xFFFFFFFE;  //Clear START
  I2C_ICR_REG_ADDR &= 0xFFFFFFFE;  //Clear STOP
  I2C_ICR_REG_ADDR |= 0x00000004;  //Set ACKNAK (Negative Acknowledge bit)
  I2C_ICR_REG_ADDR |= 0x00001000;  //Set ALDIE
  I2C_ICR_REG_ADDR |= 0x00000008;  //Set Transmit Byte (TB)

  //Wait for a byte to be placed in the IDBR
  while ((I2C_ISR_REG_ADDR & 0x00000080) == 0){}

  //Write the ISR[IRF] interrupt
  I2C_ISR_REG_ADDR |= 0x00000080;

  //Get the MSB of the value
  ReadValLSB = I2C_IDBR_REG_ADDR;

  //Set the ICR[MA] Master Abort
  I2C_ICR_REG_ADDR |= 0x00000010;  //Set MA

  //Form the result
  ReadVal = (ReadValMSB << 8) + ReadValLSB;
  
}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*  FUNCTION: uart_pause()                                                                                                              */ 
/*                                                                                                                                      */
/*  PURPOSE: To pause long enough for the UART FIFO to clear when a transmit is issued.                                                 */
/*           Now serves as a general pause function since the counter is built into putchar                                             */
/*                                                                                                                                      */
/*  INPUTS: NONE                                                                                                                        */
/*                                                                                                                                      */ 
/*  RETURNS: NONE                                                                                                                       */
/*                                                                                                                                      */  
/*  NOTE: Each clock cycle takes approximately 21.23 nanoseconds.  Therfore uart_pause takes about 100 ms                               */
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/

void uart_pause(void){

    //Declare a counter for printing to the serial port at a rate it can handle
  volatile INT32U counter;
  for (counter = 5000000; counter; counter--);
  
}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/*  FUNCTION: cf_wait()                                                                                                                 */ 
/*                                                                                                                                      */
/*  PURPOSE: To wait until the compact flash card controller is ready to receive another command or read/write of the data buffer.      */
/*                                                                                                                                      */
/*  INPUTS: NONE                                                                                                                        */
/*                                                                                                                                      */ 
/*  RETURNS: NONE                                                                                                                       */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/

void cf_wait(void){

  //Wait for both the BSY<7>=0 and RDY<6>=1 combination before writing a task register
  while (((CF_CMD_OR_STATUS_REG_ADDR & CF_RDY_BIT) == 0) || ((CF_CMD_OR_STATUS_REG_ADDR & CF_BSY_BIT) != 0 ) ){}
  
#ifdef PRINT_CF_STATUS_AND_ERRORS  
  //print out info if requested
  uart_pause();
  printf("CF_PIN_REPLACEMENT %X\n\r", CF_PINREPLACEMENT_REG_ADDR);
  uart_pause();
  printf("CF_CMD_OR_STATUS_REG_ADDR %X\n\r", CF_CMD_OR_STATUS_REG_ADDR);  
#endif
  
}

/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
/* getTimeStamp                                                                                                                         */
/*                                                                                                                                      */
/* PURPOSE:                                                                                                                             */
/*   To get a timestamp from the seconds and milliseconds clocks we are using                                                           */
/*                                                                                                                                      */
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/

void getTimeStamp()
{
  //Print out the time
  printf("ms tick: %d\n\r", OS_TIMER_OSCR8_REG_ADDR);
  printf("s  tick: %d\n\r", OS_TIMER_OSCR9_REG_ADDR);
  
}
