/* i2c_transfer --
 *     Initiate multiple-messages transfer over specified I2C bus or
 *     put request into queue if bus or some other resource is busy. (This
 *     is non-blocking function).
 *
 * PARAMETERS:
 *     bus - I2C bus number
 *     nmsg - number of messages
 *     msg - pointer to messages array
 *     done - function which is called when transfer is finished
 *     done_arg_ptr - arbitrary argument pointer passed to done funciton
 *
 * RETURNS:
 *     RTEMS_SUCCESSFUL if transfer initiated successfully, or error
 *     code if something failed.
 */
rtems_status_code
i2c_transfer(i2c_bus_number bus, int nmsg, i2c_message *msg,
             i2c_transfer_done done, void *     done_arg_ptr)
{
    i2c_qel qel;
    rtems_interrupt_level level;

    if (bus >= I2C_NUMBER_OF_BUSES)
    {
        return RTEMS_INVALID_NUMBER;
    }

    if (msg == NULL)
    {
        return RTEMS_INVALID_ADDRESS;
    }

    qel.bus = bus;
    qel.msg = msg;
    qel.nmsg = nmsg;
    qel.done = done;
    qel.done_arg_ptr = done_arg_ptr;
    rtems_interrupt_disable(level);
    if ((tqueue_head + 1) % tqueue_size == tqueue_tail)
    {
        rtems_interrupt_enable(level);
        return RTEMS_TOO_MANY;
    }
    memcpy(tqueue + tqueue_head, &qel, sizeof(qel));
    tqueue_head = (tqueue_head + 1) % tqueue_size;
    rtems_interrupt_enable(level);
    i2cdrv_unload();
    return RTEMS_SUCCESSFUL;
}

static int
rtems_am29lv160_write_data_8 (volatile uint8_t* base,
                              uint32_t          offset,
                              const uint8_t*    data,
                              uint32_t          size)
{
  volatile uint8_t*     seg = base + offset;
  rtems_interrupt_level level;

  /*
   * Issue a reset.
   */
  *base = 0xf0;

  while (size)
  {
    rtems_interrupt_disable (level);
    *(base + 0xaaa) = 0xaa;
    *(base + 0x555) = 0x55;
    *(base + 0xaaa) = 0xa0;
    *seg = *data++;
    rtems_interrupt_enable (level);
    if (rtems_am29lv160_toggle_wait_8 (seg++) != 0)
      return EIO;
    size--;
  }

  /*
   * Issue a reset.
   */
  *base = 0xf0;

  return 0;
}

/*
 * support functions for PCMCIA IDE IF
 */
bool mpc5200_pcmciaide_probe(int minor)
  {
  bool ide_card_plugged = false; /* assume: we don't have a card plugged in */
  struct mpc5200_gpt *gpt = (struct mpc5200_gpt *)(&mpc5200.gpt[GPT2]);

  #ifdef MPC5200_BOARD_DP2
    /* Deactivate RESET signal */
    rtems_interrupt_level level;
    rtems_interrupt_disable(level);
    mpc5200.gpiowe |= GPIO_W_PIN_PSC1_4;
    mpc5200.gpiowod &= ~GPIO_W_PIN_PSC1_4;
    mpc5200.gpiowdd |= GPIO_W_PIN_PSC1_4;
    mpc5200.gpiowdo |= GPIO_W_PIN_PSC1_4;
    rtems_interrupt_enable(level);
    /* FIXME */
    volatile int i = 0;
    while (++i < 20000000);
  #endif

  /* enable card detection on GPT2 */
  gpt->emsel = (GPT_EMSEL_GPIO_IN | GPT_EMSEL_TIMER_MS_GPIO);

#if defined (MPC5200_BOARD_BRS5L)
  /* Check for card detection (-CD0) */
  if((gpt->status) & GPT_STATUS_PIN)
    ide_card_plugged = false;
  else
#endif
    ide_card_plugged = true;

  return ide_card_plugged;

  }

void Shm_Lock(
  Shm_Locked_queue_Control *lq_cb
)
{
  uint32_t         isr_level;
  uint32_t         *lockptr = (uint32_t*) &lq_cb->lock;
  uint32_t         lock_value;

  lock_value = 0x80000000;
  rtems_interrupt_disable( isr_level );

    Shm_isrstat = isr_level;
    while ( lock_value ) {
      __asm__ volatile( ""
                         : "=r" (lockptr), "=r" (lock_value)
                         : "0" (lockptr),  "1" (lock_value)
                  );
      /*
       *  If not available, then may want to delay to reduce load on lock.
       */

      if ( lock_value )
        rtems_bsp_delay( 10 );   /* approximately 10 microseconds */
   }
}

rtems_device_driver Clock_control(
  rtems_device_major_number major,
  rtems_device_minor_number minor,
  void *pargp
)
{
    uint32_t         isrlevel;
    rtems_libio_ioctl_args_t *args = pargp;

    if (args == 0)
        goto done;

    /*
     * This is hokey, but until we get a defined interface
     * to do this, it will just be this simple...
     */

    if (args->command == rtems_build_name('I', 'S', 'R', ' '))
    {
        Clock_isr(CLOCK_VECTOR);
    }
    else if (args->command == rtems_build_name('N', 'E', 'W', ' '))
    {
      rtems_interrupt_disable( isrlevel );
       (void) set_vector( args->buffer, CLOCK_VECTOR, 1 );
      rtems_interrupt_enable( isrlevel );
    }

done:
    return RTEMS_SUCCESSFUL;
}

/*-------------------------------------------------------------------------+
|         Function:  BSP_irq_disable_at_i8259s
|      Description: Mask IRQ line in appropriate PIC chip.
| Global Variables: i8259s_cache
|        Arguments: vector_offset - number of IRQ line to mask.
|          Returns: original state or -1 on error.
+--------------------------------------------------------------------------*/
int BSP_irq_disable_at_i8259s    (const rtems_irq_number irqLine)
{
  unsigned short        mask;
  rtems_interrupt_level level;
  int                   rval;

  if ( ((int)irqLine < BSP_ISA_IRQ_LOWEST_OFFSET) ||
       ((int)irqLine > BSP_ISA_IRQ_MAX_OFFSET)
       )
    return -1;

  rtems_interrupt_disable(level);

  mask = 1 << irqLine;
  rval = i8259s_cache & mask ? 0 : 1;
  i8259s_cache |= mask;

  if (irqLine < 8)
  {
    outport_byte(PIC_MASTER_IMR_IO_PORT, i8259s_cache & 0xff);
  }
  else
  {
    outport_byte(PIC_SLAVE_IMR_IO_PORT, ((i8259s_cache & 0xff00) >> 8));
  }
  rtems_interrupt_enable(level);

  return rval;
}

void
ReInstall_clock(void (*new_clock_isr)(void *))
{
	uint32_t   isrlevel = 0;
	rtems_irq_connect_data clockIrqConnData;

	rtems_interrupt_disable(isrlevel);
	clockIrqConnData.name = BSP_PIT;
	if ( ! BSP_get_current_rtems_irq_handler(&clockIrqConnData)) {
		printk("Unable to stop system clock\n");
		rtems_fatal_error_occurred(1);
	}

	BSP_remove_rtems_irq_handler (&clockIrqConnData);
	clockIrqConnData.on   = ClockOn;
	clockIrqConnData.off  = ClockOff;
	clockIrqConnData.isOn = ClockIsOn;
	clockIrqConnData.name = BSP_PIT;
	clockIrqConnData.hdl  = new_clock_isr;
	if (!BSP_install_rtems_irq_handler (&clockIrqConnData)) {
		printk("Unable to connect Clock Irq handler\n");
		rtems_fatal_error_occurred(1);
	}
	rtems_interrupt_enable(isrlevel);
}

/*-------------------------------------------------------------------------+
|         Function:  BSP_irq_enable_at_i8259s
|      Description: Unmask IRQ line in appropriate PIC chip.
| Global Variables: i8259s_cache
|        Arguments: irqLine - number of IRQ line to mask.
|          Returns: Nothing.
+--------------------------------------------------------------------------*/
int BSP_irq_enable_at_i8259s    (const rtems_irq_number irqLine)
{
  unsigned short        mask;
  rtems_interrupt_level level;

  if ( ((int)irqLine < BSP_LOWEST_OFFSET) ||
       ((int)irqLine > BSP_MAX_OFFSET )
       )
    return 1;

  rtems_interrupt_disable(level);

  mask = ~(1 << irqLine);
  i8259s_cache &= mask;

  if (irqLine < 8)
  {
    outport_byte(PIC_MASTER_IMR_IO_PORT, i8259s_cache & 0xff);
  }
  else
  {
    outport_byte(PIC_SLAVE_IMR_IO_PORT, ((i8259s_cache & 0xff00) >> 8));
  }
  rtems_interrupt_enable(level);

  return 0;
} /* mask_irq */

static void compute_i8259_masks_from_prio (void)
{
  rtems_interrupt_level level;
  unsigned int i;
  unsigned int j;

  rtems_interrupt_disable(level); /* XXX */

  /*
   * Always mask at least current interrupt to prevent re-entrance
   */
  for (i=0; i < BSP_IRQ_LINES_NUMBER; i++) {
    * ((unsigned short*) &irq_mask_or_tbl[i]) = (1 << i);
    for (j = 0; j < BSP_IRQ_LINES_NUMBER; j++) {
      /*
       * Mask interrupts at i8259 level that have a lower priority
       */
      if (irqPrioTable [i] > irqPrioTable [j]) {
	* ((unsigned short*) &irq_mask_or_tbl[i]) |= (1 << j);
      }
    }
  }

  rtems_interrupt_enable(level);
}

/**
 * Configures a PIO or a group of PIO to generate an interrupt on status
 * change. The provided interrupt handler will be called with the triggering
 * pin as its parameter (enabling different pin instances to share the same
 * handler).
 * \param pPin  Pointer to a Pin instance.
 * \param handler  Interrupt handler function pointer.
 * \param arg Pointer to interrupt handler argument
 */
void PIO_ConfigureIt(const Pin *pPin, void (*handler)(const Pin *, void *arg),
    void *arg)
{
	InterruptSource *pSource;
	rtems_interrupt_level level;

	TRACE_DEBUG("PIO_ConfigureIt()\n\r");

	rtems_interrupt_disable(level);

	if (_dwNumSources == MAX_INTERRUPT_SOURCES) {
		bsp_fatal(ATSAM_FATAL_PIO_CONFIGURE_IT);
	}

	pSource = &(_aIntSources[_dwNumSources]);
	pSource->pPin = pPin;
	pSource->handler = handler;
	pSource->arg = arg;

	_dwNumSources++;

	rtems_interrupt_enable(level);

	/* Define new source */
	TRACE_DEBUG("PIO_ConfigureIt: Defining new source #%d.\n\r", _dwNumSources);
}

/**
 * @brief Checks if the module has power.
 *
 * @param has_power Power.
 * @param index Index to shift.
 * @param turn_on Turn on/off the power.
 * @param level Interrupts value.
 */
static rtems_status_code check_power(
  const bool            has_power,
  const unsigned        index,
  const bool            turn_on,
  rtems_interrupt_level level
)
{
  rtems_status_code status_code = RTEMS_INVALID_NUMBER;

  if ( index <= LPC176X_MODULE_BITS_COUNT ) {
    if ( has_power ) {
      rtems_interrupt_disable( level );

      if ( turn_on ) {
        LPC176X_SCB.pconp |= 1u << index;
      } else {
        LPC176X_SCB.pconp &= ~( 1u << index );
      }

      rtems_interrupt_enable( level );
    }

    /* else implies that the module has not power. Also,
       there is nothing to do. */

    status_code = RTEMS_SUCCESSFUL;
  }

  /* else implies an invalid index number. Also, the function
     does not return successful. */

  return status_code;
}

MC68681_STATIC ssize_t mc68681_write_support_int(
  int         minor,
  const char *buf,
  size_t      len
)
{
  uint32_t        Irql;
  uint32_t        pMC68681_port;
  setRegister_f   setReg;

  pMC68681_port = Console_Port_Tbl[minor]->ulCtrlPort2;
  setReg        = Console_Port_Tbl[minor]->setRegister;

  /*
   *  We are using interrupt driven output and termios only sends us
   *  one character at a time.
   */

  if ( !len )
    return 0;

  /*
   *  Put the character out and enable interrupts if necessary.
   */

  rtems_interrupt_disable(Irql);
    if ( Console_Port_Data[minor].bActive == FALSE ) {
      Console_Port_Data[minor].bActive = TRUE;
      mc68681_enable_interrupts(minor, MC68681_IMR_ENABLE_ALL);
    }
    (*setReg)(pMC68681_port, MC68681_TX_BUFFER, *buf);
  rtems_interrupt_enable(Irql);

  return 0;
}

rtems_task High_task(
  rtems_task_argument argument
)
{
  rtems_interrupt_level level;

  benchmark_timer_initialize();
    rtems_interrupt_disable( level );
  isr_disable_time = benchmark_timer_read();

  benchmark_timer_initialize();
    rtems_interrupt_flash( level );
  isr_flash_time = benchmark_timer_read();

  benchmark_timer_initialize();
    rtems_interrupt_enable( level );
  isr_enable_time = benchmark_timer_read();

  benchmark_timer_initialize();
    _Thread_Disable_dispatch();
  thread_disable_dispatch_time = benchmark_timer_read();

  benchmark_timer_initialize();
    _Thread_Enable_dispatch();
  thread_enable_dispatch_time = benchmark_timer_read();

  benchmark_timer_initialize();
    _Thread_Set_state( _Thread_Executing, STATES_SUSPENDED );
  thread_set_state_time = benchmark_timer_read();

  _Context_Switch_necessary = true;

  benchmark_timer_initialize();
    _Thread_Dispatch();           /* dispatches Middle_task */
}

void Shm_Lock(
  Shm_Locked_queue_Control *lq_cb
)
{
  rtems_interrupt_disable( level );
    (void) PSIM.Semaphore.lock;
}

void bfin_interrupt_enable_global(int source, bool enable) {
  int vector;
  rtems_interrupt_level isrLevel;

  for (vector = 0; vector < CEC_INTERRUPT_COUNT; vector++)
    if ( (vectors[vector].mask0 & (1 << source) ) || \
        (vectors[vector].mask1 & (1 << (source - SIC_ISR0_MAX)) ))
      break;
  if (vector < CEC_INTERRUPT_COUNT) {
    rtems_interrupt_disable(isrLevel);
    if ( SIC_ISR0_MAX > source ) {
      if (enable)
        globalMask0 |= 1 << source;
      else
        globalMask0 &= ~(1 << source);
    }else {
      if (enable)
        globalMask1 |= 1 << (source - SIC_ISR0_MAX);
      else
        globalMask1 &= ~(1 << (source - SIC_ISR0_MAX));
    }
    setMask(vector);
    rtems_interrupt_enable(isrLevel);
  }
}

void Shm_Lock(
  Shm_Locked_queue_Control *lq_cb
)
{
  uint32_t         isr_level;
  uint32_t         *lockptr = (uint32_t*) &lq_cb->lock;
  uint32_t         lock_value;

  lock_value = 0x80000000;
  rtems_interrupt_disable( isr_level );

    Shm_isrstat = isr_level;
    while ( lock_value ) {
      __asm__ volatile( ""
                         : "=r" (lockptr), "=r" (lock_value)
                         : "0" (lockptr),  "1" (lock_value)
                  );
      /*
       *  If not available, then may want to delay to reduce load on lock.
       *
       *  NOTE: BSP must initialize the counter facility. Delay value is BSP
       *        dependent.
       */
      if ( lock_value )
        rtems_counter_delay_nanoseconds( 100 );
   }
}

void bfin_interrupt_enable_all(int source, bool enable) {
  rtems_interrupt_level isrLevel;
  int vector;
  bfin_isr_t *walk;

  for (vector = 0; vector < CEC_INTERRUPT_COUNT; vector++)
    if ( (vectors[vector].mask0 & (1 << source) ) || \
        (vectors[vector].mask1 & (1 << (source - SIC_ISR0_MAX)) ))
      break;
  if (vector < CEC_INTERRUPT_COUNT) {
    rtems_interrupt_disable(isrLevel);
    walk = vectors[vector].head;
    while (walk) {
      walk->mask0 = enable ? (1 << source) : 0;
      walk = walk->next;
    }

    walk = vectors[vector].head;
    while (walk) {
      walk->mask1 = enable ? (1 << (source - SIC_ISR0_MAX)) : 0;
      walk = walk->next;
    }
    setMask(vector);
    rtems_interrupt_enable(isrLevel);
  }
}

/* add an ISR to the list for whichever vector it belongs to */
rtems_status_code bfin_interrupt_register(bfin_isr_t *isr) {
  bfin_isr_t *walk;
  rtems_interrupt_level isrLevel;

  /* find the appropriate vector */
  for (isr->vector = 0; isr->vector < CEC_INTERRUPT_COUNT; isr->vector++)
    if ( (vectors[isr->vector].mask0 & (1 << isr->source) ) || \
        (vectors[isr->vector].mask1 & (1 << (isr->source - SIC_ISR0_MAX)) ))
      break;
  if (isr->vector < CEC_INTERRUPT_COUNT) {
    isr->next = NULL;
    isr->mask0 = 0;
    isr->mask1 = 0;
    rtems_interrupt_disable(isrLevel);
    /* find the current end of the list */
    walk = vectors[isr->vector].head;
    while (walk && walk->next)
      walk = walk->next;
    /* append new isr to list */
    if (walk)
      walk->next = isr;
    else
      vectors[isr->vector].head = isr;
    rtems_interrupt_enable(isrLevel);
  } else
    /* we failed, but make vector a legal value so other calls into
	       this module with this isr descriptor won't do anything bad */
    isr->vector = 0;
  return RTEMS_SUCCESSFUL;
}

static rtems_isr interruptHandler(rtems_vector_number vector) {
  bfin_isr_t *isr = NULL;
  uint32_t sourceMask0 = 0;
  uint32_t sourceMask1 = 0;
  rtems_interrupt_level isrLevel;

  rtems_interrupt_disable(isrLevel);
  vector -= CEC_INTERRUPT_BASE_VECTOR;
  if (vector >= 0 && vector < CEC_INTERRUPT_COUNT) {
    isr = vectors[vector].head;
    sourceMask0 = *(uint32_t volatile *) SIC_ISR &
        *(uint32_t volatile *) SIC_IMASK;
    sourceMask1 = *(uint32_t volatile *) (SIC_ISR + SIC_ISR_PITCH) &
        *(uint32_t volatile *) (SIC_IMASK + SIC_IMASK_PITCH);
    while (isr) {
      if ((sourceMask0 & isr->mask0) || (sourceMask1 & isr->mask1)) {
        isr->isr(isr->_arg);
        sourceMask0 = *(uint32_t volatile *) SIC_ISR &
            *(uint32_t volatile *) SIC_IMASK;
        sourceMask1 = *(uint32_t volatile *) (SIC_ISR + SIC_ISR_PITCH) &
            *(uint32_t volatile *) (SIC_IMASK + SIC_IMASK_PITCH);
      }
      isr = isr->next;
    }
  }
  rtems_interrupt_enable(isrLevel);
}

/*
 *  console_outbyte_interrupts
 *
 *  This routine transmits a character out.
 *
 *  Input parameters:
 *    port - port to transmit character to
 *    ch  - character to be transmitted
 *
 *  Output parameters:  NONE
 *
 *  Return values:      NONE
 */
void console_outbyte_interrupts(
  const Port_85C30_info *Port,
  char ch
)
{
  Console_Protocol   *protocol;
  uint32_t            isrlevel;

  protocol = Port->Protocol;

  /*
   *  If this is the first character then we need to prime the pump
   */

  if ( protocol->Is_TX_active == false ) {

    rtems_interrupt_disable( isrlevel );
    protocol->Is_TX_active = true;
    outbyte_polled_85c30( Port->ctrl, ch );
    rtems_interrupt_enable( isrlevel );

    return;
  }

  while ( Ring_buffer_Is_full( &protocol->TX_Buffer ) );

  Ring_buffer_Add_character( &protocol->TX_Buffer, ch );
}