externC void cyg_drv_interrupt_unmask( cyg_vector_t vector )
{
    CYG_INTERRUPT_STATE old_ints;
    
    CYG_REPORT_FUNCTION();
    CYG_REPORT_FUNCARG1("vector=%d", vector);

    CYG_ASSERT( vector >= CYGNUM_HAL_ISR_MIN, "Invalid vector");    
    CYG_ASSERT( vector <= CYGNUM_HAL_ISR_MAX, "Invalid vector");
    
    HAL_DISABLE_INTERRUPTS(old_ints);
    HAL_INTERRUPT_UNMASK( vector );
    HAL_RESTORE_INTERRUPTS(old_ints);

    CYG_REPORT_RETURN();
}

/*******************************************************************************
  MCF5272_uart_start_xmit() - Enable the transmitter on the device.

  INPUT:
    chan - pointer to the serial private data.

*/
static void MCF5272_uart_start_xmit(serial_channel *chan)
{
    CYG_INTERRUPT_STATE int_state;
    MCF5272_uart_info_t * port = (MCF5272_uart_info_t *) chan->dev_priv;

    /* Enable the UART transmit. */
    MCF5272_UART_WRITE(port->base->ucr, MCF5272_UART_UCR_TXEN);

    /* Enable transmit interrupt */
    HAL_DISABLE_INTERRUPTS(int_state);
    port->imr_mirror |= MCF5272_UART_UIMR_TXRDY;
    MCF5272_UART_WRITE(port->base->uisr_uimr, port->imr_mirror);
    HAL_RESTORE_INTERRUPTS(int_state);


}

//
// This routine is called to send data to the hardware.
static void 
s3esk_eth_send(struct eth_drv_sc *sc, struct eth_drv_sg *sg_list, int sg_len, 
             int total_len, unsigned long key)
{
    struct s3esk_eth_info *qi = (struct s3esk_eth_info *)sc->driver_private;
    volatile char *bp;
    int i;

#ifdef CYGPKG_NET
    cyg_uint32 int_state;
    HAL_DISABLE_INTERRUPTS(int_state);
    // FIXME: closer to Send
#endif

	//can be send max 1500 bytes
	
    // Set up buffer
	qi->txlength = total_len;
	bp = qi->txbuf;
	qi->sended = 0;
	for (i = 0;  i < sg_len;  i++) 
	{
        memcpy((void *)bp, (void *)sg_list[i].buf, sg_list[i].len);
        bp += sg_list[i].len;
    }
	
	cyg_uint32 len = qi->txlength - qi->sended;
	if(len > CYGNUM_DEVS_ETH_POWERPC_S3ESK_BUFSIZE) len = CYGNUM_DEVS_ETH_POWERPC_S3ESK_BUFSIZE;
	
	//XEmacLite_SetMacAddress(&qi->dev, qi->enaddr);
	if (XEmacLite_Send(&qi->dev, qi->txbuf + qi->sended, len) != XST_SUCCESS) {
		deferred = 1;
    }
	else
	{
		qi->sended += len;
		if(qi->sended >= qi->txlength) deferred = 0;
		else deferred = 1;
	}

    // sg_list can be freed! (maybe deferred)
    (sc->funs->eth_drv->tx_done)(sc, key, 0);
#ifdef CYGPKG_NET
    HAL_RESTORE_INTERRUPTS(int_state);
#endif
}

static void
trampoline(unsigned long entry)
{
    typedef void code_fun(void);
    code_fun *fun = (code_fun *)entry;
    unsigned long oldints;

    HAL_DISABLE_INTERRUPTS(oldints);

#ifdef HAL_ARCH_PROGRAM_NEW_STACK
    HAL_ARCH_PROGRAM_NEW_STACK(fun);
#else
    (*fun)();
#endif

    HAL_THREAD_LOAD_CONTEXT(&saved_context);
}

void cyg_hal_report_abort_data(HAL_SavedRegisters *frame)
{
#ifdef CYGPKG_HAL_SMP_SUPPORT
    HAL_SMP_CPU_TYPE cpu;
    cpu  = HAL_SMP_CPU_THIS();
#endif

    int old;
    HAL_DISABLE_INTERRUPTS(old);
#ifdef CYGPKG_HAL_SMP_SUPPORT
    diag_printf("[ABORT DATA] CPU: %d Frame:\n", cpu);
#else
    diag_printf("[ABORT DATA] Frame:\n");
#endif
    dump_frame((unsigned char *)frame);
    HAL_RESTORE_INTERRUPTS(old);
}

void cyg_hal_report_software_interrupt(HAL_SavedRegisters *frame)
{
#ifdef CYGPKG_HAL_SMP_SUPPORT
    HAL_SMP_CPU_TYPE cpu;
    cpu  = HAL_SMP_CPU_THIS();
#endif

    int old;
    HAL_DISABLE_INTERRUPTS(old);
#ifdef CYGPKG_HAL_SMP_SUPPORT
    diag_printf("[SOFTWARE INTERRUPT] CPU: %d Frame:\n", cpu);
#else
    diag_printf("[SOFTWARE INTERRUPT] Frame:\n");
#endif
    dump_frame((unsigned char *)frame);
    HAL_RESTORE_INTERRUPTS(old);
}

// Debug routines
void cyg_hal_report_undefined_instruction(HAL_SavedRegisters *frame)
{
#ifdef CYGPKG_HAL_SMP_SUPPORT
    HAL_SMP_CPU_TYPE cpu;
    cpu  = HAL_SMP_CPU_THIS();
#endif

    int old;
    HAL_DISABLE_INTERRUPTS(old);
#ifdef CYGPKG_HAL_SMP_SUPPORT
    diag_printf("[UNDEFINED INSTRUCTION] CPU: %d Frame:\n", cpu);
#else
    diag_printf("[UNDEFINED INSTRUCTION] Frame:\n");
#endif
    dump_frame((unsigned char *)frame);
    HAL_RESTORE_INTERRUPTS(old);
}

static void post_dsr( cyg_interrupt *intr )
{
    CYG_INTERRUPT_STATE old_intr;

    CYG_REPORT_FUNCTION();

    HAL_DISABLE_INTERRUPTS(old_intr);

    if( intr->dsr_count++ == 0 )
    {
        intr->next_dsr = dsr_list;
        dsr_list = intr;
    }

    HAL_RESTORE_INTERRUPTS(old_intr);

    CYG_REPORT_RETURN();    
}

/**************************************************************************************************
 * @fn          halAssertHazardLights
 *
 * @brief       Blink LEDs to indicate an error.
 *
 * @param       none
 *
 * @return      none
 **************************************************************************************************
 */
void halAssertHazardLights(void)
{

  /* disable all interrupts before anything else */
  HAL_DISABLE_INTERRUPTS();

  /*-------------------------------------------------------------------------------
   *  Master infinite loop.
   */
  for (;;)
  {
      HAL_LED_BLINK_DELAY();

      /* toggle LEDS */
      HAL_TOGGLE_LED1();
      HAL_TOGGLE_LED2();
  }
}

void
show_frame_out(HAL_SavedRegisters *frame)
{
#ifdef CYGPKG_HAL_SMP_SUPPORT
    HAL_SMP_CPU_TYPE cpu;
    cpu  = HAL_SMP_CPU_THIS();
#endif

    int old;
    HAL_DISABLE_INTERRUPTS(old);
#ifdef CYGPKG_HAL_SMP_SUPPORT
    diag_printf("[OUT] CPU: %d IRQ Frame:\n", cpu);
#else
    diag_printf("[OUT] IRQ Frame:\n");
#endif
    dump_frame((unsigned char *)frame);
    HAL_RESTORE_INTERRUPTS(old);
}

static int 
flash_run_command(cyg_uint32 address, 
                  cyg_uint32 command, 
                  cyg_uint32 timeout) 
{
  cyg_uint32 retcode;
  cyg_uint32 fsr;
  cyg_uint32 mask;
  cyg_uint32 page;
  
  page = ((cyg_uint32) address - (cyg_uint32) flash_info.start) / 
    flash_info.block_size;
  
  // Wait for the last command to finish
  retcode = flash_wait_for_controller(timeout);
  if (retcode != FLASH_ERR_OK){
    return retcode;
  }
  
  HAL_DISABLE_INTERRUPTS(mask);
  
  HAL_WRITE_UINT32(AT91_MC+AT91_MC_FCR, 
                   command |
                   ((page & AT91_MC_FCR_PAGE_MASK) << AT91_MC_FCR_PAGE_SHIFT) |
                   AT91_MC_FCR_KEY);

  retcode = flash_wait_for_controller(timeout);

  HAL_RESTORE_INTERRUPTS(mask);

  if (retcode != FLASH_ERR_OK){
    return retcode;
  }

  // Check for an error
  HAL_READ_UINT32(AT91_MC+AT91_MC_FSR, fsr);

  if ((fsr & AT91_MC_FSR_LOCKE) == AT91_MC_FSR_LOCKE)
    return FLASH_ERR_PROTECT;
  if ((fsr & AT91_MC_FSR_PROGE) == AT91_MC_FSR_PROGE)
    return FLASH_ERR_PROGRAM;

  return FLASH_ERR_OK;
}

void hal_diag_write_char(char c)
{
  unsigned long __state;

  HAL_DISABLE_INTERRUPTS(__state);

  if(c == '\n')
    {
#if defined (CYG_KERNEL_DIAG_SERIAL)
      cyg_hal_plf_serial_putc(NULL, '\r');
      cyg_hal_plf_serial_putc(NULL, '\n');
#endif
#if defined(CYG_KERNEL_DIAG_BUFFER)
      hal_diag_buffer[hal_diag_buffer_pos++] = c;
      if (hal_diag_buffer_pos >= sizeof(hal_diag_buffer) )
          hal_diag_buffer_pos = 0;
#endif
#if defined(CYG_KERNEL_DIAG_GDB)
      gdb_diag_write_char(c);
#endif
    }
  else if (c == '\r')
    {
      // Ignore '\r'
    }
  else
    {
#if defined(CYG_KERNEL_DIAG_SERIAL)
      cyg_hal_plf_serial_putc(NULL, c);
#endif
#if defined(CYG_KERNEL_DIAG_BUFFER)
      hal_diag_buffer[hal_diag_buffer_pos++] = c;
      if (hal_diag_buffer_pos >= sizeof(hal_diag_buffer) )
          hal_diag_buffer_pos = 0;
#endif
#if defined(CYG_KERNEL_DIAG_GDB)
      gdb_diag_write_char(c);
#endif
    }

  HAL_RESTORE_INTERRUPTS(__state);
}

// Deliver function (ex-DSR) handles the ethernet [logical] processing
static void
ppc405_eth_deliver(struct eth_drv_sc *sc)
{
#ifdef CYGINT_IO_ETH_INT_SUPPORT_REQUIRED
    struct ppc405_eth_info *qi = (struct ppc405_eth_info *)sc->driver_private;
    cyg_uint32 old_ints;
#endif
    ppc405_eth_int(sc);
#ifdef CYGINT_IO_ETH_INT_SUPPORT_REQUIRED
    // Allow interrupts to happen again
    HAL_DISABLE_INTERRUPTS(old_ints);
    cyg_drv_interrupt_acknowledge(qi->ints);
    cyg_drv_interrupt_unmask(CYGNUM_HAL_INTERRUPT_MAL_SERR);
    cyg_drv_interrupt_unmask(CYGNUM_HAL_INTERRUPT_MAL_TX_EOB);
    cyg_drv_interrupt_unmask(CYGNUM_HAL_INTERRUPT_MAL_RX_EOB);
    cyg_drv_interrupt_unmask(CYGNUM_HAL_INTERRUPT_MAL_TX_DE);
    cyg_drv_interrupt_unmask(CYGNUM_HAL_INTERRUPT_MAL_RX_DE);
    cyg_drv_interrupt_unmask(CYGNUM_HAL_INTERRUPT_EMAC0);
    HAL_RESTORE_INTERRUPTS(old_ints);
#endif
}

/**************************************************************************************************
 * @fn          appForceBoot
 *
 * @brief       Force the boot loader to run.
 *
 * input parameters
 *
 * None.
 *
 * output parameters
 *
 * None.
 *
 * @return      None.
 */
void appForceBoot(void)
{
  uint16 crc[2];
  SBL_NVM_GET(SBL_ADDR_CRC, crc, sizeof(crc));

  // A simple check if the image is built for a boot loader is a valid CRC & shadow where expected.
  if ((crc[0] == 0xFFFF) || (crc[0] == 0x0000) ||
      (crc[1] == 0xFFFF) || (crc[1] == 0x0000) ||
      (crc[1] != crc[0]))
  {
    return;
  }

  HAL_DISABLE_INTERRUPTS();

  crc[0] ^= 0xFFFF;  // Only write to zero bits that are not already zero.
  crc[1] = 0xFFFF;   // No need to write any bits to zero.
  SBL_NVM_SET(SBL_ADDR_CRC, crc, sizeof(crc));

  HAL_SYSTEM_RESET();
}

void intr_main( void )
{
    CYG_INTERRUPT_STATE oldints;

    cyg_drv_interrupt_create(CYGNUM_HAL_INTERRUPT_RTC, 1,
                             ISR_DATA, isr, NULL, &intr_handle, &intr);
    cyg_drv_interrupt_attach(intr_handle);
    HAL_CLOCK_INITIALIZE( CYGNUM_HAL_RTC_PERIOD );
    cyg_drv_interrupt_unmask(CYGNUM_HAL_INTERRUPT_RTC);

    HAL_ENABLE_INTERRUPTS();

    while( ticks < 10 )
    {
        
    }

    HAL_DISABLE_INTERRUPTS(oldints);

    CYG_TEST_PASS_FINISH("HAL interrupt test");
}

/**************************************************************************************************
 * @fn          appForceBoot
 *
 * @brief       Force the boot loader to run.
 *
 * input parameters
 *
 * None.
 *
 * output parameters
 *
 * None.
 *
 * @return      None.
 */
void appForceBoot(void)
{
    uint16 crc[2];

    // Make sure SBL is present.
    HalFlashRead(UBL_PAGE_FIRST, UBL_MD_PG_OFFSET, (uint8 *)crc, 4);

    if ((crc[0] == 0xFFFF) || (crc[0] == 0x0000) ||
            (crc[1] == 0xFFFF) || (crc[1] == 0x0000) ||
            (crc[1] != crc[0]))
    {
        return;
    }

    HAL_DISABLE_INTERRUPTS();

    crc[1] ^= 0xFFFF;  // Only write to zero bits that are not already zero.
    crc[0] = 0xFFFF;
    HalFlashWrite(((uint16)&_ublMetaData) / HAL_FLASH_WORD_SIZE, (uint8 *)crc, 1);

    HAL_SYSTEM_RESET();
}

int main( void )
{
    halInit();
    moduleInit();    
    HAL_DISABLE_INTERRUPTS();
    printf("\r\n****************************************************\r\n");    
    printf("Simple Application Example - END DEVICE\r\n");  
    uint16_t vlo = calibrateVlo();
    printf("VLO = %u Hz\r\n", vlo);   
    timerIsr = &handleTimer;  
    clearLeds();
    HAL_ENABLE_INTERRUPTS();
    
    /* Most of the of infoMessage are the same, so we can create most of the message ahead of time. */
    hdr.sequence = 0;  //this will be incremented each message
    hdr.version = INFO_MESSAGE_VERSION;
    hdr.flags = INFO_MESSAGE_FLAGS_NONE;
    
    initializeSensors();
    delayMs(100);
    stateMachine();
}

// Set the priority in the interrupt control register.
// Disable all interrupts while we access the hardware registers.
static void hal_update_interrupt_controller(int vector)
{
    cyg_uint32 index;
    cyg_uint8 level;
    cyg_uint32 vec_offset;
    cyg_uint32 icr, icr_msk_offset, icr_msk, icr_val, icr_oldval;
    CYG_INTERRUPT_STATE intr_state;
    
    HAL_TRANSLATE_VECTOR(vector, index); 
    level = cyg_hal_IMASK_table[index] ? cyg_hal_ILVL_table[index] : 0;

    vec_offset = (vector) - HAL_PROG_INT_VEC_BASE - 1;
    icr = vec_offset / 8;
    icr_msk_offset = ((8-1)*4) - (vec_offset % 8) * 4;
    icr_msk = 0x0F << (icr_msk_offset);
    icr_val = (0x08 | (level & 0x07)) << icr_msk_offset;
    
    HAL_DISABLE_INTERRUPTS(intr_state);
    HAL_READ_UINT32(&MCF5272_DEVS->intc.icr[icr], icr_oldval);
    icr_val |= icr_oldval & 0x77777777 & ~icr_msk;
    HAL_WRITE_UINT32(&MCF5272_DEVS->intc.icr[icr], icr_val);
    HAL_RESTORE_INTERRUPTS(intr_state);
}

static bool MCF5272_uart_putc(serial_channel *chan, unsigned char c)
{
    CYG_INTERRUPT_STATE int_state;
    MCF5272_uart_info_t *port = (MCF5272_uart_info_t *)chan->dev_priv;

    /* Make sure the transmitter is not full. If it is full, return false. */
    if  (!(MCF5272_UART_READ(port->base->usr_ucsr) & MCF5272_UART_USR_TXRDY))
        return false;

    /* Enable transmit interrupt. */
    HAL_DISABLE_INTERRUPTS(int_state);
    port->imr_mirror |= MCF5272_UART_UIMR_TXRDY;
    MCF5272_UART_WRITE(port->base->uisr_uimr, port->imr_mirror);
    HAL_RESTORE_INTERRUPTS(int_state);

    /* Enable the UART transmit. */
    MCF5272_UART_WRITE(port->base->ucr, MCF5272_UART_UCR_TXRXEN);

	/* Send the character */
    MCF5272_UART_WRITE(port->base->urb_utb, c);

	return true ;
}

/******************************************************************************************************
 MCF5272_uart_stop_xmit() - Disable the transmitter on the device

 INPUT:
    chan - pointer to the serial private data.

*/
static void MCF5272_uart_stop_xmit(serial_channel * chan)
{	
   CYG_INTERRUPT_STATE int_state;
   MCF5272_uart_info_t * port = (MCF5272_uart_info_t *) chan->dev_priv;

   /* Disable transmit interrupt */
   HAL_DISABLE_INTERRUPTS(int_state);
   port->imr_mirror &= ~MCF5272_UART_UIMR_TXRDY;
   MCF5272_UART_WRITE(port->base->uisr_uimr, port->imr_mirror);
   HAL_RESTORE_INTERRUPTS(int_state);

   /* Disable the UART transmit.
      !!!!!!!!!!!!!
      !!!WARNING!!!
      !!!!!!!!!!!!!
      If the transmit the disabe
      the diag_printf routines will poll forever to transmit the
      a character. Hence, don't ever disable the transmit if
      we want it to work with diag_printf.
   */
   //MCF5272_UART_WRITE(port->base->ucr, MCF5272_UART_UCR_TXDE);


}