Merge pull request #210 from ozbenh/xics

xics: Cleanups and add a simple ICS for use by Linux

common.vhdl

@@ -78,6 +78,16 @@ package common is
 
     type irq_state_t is (WRITE_SRR0, WRITE_SRR1);
 
+    -- For now, fixed 16 sources, make this either a parametric
+    -- package of some sort or an unconstrainted array.
+    type ics_to_icp_t is record
+        -- Level interrupts only, ICS just keeps prsenting the
+        -- highest priority interrupt. Once handling edge, something
+        -- smarter involving handshake & reject support will be needed
+        src : std_ulogic_vector(3 downto 0);
+        pri : std_ulogic_vector(7 downto 0);
+    end record;
+
     -- This needs to die...
     type ctrl_t is record
 	tb: std_ulogic_vector(63 downto 0);

include/microwatt_soc.h

@@ -11,7 +11,8 @@
 
 #define SYSCON_BASE	0xc0000000  /* System control regs */
 #define UART_BASE	0xc0002000  /* UART */
-#define XICS_BASE   	0xc0004000  /* Interrupt controller */
+#define XICS_ICP_BASE   0xc0004000  /* Interrupt controller */
+#define XICS_ICS_BASE   0xc0005000  /* Interrupt controller */
 #define SPI_FCTRL_BASE  0xc0006000  /* SPI flash controller registers */
 #define DRAM_CTRL_BASE	0xc8000000  /* LiteDRAM control registers */
 #define SPI_FLASH_BASE  0xf0000000  /* SPI Flash memory map */

soc.vhdl

@@ -21,6 +21,7 @@ use work.wishbone_types.all;
 -- 0xc0000000: SYSCON
 -- 0xc0002000: UART0
 -- 0xc0004000: XICS ICP
+-- 0xc0005000: XICS ICS
 -- 0xc0006000: SPI Flash controller
 -- 0xc8nnnnnn: External IO bus
 -- 0xf0000000: Flash "ROM" mapping
@@ -130,11 +131,13 @@ architecture behaviour of soc is
     signal wb_spiflash_is_reg : std_ulogic;
     signal wb_spiflash_is_map : std_ulogic;
 
-    -- XICS0 signals:
+    -- XICS signals:
-    signal wb_xics0_in   : wb_io_master_out;
+    signal wb_xics_icp_in   : wb_io_master_out;
-    signal wb_xics0_out  : wb_io_slave_out;
+    signal wb_xics_icp_out  : wb_io_slave_out;
+    signal wb_xics_ics_in   : wb_io_master_out;
+    signal wb_xics_ics_out  : wb_io_slave_out;
     signal int_level_in     : std_ulogic_vector(15 downto 0);
-
+    signal ics_to_icp       : ics_to_icp_t;
     signal core_ext_irq     : std_ulogic;
 
     -- Main memory signals:
@@ -171,7 +174,8 @@ architecture behaviour of soc is
     -- IO branch split:
     type slave_io_type is (SLAVE_IO_SYSCON,
                            SLAVE_IO_UART,
-                           SLAVE_IO_ICP_0,
+                           SLAVE_IO_ICP,
+                           SLAVE_IO_ICS,
                            SLAVE_IO_SPI_FLASH_REG,
                            SLAVE_IO_SPI_FLASH_MAP,
                            SLAVE_IO_EXTERNAL,
@@ -441,7 +445,8 @@ begin
     -- IO wishbone slave intercon.
     --
     slave_io_intercon: process(wb_sio_out, wb_syscon_out, wb_uart0_out,
-                               wb_ext_io_out, wb_xics0_out, wb_spiflash_out)
+                               wb_ext_io_out, wb_xics_icp_out, wb_xics_ics_out,
+                               wb_spiflash_out)
 	variable slave_io : slave_io_type;
 
         variable match : std_ulogic_vector(31 downto 12);
@@ -462,7 +467,9 @@ begin
 	elsif std_match(match, x"C8---") then
 	    slave_io := SLAVE_IO_EXTERNAL;
 	elsif std_match(match, x"C0004") then
-	    slave_io := SLAVE_IO_ICP_0;
+	    slave_io := SLAVE_IO_ICP;
+	elsif std_match(match, x"C0005") then
+	    slave_io := SLAVE_IO_ICS;
 	elsif std_match(match, x"C0006") then
 	    slave_io := SLAVE_IO_SPI_FLASH_REG;
 	end if;
@@ -474,11 +481,15 @@ begin
         wb_spiflash_is_reg <= '0';
         wb_spiflash_is_map <= '0';
 
-	 -- Only give xics 8 bits of wb addr
+	 -- Only give xics 8 bits of wb addr (for now...)
-	wb_xics0_in <= wb_sio_out;
+	wb_xics_icp_in <= wb_sio_out;
-	wb_xics0_in.adr <= (others => '0');
+	wb_xics_icp_in.adr <= (others => '0');
-	wb_xics0_in.adr(7 downto 0) <= wb_sio_out.adr(7 downto 0);
+	wb_xics_icp_in.adr(7 downto 0) <= wb_sio_out.adr(7 downto 0);
-	wb_xics0_in.cyc  <= '0';
+	wb_xics_icp_in.cyc  <= '0';
+	wb_xics_ics_in <= wb_sio_out;
+	wb_xics_ics_in.adr <= (others => '0');
+	wb_xics_ics_in.adr(11 downto 0) <= wb_sio_out.adr(11 downto 0);
+	wb_xics_ics_in.cyc  <= '0';
 
 	wb_ext_io_in <= wb_sio_out;
 	wb_ext_io_in.cyc <= '0';
@@ -521,9 +532,12 @@ begin
 	when SLAVE_IO_UART =>
 	    wb_uart0_in.cyc <= wb_sio_out.cyc;
 	    wb_sio_in <= wb_uart0_out;
-	when SLAVE_IO_ICP_0 =>
+	when SLAVE_IO_ICP =>
-	    wb_xics0_in.cyc <= wb_sio_out.cyc;
+	    wb_xics_icp_in.cyc <= wb_sio_out.cyc;
-	    wb_sio_in <= wb_xics0_out;
+	    wb_sio_in <= wb_xics_icp_out;
+	when SLAVE_IO_ICS =>
+	    wb_xics_ics_in.cyc <= wb_sio_out.cyc;
+	    wb_sio_in <= wb_xics_ics_out;
 	when SLAVE_IO_SPI_FLASH_MAP =>
             -- Clear top bits so they don't make their way to the
             -- fash chip.
@@ -614,17 +628,28 @@ begin
         wb_spiflash_out.stall <= wb_spiflash_in.cyc and not wb_spiflash_out.ack;
     end generate;
 
-    xics0: entity work.xics
+    xics_icp: entity work.xics_icp
+	port map(
+	    clk => system_clk,
+	    rst => rst_xics,
+	    wb_in => wb_xics_icp_in,
+	    wb_out => wb_xics_icp_out,
+            ics_in => ics_to_icp,
+	    core_irq_out => core_ext_irq
+	    );
+
+    xics_ics: entity work.xics_ics
 	generic map(
-	    LEVEL_NUM => 16
+	    SRC_NUM   => 16,
+	    PRIO_BITS => 3
 	    )
 	port map(
 	    clk => system_clk,
 	    rst => rst_xics,
-	    wb_in => wb_xics0_in,
+	    wb_in => wb_xics_ics_in,
-	    wb_out => wb_xics0_out,
+	    wb_out => wb_xics_ics_out,
 	    int_level_in => int_level_in,
-	    core_irq_out => core_ext_irq
+            icp_out => ics_to_icp
 	    );
 
     -- Assign external interrupts

tests/Makefile.test

@@ -9,7 +9,7 @@ CC = $(CROSS_COMPILE)gcc
 LD = $(CROSS_COMPILE)ld
 OBJCOPY = $(CROSS_COMPILE)objcopy
 
-CFLAGS = -Os -g -Wall -std=c99 -msoft-float -mno-string -mno-multiple -mno-vsx -mno-altivec -mlittle-endian -fno-stack-protector -mstrict-align -ffreestanding -fdata-sections -ffunction-sections -I ../../include
+CFLAGS = -Os -g -Wall -std=c99 -nostdinc -msoft-float -mno-string -mno-multiple -mno-vsx -mno-altivec -mlittle-endian -fno-stack-protector -mstrict-align -ffreestanding -fdata-sections -ffunction-sections -I ../../include -isystem $(shell $(CC) -print-file-name=include)
 ASFLAGS = $(CFLAGS)
 LDFLAGS = -T powerpc.lds
 

tests/test_xics.console_out

@@ -1,3 +1,4 @@
 Test 0:PASS
 Test 1:PASS
 Test 2:PASS
+Test 3:PASS

tests/xics/xics.c

@@ -1,7 +1,6 @@
 #include <stddef.h>
 #include <stdint.h>
 #include <stdbool.h>
-#include <limits.h>
 
 #include "console.h"
 #include "xics.h"
@@ -13,8 +12,8 @@ void delay(void)
 {
 	static volatile int i;
 
-	for (i = 0; i < 10; ++i)
+	for (i = 0; i < 16; ++i)
-		;
+		__asm__ volatile("" : : : "memory");
 }
 
 void print_number(unsigned int i) // only for i = 0-999
@@ -75,6 +74,8 @@ void ipi_isr(void) {
 	debug_puts(IPI);
 
 	isrs_run |= ISR_IPI;
+
+	icp_write8(XICS_MFRR, 0xff);
 }
 
 
@@ -110,6 +111,8 @@ struct isr_op isr_table[] = {
 bool ipi_running;
 
 #define ISR "ISR XISR="
+#define PRIO " PRIO="
+#define CPPR " CPPR="
 void isr(void)
 {
 	struct isr_op *op;
@@ -118,11 +121,15 @@ void isr(void)
 	assert(!ipi_running); // check we aren't reentrant
 	ipi_running = true;
 
-	xirr = xics_read32(XICS_XIRR); // read hardware irq source
+	xirr = icp_read32(XICS_XIRR); // read hardware irq source
 
 #ifdef DEBUG
 	puts(ISR);
 	print_number(xirr & 0xff);
+	puts(PRIO);
+	print_number(xirr >> 24);
+	puts(CPPR);
+	print_number(icp_read8(XICS_XIRR_POLL));
 	puts("\n");
 #endif
 
@@ -136,7 +143,7 @@ void isr(void)
 		op++;
 	}
 
-	xics_write32(XICS_XIRR, xirr); // EOI
+	icp_write32(XICS_XIRR, xirr); // EOI
 
 	ipi_running = false;
 }
@@ -144,45 +151,91 @@ void isr(void)
 /*****************************************/
 
 int xics_test_0(void)
+{
+	uint32_t v0, v1;
+
+	v0 = ics_read_xive(0);
+	v1 = ics_read_xive(1);
+#ifdef DEBUG
+	puts("\n");
+	puts("xive(0) bfr: ");
+	print_number(v0);
+	puts("\n");
+	puts("xive(1) bfr: ");
+	print_number(v1);
+	puts("\n");
+#endif
+	assert(v0 = 0xff);
+	assert(v1 = 0xff);
+
+	ics_write_xive(0xa, 0);
+	ics_write_xive(0x5, 1);
+	v0 = ics_read_xive(0);
+	v1 = ics_read_xive(1);
+#ifdef DEBUG
+	puts("\n");
+	puts("xive(0) aft: ");
+	print_number(v0);
+	puts("\n");
+	puts("xive(1) aft: ");
+	print_number(v1);
+	puts("\n");
+#endif
+	assert(v0 = 0xa);
+	assert(v1 = 0x5);
+
+	ics_write_xive(0xff, 0);
+	ics_write_xive(0xff, 1);
+	v0 = ics_read_xive(0);
+	v1 = ics_read_xive(1);
+	assert(v0 = 0xff);
+	assert(v1 = 0xff);
+	return 0;
+}
+
+int xics_test_1(void)
 {
 	// setup
-	xics_write8(XICS_XIRR, 0x00); // mask all interrupts
+	icp_write8(XICS_XIRR, 0x00); // mask all interrupts
 	isrs_run = 0;
 
-	xics_write8(XICS_XIRR, 0x00); // mask all interrupts
+	icp_write8(XICS_XIRR, 0x00); // mask all interrupts
 
 	// trigger two interrupts
-	potato_uart_irq_en(); // cause 0x500 interrupt
+	potato_uart_irq_en();        // cause serial interrupt
-	xics_write8(XICS_MFRR, 0x05); // cause 0x500 interrupt
+	ics_write_xive(0x6, 0);      // set source to prio 6
+	icp_write8(XICS_MFRR, 0x04); // cause ipi interrupt at prio 5
 
 	// still masked, so shouldn't happen yet
 	delay();
 	assert(isrs_run == 0);
 
 	// unmask IPI only
-	xics_write8(XICS_XIRR, 0x40);
+	icp_write8(XICS_XIRR, 0x6);
 	delay();
 	assert(isrs_run == ISR_IPI);
 
 	// unmask UART
-	xics_write8(XICS_XIRR, 0xc0);
+	icp_write8(XICS_XIRR, 0x7);
 	delay();
 	assert(isrs_run == (ISR_IPI | ISR_UART));
 
 	// cleanup
-	xics_write8(XICS_XIRR, 0x00); // mask all interrupts
+	icp_write8(XICS_XIRR, 0x00); // mask all interrupts
+	potato_uart_irq_dis();
+	ics_write_xive(0, 0xff);
 	isrs_run = 0;
 
 	return 0;
 }
 
-int xics_test_1(void)
+int xics_test_2(void)
 {
 	// setup
-	xics_write8(XICS_XIRR, 0x00); // mask all interrupts
+	icp_write8(XICS_XIRR, 0x00); // mask all interrupts
 	isrs_run = 0;
 
-	xics_write8(XICS_XIRR, 0xff); // allow all interrupts
+	icp_write8(XICS_XIRR, 0xff); // allow all interrupts
 
 	// should be none pending
 	delay();
@@ -190,13 +243,14 @@ int xics_test_1(void)
 
 	// trigger both
 	potato_uart_irq_en(); // cause 0x500 interrupt
-	xics_write8(XICS_MFRR, 0x05); // cause 0x500 interrupt
+	icp_write8(XICS_MFRR, 0x05); // cause 0x500 interrupt
 
 	delay();
 	assert(isrs_run == (ISR_IPI | ISR_UART));
 
 	// cleanup
-	xics_write8(XICS_XIRR, 0x00); // mask all interrupts
+	icp_write8(XICS_XIRR, 0x00); // mask all interrupts
+	potato_uart_irq_dis();
 	isrs_run = 0;
 
 	return 0;
@@ -207,19 +261,19 @@ void mtmsrd(uint64_t val)
 	__asm__ volatile("mtmsrd %0" : : "r" (val));
 }
 
-int xics_test_2(void)
+int xics_test_3(void)
 {
 	// setup
-	xics_write8(XICS_XIRR, 0x00); // mask all interrupts
+	icp_write8(XICS_XIRR, 0x00); // mask all interrupts
 	isrs_run = 0;
 
 	// trigger interrupts with MSR[EE]=0 and show they are not run
 	mtmsrd(0x9000000000000003); // EE off
 
-	xics_write8(XICS_XIRR, 0xff); // allow all interrupts
+	icp_write8(XICS_XIRR, 0xff); // allow all interrupts
 
 	// trigger an IPI
-	xics_write8(XICS_MFRR, 0x05); // cause 0x500 interrupt
+	icp_write8(XICS_MFRR, 0x05); // cause 0x500 interrupt
 
 	delay();
 	assert(isrs_run == 0);
@@ -229,7 +283,7 @@ int xics_test_2(void)
 	assert(isrs_run == ISR_IPI);
 
 	// cleanup
-	xics_write8(XICS_XIRR, 0x00); // mask all interrupts
+	icp_write8(XICS_XIRR, 0x00); // mask all interrupts
 	isrs_run = 0;
 
 	return 0;
@@ -243,6 +297,7 @@ int (*tests[])(void) = {
 	xics_test_0,
 	xics_test_1,
 	xics_test_2,
+	xics_test_3,
 	NULL
 };
 

tests/xics/xics.h

@@ -1,5 +1,4 @@
 #include <stdint.h>
-
 #include "microwatt_soc.h"
 #include "io.h"
 
@@ -8,22 +7,35 @@
 #define XICS_RESV      0x8
 #define XICS_MFRR      0xC
 
-uint8_t xics_read8(int offset)
+#define bswap32(x) (uint32_t)__builtin_bswap32((uint32_t)(x))
+
+uint8_t icp_read8(int offset)
+{
+	return readb(XICS_ICP_BASE + offset);
+}
+
+void icp_write8(int offset, uint8_t val)
 {
-	return readb(XICS_BASE + offset);
+	writeb(val, XICS_ICP_BASE + offset);
 }
 
-void xics_write8(int offset, uint8_t val)
+uint32_t icp_read32(int offset)
 {
-	writeb(val, XICS_BASE + offset);
+	return bswap32(readl(XICS_ICP_BASE + offset));
 }
 
-uint32_t xics_read32(int offset)
+static inline void icp_write32(int offset, uint32_t val)
 {
-	return readl(XICS_BASE + offset);
+	writel(bswap32(val), XICS_ICP_BASE + offset);
 }
 
-void xics_write32(int offset, uint32_t val)
+uint32_t ics_read_xive(int irq)
 {
-	writel(val, XICS_BASE + offset);
+	return bswap32(readl(XICS_ICS_BASE + 0x800 + (irq << 2)));
 }
+
+void ics_write_xive(uint32_t val, int irq)
+{
+	writel(bswap32(val), XICS_ICS_BASE + 0x800 + (irq << 2));
+}
+

xics.vhdl

@@ -1,11 +1,13 @@
 --
 -- This is a simple XICS compliant interrupt controller.  This is a
--- Presenter (ICP) and Source (ICS) in a single unit with no routing
+-- Presenter (ICP) and Source (ICS) in two small units directly
--- layer.
+-- connected to each other with no routing layer.
 --
--- The sources have a fixed IRQ priority set by HW_PRIORITY. The
+-- The sources have a configurable IRQ priority set a set of ICS
--- source id starts at 16 for int_level_in(0) and go up from
+-- registers in the source units.
--- there (ie int_level_in(1) is source id 17).
+--
+-- The source ids start at 16 for int_level_in(0) and go up from
+-- there (ie int_level_in(1) is source id 17). XXX Make a generic
 --
 -- The presentation layer will pick an interupt that is more
 -- favourable than the current CPPR and present it via the XISR and
@@ -22,10 +24,7 @@ library work;
 use work.common.all;
 use work.wishbone_types.all;
 
-entity xics is
+entity xics_icp is
-    generic (
-        LEVEL_NUM : positive := 16
-        );
     port (
         clk          : in std_logic;
         rst          : in std_logic;
@@ -33,27 +32,23 @@ entity xics is
         wb_in        : in wb_io_master_out;
         wb_out       : out wb_io_slave_out;
 
-	int_level_in : in std_ulogic_vector(LEVEL_NUM - 1 downto 0);
+	ics_in       : in ics_to_icp_t;
-
 	core_irq_out : out std_ulogic
         );
-end xics;
+end xics_icp;
 
-architecture behaviour of xics is
+architecture behaviour of xics_icp is
     type reg_internal_t is record
 	xisr : std_ulogic_vector(23 downto 0);
 	cppr : std_ulogic_vector(7 downto 0);
-	pending_priority : std_ulogic_vector(7 downto 0);
 	mfrr : std_ulogic_vector(7 downto 0);
-	mfrr_pending : std_ulogic;
 	irq : std_ulogic;
 	wb_rd_data : std_ulogic_vector(31 downto 0);
 	wb_ack : std_ulogic;
     end record;
     constant reg_internal_init : reg_internal_t :=
 	(wb_ack => '0',
-	 mfrr_pending => '0',
+	 mfrr => x"ff", -- mask everything on reset
-	 mfrr => x"00", -- mask everything on reset
 	 irq => '0',
 	 others => (others => '0'));
 
@@ -74,25 +69,43 @@ begin
     begin
 	if rising_edge(clk) then
 	    r <= r_next;
+
+            -- We delay core_irq_out by a cycle to help with timing
+            core_irq_out <= r.irq;
 	end if;
     end process;
 
     wb_out.dat <= r.wb_rd_data;
     wb_out.ack <= r.wb_ack;
     wb_out.stall <= '0'; -- never stall wishbone
-    core_irq_out <= r.irq;
 
     comb : process(all)
 	variable v : reg_internal_t;
 	variable xirr_accept_rd : std_ulogic;
-	variable irq_eoi : std_ulogic;
+
+        function  bswap(v : in std_ulogic_vector(31 downto 0)) return std_ulogic_vector is
+            variable r : std_ulogic_vector(31 downto 0);
+        begin
+            r( 7 downto  0) := v(31 downto 24);
+            r(15 downto  8) := v(23 downto 16);
+            r(23 downto 16) := v(15 downto  8);
+            r(31 downto 24) := v( 7 downto  0);
+            return r;
+        end function;
+
+        variable be_in  : std_ulogic_vector(31 downto 0);
+        variable be_out : std_ulogic_vector(31 downto 0);
+
+	variable pending_priority : std_ulogic_vector(7 downto 0);        
     begin
 	v := r;
 
 	v.wb_ack := '0';
 
 	xirr_accept_rd := '0';
-	irq_eoi := '0';
+
+        be_in := bswap(wb_in.dat);
+        be_out := (others => '0');
 
 	if wb_in.cyc = '1' and wb_in.stb = '1' then
 	    v.wb_ack := '1'; -- always ack
@@ -100,111 +113,82 @@ begin
 		-- writes to both XIRR are the same
 		case wb_in.adr(7 downto 0) is
                 when XIRR_POLL =>
-		    report "XICS XIRR_POLL write";
+		    report "ICP XIRR_POLL write";
-		    if wb_in.sel = x"f" then -- 4 bytes
+                    v.cppr := be_in(31 downto 24);
-			v.cppr := wb_in.dat(31 downto 24);
-		    elsif wb_in.sel = x"1"  then -- 1 byte
-			v.cppr := wb_in.dat(7 downto 0);
-                    end if;
                 when XIRR =>
+                    v.cppr := be_in(31 downto 24);
 		    if wb_in.sel = x"f"  then -- 4 byte
-                        report "XICS XIRR write word:" & to_hstring(wb_in.dat);
+                        report "ICP XIRR write word (EOI) :" & to_hstring(be_in);
-			v.cppr := wb_in.dat(31 downto 24);
-			irq_eoi := '1';
 		    elsif wb_in.sel = x"1"  then -- 1 byte
-                        report "XICS XIRR write byte:" & to_hstring(wb_in.dat(7 downto 0));
+                        report "ICP XIRR write byte (CPPR):" & to_hstring(be_in(31 downto 24));
-			v.cppr := wb_in.dat(7 downto 0);
                     else
-                        report "XICS XIRR UNSUPPORTED write ! sel=" & to_hstring(wb_in.sel);
+                        report "ICP XIRR UNSUPPORTED write ! sel=" & to_hstring(wb_in.sel);
 		    end if;
 		when MFRR =>
+                    v.mfrr := be_in(31 downto 24);
 		    if wb_in.sel = x"f" then -- 4 bytes
-                        report "XICS MFRR write word:" & to_hstring(wb_in.dat);
+                        report "ICP MFRR write word:" & to_hstring(be_in);
-			v.mfrr_pending := '1';
-			v.mfrr := wb_in.dat(31 downto 24);
 		    elsif wb_in.sel = x"1" then -- 1 byte
-                        report "XICS MFRR write byte:" & to_hstring(wb_in.dat(7 downto 0));
+                        report "ICP MFRR write byte:" & to_hstring(be_in(31 downto 24));
-			v.mfrr_pending := '1';
-			v.mfrr := wb_in.dat(7 downto 0);
                     else
-                        report "XICS MFRR UNSUPPORTED write ! sel=" & to_hstring(wb_in.sel);
+                        report "ICP MFRR UNSUPPORTED write ! sel=" & to_hstring(wb_in.sel);
 		    end if;
                 when others =>                        
 		end case;
 
 	    else -- read
-		v.wb_rd_data := (others => '0');
 
 		case wb_in.adr(7 downto 0) is
                 when XIRR_POLL =>
-                    report "XICS XIRR_POLL read";
+                    report "ICP XIRR_POLL read";
-		    if wb_in.sel = x"f" then
+                    be_out := r.cppr & r.xisr;
-			v.wb_rd_data(23 downto  0) := r.xisr;
-			v.wb_rd_data(31 downto 24) := r.cppr;
-		    elsif wb_in.sel = x"1" then
-			v.wb_rd_data(7 downto  0) := r.cppr;
-                    end if;
                 when XIRR =>
-                    report "XICS XIRR read";
+                    report "ICP XIRR read";
+                    be_out := r.cppr & r.xisr;
 		    if wb_in.sel = x"f" then
-			v.wb_rd_data(23 downto 0) := r.xisr;
-			v.wb_rd_data(31 downto 24) := r.cppr;
 			xirr_accept_rd := '1';
-		    elsif wb_in.sel = x"1" then
-			v.wb_rd_data(7 downto 0) := r.cppr;
 		    end if;
 		when MFRR =>
-		    report "XICS MFRR read";
+                    report "ICP MFRR read";
-		    if wb_in.sel = x"f" then -- 4 bytes
+                    be_out(31 downto 24) := r.mfrr;
-			v.wb_rd_data(31 downto 24) := r.mfrr;
-		    elsif wb_in.sel = x"1" then -- 1 byte
-			v.wb_rd_data( 7 downto  0) := r.mfrr;
-		    end if;
                 when others =>                        
 		end case;
 	    end if;
 	end if;
 
-	-- generate interrupt
+        pending_priority := x"ff";
-	if r.irq = '0' then
+        v.xisr := x"000000";
-	    -- Here we just present any interrupt that's valid and
+        v.irq := '0';
-	    -- below cppr. For ordering, we ignore hardware
+
-	    -- priorities.
+        if ics_in.pri /= x"ff" then
-	    if unsigned(HW_PRIORITY) < unsigned(r.cppr) then --
+            v.xisr := x"00001" & ics_in.src;
-		-- lower HW sources are higher priority
+            pending_priority := ics_in.pri;
-		for i in LEVEL_NUM - 1 downto 0 loop
-		    if int_level_in(i) = '1' then
-			v.irq := '1';
-			v.xisr := std_ulogic_vector(to_unsigned(16 + i, 24));
-			v.pending_priority := HW_PRIORITY; -- hardware HW IRQs
-		    end if;
-		end loop;
         end if;
 
-	    -- Do mfrr as a higher priority so mfrr_pending is cleared
+        -- Check MFRR
-	    if unsigned(r.mfrr) < unsigned(r.cppr) then --
+        if unsigned(r.mfrr) < unsigned(pending_priority) then --
-		report "XICS: MFRR INTERRUPT";
-		-- IPI
-		if r.mfrr_pending = '1' then
-		    v.irq := '1';
             v.xisr := x"000002"; -- special XICS MFRR IRQ source number
-		    v.pending_priority := r.mfrr;
+            pending_priority := r.mfrr;
-		    v.mfrr_pending := '0';
-		end if;
-	    end if;
         end if;
 
 	-- Accept the interrupt
 	if xirr_accept_rd = '1' then
-	    report "XICS: ACCEPT" &
+            report "XICS: ICP ACCEPT" &
                 " cppr:" &  to_hstring(r.cppr) &
                 " xisr:" & to_hstring(r.xisr) &
                 " mfrr:" & to_hstring(r.mfrr);
-	    v.cppr := r.pending_priority;
+	    v.cppr := pending_priority;
 	end if;
 
-	if irq_eoi = '1' then
+        v.wb_rd_data := bswap(be_out);
-	    v.irq := '0';
+
+        if unsigned(pending_priority) < unsigned(v.cppr) then
+            if r.irq = '0' then
+                report "IRQ set";
+            end if;
+            v.irq := '1';
+        elsif r.irq = '1' then
+            report "IRQ clr";
         end if;
 
 	if rst = '1' then
@@ -216,3 +200,227 @@ begin
     end process;
 
 end architecture behaviour;
+
+library ieee;
+use ieee.std_logic_1164.all;
+use ieee.numeric_std.all;
+
+library work;
+use work.common.all;
+use work.wishbone_types.all;
+
+entity xics_ics is
+    generic (
+        SRC_NUM    : integer range 1 to 256  := 16;
+        PRIO_BITS  : integer range 1 to 8    := 8
+        );
+    port (
+        clk          : in std_logic;
+        rst          : in std_logic;
+
+        wb_in        : in wb_io_master_out;
+        wb_out       : out wb_io_slave_out;
+
+	int_level_in : in std_ulogic_vector(SRC_NUM - 1 downto 0);
+	icp_out      : out ics_to_icp_t
+        );
+end xics_ics;
+
+architecture rtl of xics_ics is
+
+    subtype pri_t is std_ulogic_vector(PRIO_BITS-1 downto 0);
+    type xive_t is record
+        pri : pri_t;
+    end record;
+    constant pri_masked : pri_t := (others => '1');
+
+    type xive_array_t is array(0 to SRC_NUM-1) of xive_t;
+    signal xives : xive_array_t;
+
+    signal wb_valid : std_ulogic;
+    signal reg_idx : integer range 0 to SRC_NUM - 1;
+    signal icp_out_next : ics_to_icp_t;
+    signal int_level_l : std_ulogic_vector(SRC_NUM - 1 downto 0);
+
+    function bswap(v : in std_ulogic_vector(31 downto 0)) return std_ulogic_vector is
+        variable r : std_ulogic_vector(31 downto 0);
+    begin
+        r( 7 downto  0) := v(31 downto 24);
+        r(15 downto  8) := v(23 downto 16);
+        r(23 downto 16) := v(15 downto  8);
+        r(31 downto 24) := v( 7 downto  0);
+        return r;
+    end function;
+
+    function get_config return std_ulogic_vector is
+        variable r: std_ulogic_vector(31 downto 0);
+    begin
+        r := (others => '0');
+        r(23 downto  0) := std_ulogic_vector(to_unsigned(SRC_NUM, 24));
+        r(27 downto 24) := std_ulogic_vector(to_unsigned(PRIO_BITS, 4));
+        return r;
+    end function;
+
+    function prio_pack(pri8: std_ulogic_vector(7 downto 0)) return pri_t is
+    begin
+        return pri8(PRIO_BITS-1 downto 0);
+    end function;
+
+    function prio_unpack(pri: pri_t) return std_ulogic_vector is
+        variable r : std_ulogic_vector(7 downto 0);
+    begin
+        if pri = pri_masked then
+            r := x"ff";
+        else
+            r := (others => '0');
+            r(PRIO_BITS-1 downto 0) := pri;
+        end if;
+        return r;
+   end function;
+
+
+-- Register map
+    --     0  : Config
+    --     4  : Debug/diagnostics
+    --   800  : XIVE0
+    --   804  : XIVE1 ...
+    --
+    -- Config register format:
+    --
+    --  23..  0 : Interrupt base (hard wired to 16)
+    --  27.. 24 : #prio bits (1..8)
+    --
+    -- XIVE register format:
+    --
+    --       31 : input bit (reflects interrupt input)
+    --       30 : reserved
+    --       29 : P (mirrors input for now)
+    --       28 : Q (not implemented in this version)
+    -- 30 ..    : reserved
+    -- 19 ..  8 : target (not implemented in this version)
+    --  7 ..  0 : prio/mask
+
+    signal reg_is_xive   : std_ulogic;
+    signal reg_is_config : std_ulogic;
+    signal reg_is_debug  : std_ulogic;
+
+begin
+
+    assert SRC_NUM = 16 report "Fixup address decode with log2";
+
+    reg_is_xive   <= wb_in.adr(11);
+    reg_is_config <= '1' when wb_in.adr(11 downto 0) = x"000" else '0';
+    reg_is_debug  <= '1' when wb_in.adr(11 downto 0) = x"004" else '0';
+
+    -- Register index XX FIXME: figure out bits from SRC_NUM
+    reg_idx <= to_integer(unsigned(wb_in.adr(5 downto 2)));
+
+    -- Latch interrupt inputs for timing
+    int_latch: process(clk)
+    begin
+        if rising_edge(clk) then
+            int_level_l <= int_level_in;
+        end if;
+    end process;
+
+    -- We don't stall. Acks are sent by the read machine one cycle
+    -- after a request, but we can handle one access per cycle.
+    wb_out.stall <= '0';
+    wb_valid <= wb_in.cyc and wb_in.stb;
+
+    -- Big read mux. This could be replaced by a slower state
+    -- machine iterating registers instead if timing gets tight.
+    reg_read: process(clk)
+        variable be_out : std_ulogic_vector(31 downto 0);
+    begin
+        if rising_edge(clk) then
+            be_out := (others => '0');
+
+            if reg_is_xive = '1' then
+                be_out := int_level_l(reg_idx) &
+                          '0' &
+                          int_level_l(reg_idx) &
+                          '0' &
+                          x"00000" &
+                          prio_unpack(xives(reg_idx).pri);
+            elsif reg_is_config = '1' then
+                be_out := get_config;
+            elsif reg_is_debug = '1' then
+                be_out := x"00000" & icp_out_next.src & icp_out_next.pri;
+            end if;
+            wb_out.dat <= bswap(be_out);
+            wb_out.ack <= wb_valid;
+        end if;
+    end process;
+
+    -- Register write machine
+    reg_write: process(clk)
+        variable be_in  : std_ulogic_vector(31 downto 0);
+    begin
+        -- Byteswapped input
+        be_in := bswap(wb_in.dat);
+
+        if rising_edge(clk) then
+            if rst = '1' then
+                for i in 0 to SRC_NUM - 1 loop
+                    xives(i) <= (pri => pri_masked);
+                end loop;
+            elsif wb_valid = '1' and wb_in.we = '1' then
+                if reg_is_xive then
+                    -- TODO: When adding support for other bits, make sure to
+                    -- properly implement wb_in.sel to allow partial writes.
+                    xives(reg_idx).pri <= prio_pack(be_in(7 downto 0));
+                    report "ICS irq " & integer'image(reg_idx) &
+                        " set to:" & to_hstring(be_in(7 downto 0));
+                end if;
+            end if;
+        end if;
+    end process;
+
+    -- generate interrupt. This is a simple combinational process,
+    -- potentially wasteful in HW for large number of interrupts.
+    --
+    -- could be replaced with iterative state machines and a message
+    -- system between ICSs' (plural) and ICP  incl. reject etc...
+    --
+    irq_gen_sync: process(clk)
+    begin
+        if rising_edge(clk) then
+            icp_out <= icp_out_next;
+        end if;
+    end process;
+
+    irq_gen: process(all)
+        variable max_idx : integer range 0 to SRC_NUM-1;
+        variable max_pri : pri_t;
+
+        -- A more favored than b ?
+        function a_mf_b(a: pri_t; b: pri_t) return boolean is
+            variable a_i : unsigned(PRIO_BITS-1 downto 0);
+            variable b_i : unsigned(PRIO_BITS-1 downto 0);
+        begin
+            a_i := unsigned(a);
+            b_i := unsigned(b);
+            report "a_mf_b a=" & to_hstring(a) &
+                " b=" & to_hstring(b) &
+                " r=" & boolean'image(a < b);
+            return a_i < b_i;
+        end function;
+    begin
+        -- XXX FIXME: Use a tree
+        max_pri := pri_masked;
+        max_idx := 0;
+        for i in 0 to SRC_NUM - 1 loop
+            if int_level_l(i) = '1' and a_mf_b(xives(i).pri, max_pri) then
+                max_pri := xives(i).pri;
+                max_idx := i;
+            end if;
+        end loop;
+        if max_pri /= pri_masked then
+            report "MFI: " & integer'image(max_idx) & " pri=" & to_hstring(prio_unpack(max_pri));
+        end if;
+        icp_out_next.src <= std_ulogic_vector(to_unsigned(max_idx, 4));
+        icp_out_next.pri <= prio_unpack(max_pri);
+    end process;
+
+end architecture rtl;