void up_enable_irq(int irq)
{
  /* System exceptions cannot be disabled */

  if (irq >= Z16F_IRQ_IRQ0)
    {
      /* Enable the interrupt by setting the corresponding bit in the
       * appropriate IRQ enable register.  The enable low
       * register is assumed to be zero, resulting in "nomimal" interrupt
       * priority.
       */

      if (irq < Z16F_IRQ_IRQ1)
        {
           putreg8((getreg8(Z16F_IRQ0_ENH) | Z16F_IRQ0_BIT(irq)), Z16F_IRQ0_ENH);
        }
      else if (irq < Z16F_IRQ_IRQ2)
        {
           putreg8((getreg8(Z16F_IRQ1_ENH) | Z16F_IRQ1_BIT(irq)), Z16F_IRQ1_ENH);
        }
      else if (irq < NR_IRQS)
        {
           putreg8((getreg8(Z16F_IRQ2_ENH) | Z16F_IRQ2_BIT(irq)), Z16F_IRQ2_ENH);
        }
    }
}

void up_disable_irq(int irq)
{
  /* System exceptions cannot be disabled */

  if (irq >= Z16F_IRQ_IRQ0)
    {
      /* Disable the interrupt by clearing the corresponding bit in the
       * appropriate IRQ enable high register.  The enable low
       * register is assumed to be zero, resulting interrupt disabled.
       */

      if (irq < Z16F_IRQ_IRQ1)
        {
           putreg8((getreg8(Z16F_IRQ0_ENH) & ~Z16F_IRQ0_BIT(irq)), Z16F_IRQ0_ENH);
        }
      else if (irq < Z16F_IRQ_IRQ2)
        {
           putreg8((getreg8(Z16F_IRQ1_ENH) & ~Z16F_IRQ1_BIT(irq)), Z16F_IRQ1_ENH);
        }
      else if (irq < NR_IRQS)
        {
           putreg8((getreg8(Z16F_IRQ2_ENH) & ~Z16F_IRQ2_BIT(irq)), Z16F_IRQ2_ENH);
        }
    }
}

int str71x_xtiinitialize(void)
{
  int ret;

  /* Mask all interrupts by setting XTI MRH/L to zero */

  putreg8(0, STR71X_XTI_MRH);
  putreg8(0, STR71X_XTI_MRL);

  /* Clear any pending interrupts by setting XTI PRH/L to zero */

  putreg8(0, STR71X_XTI_PRH);
  putreg8(0, STR71X_XTI_PRL);

  /* Attach the XTI interrupt */

  ret = irq_attach(STR71X_IRQ_XTI, str71x_xtiinterrupt);
  if (ret == OK)
    {
      /* Enable the XTI interrupt at the XTI */

      putreg8(STR71X_XTICTRL_ID1S, STR71X_XTI_CTRL);

      /* And enable the XTI interrupt at the interrupt controller */

      up_enable_irq(STR71X_IRQ_XTI);
    }
  return ret;
}

static void m16c_setleds(uint8_t gybits, uint8_t rbit)
{
  uint8_t regval;

  regval  = getreg8(GREENYELLOW_LED_PORT);
  regval &= ~GREENYELLOW_LED_MASK;
  regval |= gybits;
  putreg8(regval, GREENYELLOW_LED_PORT);

  regval  = getreg8(RED_LED_PORT);
  regval &= ~RED_LED_MASK;
  regval |= rbit;
  putreg8(regval, RED_LED_PORT);
}

void up_timerinit(void)
{
  uint32_t reload;
 
  up_disable_irq(Z8_IRQ_SYSTIMER);

  /* Write to the timer control register to disable the timer, configure
   * the timer for continuous mode, and set up the pre-scale value for
   * divide by 4.
   */

  putreg8((Z8_TIMERCTL_DIV4|Z8_TIMERCTL_CONT), T0CTL);

  /* Write to the timer high and low byte registers to set a starting
   * count value (this effects only the first pass in continuous mode)
   */

  putreg16(0x0001, T0);

  /* Write to the timer reload register to set the reload value.
   *
   * In continuous mode:
   *
   *   timer_period = reload_value x prescale / system_clock_frequency
   * or
   *   reload_value = (timer_period * system_clock_frequency) / prescale
   *
   * For system_clock_frequency=18.432MHz, timer_period=10mS, and prescale=4,
   * then reload_value=46,080 - OR:
   *
   *   reload_value = system_clock_frequency / 400
   */

   reload = get_freq() / 400;
   putreg16((uint16_t)reload, T0R);

  /* Write to the timer control register to enable the timer and to
   * initiate counting
   */

  putreg8((getreg8(T0CTL)|Z8_TIMERCTL_TEN), T0CTL);

  /* Set the timer priority */

  /* Attach and enable the timer interrupt (leaving at priority 0 */

  irq_attach(Z8_IRQ_SYSTIMER, (xcpt_t)up_timerisr);
  up_enable_irq(Z8_IRQ_SYSTIMER);
}

void up_earlyserialinit(void)
{
  uint8_t regval;

  /* Configure UART alternate pin functions.  This may duplicate logic in
   * z16f_lowuartinit() or z16f_lowinit().
   */

#ifdef CONFIG_Z16F_UART0
  /* UART0 is PA4 and PA5, alternate function 1 */

  regval  = getreg8(Z16F_GPIOA_AFL);
  regval |= 0x30;
  putreg8(regval, Z16F_GPIOA_AFL);

  regval  = getreg8(Z16F_GPIOA_AFH);
  regval &= ~0x30;
  putreg8(regval, Z16F_GPIOA_AFH);
#endif

#ifdef CONFIG_Z16F_UART1
  /* UART1 is PD4 and PD5, alternate function 1 */

  regval  = getreg8(Z16F_GPIOD_AFL);
  regval |= 0x30;
  putreg8(regval, Z16F_GPIOD_AFL);

  regval  = getreg8(Z16F_GPIOD_AFH);
  regval &= ~0x30;
  putreg8(regval, Z16F_GPIOD_AFH);
#endif

  /* Disable UART interrupts */

#ifdef TTYS0_DEV
  (void)z16f_disableuartirq(&TTYS0_DEV);
#endif

#ifdef TTYS1_DEV
  (void)z16f_disableuartirq(&TTYS1_DEV);
#endif

  /* Configuration any serial console */

#ifdef CONSOLE_DEV
  CONSOLE_DEV.isconsole = true;
  z16f_setup(&CONSOLE_DEV);
#endif
}

void calypso_fiq(void)
{
  uint8_t num, tmp;
  uint32_t *regs;

  /* XXX: What is this???
   * Passed to but ignored in IRQ handlers
   * Only valid meaning is apparently non-NULL == IRQ context */

  regs = (uint32_t *)current_regs;
  current_regs = (uint32_t *)#

  /* Detect & deliver like an IRQ but we are in FIQ context */

  num = getreg8(IRQ_REG(FIQ_NUM)) & 0x1f;
  irq_dispatch(num, regs);

  /* Start new FIQ agreement */

  tmp = getreg8(IRQ_REG(IRQ_CTRL));
  tmp |= 0x02;
  putreg8(tmp, IRQ_REG(IRQ_CTRL));

  current_regs = regs;
}

void str71x_disable_xtiirq(int irq)
{
  uint8_t regval;
  int bit;
  int ndx;

  /* Disable the external interrupt */

  if (irq >= STR71X_IRQ_FIRSTXTI && irq <= NR_IRQS)
    {
      /* Decide if we use the lower or upper regiser */

      bit = irq - STR71X_IRQ_FIRSTXTI;
      ndx = 0;
      if (bit > 7)
        {
          /* Select the high register */

          bit  -= 8;
          ndx  = 1;
        }

      /* Disable the interrupt be clearing the corresponding mask bit
       * the XTI_MRL/H register.
       */

      regval  = getreg8(g_xtiregs[ndx].mr);
      regval &= ~(1 << bit);
      putreg8(regval, g_xtiregs[ndx].mr);
    }
}

ssize_t up_progmem_erasepage(size_t page)
{
	size_t addr;
	irqstate_t irqs;

	if (page >= up_progmem_npages()) {
		return -EFAULT;
	}

	addr = up_progmem_getaddress(page);

	/* Disable IRQs while erasing sector */
	irqs = irqsave();

	s5j_sflash_disable_wp();

	/* Set sector address and then send erase command */
	putreg32(addr - S5J_FLASH_PADDR, S5J_SFLASH_ERASE_ADDRESS);
	putreg8(0xff, S5J_SFLASH_SE);

	/* Wait for the completion */
	while (s5j_sflash_read_status() & 0x1) ;

	s5j_sflash_enable_wp();

	/* Invalidate cache */
	arch_invalidate_dcache(addr, addr + up_progmem_blocksize());

	/* Restore IRQs */
	irqrestore(irqs);

	return up_progmem_blocksize();
}

void up_decodeirq(uint32_t *regs)
{
  uint8_t num, tmp;
  uint32_t *saved_regs;

  /* XXX: What is this???
   * Passed to but ignored in IRQ handlers
   * Only valid meaning is apparently non-NULL == IRQ context */

  saved_regs = (uint32_t *)current_regs;
  current_regs = regs;

  /* Detect & deliver the IRQ */

  num = getreg8(IRQ_REG(IRQ_NUM)) & 0x1f;
  irq_dispatch(num, regs);

  /* Start new IRQ agreement */

  tmp = getreg8(IRQ_REG(IRQ_CTRL));
  tmp |= 0x01;
  putreg8(tmp, IRQ_REG(IRQ_CTRL));

  current_regs = saved_regs;
}

void up_lowputc(char ch)
{
#if defined HAVE_UART_DEVICE && defined HAVE_SERIAL_CONSOLE
#ifdef CONFIG_KINETIS_UARTFIFOS
  /* Wait until there is space in the TX FIFO:  Read the number of bytes
   * currently in the FIFO and compare that to the size of the FIFO.  If
   * there are fewer bytes in the FIFO than the size of the FIFO, then we
   * are able to transmit.
   */

#  error "Missing logic"
#else
  /* Wait until the transmit data register is "empty" (TDRE).  This state
   * depends on the TX watermark setting and may not mean that the transmit
   * buffer is truly empty.  It just means that we can now add another
   * characterto the transmit buffer without exceeding the watermark.
   *
   * NOTE:  UART0 has an 8-byte deep FIFO; the other UARTs have no FIFOs
   * (1-deep).  There appears to be no way to know when the FIFO is not
   * full (other than reading the FIFO length and comparing the FIFO count).
   * Hence, the FIFOs are not used in this implementation and, as a result
   * TDRE indeed mean that the single output buffer is available.
   *
   * Performance on UART0 could be improved by enabling the FIFO and by
   * redesigning all of the FIFO status logic.
   */

  while ((getreg8(CONSOLE_BASE+KINETIS_UART_S1_OFFSET) & UART_S1_TDRE) == 0);
#endif

  /* Then write the character to the UART data register */

  putreg8((uint8_t)ch, CONSOLE_BASE+KINETIS_UART_D_OFFSET);
#endif
}

static int z16f_setup(struct uart_dev_s *dev)
{
#ifndef CONFIG_SUPPRESS_UART_CONFIG
  struct z16f_uart_s *priv = (struct z16f_uart_s*)dev->priv;
  uint32_t brg;
  uint8_t ctl0;
  uint8_t ctl1;

  /* Calculate and set the baud rate generation register.
   * BRG = (freq + baud * 8)/(baud * 16)
   */

  brg = (_DEFCLK + (priv->baud << 3)) / (priv->baud << 4);
  putreg16((uint16_t)brg, priv->uartbase + Z16F_UART_BR);

  /* Configure STOP bits */

  ctl0 = 0;
  ctl1 = 0;

  if (priv->stopbits2)
    {
      ctl0 |= Z16F_UARTCTL0_STOP;
    }

  /* Configure parity */

  if (priv->parity == 1)
    {
      ctl0 |= (Z16F_UARTCTL0_PEN|Z16F_UARTCTL0_PSEL);
    }
  else if (priv->parity == 2)
    {
      ctl0 |= Z16F_UARTCTL0_PEN;
    }

  putreg8(ctl0, priv->uartbase + Z16F_UART_CTL0);
  putreg8(ctl1, priv->uartbase + Z16F_UART_CTL1);

  /* Enable UART receive (REN) and transmit (TEN) */

  ctl0 |= (Z16F_UARTCTL0_TEN|Z16F_UARTCTL0_REN);
  putreg8(ctl0, priv->uartbase + Z16F_UART_CTL0);
#endif

  return OK;
}

static int lcd_setpower(struct lcd_dev_s *dev, int power)
{
  uint16_t reg;

  if (g_lcddev.power == power) {
    return OK;
  }

  ginfo("power: %d\n", power);
  DEBUGASSERT(power <= CONFIG_LCD_MAXPOWER);

  /* Set new power level */
  reg = getreg16(ASIC_CONF_REG);
  if (power)
    {
      reg = getreg16(ASIC_CONF_REG);
      /* LCD Set I/O(3) / SA0 to I/O(3) mode */
      reg &= ~( (1 << 12) | (1 << 10) | (1 << 7) | (1 << 1)) ;
      /* don't set function pins to I2C Mode, C155 uses UWire */
      /* TWL3025: Set SPI+RIF RX clock to rising edge */
      reg |= (1 << 13) | (1 << 14);
      putreg16(reg, ASIC_CONF_REG);

      /* LCD Set I/O(3) to output mode and enable C155 backlight (IO1) */
      /* FIXME: Put the display backlight control to backlight.c */
      reg = getreg16(IO_CNTL_REG);
      reg &= ~( (1 << 3) | (1 << 1));
      putreg16(reg, IO_CNTL_REG);

      /* LCD Set I/O(3) output low */
      reg = getreg16(ARMIO_LATCH_OUT);
      reg &= ~(1 << 3);
      reg |= (1 << 1);
      putreg16(reg, ARMIO_LATCH_OUT);
    }
    else
    {
      ginfo("powering LCD off...\n");
      /* Switch pin from PWL to LT */
      reg &= ~ASCONF_PWL_ENA;
      putreg8(reg, ASIC_CONF_REG);
      /* Disable pwl */
      putreg8(0x00, PWL_REG(PWL_CTRL));
    }
    return OK;
}

void up_buttoninit(void)
{
  uint8_t regval;

  regval  = getreg8(M16C_PD8);
  regval |= (SW1_BIT | SW2_BIT | SW3_BIT);
  putreg8(regval, M16C_PD8);
}

static void z16f_sysclkinit(int clockid, uint32_t frequency)
{
  int count;

  /* In this configuration, we support only the external oscillator/clock
   * the the source of the system clock (__DEFCLK is ignored).
   */

  if ((getreg8(Z16F_OSC_CTL) & 0x03) != 1)
    {
       /* No divider for the oscillator */

       putreg8(0x00, Z16F_OSC_DIV);

       /* Enable external oscillator */

       putreg8(0xe7, Z16F_OSC_CTL); /* Unlock the crystal oscillator */
       putreg8(0x18, Z16F_OSC_CTL);
       putreg8(0xe0, Z16F_OSC_CTL); /* INTEN+XTLEN+WDTEN */

       /* Wait for oscillator to stabilize */

       for (count = 0; count < 10000; count++);

       /* Select external oscillator (SCLKSEL=1) */

       putreg8(0xe7, Z16F_OSC_CTL); /* Unlock the crystal oscillator */
       putreg8(0x18, Z16F_OSC_CTL);
       putreg8(0xe0 | 1, Z16F_OSC_CTL); /* Use the external osc/clock as system clock */
    }
}

void up_timerinit(void)
{
  uint8_t reg8;

  /* Clear timer counter 0 */

  putreg16(0, SH1_ITU0_TCNT);

  /* Set the GRA0 match value.  The interrupt will be generated when TNCT
   * increments to this value
   */

  putreg16(TCNT_PER_TICK, SH1_ITU0_GRA);

  /* Set the timer control register.  TCNT cleared by FRA */

  putreg8(SH1_ITUTCR_CGRA|SH1_ITUTCR_DIV, SH1_ITU0_TCR);

  /* Set the timer I/O control register */

  putreg8(0, SH1_ITU0_TIOR); /* GRA used but with no inputs/output pins */

  /* Make sure that all status flags are clear */

  putreg8(0, SH1_ITU0_TSR);

  /* Attach the IMIA0 IRQ */

  irq_attach(SH1_SYSTIMER_IRQ, (xcpt_t)up_timerisr);

  /* Enable interrupts on GRA compare match */

  putreg8(SH1_ITUTIER_IMIEA, SH1_ITU0_TIER);

  /* Set the interrupt priority */

  up_prioritize_irq(SH1_SYSTIMER_IRQ, 7);  /* Set ITU priority midway */

  /* Start the timer */

  reg8  = getreg8(SH1_ITU_TSTR);
  reg8 |= SH1_ITUTSTR_STR0;     /* Enable TCNT0 */
  putreg8(reg8, SH1_ITU_TSTR);  /* TCNT0 is counting */
}

void hwtimer_config(int num, uint8_t pre_scale, int auto_reload)
{
  uint8_t ctl;

  ctl = (pre_scale & 0x7) << 2;
  if (auto_reload)
      ctl |= CNTL_AUTO_RELOAD;

  putreg8(ctl, TIMER_REG(num, CNTL_TIMER));
}

uint32_t itm_sendchar(uint32_t ch)
{
	if ((getreg32(ITM_TCR) & ITM_TCR_ITMENA_Msk) &&	/* ITM enabled */
		(getreg32(ITM_TER) & (1UL << 0))) {	/* ITM Port #0 enabled */
		while (getreg32(ITM_PORT(0)) == 0) ;
		putreg8((uint8_t)ch, ITM_PORT(0));
	}

	return ch;
}

void kinetis_uartreset(uintptr_t uart_base)
{
  uint8_t regval;

  /* Just disable the transmitter and receiver */

  regval = getreg8(uart_base+KINETIS_UART_C2_OFFSET);
  regval &= ~(UART_C2_RE | UART_C2_TE);
  putreg8(regval, uart_base+KINETIS_UART_C2_OFFSET);
}

static int str71x_xtiinterrupt(int irq, FAR void *context)
{
  uint16_t enabled = (uint16_t)getreg8(STR71X_XTI_MRH) << 8 |
                     (uint16_t)getreg8(STR71X_XTI_MRL);
  uint16_t pending = (uint16_t)getreg8(STR71X_XTI_PRH) << 8 |
                     (uint16_t)getreg8(STR71X_XTI_PRL);
  uint16_t mask;

  /* Dispatch the interrupts, the actions performed by the interrupt
   * handlers should clear the interrupt at the external source of the
   * interrupt.  We need to clear the interrupts at the source before
   * clearing the pending interrupts (see below).
   */

  pending &= enabled;

  for (irq = STR71X_IRQ_FIRSTXTI, mask = 0x0001;
       irq < NR_IRQS && pending != 0;
       irq++, mask <<= 1)
    {
      /* Is this interrupt pending? */

      if ((pending & mask) != 0)
        {
	  /* Deliver the IRQ */

          irq_dispatch(irq, context);
          pending &= ~mask;
        }
    }

  /* Clear the pending interrupts.  This should be safe: "it is necessary to
   * clear at least one pending bit: this operation allows a rising edge to be
   * generated on the internal line (if there is at least one more pending bit
   * set and not masked) and so to set the interrupt controller pending bit
   * again.
   */

  putreg8(0, STR71X_XTI_PRH);
  putreg8(0, STR71X_XTI_PRL);
  return OK;
}