i386/pc386: Fix interrupt support.

Fix the interrupt and stop the spurious interrupt from happening.

The fix moves the EOI to C code and cleans that functionality out
of the asm part of the ISR handler.

The code checks the ISR and IRR registers on the enable.

Only ack the master for a slave IRQ if the slave has no other pending
requests.

9 files changed, 471 insertions, 172 deletions

diff --git a/c/src/lib/libbsp/i386/Makefile.am b/c/src/lib/libbsp/i386/Makefile.am
index 3b6df65b1d..567f75fdbd 100644
--- a/c/src/lib/libbsp/i386/Makefile.am
+++ b/c/src/lib/libbsp/i386/Makefile.am
@@ -19,6 +19,7 @@ EXTRA_DIST += shared/irq/irq.h shared/irq/irq.c
 EXTRA_DIST += shared/irq/irq_asm.h shared/irq/irq_asm.S
 EXTRA_DIST += shared/irq/idt.c
 EXTRA_DIST += shared/irq/irq_init.c
+EXTRA_DIST += shared/irq/elcr.c
 
 # shared/pci
 EXTRA_DIST += shared/pci/pcibios.c
diff --git a/c/src/lib/libbsp/i386/pc386/Makefile.am b/c/src/lib/libbsp/i386/pc386/Makefile.am
index 22d4bf1e46..d051da8ec7 100644
--- a/c/src/lib/libbsp/i386/pc386/Makefile.am
+++ b/c/src/lib/libbsp/i386/pc386/Makefile.am
@@ -161,6 +161,7 @@ libbsp_a_SOURCES += startup/bspreset.c
 libbsp_a_SOURCES += ../../i386/shared/irq/idt.c
 libbsp_a_SOURCES += ../../i386/shared/irq/irq.c
 libbsp_a_SOURCES += ../../i386/shared/irq/irq_init.c
+libbsp_a_SOURCES += ../../i386/shared/irq/elcr.c
 libbsp_a_SOURCES += ../../shared/bootcard.c
 libbsp_a_SOURCES += ../../shared/sbrk.c
 libbsp_a_SOURCES += startup/ldsegs.S
diff --git a/c/src/lib/libbsp/i386/shared/irq/elcr.c b/c/src/lib/libbsp/i386/shared/irq/elcr.c
new file mode 100644
index 0000000000..f72e36c33f
--- /dev/null
+++ b/c/src/lib/libbsp/i386/shared/irq/elcr.c
@@ -0,0 +1,170 @@
+/*-
+ * Copyright (c) 2004 John Baldwin <jhb@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+#ifndef __rtems__
+__FBSDID("$FreeBSD$");
+#endif /* __rtems__ */
+
+/*
+ * The ELCR is a register that controls the trigger mode and polarity of
+ * EISA and ISA interrupts.  In FreeBSD 3.x and 4.x, the ELCR was only
+ * consulted for determining the appropriate trigger mode of EISA
+ * interrupts when using an APIC.  However, it seems that almost all
+ * systems that include PCI also include an ELCR that manages the ISA
+ * IRQs 0 through 15.  Thus, we check for the presence of an ELCR on
+ * every machine by checking to see if the values found at bootup are
+ * sane.  Note that the polarity of ISA and EISA IRQs are linked to the
+ * trigger mode.  All edge triggered IRQs use active-hi polarity, and
+ * all level triggered interrupts use active-lo polarity.
+ *
+ * The format of the ELCR is simple: it is a 16-bit bitmap where bit 0
+ * controls IRQ 0, bit 1 controls IRQ 1, etc.  If the bit is zero, the
+ * associated IRQ is edge triggered.  If the bit is one, the IRQ is
+ * level triggered.
+ */
+
+#ifndef __rtems__
+#include <sys/param.h>
+#include <sys/bus.h>
+#include <sys/systm.h>
+#include <machine/intr_machdep.h>
+#endif /* __rtems__ */
+
+#ifdef __rtems__
+#include <bsp.h>
+#include "i386_io.h"
+#include <errno.h>
+#include "elcr.h"
+#endif /* __rtems__ */
+
+#define	ELCR_PORT	0x4d0
+#define	ELCR_MASK(irq)	(1 << (irq))
+
+static int elcr_status;
+#ifdef __rtems__
+static
+#endif /* __rtems__ */
+int elcr_found;
+
+#ifdef __rtems__
+#undef printf
+#define printf printk
+#define bootverbose 1
+#define KASSERT(...)
+#endif /* __rtems__ */
+
+/*
+ * Check to see if we have what looks like a valid ELCR.  We do this by
+ * verifying that IRQs 0, 1, 2, and 13 are all edge triggered.
+ */
+int
+elcr_probe(void)
+{
+	int i;
+
+	elcr_status = inb(ELCR_PORT) | inb(ELCR_PORT + 1) << 8;
+	if ((elcr_status & (ELCR_MASK(0) | ELCR_MASK(1) | ELCR_MASK(2) |
+	    ELCR_MASK(8) | ELCR_MASK(13))) != 0)
+		return (ENXIO);
+	if (bootverbose) {
+		printf("ELCR Found.  ISA IRQs programmed as:\n");
+		for (i = 0; i < 16; i++)
+			printf(" %2d", i);
+		printf("\n");
+		for (i = 0; i < 16; i++)
+			if (elcr_status & ELCR_MASK(i))
+				printf("  L");
+			else
+				printf("  E");
+		printf("\n");
+	}
+#ifndef __rtems__
+	if (resource_disabled("elcr", 0))
+		return (ENXIO);
+#endif /* __rtems__ */
+	elcr_found = 1;
+	return (0);
+}
+
+/*
+ * Returns 1 for level trigger, 0 for edge.
+ */
+enum intr_trigger
+elcr_read_trigger(u_int irq)
+{
+#ifdef __rtems__
+	if (!elcr_found)
+		return INTR_TRIGGER_EDGE;
+#endif /* __rtems__ */
+	KASSERT(elcr_found, ("%s: no ELCR was found!", __func__));
+	KASSERT(irq <= 15, ("%s: invalid IRQ %u", __func__, irq));
+	if (elcr_status & ELCR_MASK(irq))
+		return (INTR_TRIGGER_LEVEL);
+	else
+		return (INTR_TRIGGER_EDGE);
+}
+
+/*
+ * Set the trigger mode for a specified IRQ.  Mode of 0 means edge triggered,
+ * and a mode of 1 means level triggered.
+ */
+void
+elcr_write_trigger(u_int irq, enum intr_trigger trigger)
+{
+	int new_status;
+
+#ifdef __rtems__
+	if (!elcr_found)
+		return;
+#endif /* __rtems__ */
+	KASSERT(elcr_found, ("%s: no ELCR was found!", __func__));
+	KASSERT(irq <= 15, ("%s: invalid IRQ %u", __func__, irq));
+	if (trigger == INTR_TRIGGER_LEVEL)
+		new_status = elcr_status | ELCR_MASK(irq);
+	else
+		new_status = elcr_status & ~ELCR_MASK(irq);
+	if (new_status == elcr_status)
+		return;
+	elcr_status = new_status;
+	if (irq >= 8)
+		outb(ELCR_PORT + 1, elcr_status >> 8);
+	else
+		outb(ELCR_PORT, elcr_status & 0xff);
+}
+
+void
+elcr_resume(void)
+{
+#ifdef __rtems__
+	if (!elcr_found)
+		return;
+#endif /* __rtems__ */
+
+	KASSERT(elcr_found, ("%s: no ELCR was found!", __func__));
+	outb(ELCR_PORT, elcr_status & 0xff);
+	outb(ELCR_PORT + 1, elcr_status >> 8);
+}
diff --git a/c/src/lib/libbsp/i386/shared/irq/elcr.h b/c/src/lib/libbsp/i386/shared/irq/elcr.h
new file mode 100644
index 0000000000..a006d4f149
--- /dev/null
+++ b/c/src/lib/libbsp/i386/shared/irq/elcr.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright 2016 Chris Johns <chrisj@rtems.org>
+ *
+ * Header for the FreeBSD ported elcr.c
+ */
+
+#ifndef _IRQ_ELCR_H_
+#define _IRQ_ELCR_H_
+
+#include <sys/cdefs.h>
+
+enum intr_trigger {
+  INTR_TRIGGER_EDGE,
+  INTR_TRIGGER_LEVEL
+};
+
+/*
+ * Check to see if we have what looks like a valid ELCR.  We do this by
+ * verifying that IRQs 0, 1, 2, and 13 are all edge triggered.
+ */
+int elcr_probe(void);
+
+/*
+ * Returns 1 for level trigger, 0 for edge.
+ */
+enum intr_trigger elcr_read_trigger(u_int irq);
+
+/*
+ * Set the trigger mode for a specified IRQ.  Mode of 0 means edge triggered,
+ * and a mode of 1 means level triggered.
+ */
+void elcr_write_trigger(u_int irq, enum intr_trigger trigger);
+
+void elcr_resume(void);
+
+
+#endif
diff --git a/c/src/lib/libbsp/i386/shared/irq/irq.c b/c/src/lib/libbsp/i386/shared/irq/irq.c
index 2bee7643a9..0511257fa1 100644
--- a/c/src/lib/libbsp/i386/shared/irq/irq.c
+++ b/c/src/lib/libbsp/i386/shared/irq/irq.c
@@ -19,6 +19,9 @@
 #include <stdio.h>
 #include <inttypes.h>
 
+
+#include "elcr.h"
+
 /*
  * pointer to the mask representing the additionnal irq vectors
  * that must be disabled when a particular entry is activated.
@@ -27,138 +30,174 @@
  * CAUTION : this table is accessed directly by interrupt routine
  * 	     prologue.
  */
-rtems_i8259_masks 	irq_mask_or_tbl[BSP_IRQ_LINES_NUMBER];
+static rtems_i8259_masks irq_mask_or_tbl[BSP_IRQ_LINES_NUMBER];
 
-uint32_t            irq_count[BSP_IRQ_LINES_NUMBER] = {0};
+/*
+ * Stats of interrupts dispatched.
+ */
+static uint32_t irq_count[BSP_IRQ_VECTOR_NUMBER] = {0};
+static uint32_t spurious_count;
 
-uint32_t
-BSP_irq_count_dump(FILE *f)
-{
-uint32_t tot = 0;
-int      i;
-	if ( !f )
-		f = stdout;
-	for ( i=0; i<BSP_IRQ_LINES_NUMBER; i++ ) {
-		tot += irq_count[i];
-		fprintf(f,"IRQ %2u: %9"PRIu32"\n", i, irq_count[i]);
-	}
-	return tot;
-}
+/*
+ * Edge or level trigger interrupts.
+ */
+static enum intr_trigger irq_trigger[BSP_IRQ_LINES_NUMBER];
 
 /*-------------------------------------------------------------------------+
-| Cache for 1st and 2nd PIC IRQ line's status (enabled or disabled) register.
+| Cache for 1st and 2nd PIC IRQ line's mssk (enabled or disabled) register.
 +--------------------------------------------------------------------------*/
 /*
  * lower byte is interrupt mask on the master PIC.
  * while upper bits are interrupt on the slave PIC.
  * This cache is initialized in ldseg.s
  */
-rtems_i8259_masks i8259s_cache = 0xFFFB;
-rtems_i8259_masks i8259s_super_imr = 0xFFFB;
+static rtems_i8259_masks i8259a_cache = 0xFFFB;
+
+/*
+ * Print the stats.
+ */
+uint32_t BSP_irq_count_dump(FILE *f)
+{
+  uint32_t tot = 0;
+  int      i;
+ if ( !f )
+   f = stdout;
+ fprintf(f,"SPURIOUS: %9"PRIu32"\n", spurious_count);
+ for ( i = 0; i < BSP_IRQ_VECTOR_NUMBER; i++ ) {
+   char type = '-';
+   if (i < BSP_IRQ_LINES_NUMBER)
+     type = irq_trigger[i] == INTR_TRIGGER_EDGE ? 'E' : 'L';
+   tot += irq_count[i];
+   fprintf(f,"IRQ %2u: %c %9"PRIu32"\n", i, type, irq_count[i]);
+ }
+ return tot;
+}
+
+/*
+ * Is the IRQ valid?
+ */
+static inline bool BSP_i8259a_irq_valid(const rtems_irq_number irqLine)
+{
+  return ((int)irqLine >= BSP_IRQ_VECTOR_LOWEST_OFFSET) &&
+    ((int)irqLine <= BSP_IRQ_MAX_ON_i8259A);
+}
+
+/*
+ * Read the IRR register. The default.
+ */
+static inline uint8_t BSP_i8259a_irq_int_request_reg(uint32_t ioport)
+{
+  uint8_t isr;
+  inport_byte(ioport, isr);
+  return isr;
+}
+
+/*
+ * Read the ISR register. Keep the default of the IRR.
+ */
+static inline uint8_t BSP_i8259a_irq_in_service_reg(uint32_t ioport)
+{
+  uint8_t isr;
+  outport_byte(ioport, PIC_OCW3_SEL | PIC_OCW3_RR | PIC_OCW3_RIS);
+  inport_byte(ioport, isr);
+  outport_byte(ioport, PIC_OCW3_SEL | PIC_OCW3_RR);
+  return isr;
+}
 
 /*-------------------------------------------------------------------------+
-|         Function:  BSP_irq_disable_at_i8259s
+|         Function:  BSP_irq_disable_at_i8259a
 |      Description: Mask IRQ line in appropriate PIC chip.
-| Global Variables: i8259s_cache
+| Global Variables: i8259a_cache
 |        Arguments: vector_offset - number of IRQ line to mask.
-|          Returns: Nothing.
+|          Returns: 0 is OK.
 +--------------------------------------------------------------------------*/
-int BSP_irq_disable_at_i8259s    (const rtems_irq_number irqLine)
+static int BSP_irq_disable_at_i8259a(const rtems_irq_number irqLine)
 {
   unsigned short        mask;
   rtems_interrupt_level level;
 
-  if ( ((int)irqLine < BSP_LOWEST_OFFSET) ||
-       ((int)irqLine > BSP_MAX_ON_i8259S )
-       )
-    return 1;
-
   rtems_interrupt_disable(level);
 
   mask = 1 << irqLine;
-  i8259s_cache |= mask;
-  i8259s_super_imr |= mask;
+  i8259a_cache |= mask;
 
   if (irqLine < 8)
   {
-    outport_byte(PIC_MASTER_IMR_IO_PORT, i8259s_cache & 0xff);
+    outport_byte(PIC_MASTER_IMR_IO_PORT, i8259a_cache & 0xff);
   }
   else
   {
-    outport_byte(PIC_SLAVE_IMR_IO_PORT, ((i8259s_cache & 0xff00) >> 8));
+    outport_byte(PIC_SLAVE_IMR_IO_PORT, (i8259a_cache >> 8) & 0xff);
   }
+
   rtems_interrupt_enable(level);
 
   return 0;
 }
 
 /*-------------------------------------------------------------------------+
-|         Function:  BSP_irq_enable_at_i8259s
+|         Function:  BSP_irq_enable_at_i8259a
 |      Description: Unmask IRQ line in appropriate PIC chip.
-| Global Variables: i8259s_cache
+| Global Variables: i8259a_cache
 |        Arguments: irqLine - number of IRQ line to mask.
 |          Returns: Nothing.
 +--------------------------------------------------------------------------*/
-int BSP_irq_enable_at_i8259s    (const rtems_irq_number irqLine)
+static int BSP_irq_enable_at_i8259a(const rtems_irq_number irqLine)
 {
   unsigned short        mask;
   rtems_interrupt_level level;
-
-  if ( ((int)irqLine < BSP_LOWEST_OFFSET) ||
-       ((int)irqLine > BSP_MAX_ON_i8259S )
-       )
-    return 1;
+  uint8_t               isr;
+  uint8_t               irr;
 
   rtems_interrupt_disable(level);
 
-  mask = ~(1 << irqLine);
-  i8259s_cache &= mask;
-  i8259s_super_imr &= mask;
+  mask = 1 << irqLine;
+  i8259a_cache &= ~mask;
 
   if (irqLine < 8)
   {
-    outport_byte(PIC_MASTER_IMR_IO_PORT, i8259s_cache & 0xff);
+    isr = BSP_i8259a_irq_in_service_reg(PIC_MASTER_COMMAND_IO_PORT);
+    irr = BSP_i8259a_irq_int_request_reg(PIC_MASTER_COMMAND_IO_PORT);
+    outport_byte(PIC_MASTER_IMR_IO_PORT, i8259a_cache & 0xff);
   }
   else
   {
-    outport_byte(PIC_SLAVE_IMR_IO_PORT, ((i8259s_cache & 0xff00) >> 8));
+    isr = BSP_i8259a_irq_in_service_reg(PIC_SLAVE_COMMAND_IO_PORT);
+    irr = BSP_i8259a_irq_int_request_reg(PIC_SLAVE_COMMAND_IO_PORT);
+    outport_byte(PIC_SLAVE_IMR_IO_PORT, (i8259a_cache >> 8) & 0xff);
   }
+
+  if (((isr ^ irr) & mask) != 0)
+    printk("i386: isr=%x irr=%x\n", isr, irr);
+
   rtems_interrupt_enable(level);
 
   return 0;
 } /* mask_irq */
 
-int BSP_irq_enabled_at_i8259s        	(const rtems_irq_number irqLine)
-{
-  unsigned short mask;
-
-  if ( ((int)irqLine < BSP_LOWEST_OFFSET) ||
-       ((int)irqLine > BSP_MAX_ON_i8259S )
-     )
-    return 1;
-
-  mask = (1 << irqLine);
-  return  (~(i8259s_cache & mask));
-}
-
 /*-------------------------------------------------------------------------+
-|         Function: BSP_irq_ack_at_i8259s
+|         Function: BSP_irq_ack_at_i8259a
 |      Description: Signal generic End Of Interrupt (EOI) to appropriate PIC.
 | Global Variables: None.
 |        Arguments: irqLine - number of IRQ line to acknowledge.
 |          Returns: Nothing.
 +--------------------------------------------------------------------------*/
-int BSP_irq_ack_at_i8259s  	(const rtems_irq_number irqLine)
+static int BSP_irq_ack_at_i8259a(const rtems_irq_number irqLine)
 {
-  if ( ((int)irqLine < BSP_LOWEST_OFFSET) ||
-       ((int)irqLine > BSP_MAX_ON_i8259S )
-       )
-    return 1;
+  uint8_t slave_isr = 0;
 
   if (irqLine >= 8) {
    outport_byte(PIC_SLAVE_COMMAND_IO_PORT, PIC_EOI);
+   slave_isr = BSP_i8259a_irq_in_service_reg(PIC_SLAVE_COMMAND_IO_PORT);
   }
-  outport_byte(PIC_MASTER_COMMAND_IO_PORT, PIC_EOI);
+
+  /*
+   * Only issue the EOI to the master if there are no more interrupts in
+   * service for the slave. i8259a data sheet page 18, The Special Fully Nested
+   * Mode, b.
+   */
+  if (slave_isr == 0)
+    outport_byte(PIC_MASTER_COMMAND_IO_PORT, PIC_EOI);
 
   return 0;
 
@@ -179,7 +218,7 @@ static rtems_irq_prio irqPrioTable[BSP_IRQ_LINES_NUMBER]={
    */
   0,0,
   255,
-  0, 0, 0, 0,  0,  0,  0,  0,  0,  0,  0,  0,  0
+  0, 0, 0, 0,  0,  0,  0,  0,  0,  0,  0,  0
 };
 
 static void compute_i8259_masks_from_prio (void)
@@ -208,22 +247,29 @@ static void compute_i8259_masks_from_prio (void)
   rtems_interrupt_enable(level);
 }
 
-rtems_status_code bsp_interrupt_vector_enable(rtems_vector_number vector)
+static inline bool bsp_interrupt_vector_is_valid(rtems_vector_number vector)
 {
-  BSP_irq_enable_at_i8259s(vector);
+  return BSP_i8259a_irq_valid((const rtems_irq_number) vector);
+}
 
+rtems_status_code bsp_interrupt_vector_enable(rtems_vector_number vector)
+{
+  if (bsp_interrupt_vector_is_valid(vector))
+    BSP_irq_enable_at_i8259a(vector);
   return RTEMS_SUCCESSFUL;
 }
 
 rtems_status_code bsp_interrupt_vector_disable(rtems_vector_number vector)
 {
-  BSP_irq_disable_at_i8259s(vector);
-
+  if (bsp_interrupt_vector_is_valid(vector))
+    BSP_irq_disable_at_i8259a(vector);
   return RTEMS_SUCCESSFUL;
 }
 
 rtems_status_code bsp_interrupt_facility_initialize(void)
 {
+  int i;
+
   /*
    * set up internal tables used by rtems interrupt prologue
    */
@@ -232,13 +278,99 @@ rtems_status_code bsp_interrupt_facility_initialize(void)
   /*
    * must enable slave pic anyway
    */
-  BSP_irq_enable_at_i8259s(2);
+  BSP_irq_enable_at_i8259a(2);
+
+  /*
+   * Probe the ELCR.
+   */
+  elcr_probe();
+
+  for (i = 0; i < BSP_IRQ_LINES_NUMBER; i++)
+    irq_trigger[i] = elcr_read_trigger(i);
 
   return RTEMS_SUCCESSFUL;
 }
 
-void C_dispatch_isr(int vector)
+/*
+ * Global so the asm handler can call it.
+ */
+void BSP_dispatch_isr(int vector);
+
+void BSP_dispatch_isr(int vector)
 {
-  irq_count[vector]++;
-  bsp_interrupt_handler_dispatch(vector);
+  uint16_t old_imr = 0;
+
+  if (vector < BSP_IRQ_VECTOR_NUMBER) {
+    /*
+     * Hardware?
+     */
+    if (vector <= BSP_IRQ_MAX_ON_i8259A) {
+      /*
+       * See if this is a spurious interrupt.
+       */
+      if ((vector == 7 || vector == 15)) {
+        /*
+         * Only check it there no handler for 7 or 15.
+         */
+        if (bsp_interrupt_handler_is_empty(vector)) {
+          /*
+           * Read the ISR register to see if IRQ 7/15 is really pending.
+           */
+          uint8_t isr = BSP_i8259a_irq_in_service_reg(PIC_MASTER_COMMAND_IO_PORT);
+          if ((isr & (1 << 7)) == 0) {
+            ++spurious_count;
+            return;
+          }
+        }
+      }
+
+      /*
+       * Save the current cached value for the IMR. It will have the bit for this
+       * vector clear.
+       */
+      if (vector <= BSP_IRQ_MAX_ON_i8259A) {
+        old_imr = i8259a_cache;
+        i8259a_cache |= irq_mask_or_tbl[vector];
+        outport_byte(PIC_MASTER_IMR_IO_PORT, i8259a_cache & 0xff);
+        outport_byte(PIC_SLAVE_IMR_IO_PORT, (i8259a_cache >> 8) & 0xff);
+      }
+
+      /*
+       * Do not use auto-EOI as some slave PIC do not work correctly.
+       */
+      BSP_irq_ack_at_i8259a(vector);
+    }
+
+    /*
+     * Count the interrupt.
+     */
+    irq_count[vector]++;
+
+    /*
+     * Allow nesting.
+     */
+    __asm__ __volatile__("sti");
+
+    bsp_interrupt_handler_dispatch(vector);
+
+    /*
+     * Disallow nesting.
+     */
+    __asm__ __volatile__("cli");
+
+    if (vector <= BSP_IRQ_MAX_ON_i8259A) {
+      /*
+       * Put the mask back but keep this vector masked if the trigger type is
+       * level. The driver or a thread level interrupt server needs to enable it
+       * again.
+       */
+      if (vector <= BSP_IRQ_MAX_ON_i8259A) {
+        if (irq_trigger[vector] == INTR_TRIGGER_LEVEL)
+          old_imr |= 1 << vector;
+        i8259a_cache = old_imr;
+        outport_byte(PIC_MASTER_IMR_IO_PORT, i8259a_cache & 0xff);
+        outport_byte(PIC_SLAVE_IMR_IO_PORT, (i8259a_cache >> 8) & 0xff);
+      }
+    }
+  }
 }
diff --git a/c/src/lib/libbsp/i386/shared/irq/irq.h b/c/src/lib/libbsp/i386/shared/irq/irq.h
index 095af423cf..953774e0ab 100644
--- a/c/src/lib/libbsp/i386/shared/irq/irq.h
+++ b/c/src/lib/libbsp/i386/shared/irq/irq.h
@@ -50,30 +50,37 @@ extern "C" {
 | Constants
 +--------------------------------------------------------------------------*/
 
-    /** @brief Base vector for our IRQ handlers. */
+/** @brief Base vector for our IRQ handlers. */
 #define BSP_IRQ_VECTOR_BASE		BSP_ASM_IRQ_VECTOR_BASE
-#define BSP_IRQ_LINES_NUMBER    	17
-#define BSP_LOWEST_OFFSET		0
-#define BSP_MAX_ON_i8259S		(BSP_IRQ_LINES_NUMBER - 2)
-#define BSP_MAX_OFFSET		 	(BSP_IRQ_LINES_NUMBER - 1)
-    /** @brief
-     * Interrupt offset in comparison to BSP_ASM_IRQ_VECTOR_BASE
-     * NB : 1) Interrupt vector number in IDT = offset + BSP_ASM_IRQ_VECTOR_BASE
-     * 	    2) The same name should be defined on all architecture
-     *	       so that handler connection can be unchanged.
-     */
-#define BSP_PERIODIC_TIMER      0
-#define BSP_KEYBOARD          	1
-#define BSP_UART_COM2_IRQ	3
-#define BSP_UART_COM1_IRQ	4
+#define BSP_IRQ_LINES_NUMBER    	16
+#define BSP_IRQ_MAX_ON_i8259A		(BSP_IRQ_LINES_NUMBER - 1)
+
+/*
+ * Define the number of valid vectors. This is different to the number of IRQ
+ * signals supported. Use this value to allocation vector data or range check.
+ */
+#define BSP_IRQ_VECTOR_NUMBER        17
+#define BSP_IRQ_VECTOR_LOWEST_OFFSET 0
+#define BSP_IRQ_VECTOR_MAX_OFFSET    (BSP_IRQ_VECTOR_NUMBER - 1)
+
+/** @brief
+ * Interrupt offset in comparison to BSP_ASM_IRQ_VECTOR_BASE
+ * NB : 1) Interrupt vector number in IDT = offset + BSP_ASM_IRQ_VECTOR_BASE
+ * 	    2) The same name should be defined on all architecture
+ *	       so that handler connection can be unchanged.
+ */
+#define BSP_PERIODIC_TIMER      0 /* fixed on all builds of PC */
+#define BSP_KEYBOARD          	1 /* fixed on all builds of PC */
+#define BSP_UART_COM2_IRQ	3 /* fixed for ISA bus */
+#define BSP_UART_COM1_IRQ	4 /* fixed for ISA bus */
 #define BSP_UART_COM3_IRQ	5
 #define BSP_UART_COM4_IRQ	6
 #define BSP_RT_TIMER1	      	8
 #define BSP_RT_TIMER3		10
-#define BSP_SMP_IPI   		16
+#define BSP_SMP_IPI   		16 /* not part of the ATPIC */
 
-#define BSP_INTERRUPT_VECTOR_MIN BSP_LOWEST_OFFSET
-#define BSP_INTERRUPT_VECTOR_MAX BSP_MAX_OFFSET
+#define BSP_INTERRUPT_VECTOR_MIN BSP_IRQ_VECTOR_LOWEST_OFFSET
+#define BSP_INTERRUPT_VECTOR_MAX BSP_IRQ_VECTOR_MAX_OFFSET
 
 /** @brief
  * Type definition for RTEMS managed interrupts
@@ -83,7 +90,7 @@ typedef unsigned short rtems_i8259_masks;
 /**
  * @brief Contains the current IMR of both i8259s.
  */
-extern rtems_i8259_masks i8259s_cache;
+//extern rtems_i8259_masks i8259s_cache;
 
 /**
  * @brief Contains the super IMR of both i8259s to overrule i8259s_cache during
@@ -92,7 +99,7 @@ extern rtems_i8259_masks i8259s_cache;
  * This enables a bsp_interrupt_vector_disable() in interrupt handlers.  This
  * is required for the interrupt server support used by the new network stack.
  */
-extern rtems_i8259_masks i8259s_super_imr;
+//extern rtems_i8259_masks i8259s_super_imr;
 
 /*-------------------------------------------------------------------------+
 | Function Prototypes.
@@ -106,13 +113,13 @@ extern rtems_i8259_masks i8259s_super_imr;
  * this function, even if the device asserts the interrupt line it will
  * not be propagated further to the processor
  */
-int BSP_irq_disable_at_i8259s        (const rtems_irq_number irqLine);
+//int BSP_irq_disable_at_i8259s        (const rtems_irq_number irqLine);
 /** @brief
  * function to enable a particular irq at 8259 level. After calling
  * this function, if the device asserts the interrupt line it will
  * be propagated further to the processor
  */
-int BSP_irq_enable_at_i8259s		(const rtems_irq_number irqLine);
+//int BSP_irq_enable_at_i8259s		(const rtems_irq_number irqLine);
 /** @brief
  * function to acknoledge a particular irq at 8259 level. After calling
  * this function, if a device asserts an enabled interrupt line it will
@@ -120,11 +127,11 @@ int BSP_irq_enable_at_i8259s		(const rtems_irq_number irqLine);
  * writting raw handlers as this is automagically done for rtems managed
  * handlers.
  */
-int BSP_irq_ack_at_i8259s           	(const rtems_irq_number irqLine);
+//int BSP_irq_ack_at_i8259s           	(const rtems_irq_number irqLine);
 /** @brief
  * function to check if a particular irq is enabled at 8259 level. After calling
  */
-int BSP_irq_enabled_at_i8259s        	(const rtems_irq_number irqLine);
+//int BSP_irq_enabled_at_i8259s        	(const rtems_irq_number irqLine);
 
 /** @} */
 
diff --git a/c/src/lib/libbsp/i386/shared/irq/irq_asm.S b/c/src/lib/libbsp/i386/shared/irq/irq_asm.S
index d0ed9b626c..181293351e 100644
--- a/c/src/lib/libbsp/i386/shared/irq/irq_asm.S
+++ b/c/src/lib/libbsp/i386/shared/irq/irq_asm.S
@@ -16,7 +16,6 @@
 #include <rtems/asm.h>
 #include <rtems/system.h>
 #include <bspopts.h>
-#include <bsp/irq_asm.h>
 #include <rtems/score/cpu.h>
 #include <rtems/score/percpu.h>
 
@@ -28,7 +27,7 @@
 
 /* Stack frame we use for intermediate storage               */
 #define ARG_OFF	0
-#define MSK_OFF 4
+#define MSK_OFF 4        /* not used any more                */
 #define EBX_OFF 8        /* ebx                              */
 #define EBP_OFF 12       /* code restoring ebp/esp relies on */
 #define ESP_OFF 16       /* esp being on top of ebp!         */
@@ -77,7 +76,7 @@ SYM (_ISR_Handler):
          *   saved ebx
 	 *   saved ebp
 	 *   saved irq mask
-	 *   vector arg to C_dispatch_isr   <- aligned SP
+	 *   vector arg to BSP_dispatch_isr   <- aligned SP
 	 */
 	movl      esp, eax
 	subl      $FRM_SIZ, esp
@@ -86,6 +85,13 @@ SYM (_ISR_Handler):
 	movl      eax, ESP_OFF(esp)
 	movl      ebp, EBP_OFF(esp)
 
+	/*
+	 * GCC versions starting with 4.3 no longer place the cld
+	 * instruction before string operations.  We  need to ensure
+	 * it is set correctly for ISR handlers.
+	 */
+	cld
+
 #ifdef __SSE__
 	/* NOTE: SSE only is supported if the BSP enables fxsave/fxrstor
 	 * to save/restore SSE context! This is so far only implemented
@@ -105,39 +111,6 @@ SYM (_ISR_Handler):
 	ldmxcsr ARG_OFF(esp)            /* clean-slate MXCSR              */
 #endif
 
-        /* Do not disable any 8259 interrupts if this isn't from one */
-	cmp       ecx, 16               /* is this a PIC IRQ? */
-	jge       .check_stack_switch
-
-	/*
-	 * acknowledge the interrupt
-	 */
-	movw      SYM (i8259s_cache), ax /* save current i8259 interrupt mask */
-	movl      eax, MSK_OFF(esp)      /* save in stack frame */
-
-	/*
-	 * compute the new PIC mask:
-	 *
-	 * <new mask> = <old mask> | irq_mask_or_tbl[<intr number aka ecx>]
-	 */
-	movw      SYM (irq_mask_or_tbl) (,ecx,2), dx
-	orw       dx, ax
-	/*
-	 * Install new computed value on the i8259 and update cache
-	 * accordingly
-	 */
-	movw      ax, SYM (i8259s_cache)
-	outb      $PIC_MASTER_IMR_IO_PORT
-	movb      ah, al
-	outb      $PIC_SLAVE_IMR_IO_PORT
-
-	movb      $PIC_EOI, al
-	cmpl      $7, ecx
-	jbe      .master
-	outb      $PIC_SLAVE_COMMAND_IO_PORT
-.master:
-	outb      $PIC_MASTER_COMMAND_IO_PORT
-
 	/*
 	 *  Now switch stacks if necessary
 	 */
@@ -172,19 +145,6 @@ nested:
 	incl      PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(ebx) /* disable
 	                                                        multitasking */
 	/*
-	 * GCC versions starting with 4.3 no longer place the cld
-	 * instruction before string operations.  We  need to ensure
-	 * it is set correctly for ISR handlers.
-	 */
-	cld
-
-	/*
-	 * re-enable interrupts at processor level as the current
-	 * interrupt source is now masked via i8259
-	 */
-	sti
-
-	/*
 	 *  ECX is preloaded with the vector number; store as arg
 	 *  on top of stack. Note that _CPU_Interrupt_stack_high
 	 *  was adjusted in _CPU_Interrupt_stack_setup() (score/rtems/cpu.h)
@@ -192,12 +152,7 @@ nested:
 	 */
 
 	movl      ecx, ARG_OFF(esp)  /* store vector arg in stack */
-	call      C_dispatch_isr
-
-	/*
-	 * disable interrupts_again
-	 */
-	cli
+	call      BSP_dispatch_isr
 
 	movl      ARG_OFF(esp), ecx     /* grab vector arg from stack */
 
@@ -207,22 +162,6 @@ nested:
 	 */
 	movl      ebp, esp
 
-	/*
-	 * restore the original i8259 masks
-	 */
-        /* Do not touch 8259 interrupts if this isn't from one */
-	cmp       ecx, 16               /* is this a PIC IRQ? */
-	jge       .dont_restore_i8259
-
-	movw      SYM (i8259s_super_imr), dx
-	movl      MSK_OFF(esp), eax
-	orw       dx, ax
-	movw      ax, SYM (i8259s_cache)
-	outb	  $PIC_MASTER_IMR_IO_PORT
-	movb	  ah, al
-	outb	  $PIC_SLAVE_IMR_IO_PORT
-
-.dont_restore_i8259:
 	decl      PER_CPU_ISR_NEST_LEVEL(ebx)  /* one less ISR nest level */
 	                                    /* If interrupts are nested, */
 	                                    /*   then dispatching is disabled */
diff --git a/c/src/lib/libbsp/i386/shared/irq/irq_asm.h b/c/src/lib/libbsp/i386/shared/irq/irq_asm.h
index 31575adb94..05cb4e6cc3 100644
--- a/c/src/lib/libbsp/i386/shared/irq/irq_asm.h
+++ b/c/src/lib/libbsp/i386/shared/irq/irq_asm.h
@@ -12,12 +12,14 @@
  *
  *  COPYRIGHT (c) 1998 valette@crf.canon.fr
  *
+ *  Copyright (c) 2016 Chris Johns <chrisj@rtems.org>
+ *
  *  The license and distribution terms for this file may be
  *  found in the file LICENSE in this distribution or at
  *  http://www.rtems.org/license/LICENSE.
  */
-#ifndef __IRQ_ASM_H__
-#define __IRQ_ASM_H__
+#ifndef __I8259S_H__
+#define __I8259S_H__
 
 #define BSP_ASM_IRQ_VECTOR_BASE 0x20
     /** @brief PIC's command and mask registers */
@@ -30,4 +32,14 @@
 #define PIC_EOSI	0x60	///< End of Specific Interrupt (EOSI)
 #define PIC_EOI		0x20	///< Generic End of Interrupt (EOI)
 
+/* Operation control word type 3.  Bit 3 (0x08) must be set. Even address. */
+#define PIC_OCW3_RIS        0x01            /* 1 = read IS, 0 = read IR */
+#define PIC_OCW3_RR         0x02            /* register read */
+#define PIC_OCW3_P          0x04            /* poll mode command */
+/* 0x08 must be 1 to select OCW3 vs OCW2 */
+#define PIC_OCW3_SEL        0x08            /* must be 1 */
+/* 0x10 must be 0 to select OCW3 vs ICW1 */
+#define PIC_OCW3_SMM        0x20            /* special mode mask */
+#define PIC_OCW3_ESMM       0x40            /* enable SMM */
+
 #endif
diff --git a/c/src/lib/libbsp/i386/shared/irq/irq_init.c b/c/src/lib/libbsp/i386/shared/irq/irq_init.c
index fe8d7e7f9f..c401f29b71 100644
--- a/c/src/lib/libbsp/i386/shared/irq/irq_init.c
+++ b/c/src/lib/libbsp/i386/shared/irq/irq_init.c
@@ -66,7 +66,7 @@ static int raw_not_connected(
 
 static rtems_raw_irq_connect_data 	idtHdl[IDT_SIZE];
 
-static rtems_raw_irq_hdl rtemsIrq[BSP_IRQ_LINES_NUMBER] = {
+static rtems_raw_irq_hdl rtemsIrq[BSP_IRQ_VECTOR_NUMBER] = {
   rtems_irq_prologue_0,
   rtems_irq_prologue_1,
   rtems_irq_prologue_2,
@@ -149,7 +149,7 @@ void  rtems_irq_mngt_init(void)
      * Patch the entry that will be used by RTEMS for interrupt management
      * with RTEMS prologue.
      */
-    for (i = 0; i < BSP_IRQ_LINES_NUMBER; i++) {
+    for (i = 0; i < BSP_IRQ_VECTOR_NUMBER; i++) {
       create_interrupt_gate_descriptor(&idtEntry, rtemsIrq[i]);
       idt_entry_tbl[i + BSP_ASM_IRQ_VECTOR_BASE] = idtEntry;
     }