Merge XLP 3XX updates and related rework.

* Update message station (CMS) code, read queue ids from PCI header.
* Use interrupts to wakeup message handling threads on 3XX
* Update PIC code, read interrupt information from PCI header instead
  of using fixed values.
* Update PCI interrupt handling for the PIC change.
* Update code for getting chip frequency, new code support XLP 3XX
* Misc style(9) fixes

In collaboration with: prabhath at netlogicmicro com (CMS/PIC)
                       venkatesh at netlogicmicro.com (PCI)

13 files changed, 891 insertions, 1141 deletions

diff --git a/sys/mips/nlm/cms.c b/sys/mips/nlm/cms.c
index a991add..4ba3209 100644
--- a/sys/mips/nlm/cms.c
+++ b/sys/mips/nlm/cms.c
@@ -75,8 +75,8 @@ __FBSDID("$FreeBSD$");
  * load
  */
 struct msgring_thread {
-	struct thread	*thread; /* msgring handler threads */
-	int	needed;		/* thread needs to wake up */
+	struct thread	*thread;	/* msgring handler threads */
+	int	needed;			/* thread needs to wake up */
 };
 static struct msgring_thread msgring_threads[XLP_MAX_CORES * XLP_MAX_THREADS];
 static struct proc *msgring_proc;	/* all threads are under a proc */
@@ -91,142 +91,80 @@ struct tx_stn_handler {
 };
 static struct tx_stn_handler msgmap[MSGRNG_NSTATIONS];
 static struct mtx	msgmap_lock;
-uint64_t xlp_cms_base;
 uint32_t xlp_msg_thread_mask;
-static int xlp_msg_threads_per_core = 3; /* Make tunable */
+static int xlp_msg_threads_per_core = 3;
 
 static void create_msgring_thread(int hwtid);
 static int msgring_process_fast_intr(void *arg);
-/*
- * Boot time init, called only once
- */
-void
-xlp_msgring_config(void)
-{
-	unsigned int thrmask, mask;
-	int i;
-
-	/* TODO: Add other nodes */
-	xlp_cms_base = nlm_get_cms_regbase(0);
-
-	mtx_init(&msgmap_lock, "msgring", NULL, MTX_SPIN);
-	if (xlp_threads_per_core < xlp_msg_threads_per_core)
-		xlp_msg_threads_per_core = xlp_threads_per_core;
-	thrmask = ((1 << xlp_msg_threads_per_core) - 1);
-	/*thrmask <<= xlp_threads_per_core - xlp_msg_threads_per_core;*/
-	mask = 0;
-	for (i = 0; i < XLP_MAX_CORES; i++) {
-		mask <<= XLP_MAX_THREADS;
-		mask |= thrmask;
-	}
-	xlp_msg_thread_mask = xlp_hw_thread_mask & mask;
-	printf("Initializing CMS...@%jx, Message handler thread mask %#jx\n",
-	    (uintmax_t)xlp_cms_base, (uintmax_t)xlp_msg_thread_mask);
-}
 
-/*
- * Initialize the messaging subsystem.
- *
- * Message Stations are shared among all threads in a cpu core, this
- * has to be called once from every core which is online.
+/* Debug counters */
+static int msgring_nintr[XLP_MAX_CORES * XLP_MAX_THREADS];
+static int msgring_wakeup_sleep[XLP_MAX_CORES * XLP_MAX_THREADS];
+static int msgring_wakeup_nosleep[XLP_MAX_CORES * XLP_MAX_THREADS];
+static int fmn_msgcount[XLP_MAX_CORES * XLP_MAX_THREADS][4];
+static int fmn_loops[XLP_MAX_CORES * XLP_MAX_THREADS];
+
+/* Whether polled driver implementation */
+static int polled = 1;
+
+/* We do only i/o device credit setup here. CPU credit setup is now
+ * moved to xlp_msgring_cpu_init() so that the credits get setup 
+ * only if the CPU exists. xlp_msgring_cpu_init() gets called from
+ * platform_init_ap; and this makes it easy for us to setup CMS
+ * credits for various types of XLP chips, with varying number of
+ * cpu's and cores.
  */
-void
-xlp_msgring_iodi_config(void)
-{
-	void *cookie;
-
-	xlp_msgring_config();
-/*	nlm_cms_default_setup(0,0,0,0); */
-	nlm_cms_credit_setup(50);
-	create_msgring_thread(0);
-	cpu_establish_hardintr("msgring", msgring_process_fast_intr, NULL,
-	    NULL, IRQ_MSGRING, INTR_TYPE_NET, &cookie);
-}
-
-void
-nlm_cms_credit_setup(int credit)
+static void
+xlp_cms_credit_setup(int credit)
 {
+	uint64_t cmspcibase, cmsbase, pcibase;
+	uint32_t devoffset;
+	int dev, fn, maxqid;
 	int src, qid, i;
 
-#if 0
-	/* there are a total of 18 src stations on XLP. */
-	printf("Setting up CMS credits!\n");
-	for (src=0; src<18; src++) {
-		for(qid=0; qid<1024; qid++) {
-			nlm_cms_setup_credits(xlp_cms_base, qid, src, credit);
-		}
-	}
+	for (i = 0; i < XLP_MAX_NODES; i++) {
+		cmspcibase = nlm_get_cms_pcibase(i);
+		if (!nlm_dev_exists(XLP_IO_CMS_OFFSET(i)))
+		    continue;
+		cmsbase = nlm_get_cms_regbase(i);
+		maxqid = nlm_read_reg(cmspcibase, XLP_PCI_DEVINFO_REG0);
+		for (dev = 0; dev < 8; dev++) {
+			for (fn = 0; fn < 8; fn++) {
+				devoffset = XLP_HDR_OFFSET(i, 0, dev, fn);
+				if (nlm_dev_exists(devoffset) == 0)
+					continue;
+				pcibase = nlm_pcicfg_base(devoffset);
+				src = nlm_qidstart(pcibase);
+				if (src == 0)
+				    continue;
+#if 0 /* Debug */
+				printf("Setup CMS credits for queues ");
+				printf("[%d to %d] from src %d\n", 0,
+				    maxqid, src);
 #endif
-	printf("Setting up CMS credits!\n");
-	/* CPU Credits */
-	for (i = 1; i < 8; i++) {
-		src = (i << 4);
-		for (qid = 0; qid < 1024; qid++)
-			nlm_cms_setup_credits(xlp_cms_base, qid, src, credit);
-	}
-	/* PCIE Credits */
-	for(i = 0; i < 4; i++) {
-		src = (256 + (i * 2));
-		for(qid = 0; qid < 1024; qid++)
-			nlm_cms_setup_credits(xlp_cms_base, qid, src, credit);
+				for (qid = 0; qid < maxqid; qid++)
+					nlm_cms_setup_credits(cmsbase, qid,
+					    src, credit);
+			}
+		}
 	}
-	/* DTE Credits */
-	src = 264;
-	for (qid = 0; qid < 1024; qid++)
-		nlm_cms_setup_credits(xlp_cms_base, qid, src, credit);
-	/* RSA Credits */
-	src = 272;
-	for (qid = 0; qid < 1024; qid++)
-		nlm_cms_setup_credits(xlp_cms_base, qid, src, credit);
-
-	/* Crypto Credits */
-	src = 281;
-	for (qid = 0; qid < 1024; qid++)
-		nlm_cms_setup_credits(xlp_cms_base, qid, src, credit);
-
-	/* CMP Credits */
-	src = 298;
-	for (qid = 0; qid < 1024; qid++)
-		nlm_cms_setup_credits(xlp_cms_base, qid, src, credit);
-
-	/* POE Credits */
-	src = 384;
-	for(qid = 0; qid < 1024; qid++)
-		nlm_cms_setup_credits(xlp_cms_base, qid, src, credit);
-
-	/* NAE Credits */
-	src = 476;
-	for(qid = 0; qid < 1024; qid++)
-		nlm_cms_setup_credits(xlp_cms_base, qid, src, credit);
 }
 
 void
-xlp_msgring_cpu_init(uint32_t cpuid)
+xlp_msgring_cpu_init(int node, int cpu, int credit)
 {
-	int queue,i;
-
-	queue = CMS_CPU_PUSHQ(0, ((cpuid >> 2) & 0x7), (cpuid & 0x3), 0);
-	/* temp allocate 4 segments to each output queue */
-	nlm_cms_alloc_onchip_q(xlp_cms_base, queue, 4);
-	/* Enable high watermark and non empty interrupt */
-	nlm_cms_per_queue_level_intr(xlp_cms_base, queue,2,0);
-	for(i=0;i<8;i++) {
-		/* temp distribute the credits to all CPU stations */
-		nlm_cms_setup_credits(xlp_cms_base, queue, i * 16, 8);
-	}
-}
+	uint64_t cmspcibase = nlm_get_cms_pcibase(node);
+	uint64_t cmsbase = nlm_get_cms_regbase(node);
+	int qid, maxqid, src;
 
-void
-xlp_cpu_msgring_handler(int bucket, int size, int code, int stid,
-		    struct nlm_fmn_msg *msg, void *data)
-{
-	int i;
+	maxqid = nlm_read_reg(cmspcibase, XLP_PCI_DEVINFO_REG0);
 
-	printf("vc:%d srcid:%d size:%d\n",bucket,stid,size);
-	for(i=0;i<size;i++) {
-		printf("msg->msg[%d]:0x%jx ", i, (uintmax_t)msg->msg[i]);
+	/* cpu credit setup is done only from thread-0 of each core */
+	if((cpu % 4) == 0) {
+		src = cpu << 2; /* each thread has 4 vc's */
+		for (qid = 0; qid < maxqid; qid++)
+		    nlm_cms_setup_credits(cmsbase, qid, src, credit);
 	}
-	printf("\n");
 }
 
 /*
@@ -234,35 +172,100 @@ xlp_cpu_msgring_handler(int bucket, int size, int code, int stid,
  * Use max_msgs = 0 to drain out all messages.
  */
 int
-xlp_handle_msg_vc(int vc, int max_msgs)
+xlp_handle_msg_vc(u_int vcmask, int max_msgs)
 {
 	struct nlm_fmn_msg msg;
-	int i, srcid = 0, size = 0, code = 0;
+	int srcid = 0, size = 0, code = 0;
 	struct tx_stn_handler *he;
 	uint32_t mflags, status;
+	int n_msgs = 0, vc, m, hwtid;
+	u_int msgmask;
+
 
-	for (i = 0; i < max_msgs; i++) {
+	hwtid = nlm_cpuid();
+	for (;;) {
+		/* check if VC empty */
 		mflags = nlm_save_flags_cop2();
-		status = nlm_fmn_msgrcv(vc, &srcid, &size, &code, &msg);
+		status = nlm_read_c2_msgstatus1();
 		nlm_restore_flags(mflags);
-		if (status != 0) /* If there is no msg or error */
-			break;
-		if (srcid < 0 && srcid >= 1024) {
-			printf("[%s]: bad src id %d\n", __func__, srcid);
-			continue;
-		}
-		he = &msgmap[srcid];
-		if(he->action != NULL)
-			(he->action)(vc, size, code, srcid, &msg, he->arg);
-#if 0 /* debug */
-		else
-			printf("[%s]: No Handler for message from stn_id=%d,"
-			    " vc=%d, size=%d, msg0=%jx, dropping message\n",
-			    __func__, srcid, vc, size, (uintmax_t)msg.msg[0]);
+
+		msgmask = ((status >> 24) & 0xf) ^ 0xf;
+		msgmask &= vcmask;
+		if (msgmask == 0)
+			    break;
+		m = 0;
+		for (vc = 0; vc < 4; vc++) {
+			if ((msgmask & (1 << vc)) == 0)
+				continue;
+
+			mflags = nlm_save_flags_cop2();
+			status = nlm_fmn_msgrcv(vc, &srcid, &size, &code,
+		 	    &msg);
+			nlm_restore_flags(mflags);
+			if (status != 0) 	/*  no msg or error */
+				continue;
+			if (srcid < 0 && srcid >= 1024) {
+				printf("[%s]: bad src id %d\n", __func__,
+				    srcid);
+				continue;
+			}
+			he = &msgmap[srcid];
+			if(he->action != NULL)
+				(he->action)(vc, size, code, srcid, &msg, he->arg);
+#if 1 /* defined DEBUG */
+			else
+				printf("[%s]: No Handler for msg from stn %d,"
+				    " vc=%d, size=%d, msg0=%jx, droppinge\n",
+				    __func__, srcid, vc, size,
+				    (uintmax_t)msg.msg[0]);
 #endif
+			fmn_msgcount[hwtid][vc] += 1;
+			m++;	/* msgs handled in this iter */
+		}
+		if (m == 0)
+			break;	/* nothing done in this iter */
+		n_msgs += m;
+		if (max_msgs > 0 && n_msgs >= max_msgs)
+		    break;
+	}
+
+	return (n_msgs);
+}
+
+static void
+xlp_discard_msg_vc(u_int vcmask)
+{
+	struct nlm_fmn_msg msg;
+	int srcid = 0, size = 0, code = 0, vc;
+	uint32_t mflags, status;
+
+	for (vc = 0; vc < 4; vc++) {
+		for (;;) {
+			mflags = nlm_save_flags_cop2();
+			status = nlm_fmn_msgrcv(vc, &srcid,
+			    &size, &code, &msg);
+			nlm_restore_flags(mflags);
+
+			/* break if there is no msg or error */
+			if (status != 0)
+			    break;
+		}
 	}
+}
 
-	return (i);
+void
+xlp_cms_enable_intr(int node, int cpu, int type, int watermark)
+{
+	uint64_t cmsbase;
+	int i, qid;
+
+	cmsbase = nlm_get_cms_regbase(node);
+
+	for (i = 0; i < 4; i++) {
+		qid = (i + (cpu * 4)) & 0x7f;
+		nlm_cms_per_queue_level_intr(cmsbase, qid, type, watermark);
+		nlm_cms_per_queue_timer_intr(cmsbase, qid, 0x1, 0);
+	}
 }
 
 static int
@@ -274,6 +277,7 @@ msgring_process_fast_intr(void *arg)
 
 	cpu = nlm_cpuid();
 	mthd = &msgring_threads[cpu];
+	msgring_nintr[cpu]++;
 	td = mthd->thread;
 
 	/* clear pending interrupts */
@@ -283,24 +287,24 @@ msgring_process_fast_intr(void *arg)
 	mthd->needed = 1;
 	thread_lock(td);
 	if (TD_AWAITING_INTR(td)) {
+		msgring_wakeup_sleep[cpu]++;
 		TD_CLR_IWAIT(td);
 		sched_add(td, SRQ_INTR);
-	}
+	} else
+		msgring_wakeup_nosleep[cpu]++;
 
 	thread_unlock(td);
+
 	return (FILTER_HANDLED);
 }
 
-u_int fmn_msgcount[32][4];
-u_int fmn_loops[32];
-
 static void
 msgring_process(void * arg)
 {
 	volatile struct msgring_thread *mthd;
 	struct thread *td;
-	uint32_t mflags;
-	int hwtid, vc, handled, nmsgs;
+	uint32_t mflags, msgstatus1;
+	int hwtid, nmsgs;
 
 	hwtid = (intptr_t)arg;
 	mthd = &msgring_threads[hwtid];
@@ -314,39 +318,47 @@ msgring_process(void * arg)
 	thread_unlock(td);
 
 	if (hwtid != nlm_cpuid())
-		printf("Misscheduled hwtid %d != cpuid %d\n", hwtid, nlm_cpuid());
-	mflags = nlm_save_flags_cop2();
-	nlm_fmn_cpu_init(IRQ_MSGRING, 0, 0, 0, 0, 0);
-	nlm_restore_flags(mflags);
+		printf("Misscheduled hwtid %d != cpuid %d\n", hwtid,
+		    nlm_cpuid());
+
+	xlp_discard_msg_vc(0xf);
+	xlp_msgring_cpu_init(nlm_nodeid(), nlm_cpuid(), CMS_DEFAULT_CREDIT);
+	if (polled == 0) {
+		mflags = nlm_save_flags_cop2();
+		nlm_fmn_cpu_init(IRQ_MSGRING, 0, 0, 0, 0, 0);
+		nlm_restore_flags(mflags);
+		xlp_cms_enable_intr(nlm_nodeid(), nlm_cpuid(), 0x2, 0);
+		/* clear pending interrupts.
+		 *  they will get re-raised if still valid */
+		nlm_write_c0_eirr(1ULL << IRQ_MSGRING);
+	}
 
 	/* start processing messages */
-	for( ; ; ) {
-		/*atomic_store_rel_int(&mthd->needed, 0);*/
-
-	        /* enable cop2 access */
-		do {
-			handled = 0;
-			for (vc = 0; vc < 4; vc++) {
-				nmsgs = xlp_handle_msg_vc(vc, 1);
-				fmn_msgcount[hwtid][vc] += nmsgs;
-				handled += nmsgs;
-			}
-		} while (handled);
+	for (;;) {
+		atomic_store_rel_int(&mthd->needed, 0);
+		nmsgs = xlp_handle_msg_vc(0xf, 0);
 
 		/* sleep */
-#if 0
-		thread_lock(td);
-		if (mthd->needed) {
+		if (polled == 0) {
+			/* clear VC-pend bits */
+			mflags = nlm_save_flags_cop2();
+			msgstatus1 = nlm_read_c2_msgstatus1();
+			msgstatus1 |= (0xf << 16);
+			nlm_write_c2_msgstatus1(msgstatus1);
+			nlm_restore_flags(mflags);
+
+			thread_lock(td);
+			if (mthd->needed) {
+				thread_unlock(td);
+				continue;
+			}
+			sched_class(td, PRI_ITHD);
+			TD_SET_IWAIT(td);
+			mi_switch(SW_VOL, NULL);
 			thread_unlock(td);
-			continue;
-		}
-		sched_class(td, PRI_ITHD);
-		TD_SET_IWAIT(td);
-		mi_switch(SW_VOL, NULL);
-		thread_unlock(td);
-#else
-		pause("wmsg", 1);
-#endif
+		} else
+			pause("wmsg", 1);
+
 		fmn_loops[hwtid]++;
 	}
 }
@@ -370,7 +382,9 @@ create_msgring_thread(int hwtid)
 	sched_class(td, PRI_ITHD);
 	sched_add(td, SRQ_INTR);
 	thread_unlock(td);
-	CTR2(KTR_INTR, "%s: created %s", __func__, td->td_name);
+	if (bootverbose)
+		printf("Msgring handler create on cpu %d (%s)\n",
+		    hwtid, td->td_name);
 }
 
 int
@@ -381,7 +395,7 @@ register_msgring_handler(int startb, int endb, msgring_handler action,
 
 	printf("Register handler %d-%d %p(%p)\n", startb, endb, action, arg);
 	KASSERT(startb >= 0 && startb <= endb && endb < MSGRNG_NSTATIONS,
-	    ("Invalid value for for bucket range %d,%d", startb, endb));
+	    ("Invalid value for bucket range %d,%d", startb, endb));
 
 	mtx_lock_spin(&msgmap_lock);
 	for (i = startb; i <= endb; i++) {
@@ -395,6 +409,45 @@ register_msgring_handler(int startb, int endb, msgring_handler action,
 }
 
 /*
+ * Initialize the messaging subsystem.
+ *
+ * Message Stations are shared among all threads in a cpu core, this
+ * has to be called once from every core which is online.
+ */
+static void
+xlp_msgring_config(void *arg)
+{
+	void *cookie;
+	unsigned int thrmask, mask;
+	int i;
+
+	mtx_init(&msgmap_lock, "msgring", NULL, MTX_SPIN);
+	if (xlp_threads_per_core < xlp_msg_threads_per_core)
+		xlp_msg_threads_per_core = xlp_threads_per_core;
+	thrmask = ((1 << xlp_msg_threads_per_core) - 1);
+	/*thrmask <<= xlp_threads_per_core - xlp_msg_threads_per_core;*/
+	mask = 0;
+	for (i = 0; i < XLP_MAX_CORES; i++) {
+		mask <<= XLP_MAX_THREADS;
+		mask |= thrmask;
+	}
+	xlp_msg_thread_mask = xlp_hw_thread_mask & mask;
+#if 0
+	printf("CMS Message handler thread mask %#jx\n",
+	    (uintmax_t)xlp_msg_thread_mask);
+#endif
+
+	if (nlm_is_xlp3xx())
+	    polled = 0;		/* switch to interrupt driven driver */
+
+/*	nlm_cms_default_setup(0,0,0,0); */
+	xlp_cms_credit_setup(CMS_DEFAULT_CREDIT);
+	create_msgring_thread(0);
+	cpu_establish_hardintr("msgring", msgring_process_fast_intr, NULL,
+	    NULL, IRQ_MSGRING, INTR_TYPE_NET, &cookie);
+}
+
+/*
  * Start message ring processing threads on other CPUs, after SMP start
  */
 static void
@@ -409,6 +462,8 @@ start_msgring_threads(void *arg)
 	}
 }
 
+SYSINIT(xlp_msgring_config, SI_SUB_DRIVERS, SI_ORDER_FIRST,
+    xlp_msgring_config, NULL);
 SYSINIT(start_msgring_threads, SI_SUB_SMP, SI_ORDER_MIDDLE,
     start_msgring_threads, NULL);
 
diff --git a/sys/mips/nlm/files.xlp b/sys/mips/nlm/files.xlp
index f9b2b71..682bbbf 100644
--- a/sys/mips/nlm/files.xlp
+++ b/sys/mips/nlm/files.xlp
@@ -1,4 +1,5 @@
 # $FreeBSD$
+mips/nlm/hal/nlm_hal.c				standard
 mips/nlm/hal/fmn.c				standard
 mips/nlm/xlp_machdep.c				standard
 mips/nlm/intr_machdep.c				standard
diff --git a/sys/mips/nlm/hal/cop2.h b/sys/mips/nlm/hal/cop2.h
index 308461b..9cbd266 100644
--- a/sys/mips/nlm/hal/cop2.h
+++ b/sys/mips/nlm/hal/cop2.h
@@ -38,7 +38,7 @@
 #define	COP2_RXMSGSTATUS	3
 #define	COP2_MSGSTATUS1		4
 #define	COP2_MSGCONFIG		5
-#define	COP2_MSGCONFIG1		6
+#define	COP2_MSGERROR		6
 
 #define	CROSSTHR_POPQ_EN	0x01
 #define	VC0_POPQ_EN		0x02
@@ -160,7 +160,10 @@ NLM_DEFINE_COP2_ACCESSORS32(txmsgstatus, COP2_TXMSGSTATUS, 0);
 NLM_DEFINE_COP2_ACCESSORS32(rxmsgstatus, COP2_RXMSGSTATUS, 0);
 NLM_DEFINE_COP2_ACCESSORS32(msgstatus1, COP2_MSGSTATUS1, 0);
 NLM_DEFINE_COP2_ACCESSORS32(msgconfig, COP2_MSGCONFIG, 0);
-NLM_DEFINE_COP2_ACCESSORS32(msgconfig1, COP2_MSGCONFIG1, 0);
+NLM_DEFINE_COP2_ACCESSORS32(msgerror0, COP2_MSGERROR, 0);
+NLM_DEFINE_COP2_ACCESSORS32(msgerror1, COP2_MSGERROR, 1);
+NLM_DEFINE_COP2_ACCESSORS32(msgerror2, COP2_MSGERROR, 2);
+NLM_DEFINE_COP2_ACCESSORS32(msgerror3, COP2_MSGERROR, 3);
 
 /* successful completion returns 1, else 0 */
 static inline int
@@ -279,7 +282,7 @@ nlm_fmn_msgrcv(int vc, int *srcid, int *size, int *code, struct nlm_fmn_msg *m)
 }
 
 static inline void
-nlm_fmn_cpu_init(int int_vec, int ctpe, int v0pe, int v1pe, int v2pe, int v3pe)
+nlm_fmn_cpu_init(int int_vec, int ecc_en, int v0pe, int v1pe, int v2pe, int v3pe)
 {
 	uint32_t val = nlm_read_c2_msgconfig();
 
@@ -287,12 +290,12 @@ nlm_fmn_cpu_init(int int_vec, int ctpe, int v0pe, int v1pe, int v2pe, int v3pe)
 	 * in msgconfig register of cop2.
 	 * As per chip/cpu RTL, [16:20] bits consist of int_vec.
 	 */
-	val |= ((int_vec & 0x1f) << 16) |
+	val |= (((int_vec & 0x1f) << 16) |
+		((ecc_en & 0x1) << 8) |
 		((v3pe & 0x1) << 4) |
 		((v2pe & 0x1) << 3) |
 		((v1pe & 0x1) << 2) |
-		((v0pe & 0x1) << 1) |
-		(ctpe & 0x1);
+		((v0pe & 0x1) << 1));
 
 	nlm_write_c2_msgconfig(val);
 }
diff --git a/sys/mips/nlm/hal/fmn.c b/sys/mips/nlm/hal/fmn.c
index e58a2a5..6d40134 100644
--- a/sys/mips/nlm/hal/fmn.c
+++ b/sys/mips/nlm/hal/fmn.c
@@ -67,7 +67,6 @@ uint64_t nlm_cms_spill_total_messages = 1 * 1024;
 * For all 4 nodes, there are 18*4 = 72 FMN stations
 */
 uint32_t nlm_cms_total_stations = 18 * 4 /*xlp_num_nodes*/;
-uint32_t cms_onchip_seg_availability[CMS_ON_CHIP_PER_QUEUE_SPACE];
 
 /**
  * Takes inputs as node, queue_size and maximum number of queues.
@@ -146,114 +145,6 @@ void nlm_cms_setup_credits(uint64_t base, int destid, int srcid, int credit)
 
 }
 
-int nlm_cms_config_onchip_queue (uint64_t base, uint64_t spill_base,
-					int qid, int spill_en)
-{
-
-	/* Configure 32 as onchip queue depth */
-	nlm_cms_alloc_onchip_q(base, qid, 1);
-
-	/* Spill configuration */
-	if (spill_en) {
-		/* Configure 4*4KB = 16K as spill size */
-		nlm_cms_alloc_spill_q(base, qid, spill_base, 4);
-	}
-
-#if 0
-	/* configure credits for src cpu0, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_CPU0_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src cpu1, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_CPU1_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src cpu2, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_CPU2_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src cpu3, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_CPU3_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src cpu4, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_CPU4_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src cpu5, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_CPU5_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src cpu6, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_CPU6_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src cpu7, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_CPU7_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src pcie0, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_PCIE0_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src pcie1, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_PCIE1_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src pcie2, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_PCIE2_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src pcie3, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_PCIE3_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src dte, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_DTE_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src rsa_ecc, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_RSA_ECC_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src crypto, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_CRYPTO_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src cmp, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_CMP_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src poe, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_POE_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-
-	/* configure credits for src nae, on this queue */
-	nlm_cms_setup_credits(base, qid, CMS_NAE_SRC_STID,
-		CMS_DEFAULT_CREDIT(nlm_cms_total_stations,
-			nlm_cms_spill_total_messages));
-#endif
-
-	return 0;
-}
-
 /*
  * base		- CMS module base address for this node.
  * qid		- is the output queue id otherwise called as vc id
@@ -268,7 +159,7 @@ int nlm_cms_alloc_spill_q(uint64_t base, int qid, uint64_t spill_base,
 	uint64_t queue_config;
 	uint32_t spill_start;
 
-	if(nsegs > CMS_MAX_SPILL_SEGMENTS_PER_QUEUE) {
+	if (nsegs > CMS_MAX_SPILL_SEGMENTS_PER_QUEUE) {
 		return 1;
 	}
 
@@ -286,152 +177,6 @@ int nlm_cms_alloc_spill_q(uint64_t base, int qid, uint64_t spill_base,
 	return 0;
 }
 
-/*
- * base		- CMS module base address for this node.
- * qid		- is the output queue id otherwise called as vc id
- * nsegs	- No of segments where a "1" indicates 32 credits. On chip
- *                credits must be a multiple of 32.
- */
-int nlm_cms_alloc_onchip_q(uint64_t base, int qid, int nsegs)
-{
-	static uint32_t curr_end = 0;
-	uint64_t queue_config;
-	int onchipbase, start, last;
-	uint8_t i;
-
-        if( ((curr_end + nsegs) > CMS_MAX_ONCHIP_SEGMENTS) ||
-		(nsegs > CMS_ON_CHIP_PER_QUEUE_SPACE) ) {
-		/* Invalid configuration */
-                return 1;
-        }
-        if(((curr_end % 32) + nsegs - 1) <= 31) {
-                onchipbase = (curr_end / 32);
-                start  = (curr_end % 32);
-                curr_end += nsegs;
-        } else {
-                onchipbase = (curr_end / 32) + 1;
-                start  = 0;
-                curr_end = ((onchipbase * 32) + nsegs);
-        }
-        last   = start + nsegs - 1;
-
-	for(i = start;i <= last;i++) {
-		if(cms_onchip_seg_availability[onchipbase] & (1 << i)) {
-			/* Conflict!!! segment is already allocated */
-			return 1;
-		}
-	}
-	/* Update the availability bitmap as consumed */
-	for(i = start; i <= last; i++) {
-		cms_onchip_seg_availability[onchipbase] |= (1 << i);
-	}
-
-	queue_config = nlm_read_cms_reg(base,(CMS_OUTPUTQ_CONFIG(qid)));
-
-	/* On chip configuration */
-	queue_config = (((uint64_t)CMS_QUEUE_ENA << 63) |
-			((onchipbase & 0x1f) << 10) |
-			((last & 0x1f) << 5) |
-			(start & 0x1f));
-
-	nlm_write_cms_reg(base,(CMS_OUTPUTQ_CONFIG(qid)),queue_config);
-
-	return 0;
-}
-
-void nlm_cms_default_setup(int node, uint64_t spill_base, int spill_en,
-				int popq_en)
-{
-	int j, k, vc;
-	int queue;
-	uint64_t base;
-
-	base = nlm_get_cms_regbase(node);
-	for(j=0; j<1024; j++) {
-		printf("Qid:0x%04d Val:0x%016jx\n",j, 
-		    (uintmax_t)nlm_cms_get_onchip_queue (base, j));
-	}
-	/* Enable all cpu push queues */
-	for (j=0; j<XLP_MAX_CORES; j++)
-		for (k=0; k<XLP_MAX_THREADS; k++)
-			for (vc=0; vc<CMS_MAX_VCPU_VC; vc++) {
-		/* TODO : remove this once SMP works */
-		if( (j == 0) && (k == 0) )
-			continue;
-		queue = CMS_CPU_PUSHQ(node, j, k, vc);
-		nlm_cms_config_onchip_queue(base, spill_base, queue, spill_en);
-	}
-
-	/* Enable pcie 0 push queue */
-	for (j=CMS_PCIE0_QID(0); j<CMS_PCIE0_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_config_onchip_queue(base, spill_base, queue, spill_en);
-	}
-
-	/* Enable pcie 1 push queue */
-	for (j=CMS_PCIE1_QID(0); j<CMS_PCIE1_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_config_onchip_queue(base, spill_base, queue, spill_en);
-	}
-
-	/* Enable pcie 2 push queue */
-	for (j=CMS_PCIE2_QID(0); j<CMS_PCIE2_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_config_onchip_queue(base, spill_base, queue, spill_en);
-	}
-
-	/* Enable pcie 3 push queue */
-	for (j=CMS_PCIE3_QID(0); j<CMS_PCIE3_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_config_onchip_queue(base, spill_base, queue, spill_en);
-	}
-
-	/* Enable DTE push queue */
-	for (j=CMS_DTE_QID(0); j<CMS_DTE_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_config_onchip_queue(base, spill_base, queue, spill_en);
-	}
-
-	/* Enable RSA/ECC push queue */
-	for (j=CMS_RSA_ECC_QID(0); j<CMS_RSA_ECC_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_config_onchip_queue(base, spill_base, queue, spill_en);
-	}
-
-	/* Enable crypto push queue */
-	for (j=CMS_CRYPTO_QID(0); j<CMS_CRYPTO_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_config_onchip_queue(base, spill_base, queue, spill_en);
-	}
-
-	/* Enable CMP push queue */
-	for (j=CMS_CMP_QID(0); j<CMS_CMP_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_config_onchip_queue(base, spill_base, queue, spill_en);
-	}
-
-	/* Enable POE push queue */
-	for (j=CMS_POE_QID(0); j<CMS_POE_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_config_onchip_queue(base, spill_base, queue, spill_en);
-	}
-
-	/* Enable NAE push queue */
-	for (j=CMS_NAE_QID(0); j<CMS_NAE_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_config_onchip_queue(base, spill_base, queue, spill_en);
-	}
-
-	/* Enable all pop queues */
-	if (popq_en) {
-		for (j=CMS_POPQ_QID(0); j<CMS_POPQ_MAXQID; j++) {
-			queue = CMS_POPQ(node, j);
-			nlm_cms_config_onchip_queue(base, spill_base, queue,
-							spill_en);
-		}
-	}
-}
-
 uint64_t nlm_cms_get_onchip_queue (uint64_t base, int qid)
 {
 	return nlm_read_cms_reg(base, CMS_OUTPUTQ_CONFIG(qid));
@@ -453,94 +198,14 @@ void nlm_cms_per_queue_level_intr(uint64_t base, int qid, int sub_type,
 
 	val = nlm_read_cms_reg(base, CMS_OUTPUTQ_CONFIG(qid));
 
+	val &= ~((0x7ULL << 56) | (0x3ULL << 54));
+
 	val |= (((uint64_t)sub_type<<54) |
 		((uint64_t)intr_val<<56));
 
 	nlm_write_cms_reg(base, CMS_OUTPUTQ_CONFIG(qid), val);
 }
 
-void nlm_cms_level_intr(int node, int sub_type, int intr_val)
-{
-	int j, k, vc;
-	int queue;
-	uint64_t base;
-
-	base = nlm_get_cms_regbase(node);
-	/* setup level intr config on all cpu push queues */
-	for (j=0; j<XLP_MAX_CORES; j++)
-		for (k=0; k<XLP_MAX_THREADS; k++)
-			for (vc=0; vc<CMS_MAX_VCPU_VC; vc++) {
-		queue = CMS_CPU_PUSHQ(node, j, k, vc);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup level intr config on all pcie 0 push queue */
-	for (j=CMS_PCIE0_QID(0); j<CMS_PCIE0_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup level intr config on all pcie 1 push queue */
-	for (j=CMS_PCIE1_QID(0); j<CMS_PCIE1_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup level intr config on all pcie 2 push queue */
-	for (j=CMS_PCIE2_QID(0); j<CMS_PCIE2_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup level intr config on all pcie 3 push queue */
-	for (j=CMS_PCIE3_QID(0); j<CMS_PCIE3_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup level intr config on all DTE push queue */
-	for (j=CMS_DTE_QID(0); j<CMS_DTE_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup level intr config on all RSA/ECC push queue */
-	for (j=CMS_RSA_ECC_QID(0); j<CMS_RSA_ECC_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup level intr config on all crypto push queue */
-	for (j=CMS_CRYPTO_QID(0); j<CMS_CRYPTO_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup level intr config on all CMP push queue */
-	for (j=CMS_CMP_QID(0); j<CMS_CMP_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup level intr config on all POE push queue */
-	for (j=CMS_POE_QID(0); j<CMS_POE_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup level intr config on all NAE push queue */
-	for (j=CMS_NAE_QID(0); j<CMS_NAE_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup level intr config on all pop queues */
-	for (j=CMS_POPQ_QID(0); j<CMS_POPQ_MAXQID; j++) {
-		queue = CMS_POPQ(node, j);
-		nlm_cms_per_queue_level_intr(base, queue, sub_type, intr_val);
-	}
-}
-
 void nlm_cms_per_queue_timer_intr(uint64_t base, int qid, int sub_type,
 					int intr_val)
 {
@@ -548,94 +213,14 @@ void nlm_cms_per_queue_timer_intr(uint64_t base, int qid, int sub_type,
 
 	val = nlm_read_cms_reg(base, CMS_OUTPUTQ_CONFIG(qid));
 
+	val &= ~((0x7ULL << 51) | (0x3ULL << 49));
+
 	val |= (((uint64_t)sub_type<<49) |
 		((uint64_t)intr_val<<51));
 
 	nlm_write_cms_reg(base, CMS_OUTPUTQ_CONFIG(qid), val);
 }
 
-void nlm_cms_timer_intr(int node, int en, int sub_type, int intr_val)
-{
-	int j, k, vc;
-	int queue;
-	uint64_t base;
-
-	base = nlm_get_cms_regbase(node);
-	/* setup timer intr config on all cpu push queues */
-	for (j=0; j<XLP_MAX_CORES; j++)
-		for (k=0; k<XLP_MAX_THREADS; k++)
-			for (vc=0; vc<CMS_MAX_VCPU_VC; vc++) {
-		queue = CMS_CPU_PUSHQ(node, j, k, vc);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup timer intr config on all pcie 0 push queue */
-	for (j=CMS_PCIE0_QID(0); j<CMS_PCIE0_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup timer intr config on all pcie 1 push queue */
-	for (j=CMS_PCIE1_QID(0); j<CMS_PCIE1_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup timer intr config on all pcie 2 push queue */
-	for (j=CMS_PCIE2_QID(0); j<CMS_PCIE2_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup timer intr config on all pcie 3 push queue */
-	for (j=CMS_PCIE3_QID(0); j<CMS_PCIE3_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup timer intr config on all DTE push queue */
-	for (j=CMS_DTE_QID(0); j<CMS_DTE_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup timer intr config on all RSA/ECC push queue */
-	for (j=CMS_RSA_ECC_QID(0); j<CMS_RSA_ECC_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup timer intr config on all crypto push queue */
-	for (j=CMS_CRYPTO_QID(0); j<CMS_CRYPTO_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup timer intr config on all CMP push queue */
-	for (j=CMS_CMP_QID(0); j<CMS_CMP_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup timer intr config on all POE push queue */
-	for (j=CMS_POE_QID(0); j<CMS_POE_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup timer intr config on all NAE push queue */
-	for (j=CMS_NAE_QID(0); j<CMS_NAE_MAXQID; j++) {
-		queue = CMS_IO_PUSHQ(node, j);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-
-	/* setup timer intr config on all pop queues */
-	for (j=CMS_POPQ_QID(0); j<CMS_POPQ_MAXQID; j++) {
-		queue = CMS_POPQ(node, j);
-		nlm_cms_per_queue_timer_intr(base, queue, sub_type, intr_val);
-	}
-}
-
 /* returns 1 if interrupt has been generated for this output queue */
 int nlm_cms_outputq_intr_check(uint64_t base, int qid)
 {
diff --git a/sys/mips/nlm/hal/fmn.h b/sys/mips/nlm/hal/fmn.h
index 88ba113..92f3bd7 100644
--- a/sys/mips/nlm/hal/fmn.h
+++ b/sys/mips/nlm/hal/fmn.h
@@ -30,7 +30,7 @@
  */
 
 #ifndef __NLM_FMNV2_H__
-#define __NLM_FMNV2_H__
+#define	__NLM_FMNV2_H__
 
 /**
 * @file_name fmn.h
@@ -39,156 +39,126 @@
 */
 
 /* FMN configuration registers */
-#define CMS_OUTPUTQ_CONFIG(i)		((i)*2)
-#define CMS_MAX_OUTPUTQ			1024
-#define CMS_OUTPUTQ_CREDIT_CFG		(0x2000/4)
-#define CMS_MSG_CONFIG			(0x2008/4)
-#define CMS_MSG_ERR			(0x2010/4)
-#define CMS_TRACE_CONFIG		(0x2018/4)
-#define CMS_TRACE_BASE_ADDR		(0x2020/4)
-#define CMS_TRACE_LIMIT_ADDR		(0x2028/4)
-#define CMS_TRACE_CURRENT_ADDR		(0x2030/4)
-#define CMS_MSG_ENDIAN_SWAP		(0x2038/4)
-
-#define CMS_CPU_PUSHQ(node, core, thread, vc)	\
+#define	CMS_OUTPUTQ_CONFIG(i)		((i)*2)
+#define	CMS_MAX_OUTPUTQ			1024
+#define	CMS_OUTPUTQ_CREDIT_CFG		(0x2000/4)
+#define	CMS_MSG_CONFIG			(0x2008/4)
+#define	CMS_MSG_ERR			(0x2010/4)
+#define	CMS_TRACE_CONFIG		(0x2018/4)
+#define	CMS_TRACE_BASE_ADDR		(0x2020/4)
+#define	CMS_TRACE_LIMIT_ADDR		(0x2028/4)
+#define	CMS_TRACE_CURRENT_ADDR		(0x2030/4)
+#define	CMS_MSG_ENDIAN_SWAP		(0x2038/4)
+
+#define	CMS_CPU_PUSHQ(node, core, thread, vc)	\
 		(((node)<<10) | ((core)<<4) | ((thread)<<2) | ((vc)<<0))
-#define CMS_POPQ(node, queue)	(((node)<<10) | (queue))
-#define CMS_IO_PUSHQ(node, queue)	(((node)<<10) | (queue))
-
-#define CMS_POPQ_QID(i)		(128+(i))
-#define CMS_POPQ_MAXQID		255
-#define CMS_PCIE0_QID(i)	(256+(i))
-#define CMS_PCIE0_MAXQID	257
-#define CMS_PCIE1_QID(i)	(258+(i))
-#define CMS_PCIE1_MAXQID	259
-#define CMS_PCIE2_QID(i)	(260+(i))
-#define CMS_PCIE2_MAXQID	261
-#define CMS_PCIE3_QID(i)	(262+(i))
-#define CMS_PCIE3_MAXQID	263
-#define CMS_DTE_QID(i)		(264+(i))
-#define CMS_DTE_MAXQID		267
-#define CMS_RSA_ECC_QID(i)	(272+(i))
-#define CMS_RSA_ECC_MAXQID	280
-#define CMS_CRYPTO_QID(i)	(281+(i))
-#define CMS_CRYPTO_MAXQID	296
-/* TODO PCI header register 0x3C says CMP starts at 297(0x129) VERIFY */
-#define CMS_CMP_QID(i)		(298+(i))
-#define CMS_CMP_MAXQID		305
-#define CMS_POE_QID(i)		(384+(i))
-#define CMS_POE_MAXQID		391
-#define CMS_NAE_QID(i)		(476+(i))
-#define CMS_NAE_MAXQID		1023
+#define	CMS_POPQ(node, queue)	(((node)<<10) | (queue))
+#define	CMS_IO_PUSHQ(node, queue)	(((node)<<10) | (queue))
 
-#define CMS_NAE_TX_VC_BASE	476
-#define CMS_NAE_TX_VC_LIMIT	999
-#define CMS_NAE_RX_VC_BASE	1000
-#define CMS_NAE_RX_VC_LIMIT	1019
-
-#define MAX_CMS_QUEUES	1024
+#define	CMS_POPQ_QID(i)		(128+(i))
 
 /* FMN Level Interrupt Type */
-#define CMS_LVL_INTR_DISABLE	0
-#define CMS_LVL_LOW_WATERMARK	1
-#define CMS_LVL_HI_WATERMARK	2
+#define	CMS_LVL_INTR_DISABLE	0
+#define	CMS_LVL_LOW_WATERMARK	1
+#define	CMS_LVL_HI_WATERMARK	2
 
 /* FMN Level interrupt trigger values */
-#define CMS_QUEUE_NON_EMPTY		0
-#define CMS_QUEUE_QUARTER_FULL		1
-#define CMS_QUEUE_HALF_FULL		2
-#define CMS_QUEUE_THREE_QUARTER_FULL	3
-#define CMS_QUEUE_FULL			4
+#define	CMS_QUEUE_NON_EMPTY		0
+#define	CMS_QUEUE_QUARTER_FULL		1
+#define	CMS_QUEUE_HALF_FULL		2
+#define	CMS_QUEUE_THREE_QUARTER_FULL	3
+#define	CMS_QUEUE_FULL			4
 
 /* FMN Timer Interrupt Type */
-#define CMS_TIMER_INTR_DISABLE	0
-#define CMS_TIMER_CONSUMER		1
-#define CMS_TIMER_PRODUCER		1
+#define	CMS_TIMER_INTR_DISABLE	0
+#define	CMS_TIMER_CONSUMER		1
+#define	CMS_TIMER_PRODUCER		1
 
 /* FMN timer interrupt trigger values */
-#define CMS_TWO_POW_EIGHT_CYCLES	0
-#define CMS_TWO_POW_TEN_CYCLES		1
-#define CMS_TWO_POW_TWELVE_CYCLES	2
-#define CMS_TWO_POW_FOURTEEN_CYCLES	3
-#define CMS_TWO_POW_SIXTEEN_CYCLES	4
-#define CMS_TWO_POW_EIGHTTEEN_CYCLES	5
-#define CMS_TWO_POW_TWENTY_CYCLES	6
-#define CMS_TWO_POW_TWENTYTWO_CYCLES	7
-
-#define CMS_QUEUE_ENA		1ULL
-#define CMS_QUEUE_DIS		0
-#define CMS_SPILL_ENA		1ULL
-#define CMS_SPILL_DIS		0
-
-#define CMS_MAX_VCPU_VC		4
+#define	CMS_TWO_POW_EIGHT_CYCLES	0
+#define	CMS_TWO_POW_TEN_CYCLES		1
+#define	CMS_TWO_POW_TWELVE_CYCLES	2
+#define	CMS_TWO_POW_FOURTEEN_CYCLES	3
+#define	CMS_TWO_POW_SIXTEEN_CYCLES	4
+#define	CMS_TWO_POW_EIGHTTEEN_CYCLES	5
+#define	CMS_TWO_POW_TWENTY_CYCLES	6
+#define	CMS_TWO_POW_TWENTYTWO_CYCLES	7
+
+#define	CMS_QUEUE_ENA		1ULL
+#define	CMS_QUEUE_DIS		0
+#define	CMS_SPILL_ENA		1ULL
+#define	CMS_SPILL_DIS		0
+
+#define	CMS_MAX_VCPU_VC		4
 
 /* Each XLP chip can hold upto 32K messages on the chip itself */
-#define CMS_ON_CHIP_MESG_SPACE	(32*1024)
-#define CMS_ON_CHIP_PER_QUEUE_SPACE	\
-		((CMS_ON_CHIP_MESG_SPACE)/(MAX_CMS_QUEUES))
-#define CMS_MAX_ONCHIP_SEGMENTS	1024
-#define CMS_MAX_SPILL_SEGMENTS_PER_QUEUE 	64
+#define	CMS_ON_CHIP_MESG_SPACE	(32*1024)
+#define	CMS_MAX_ONCHIP_SEGMENTS	1024
+#define	CMS_MAX_SPILL_SEGMENTS_PER_QUEUE 	64
 
 /* FMN Network error */
-#define CMS_ILLEGAL_DST_ERROR		0x100
-#define CMS_BIU_TIMEOUT_ERROR		0x080
-#define CMS_BIU_ERROR			0x040
-#define CMS_SPILL_FILL_UNCORRECT_ECC_ERROR	0x020
-#define CMS_SPILL_FILL_CORRECT_ECC_ERROR	0x010
-#define CMS_SPILL_UNCORRECT_ECC_ERROR	0x008
-#define CMS_SPILL_CORRECT_ECC_ERROR		0x004
-#define CMS_OUTPUTQ_UNCORRECT_ECC_ERROR	0x002
-#define CMS_OUTPUTQ_CORRECT_ECC_ERROR	0x001
+#define	CMS_ILLEGAL_DST_ERROR		0x100
+#define	CMS_BIU_TIMEOUT_ERROR		0x080
+#define	CMS_BIU_ERROR			0x040
+#define	CMS_SPILL_FILL_UNCORRECT_ECC_ERROR	0x020
+#define	CMS_SPILL_FILL_CORRECT_ECC_ERROR	0x010
+#define	CMS_SPILL_UNCORRECT_ECC_ERROR	0x008
+#define	CMS_SPILL_CORRECT_ECC_ERROR		0x004
+#define	CMS_OUTPUTQ_UNCORRECT_ECC_ERROR	0x002
+#define	CMS_OUTPUTQ_CORRECT_ECC_ERROR	0x001
 
 /* worst case, a single entry message consists of a 4 byte header
  * and an 8-byte entry = 12 bytes in total
  */
-#define CMS_SINGLE_ENTRY_MSG_SIZE	12
+#define	CMS_SINGLE_ENTRY_MSG_SIZE	12
 /* total spill memory needed for one FMN queue */
-#define CMS_PER_QUEUE_SPILL_MEM(spilltotmsgs)		\
+#define	CMS_PER_QUEUE_SPILL_MEM(spilltotmsgs)		\
 		((spilltotmsgs) * (CMS_SINGLE_ENTRY_MSG_SIZE))
-/* total spill memory needed */
-#define CMS_TOTAL_SPILL_MEM(spilltotmsgs)			\
-		((CMS_PER_QUEUE_SPILL_MEM(spilltotmsgs)) *	\
-		(MAX_CMS_QUEUES))
-/* total number of FMN messages possible in a queue */
-#define CMS_TOTAL_QUEUE_SIZE(spilltotmsgs)			\
-		((spilltotmsgs) + (CMS_ON_CHIP_PER_QUEUE_SPACE))
 
 /* FMN Src station id's */
-#define CMS_CPU0_SRC_STID		(0 << 4)
-#define CMS_CPU1_SRC_STID		(1 << 4)
-#define CMS_CPU2_SRC_STID		(2 << 4)
-#define CMS_CPU3_SRC_STID		(3 << 4)
-#define CMS_CPU4_SRC_STID		(4 << 4)
-#define CMS_CPU5_SRC_STID		(5 << 4)
-#define CMS_CPU6_SRC_STID		(6 << 4)
-#define CMS_CPU7_SRC_STID		(7 << 4)
-#define CMS_PCIE0_SRC_STID		256
-#define CMS_PCIE1_SRC_STID		258
-#define CMS_PCIE2_SRC_STID		260
-#define CMS_PCIE3_SRC_STID		262
-#define CMS_DTE_SRC_STID		264
-#define CMS_RSA_ECC_SRC_STID		272
-#define CMS_CRYPTO_SRC_STID		281
-#define CMS_CMP_SRC_STID		298
-#define CMS_POE_SRC_STID		384
-#define CMS_NAE_SRC_STID		476
-#if 0
-#define CMS_DEFAULT_CREDIT(cmstotstns,spilltotmsgs)	\
-		((CMS_TOTAL_QUEUE_SIZE(spilltotmsgs)) /	\
-		(cmstotstns))
-#endif
-#define CMS_DEFAULT_CREDIT(cmstotstns,spilltotmsgs) 8
+#define	CMS_CPU0_SRC_STID		(0 << 4)
+#define	CMS_CPU1_SRC_STID		(1 << 4)
+#define	CMS_CPU2_SRC_STID		(2 << 4)
+#define	CMS_CPU3_SRC_STID		(3 << 4)
+#define	CMS_CPU4_SRC_STID		(4 << 4)
+#define	CMS_CPU5_SRC_STID		(5 << 4)
+#define	CMS_CPU6_SRC_STID		(6 << 4)
+#define	CMS_CPU7_SRC_STID		(7 << 4)
+#define	CMS_PCIE0_SRC_STID		256
+#define	CMS_PCIE1_SRC_STID		258
+#define	CMS_PCIE2_SRC_STID		260
+#define	CMS_PCIE3_SRC_STID		262
+#define	CMS_DTE_SRC_STID		264
+#define	CMS_RSA_ECC_SRC_STID		272
+#define	CMS_CRYPTO_SRC_STID		281
+#define	CMS_CMP_SRC_STID		298
+#define	CMS_POE_SRC_STID		384
+#define	CMS_NAE_SRC_STID		476
 
 /* POPQ related defines */
-#define CMS_POPQID_START	128
-#define CMS_POPQID_END		255
+#define	CMS_POPQID_START	128
+#define	CMS_POPQID_END		255
 
-#define CMS_INT_RCVD		0x800000000000000ULL
+#define	CMS_INT_RCVD		0x800000000000000ULL
 
 #define	nlm_read_cms_reg(b, r)	nlm_read_reg64_xkphys(b,r)
 #define	nlm_write_cms_reg(b, r, v)	nlm_write_reg64_xkphys(b,r,v)
-#define nlm_get_cms_pcibase(node)	nlm_pcicfg_base(XLP_IO_CMS_OFFSET(node))
-#define nlm_get_cms_regbase(node)	nlm_xkphys_map_pcibar0(nlm_get_cms_pcibase(node))
+#define	nlm_get_cms_pcibase(node)	\
+	nlm_pcicfg_base(XLP_IO_CMS_OFFSET(node))
+#define	nlm_get_cms_regbase(node)	\
+	nlm_xkphys_map_pcibar0(nlm_get_cms_pcibase(node))
+
+#define	XLP_CMS_ON_CHIP_PER_QUEUE_SPACE(node)			\
+		((XLP_CMS_ON_CHIP_MESG_SPACE)/			\
+		(nlm_read_reg(nlm_pcibase_cms(node),		\
+		XLP_PCI_DEVINFO_REG0))
+/* total spill memory needed */
+#define	XLP_CMS_TOTAL_SPILL_MEM(node, spilltotmsgs)		\
+               ((XLP_CMS_PER_QUEUE_SPILL_MEM(spilltotmsgs)) *	\
+		(nlm_read_reg(nlm_pcibase_cms(node),		\
+		XLP_PCI_DEVINFO_REG0))
+#define	CMS_TOTAL_QUEUE_SIZE(node, spilltotmsgs)		\
+		((spilltotmsgs) + (CMS_ON_CHIP_PER_QUEUE_SPACE(node)))
 
 enum fmn_swcode {
 	FMN_SWCODE_CPU0=1,
@@ -238,7 +208,6 @@ enum fmn_swcode {
 
 extern uint64_t nlm_cms_spill_total_messages;
 extern uint32_t nlm_cms_total_stations;
-extern uint32_t cms_onchip_seg_availability[CMS_ON_CHIP_PER_QUEUE_SPACE];
 
 extern uint64_t cms_base_addr(int node);
 extern int nlm_cms_verify_credit_config (int spill_en, int tot_credit);
diff --git a/sys/mips/nlm/hal/iomap.h b/sys/mips/nlm/hal/iomap.h
index a5ba9ba..56b585b 100644
--- a/sys/mips/nlm/hal/iomap.h
+++ b/sys/mips/nlm/hal/iomap.h
@@ -30,9 +30,9 @@
  */
 
 #ifndef __NLM_HAL_IOMAP_H__
-#define __NLM_HAL_IOMAP_H__
+#define	__NLM_HAL_IOMAP_H__
 
-#define	XLP_DEFAULT_IO_BASE		0x18000000
+#define	XLP_DEFAULT_IO_BASE             0x18000000
 #define	NMI_BASE			0xbfc00000
 #define	XLP_IO_CLK			133333333
 
@@ -68,17 +68,21 @@
 
 #define	XLP_IO_NAE_OFFSET(node)		XLP_HDR_OFFSET(node, 0, 3, 0)
 #define	XLP_IO_POE_OFFSET(node)		XLP_HDR_OFFSET(node, 0, 3, 1)
+#define	XLP_IO_SATA_OFFSET(node)	XLP_HDR_OFFSET(node, 0, 3, 2)
 
 #define	XLP_IO_CMS_OFFSET(node)		XLP_HDR_OFFSET(node, 0, 4, 0)
 
-#define	XLP_IO_DMA_OFFSET(node)		XLP_HDR_OFFSET(node, 0, 5, 1)
-#define	XLP_IO_SEC_OFFSET(node)		XLP_HDR_OFFSET(node, 0, 5, 2)
+#define	XLP_IO_DMA_OFFSET(node)		XLP_HDR_OFFSET(node, 0, 5, 0)
+#define	XLP_IO_SEC_OFFSET(node)		XLP_HDR_OFFSET(node, 0, 5, 1)
+#define	XLP_IO_RSA_OFFSET(node)		XLP_HDR_OFFSET(node, 0, 5, 2)
 #define	XLP_IO_CMP_OFFSET(node)		XLP_HDR_OFFSET(node, 0, 5, 3)
+#define	XLP_IO_SRIO_OFFSET(node)	XLP_HDR_OFFSET(node, 0, 5, 4)
+#define	XLP_IO_REGEX_OFFSET(node)	XLP_HDR_OFFSET(node, 0, 5, 5)
 
 #define	XLP_IO_UART_OFFSET(node, i)	XLP_HDR_OFFSET(node, 0, 6, i)
 #define	XLP_IO_UART0_OFFSET(node)	XLP_HDR_OFFSET(node, 0, 6, 0)
 #define	XLP_IO_UART1_OFFSET(node)	XLP_HDR_OFFSET(node, 0, 6, 1)
-#define	XLP_IO_I2C_OFFSET(node, i)	XLP_HDR_OFFSET(node, 0, 6, 2 + (i))
+#define	XLP_IO_I2C_OFFSET(node, i)	XLP_HDR_OFFSET(node, 0, 6, 2 + i)
 #define	XLP_IO_I2C0_OFFSET(node)	XLP_HDR_OFFSET(node, 0, 6, 2)
 #define	XLP_IO_I2C1_OFFSET(node)	XLP_HDR_OFFSET(node, 0, 6, 3)
 #define	XLP_IO_GPIO_OFFSET(node)	XLP_HDR_OFFSET(node, 0, 6, 4)
@@ -90,9 +94,9 @@
 #define	XLP_IO_NAND_OFFSET(node)	XLP_HDR_OFFSET(node, 0, 7, 1)
 #define	XLP_IO_SPI_OFFSET(node)		XLP_HDR_OFFSET(node, 0, 7, 2)
 /* SD flash */
-#define	XLP_IO_SD_OFFSET(node)		XLP_HDR_OFFSET(node, 0, 7, 3)
+#define	XLP_IO_SD_OFFSET(node)          XLP_HDR_OFFSET(node, 0, 7, 3)
 #define	XLP_IO_MMC_OFFSET(node, slot)   \
-	((XLP_IO_SD_OFFSET(node)) + (slot * 0x100) + XLP_IO_PCI_HDRSZ)
+		((XLP_IO_SD_OFFSET(node))+(slot*0x100)+XLP_IO_PCI_HDRSZ)
 
 /* PCI config header register id's */
 #define	XLP_PCI_CFGREG0			0x00
@@ -147,6 +151,45 @@
 
 extern uint64_t xlp_sys_base;
 extern uint64_t xlp_pic_base;
+
+static __inline__ int
+nlm_dev_exists(uint32_t devoffset)
+{
+	uint64_t pcibase = nlm_pcicfg_base(devoffset);
+
+	return (nlm_read_reg(pcibase, XLP_PCI_CFGREG0) != 0xffffffff);
+}
+
+static __inline__ int
+nlm_qidstart(uint64_t pcibase)
+{
+	return (nlm_read_reg(pcibase, XLP_PCI_MSGSTN_REG) & 0xffff);
+}
+
+static __inline__ int
+nlm_qnum(uint64_t pcibase)
+{
+	return (nlm_read_reg(pcibase, XLP_PCI_MSGSTN_REG) >> 16);
+}
+
+static __inline__ int
+nlm_irtstart(uint64_t pcibase)
+{
+	return (nlm_read_reg(pcibase, XLP_PCI_IRTINFO_REG) & 0xffff);
+}
+
+static __inline__ int
+nlm_irtnum(uint64_t pcibase)
+{
+	return (nlm_read_reg(pcibase, XLP_PCI_IRTINFO_REG) >> 16);
+}
+
+static __inline__ int
+nlm_uenginenum(uint64_t pcibase)
+{
+	return nlm_read_reg(pcibase, XLP_PCI_UCODEINFO_REG);
+}
+
 #endif /* !LOCORE or !__ASSEMBLY */
 
 #endif /* __NLM_HAL_IOMAP_H__ */
diff --git a/sys/mips/nlm/hal/nlm_hal.c b/sys/mips/nlm/hal/nlm_hal.c
new file mode 100644
index 0000000..049593c
--- /dev/null
+++ b/sys/mips/nlm/hal/nlm_hal.c
@@ -0,0 +1,170 @@
+/*-
+ * Copyright 2003-2011 Netlogic Microsystems (Netlogic). 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 Netlogic Microsystems ``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 NETLOGIC 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.
+ *
+ * NETLOGIC_BSD */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+#include <sys/param.h>
+#include <sys/types.h>
+#include <sys/systm.h>
+
+#include <mips/nlm/hal/mips-extns.h>
+#include <mips/nlm/hal/haldefs.h>
+#include <mips/nlm/hal/iomap.h>
+#include <mips/nlm/hal/sys.h>
+#include <mips/nlm/hal/pic.h>
+#include <mips/nlm/xlp.h>
+
+#include <mips/nlm/hal/uart.h>
+#include <mips/nlm/hal/mmu.h>
+#include <mips/nlm/hal/pcibus.h>
+#include <mips/nlm/hal/usb.h>
+
+int pic_irt_ehci0;
+int pic_irt_ehci1;
+int pic_irt_uart0;
+int pic_irt_uart1;
+int pic_irt_pcie_lnk0;
+int pic_irt_pcie_lnk1;
+int pic_irt_pcie_lnk2;
+int pic_irt_pcie_lnk3;
+
+uint32_t
+xlp_get_cpu_frequency(int core)
+{
+	uint64_t sysbase = nlm_get_sys_regbase(nlm_nodeid());
+	uint64_t num;
+	unsigned int pll_divf, pll_divr, dfs_div, ext_div;
+	unsigned int rstval, dfsval, denom;
+
+	rstval = nlm_read_sys_reg(sysbase, SYS_POWER_ON_RESET_CFG);
+	dfsval = nlm_read_sys_reg(sysbase, SYS_CORE_DFS_DIV_VALUE);
+	pll_divf = ((rstval >> 10) & 0x7f) + 1;
+	pll_divr = ((rstval >> 8)  & 0x3) + 1;
+	ext_div  = ((rstval >> 30) & 0x3) + 1;
+	dfs_div  = ((dfsval >> (core * 4)) & 0xf) + 1;
+
+	num = 800000000ULL * pll_divf;
+	denom = 3 * pll_divr * ext_div * dfs_div;
+	num = num/denom;
+	return (num);
+}
+
+void
+nlm_pic_irt_init(void)
+{
+	pic_irt_ehci0 = nlm_irtstart(nlm_get_usb_pcibase(nlm_nodeid(), 0));
+	pic_irt_ehci1 = nlm_irtstart(nlm_get_usb_pcibase(nlm_nodeid(), 3));
+	pic_irt_uart0 = nlm_irtstart(nlm_get_uart_pcibase(nlm_nodeid(), 0));
+	pic_irt_uart1 = nlm_irtstart(nlm_get_uart_pcibase(nlm_nodeid(), 1));
+
+	/* Hardcoding the PCIE IRT information as PIC doesn't 
+	   understand any value other than 78,79,80,81 for PCIE0/1/2/3 */
+	pic_irt_pcie_lnk0 = 78;
+	pic_irt_pcie_lnk1 = 79;
+	pic_irt_pcie_lnk2 = 80;
+	pic_irt_pcie_lnk3 = 81;
+}
+/*
+ * Find the IRQ for the link, each link has a different interrupt 
+ * at the XLP pic
+ */
+int xlp_pcie_link_irt(int link)
+{
+
+	if( (link < 0) || (link > 3))
+		return (-1);
+
+	return (pic_irt_pcie_lnk0 + link);
+}
+
+int
+xlp_irt_to_irq(int irt)
+{
+	if (irt == pic_irt_ehci0)
+	    return PIC_EHCI_0_IRQ;
+	else if (irt == pic_irt_ehci1)
+	    return PIC_EHCI_1_IRQ;
+	else if (irt == pic_irt_uart0)
+	    return PIC_UART_0_IRQ;
+	else if (irt == pic_irt_uart1)
+	    return PIC_UART_1_IRQ;
+	else if (irt == pic_irt_pcie_lnk0)
+	    return PIC_PCIE_0_IRQ;
+	else if (irt == pic_irt_pcie_lnk1)
+	    return PIC_PCIE_1_IRQ;
+	else if (irt == pic_irt_pcie_lnk2)
+	    return PIC_PCIE_2_IRQ;
+	else if (irt == pic_irt_pcie_lnk3)
+	    return PIC_PCIE_3_IRQ;
+	else {
+	    printf("Cannot find irq for IRT %d\n", irt);
+	    return 0;
+	 }
+}
+
+int
+xlp_irq_to_irt(int irq)
+{
+	switch (irq) {
+ 		case PIC_EHCI_0_IRQ :
+ 			return pic_irt_ehci0;
+ 		case PIC_EHCI_1_IRQ :
+ 			return pic_irt_ehci1;
+		case PIC_UART_0_IRQ :
+			return pic_irt_uart0;
+		case PIC_UART_1_IRQ :
+			return pic_irt_uart1;
+		case PIC_PCIE_0_IRQ :
+			return pic_irt_pcie_lnk0;
+		case PIC_PCIE_1_IRQ :
+			return pic_irt_pcie_lnk1;
+		case PIC_PCIE_2_IRQ :
+			return pic_irt_pcie_lnk2;
+		case PIC_PCIE_3_IRQ :
+			return pic_irt_pcie_lnk3;
+		default: panic("Bad IRQ %d\n", irq);
+	}
+}
+
+int
+xlp_irq_is_picintr(int irq)
+{
+	switch (irq) {
+		case PIC_MMC_IRQ : return 1;
+ 		case PIC_EHCI_0_IRQ : return 1;
+ 		case PIC_EHCI_1_IRQ : return 1;
+		case PIC_UART_0_IRQ : return 1;
+		case PIC_UART_1_IRQ : return 1;
+		case PIC_PCIE_0_IRQ : return 1;
+		case PIC_PCIE_1_IRQ : return 1;
+		case PIC_PCIE_2_IRQ : return 1;
+		case PIC_PCIE_3_IRQ : return 1;
+		default: return 0;
+	}
+}
diff --git a/sys/mips/nlm/hal/pcibus.h b/sys/mips/nlm/hal/pcibus.h
index 5f3c206..c7e5810 100644
--- a/sys/mips/nlm/hal/pcibus.h
+++ b/sys/mips/nlm/hal/pcibus.h
@@ -57,28 +57,16 @@
 
 #define	MSI_MIPS_DATA_INTVEC		0x000000ff
 
-/*
- * Build Intel MSI message and data values from a source.  AMD64 systems
- * seem to be compatible, so we use the same function for both.
- */
-#define	MIPS_MSI_ADDR(cpu)					\
-	(MSI_MIPS_ADDR_BASE | (cpu) << 12 |			\
-	MSI_MIPS_ADDR_RH_OFF | MSI_MIPS_ADDR_DM_PHYSICAL)
-
-#define MIPS_MSI_DATA(irq)					\
-	(MSI_MIPS_DATA_TRGRLVL | MSI_MIPS_DATA_DELFIXED |	\
-	MSI_MIPS_DATA_ASSERT | (irq))
-
-#define	PCIE_BRIDGE_CMD			0x1
-#define	PCIE_BRIDGE_MSI_CAP		0x14
-#define	PCIE_BRIDGE_MSI_ADDRL		0x15
-#define	PCIE_BRIDGE_MSI_ADDRH		0x16
-#define	PCIE_BRIDGE_MSI_DATA		0x17
+#define	PCIE_BRIDGE_CMD		0x1
+#define	PCIE_BRIDGE_MSI_CAP	0x14
+#define	PCIE_BRIDGE_MSI_ADDRL	0x15
+#define	PCIE_BRIDGE_MSI_ADDRH	0x16
+#define	PCIE_BRIDGE_MSI_DATA	0x17
 
 /* XLP Global PCIE configuration space registers */
-#define	PCIE_MSI_STATUS			0x25A
-#define	PCIE_MSI_EN			0x25B
-#define	PCIE_INT_EN0			0x261
+#define	PCIE_MSI_STATUS		0x25A
+#define	PCIE_MSI_EN		0x25B
+#define	PCIE_INT_EN0		0x261
 
 /* PCIE_MSI_EN */
 #define	PCIE_MSI_VECTOR_INT_EN		0xFFFFFFFF
@@ -86,4 +74,30 @@
 /* PCIE_INT_EN0 */
 #define	PCIE_MSI_INT_EN			(1 << 9)
 
+#if !defined(LOCORE) && !defined(__ASSEMBLY__)
+
+#define	nlm_read_pcie_reg(b, r)		nlm_read_reg(b, r)
+#define	nlm_write_pcie_reg(b, r, v)	nlm_write_reg(b, r, v)
+#define	nlm_get_pcie_base(node, inst)	\
+				nlm_pcicfg_base(XLP_IO_PCIE_OFFSET(node, inst))
+#define	nlm_get_pcie_regbase(node, inst)	\
+				(nlm_get_pcie_base(node, inst) + XLP_IO_PCI_HDRSZ)
+
+/*
+ * Build Intel MSI message and data values from a source.  AMD64 systems
+ * seem to be compatible, so we use the same function for both.
+ */
+#define	MIPS_MSI_ADDR(cpu)					       \
+        (MSI_MIPS_ADDR_BASE | (cpu) << 12 |			       \
+	 MSI_MIPS_ADDR_RH_OFF | MSI_MIPS_ADDR_DM_PHYSICAL)
+
+#define	MIPS_MSI_DATA(irq)					       \
+        (MSI_MIPS_DATA_TRGRLVL | MSI_MIPS_DATA_DELFIXED |	       \
+	 MSI_MIPS_DATA_ASSERT | (irq))
+
+#endif
+
+#ifndef LOCORE
+int xlp_pcie_link_irt(int link);
+#endif
 #endif /* __XLP_PCIBUS_H__ */
diff --git a/sys/mips/nlm/hal/pic.h b/sys/mips/nlm/hal/pic.h
index efc676e..7313e5c 100644
--- a/sys/mips/nlm/hal/pic.h
+++ b/sys/mips/nlm/hal/pic.h
@@ -30,234 +30,163 @@
  */
 
 #ifndef _NLM_HAL_PIC_H
-#define _NLM_HAL_PIC_H
+#define	_NLM_HAL_PIC_H
 
 /* PIC Specific registers */
-#define PIC_CTRL                0x00
+#define	PIC_CTRL                0x00
 
 /* PIC control register defines */
-#define PIC_CTRL_ITV		32 /* interrupt timeout value */
-#define PIC_CTRL_ICI		19 /* ICI interrupt timeout enable */
-#define PIC_CTRL_ITE		18 /* interrupt timeout enable */
-#define PIC_CTRL_STE		10 /* system timer interrupt enable */
-#define PIC_CTRL_WWR1		8  /* watchdog 1 wraparound count for reset */
-#define PIC_CTRL_WWR0		6  /* watchdog 0 wraparound count for reset */
-#define PIC_CTRL_WWN1		4  /* watchdog 1 wraparound count for NMI */
-#define PIC_CTRL_WWN0		2  /* watchdog 0 wraparound count for NMI */
-#define PIC_CTRL_WTE		0  /* watchdog timer enable */
+#define	PIC_CTRL_ITV		32 /* interrupt timeout value */
+#define	PIC_CTRL_ICI		19 /* ICI interrupt timeout enable */
+#define	PIC_CTRL_ITE		18 /* interrupt timeout enable */
+#define	PIC_CTRL_STE		10 /* system timer interrupt enable */
+#define	PIC_CTRL_WWR1		8  /* watchdog 1 wraparound count for reset */
+#define	PIC_CTRL_WWR0		6  /* watchdog 0 wraparound count for reset */
+#define	PIC_CTRL_WWN1		4  /* watchdog 1 wraparound count for NMI */
+#define	PIC_CTRL_WWN0		2  /* watchdog 0 wraparound count for NMI */
+#define	PIC_CTRL_WTE		0  /* watchdog timer enable */
 
 /* PIC Status register defines */
-#define PIC_ICI_STATUS		33 /* ICI interrupt timeout status */
-#define PIC_ITE_STATUS		32 /* interrupt timeout status */
-#define PIC_STS_STATUS		4  /* System timer interrupt status */
-#define PIC_WNS_STATUS		2  /* NMI status for watchdog timers */
-#define PIC_WIS_STATUS		0  /* Interrupt status for watchdog timers */
+#define	PIC_ICI_STATUS		33 /* ICI interrupt timeout status */
+#define	PIC_ITE_STATUS		32 /* interrupt timeout status */
+#define	PIC_STS_STATUS		4  /* System timer interrupt status */
+#define	PIC_WNS_STATUS		2  /* NMI status for watchdog timers */
+#define	PIC_WIS_STATUS		0  /* Interrupt status for watchdog timers */
 
 /* PIC IPI control register offsets */
-#define PIC_IPICTRL_NMI		32
-#define PIC_IPICTRL_RIV		20 /* received interrupt vector */
-#define PIC_IPICTRL_IDB		16 /* interrupt destination base */
-#define PIC_IPICTRL_DTE		 0 /* interrupt destination thread enables */
+#define	PIC_IPICTRL_NMI		32
+#define	PIC_IPICTRL_RIV		20 /* received interrupt vector */
+#define	PIC_IPICTRL_IDB		16 /* interrupt destination base */
+#define	PIC_IPICTRL_DTE		 0 /* interrupt destination thread enables */
 
 /* PIC IRT register offsets */
-#define PIC_IRT_ENABLE		31
-#define PIC_IRT_NMI		29
-#define PIC_IRT_SCH		28 /* Scheduling scheme */
-#define PIC_IRT_RVEC		20 /* Interrupt receive vectors */
-#define PIC_IRT_DT		19 /* Destination type */
-#define PIC_IRT_DB		16 /* Destination base */
-#define PIC_IRT_DTE		0  /* Destination thread enables */
-
-#define PIC_BYTESWAP		0x02
-#define PIC_STATUS		0x04
-#define PIC_INTR_TIMEOUT	0x06
-#define PIC_ICI0_INTR_TIMEOUT	0x08
-#define PIC_ICI1_INTR_TIMEOUT	0x0a
-#define PIC_ICI2_INTR_TIMEOUT	0x0c
-#define PIC_IPI_CTL		0x0e
-#define PIC_INT_ACK		0x10
-#define PIC_INT_PENDING0	0x12
-#define PIC_INT_PENDING1	0x14
-#define PIC_INT_PENDING2	0x16
-
-#define PIC_WDOG0_MAXVAL	0x18
-#define PIC_WDOG0_COUNT		0x1a
-#define PIC_WDOG0_ENABLE0	0x1c
-#define PIC_WDOG0_ENABLE1	0x1e
-#define PIC_WDOG0_BEATCMD	0x20
-#define PIC_WDOG0_BEAT0		0x22
-#define PIC_WDOG0_BEAT1		0x24
-
-#define PIC_WDOG1_MAXVAL	0x26
-#define PIC_WDOG1_COUNT		0x28
-#define PIC_WDOG1_ENABLE0	0x2a
-#define PIC_WDOG1_ENABLE1	0x2c
-#define PIC_WDOG1_BEATCMD	0x2e
-#define PIC_WDOG1_BEAT0		0x30
-#define PIC_WDOG1_BEAT1		0x32
-
-#define PIC_WDOG_MAXVAL(i)	(PIC_WDOG0_MAXVAL + ((i) ? 7 : 0))
-#define PIC_WDOG_COUNT(i)	(PIC_WDOG0_COUNT + ((i) ? 7 : 0))
-#define PIC_WDOG_ENABLE0(i)	(PIC_WDOG0_ENABLE0 + ((i) ? 7 : 0))
-#define PIC_WDOG_ENABLE1(i)	(PIC_WDOG0_ENABLE1 + ((i) ? 7 : 0))
-#define PIC_WDOG_BEATCMD(i)	(PIC_WDOG0_BEATCMD + ((i) ? 7 : 0))
-#define PIC_WDOG_BEAT0(i)	(PIC_WDOG0_BEAT0 + ((i) ? 7 : 0))
-#define PIC_WDOG_BEAT1(i)	(PIC_WDOG0_BEAT1 + ((i) ? 7 : 0))
-
-#define PIC_TIMER0_MAXVAL	0x34
-#define PIC_TIMER1_MAXVAL	0x36
-#define PIC_TIMER2_MAXVAL	0x38
-#define PIC_TIMER3_MAXVAL	0x3a
-#define PIC_TIMER4_MAXVAL	0x3c
-#define PIC_TIMER5_MAXVAL	0x3e
-#define PIC_TIMER6_MAXVAL	0x40
-#define PIC_TIMER7_MAXVAL	0x42
-#define PIC_TIMER_MAXVAL(i)	(PIC_TIMER0_MAXVAL + ((i) * 2))
-
-#define PIC_TIMER0_COUNT	0x44
-#define PIC_TIMER1_COUNT	0x46
-#define PIC_TIMER2_COUNT	0x48
-#define PIC_TIMER3_COUNT	0x4a
-#define PIC_TIMER4_COUNT	0x4c
-#define PIC_TIMER5_COUNT	0x4e
-#define PIC_TIMER6_COUNT	0x50
-#define PIC_TIMER7_COUNT	0x52
-#define PIC_TIMER_COUNT(i)	(PIC_TIMER0_COUNT + ((i) * 2))
-
-#define PIC_ITE0_N0_N1		0x54
-#define PIC_ITE1_N0_N1		0x58
-#define PIC_ITE2_N0_N1		0x5c
-#define PIC_ITE3_N0_N1		0x60
-#define PIC_ITE4_N0_N1		0x64
-#define PIC_ITE5_N0_N1		0x68
-#define PIC_ITE6_N0_N1		0x6c
-#define PIC_ITE7_N0_N1		0x70
-#define PIC_ITE_N0_N1(i)	(PIC_ITE0_N0_N1 + ((i) * 4))
-
-#define PIC_ITE0_N2_N3		0x56
-#define PIC_ITE1_N2_N3		0x5a
-#define PIC_ITE2_N2_N3		0x5e
-#define PIC_ITE3_N2_N3		0x62
-#define PIC_ITE4_N2_N3		0x66
-#define PIC_ITE5_N2_N3		0x6a
-#define PIC_ITE6_N2_N3		0x6e
-#define PIC_ITE7_N2_N3		0x72
-#define PIC_ITE_N2_N3(i)		(PIC_ITE0_N2_N3 + ((i) * 4))
-
-#define PIC_IRT0		0x74
-#define PIC_IRT(i)		(PIC_IRT0 + ((i) * 2))
-
-#define TIMER_CYCLES_MAXVAL	0xffffffffffffffffULL
+#define	PIC_IRT_ENABLE		31
+#define	PIC_IRT_NMI		29
+#define	PIC_IRT_SCH		28 /* Scheduling scheme */
+#define	PIC_IRT_RVEC		20 /* Interrupt receive vectors */
+#define	PIC_IRT_DT		19 /* Destination type */
+#define	PIC_IRT_DB		16 /* Destination base */
+#define	PIC_IRT_DTE		0  /* Destination thread enables */
+
+#define	PIC_BYTESWAP		0x02
+#define	PIC_STATUS		0x04
+#define	PIC_INTR_TIMEOUT	0x06
+#define	PIC_ICI0_INTR_TIMEOUT	0x08
+#define	PIC_ICI1_INTR_TIMEOUT	0x0a
+#define	PIC_ICI2_INTR_TIMEOUT	0x0c
+#define	PIC_IPI_CTL		0x0e
+#define	PIC_INT_ACK		0x10
+#define	PIC_INT_PENDING0	0x12
+#define	PIC_INT_PENDING1	0x14
+#define	PIC_INT_PENDING2	0x16
+
+#define	PIC_WDOG0_MAXVAL	0x18
+#define	PIC_WDOG0_COUNT		0x1a
+#define	PIC_WDOG0_ENABLE0	0x1c
+#define	PIC_WDOG0_ENABLE1	0x1e
+#define	PIC_WDOG0_BEATCMD	0x20
+#define	PIC_WDOG0_BEAT0		0x22
+#define	PIC_WDOG0_BEAT1		0x24
+
+#define	PIC_WDOG1_MAXVAL	0x26
+#define	PIC_WDOG1_COUNT		0x28
+#define	PIC_WDOG1_ENABLE0	0x2a
+#define	PIC_WDOG1_ENABLE1	0x2c
+#define	PIC_WDOG1_BEATCMD	0x2e
+#define	PIC_WDOG1_BEAT0		0x30
+#define	PIC_WDOG1_BEAT1		0x32
+
+#define	PIC_WDOG_MAXVAL(i)	(PIC_WDOG0_MAXVAL + ((i) ? 7 : 0))
+#define	PIC_WDOG_COUNT(i)	(PIC_WDOG0_COUNT + ((i) ? 7 : 0))
+#define	PIC_WDOG_ENABLE0(i)	(PIC_WDOG0_ENABLE0 + ((i) ? 7 : 0))
+#define	PIC_WDOG_ENABLE1(i)	(PIC_WDOG0_ENABLE1 + ((i) ? 7 : 0))
+#define	PIC_WDOG_BEATCMD(i)	(PIC_WDOG0_BEATCMD + ((i) ? 7 : 0))
+#define	PIC_WDOG_BEAT0(i)	(PIC_WDOG0_BEAT0 + ((i) ? 7 : 0))
+#define	PIC_WDOG_BEAT1(i)	(PIC_WDOG0_BEAT1 + ((i) ? 7 : 0))
+
+#define	PIC_TIMER0_MAXVAL	0x34
+#define	PIC_TIMER1_MAXVAL	0x36
+#define	PIC_TIMER2_MAXVAL	0x38
+#define	PIC_TIMER3_MAXVAL	0x3a
+#define	PIC_TIMER4_MAXVAL	0x3c
+#define	PIC_TIMER5_MAXVAL	0x3e
+#define	PIC_TIMER6_MAXVAL	0x40
+#define	PIC_TIMER7_MAXVAL	0x42
+#define	PIC_TIMER_MAXVAL(i)	(PIC_TIMER0_MAXVAL + ((i) * 2))
+
+#define	PIC_TIMER0_COUNT	0x44
+#define	PIC_TIMER1_COUNT	0x46
+#define	PIC_TIMER2_COUNT	0x48
+#define	PIC_TIMER3_COUNT	0x4a
+#define	PIC_TIMER4_COUNT	0x4c
+#define	PIC_TIMER5_COUNT	0x4e
+#define	PIC_TIMER6_COUNT	0x50
+#define	PIC_TIMER7_COUNT	0x52
+#define	PIC_TIMER_COUNT(i)	(PIC_TIMER0_COUNT + ((i) * 2))
+
+#define	PIC_ITE0_N0_N1		0x54
+#define	PIC_ITE1_N0_N1		0x58
+#define	PIC_ITE2_N0_N1		0x5c
+#define	PIC_ITE3_N0_N1		0x60
+#define	PIC_ITE4_N0_N1		0x64
+#define	PIC_ITE5_N0_N1		0x68
+#define	PIC_ITE6_N0_N1		0x6c
+#define	PIC_ITE7_N0_N1		0x70
+#define	PIC_ITE_N0_N1(i)	(PIC_ITE0_N0_N1 + ((i) * 4))
+
+#define	PIC_ITE0_N2_N3		0x56
+#define	PIC_ITE1_N2_N3		0x5a
+#define	PIC_ITE2_N2_N3		0x5e
+#define	PIC_ITE3_N2_N3		0x62
+#define	PIC_ITE4_N2_N3		0x66
+#define	PIC_ITE5_N2_N3		0x6a
+#define	PIC_ITE6_N2_N3		0x6e
+#define	PIC_ITE7_N2_N3		0x72
+#define	PIC_ITE_N2_N3(i)		(PIC_ITE0_N2_N3 + ((i) * 4))
+
+#define	PIC_IRT0		0x74
+#define	PIC_IRT(i)		(PIC_IRT0 + ((i) * 2))
+
+#define	TIMER_CYCLES_MAXVAL	0xffffffffffffffffULL
 
 /*
  *    IRT Map
  */
-#define PIC_NUM_IRTS		160
-
-#define PIC_IRT_WD_0_INDEX	0
-#define PIC_IRT_WD_1_INDEX	1
-#define PIC_IRT_WD_NMI_0_INDEX	2
-#define PIC_IRT_WD_NMI_1_INDEX	3
-#define PIC_IRT_TIMER_0_INDEX	4
-#define PIC_IRT_TIMER_1_INDEX	5
-#define PIC_IRT_TIMER_2_INDEX	6
-#define PIC_IRT_TIMER_3_INDEX	7
-#define PIC_IRT_TIMER_4_INDEX	8
-#define PIC_IRT_TIMER_5_INDEX	9
-#define PIC_IRT_TIMER_6_INDEX	10
-#define PIC_IRT_TIMER_7_INDEX	11
-#define PIC_IRT_CLOCK_INDEX	PIC_IRT_TIMER_7_INDEX
-#define PIC_IRT_TIMER_INDEX(num)	((num) + PIC_IRT_TIMER_0_INDEX)
-
-
-/* 11 and 12 */
-#define PIC_NUM_MSG_Q_IRTS	32
-#define PIC_IRT_MSG_Q0_INDEX	12
-#define PIC_IRT_MSG_Q_INDEX(qid)	((qid) + PIC_IRT_MSG_Q0_INDEX)
-/* 12 to 43 */
-#define PIC_IRT_MSG_0_INDEX	44
-#define PIC_IRT_MSG_1_INDEX	45
-/* 44 and 45 */
-#define PIC_NUM_PCIE_MSIX_IRTS	32
-#define PIC_IRT_PCIE_MSIX_0_INDEX	46
-#define PIC_IRT_PCIE_MSIX_INDEX(num)	((num) + PIC_IRT_PCIE_MSIX_0_INDEX)
-/* 46 to 77 */
-#define PIC_NUM_PCIE_LINK_IRTS		4
-#define PIC_IRT_PCIE_LINK_0_INDEX	78
-#define PIC_IRT_PCIE_LINK_1_INDEX	79
-#define PIC_IRT_PCIE_LINK_2_INDEX	80
-#define PIC_IRT_PCIE_LINK_3_INDEX	81
-#define PIC_IRT_PCIE_LINK_INDEX(num)	((num) + PIC_IRT_PCIE_LINK_0_INDEX)
-/* 78 to 81 */
-#define PIC_NUM_NA_IRTS			32
-/* 82 to 113 */
-#define PIC_IRT_NA_0_INDEX		82
-#define PIC_IRT_NA_INDEX(num)		((num) + PIC_IRT_NA_0_INDEX)
-#define PIC_IRT_POE_INDEX		114
-
-#define PIC_NUM_USB_IRTS		6
-#define PIC_IRT_USB_0_INDEX		115
-#define PIC_IRT_EHCI_0_INDEX		115
-#define PIC_IRT_EHCI_1_INDEX		118
-#define PIC_IRT_USB_INDEX(num)		((num) + PIC_IRT_USB_0_INDEX)
-/* 115 to 120 */
-#define PIC_IRT_GDX_INDEX		121
-#define PIC_IRT_SEC_INDEX		122
-#define PIC_IRT_RSA_INDEX		123
-
-#define PIC_NUM_COMP_IRTS		4
-#define PIC_IRT_COMP_0_INDEX		124
-#define PIC_IRT_COMP_INDEX(num)		((num) + PIC_IRT_COMP_0_INDEX)
-/* 124 to 127 */
-#define PIC_IRT_GBU_INDEX		128
-#define PIC_IRT_ICC_0_INDEX		129 /* ICC - Inter Chip Coherency */
-#define PIC_IRT_ICC_1_INDEX		130
-#define PIC_IRT_ICC_2_INDEX		131
-#define PIC_IRT_CAM_INDEX		132
-#define PIC_IRT_UART_0_INDEX		133
-#define PIC_IRT_UART_1_INDEX		134
-#define PIC_IRT_I2C_0_INDEX		135
-#define PIC_IRT_I2C_1_INDEX		136
-#define PIC_IRT_SYS_0_INDEX		137
-#define PIC_IRT_SYS_1_INDEX		138
-#define PIC_IRT_JTAG_INDEX		139
-#define PIC_IRT_PIC_INDEX		140
-#define PIC_IRT_NBU_INDEX		141
-#define PIC_IRT_TCU_INDEX		142
-#define PIC_IRT_GCU_INDEX		143 /* GBC - Global Coherency */
-#define PIC_IRT_DMC_0_INDEX		144
-#define PIC_IRT_DMC_1_INDEX		145
-
-#define PIC_NUM_GPIO_IRTS		4
-#define PIC_IRT_GPIO_0_INDEX		146
-#define PIC_IRT_GPIO_INDEX(num)		((num) + PIC_IRT_GPIO_0_INDEX)
-
-/* 146 to 149 */
-#define PIC_IRT_NOR_INDEX		150
-#define PIC_IRT_NAND_INDEX		151
-#define PIC_IRT_SPI_INDEX		152
-#define PIC_IRT_MMC_INDEX		153
-
-#define PIC_CLOCK_TIMER			7
-#define PIC_IRQ_BASE			8
+#define	PIC_IRT_WD_0_INDEX	0
+#define	PIC_IRT_WD_1_INDEX	1
+#define	PIC_IRT_WD_NMI_0_INDEX	2
+#define	PIC_IRT_WD_NMI_1_INDEX	3
+#define	PIC_IRT_TIMER_0_INDEX	4
+#define	PIC_IRT_TIMER_1_INDEX	5
+#define	PIC_IRT_TIMER_2_INDEX	6
+#define	PIC_IRT_TIMER_3_INDEX	7
+#define	PIC_IRT_TIMER_4_INDEX	8
+#define	PIC_IRT_TIMER_5_INDEX	9
+#define	PIC_IRT_TIMER_6_INDEX	10
+#define	PIC_IRT_TIMER_7_INDEX	11
+#define	PIC_IRT_CLOCK_INDEX	PIC_IRT_TIMER_7_INDEX
+#define	PIC_IRT_TIMER_INDEX(num)	((num) + PIC_IRT_TIMER_0_INDEX)
+
+#define	PIC_CLOCK_TIMER			7
+#define	PIC_IRQ_BASE			8
 
 #if !defined(LOCORE) && !defined(__ASSEMBLY__)
 
-#define PIC_IRT_FIRST_IRQ		(PIC_IRQ_BASE)
-#define PIC_IRT_LAST_IRQ		63
-#define PIC_IRQ_IS_IRT(irq)		((irq) >= PIC_IRT_FIRST_IRQ)
+#define	PIC_IRT_FIRST_IRQ		(PIC_IRQ_BASE)
+#define	PIC_IRT_LAST_IRQ		63
+#define	PIC_IRQ_IS_IRT(irq)		((irq) >= PIC_IRT_FIRST_IRQ)
 
 /*
  *   Misc
  */
-#define PIC_IRT_VALID			1
-#define PIC_LOCAL_SCHEDULING		1
-#define PIC_GLOBAL_SCHEDULING		0
+#define	PIC_IRT_VALID			1
+#define	PIC_LOCAL_SCHEDULING		1
+#define	PIC_GLOBAL_SCHEDULING		0
 
-#define nlm_read_pic_reg(b, r)	nlm_read_reg64(b, r)
-#define nlm_write_pic_reg(b, r, v) nlm_write_reg64(b, r, v)
-#define nlm_get_pic_pcibase(node) nlm_pcicfg_base(XLP_IO_PIC_OFFSET(node))
-#define nlm_get_pic_regbase(node) (nlm_get_pic_pcibase(node) + XLP_IO_PCI_HDRSZ)
+#define	nlm_read_pic_reg(b, r)	nlm_read_reg64(b, r)
+#define	nlm_write_pic_reg(b, r, v) nlm_write_reg64(b, r, v)
+#define	nlm_get_pic_pcibase(node) nlm_pcicfg_base(XLP_IO_PIC_OFFSET(node))
+#define	nlm_get_pic_regbase(node) (nlm_get_pic_pcibase(node) + XLP_IO_PCI_HDRSZ)
 
 /* IRT and h/w interrupt routines */
 static inline int
@@ -363,15 +292,22 @@ nlm_pic_write_timer(uint64_t base, int timer, uint64_t value)
 static inline void
 nlm_pic_set_timer(uint64_t base, int timer, uint64_t value, int irq, int cpu)
 {
-	uint64_t pic_ctrl = nlm_read_pic_reg(base, PIC_CTRL);
-	int en;
-
-	en = (irq > 0);
+	uint64_t pic_ctrl;
+	int en, nmi;
+
+	en = nmi = 0;
+	if (irq > 0)
+		en = 1;
+	else if (irq < 0) {
+		en = nmi = 1;
+		irq = -irq;
+	}
 	nlm_write_pic_reg(base, PIC_TIMER_MAXVAL(timer), value);
 	nlm_pic_write_irt_direct(base, PIC_IRT_TIMER_INDEX(timer),
-		en, 0, 0, irq, cpu);
+		en, nmi, 0, irq, cpu);
 
 	/* enable the timer */
+	pic_ctrl = nlm_read_pic_reg(base, PIC_CTRL);
 	pic_ctrl |= (1 << (PIC_CTRL_STE + timer));
 	nlm_write_pic_reg(base, PIC_CTRL, pic_ctrl);
 }
diff --git a/sys/mips/nlm/msgring.h b/sys/mips/nlm/msgring.h
index e57a0fe..cf5f66e 100644
--- a/sys/mips/nlm/msgring.h
+++ b/sys/mips/nlm/msgring.h
@@ -29,15 +29,12 @@
  * $FreeBSD$
  */
 
+#define	CMS_DEFAULT_CREDIT	50
+
 extern uint32_t xlp_msg_thread_mask;
 typedef void (*msgring_handler)(int, int, int, int, struct nlm_fmn_msg *, void *);
 int register_msgring_handler(int startb, int endb, msgring_handler action,
 		                    void *arg);
-int xlp_handle_msg_vc(int vc, int max_msgs);
-void xlp_msgring_cpu_init(uint32_t);
-void xlp_msgring_config(void);
-void xlp_cpu_msgring_handler(int bucket, int size, int code, int stid,
-		    struct nlm_fmn_msg *msg, void *data);
-
-void nlm_cms_credit_setup(int credit);
-void xlp_msgring_iodi_config(void);
+int xlp_handle_msg_vc(u_int vcmask, int max_msgs);
+void xlp_msgring_cpu_init(int, int, int);
+void xlp_cms_enable_intr(int , int , int , int);
diff --git a/sys/mips/nlm/xlp.h b/sys/mips/nlm/xlp.h
index b400891..c80c80a 100644
--- a/sys/mips/nlm/xlp.h
+++ b/sys/mips/nlm/xlp.h
@@ -31,7 +31,8 @@
 
 #ifndef __NLM_XLP_H__
 #define __NLM_XLP_H__
-#include <mips/nlm/hal/pic.h>
+#include <mips/nlm/hal/mips-extns.h>
+#include <mips/nlm/hal/iomap.h>
 
 #define PIC_UART_0_IRQ	9
 #define PIC_UART_1_IRQ	10
@@ -43,7 +44,17 @@
 
 #define PIC_EHCI_0_IRQ	39 
 #define PIC_EHCI_1_IRQ	42 
-#define PIC_MMC_IRQ		43
+#define PIC_MMC_IRQ	43
+
+/* XLP 8xx/4xx A0, A1, A2 CPU COP0 PRIDs */
+#define	CHIP_PROCESSOR_ID_XLP_8XX		0x10
+#define	CHIP_PROCESSOR_ID_XLP_3XX		0x11
+
+/* Revision id's */
+#define	XLP_REVISION_A0				0x00
+#define	XLP_REVISION_A1				0x01
+#define	XLP_REVISION_A2				0x02
+#define	XLP_REVISION_B0				0x03
 
 #ifndef LOCORE
 /*
@@ -58,74 +69,25 @@ extern int xlp_hwtid_to_cpuid[];
 #ifdef SMP
 extern void xlp_enable_threads(int code);
 #endif
+uint32_t xlp_get_cpu_frequency(int core);
+void nlm_pic_irt_init(void);
+int xlp_irt_to_irq(int irt);
+int xlp_irq_to_irt(int irq);
+int xlp_irq_is_picintr(int irq);
 
-static __inline__ int
-xlp_irt_to_irq(int irt)
+static __inline int nlm_is_xlp3xx(void)
 {
-	switch (irt) {
-		case PIC_IRT_MMC_INDEX :
-			return PIC_MMC_IRQ;
-		case PIC_IRT_EHCI_0_INDEX :
- 			return PIC_EHCI_0_IRQ;
- 		case PIC_IRT_EHCI_1_INDEX :
- 			return PIC_EHCI_1_IRQ;
-		case PIC_IRT_UART_0_INDEX :
-			return PIC_UART_0_IRQ;
-		case PIC_IRT_UART_1_INDEX :
-			return PIC_UART_1_IRQ;
-		case PIC_IRT_PCIE_LINK_0_INDEX :
-			return PIC_PCIE_0_IRQ;
-		case PIC_IRT_PCIE_LINK_1_INDEX :
-			return PIC_PCIE_1_IRQ;
-		case PIC_IRT_PCIE_LINK_2_INDEX :
-			return PIC_PCIE_2_IRQ;
-		case PIC_IRT_PCIE_LINK_3_INDEX :
-			return PIC_PCIE_3_IRQ;
-		default: panic("Bad IRT %d\n", irt);
-	}
-}
+	int prid = (mips_rd_prid() >> 8) & 0xff;
 
-static __inline__ int
-xlp_irq_to_irt(int irq)
-{
-	switch (irq) {
-		case PIC_MMC_IRQ :
-			return PIC_IRT_MMC_INDEX;
- 		case PIC_EHCI_0_IRQ :
- 			return PIC_IRT_EHCI_0_INDEX;
- 		case PIC_EHCI_1_IRQ :
- 			return PIC_IRT_EHCI_1_INDEX;
-		case PIC_UART_0_IRQ :
-			return PIC_IRT_UART_0_INDEX;
-		case PIC_UART_1_IRQ :
-			return PIC_IRT_UART_1_INDEX;
-		case PIC_PCIE_0_IRQ :
-			return PIC_IRT_PCIE_LINK_0_INDEX;
-		case PIC_PCIE_1_IRQ :
-			return PIC_IRT_PCIE_LINK_1_INDEX;
-		case PIC_PCIE_2_IRQ :
-			return PIC_IRT_PCIE_LINK_2_INDEX;
-		case PIC_PCIE_3_IRQ :
-			return PIC_IRT_PCIE_LINK_3_INDEX;
-		default: panic("Bad IRQ %d\n", irq);
-	}
+	return (prid == CHIP_PROCESSOR_ID_XLP_3XX);
 }
 
-static __inline__ int
-xlp_irq_is_picintr(int irq)
+static __inline int nlm_is_xlp8xx(void)
 {
-	switch (irq) {
-		case PIC_MMC_IRQ : return 1;
- 		case PIC_EHCI_0_IRQ : return 1;
- 		case PIC_EHCI_1_IRQ : return 1;
-		case PIC_UART_0_IRQ : return 1;
-		case PIC_UART_1_IRQ : return 1;
-		case PIC_PCIE_0_IRQ : return 1;
-		case PIC_PCIE_1_IRQ : return 1;
-		case PIC_PCIE_2_IRQ : return 1;
-		case PIC_PCIE_3_IRQ : return 1;
-		default: return 0;
-	}
+	int prid = (mips_rd_prid() >> 8) & 0xff;
+
+	return (prid == CHIP_PROCESSOR_ID_XLP_8XX);
 }
+
 #endif /* LOCORE */
 #endif /* __NLM_XLP_H__ */
diff --git a/sys/mips/nlm/xlp_machdep.c b/sys/mips/nlm/xlp_machdep.c
index 5724df0..0744f5b 100644
--- a/sys/mips/nlm/xlp_machdep.c
+++ b/sys/mips/nlm/xlp_machdep.c
@@ -80,11 +80,13 @@ __FBSDID("$FreeBSD$");
 #include <mips/nlm/hal/mmu.h>
 #include <mips/nlm/hal/bridge.h>
 #include <mips/nlm/hal/cpucontrol.h>
+#include <mips/nlm/hal/cop2.h>
 
 #include <mips/nlm/clock.h>
 #include <mips/nlm/interrupt.h>
 #include <mips/nlm/board.h>
 #include <mips/nlm/xlp.h>
+#include <mips/nlm/msgring.h>
 
 #ifdef FDT
 #include <dev/fdt/fdt_common.h>
@@ -137,19 +139,30 @@ xlp_setup_core(void)
 static void 
 xlp_setup_mmu(void)
 {
+	uint32_t pagegrain;
 
-	nlm_setup_extended_pagemask(0); /* pagemask = 0 for 4K pages */
-	nlm_large_variable_tlb_en(0);
-	nlm_extended_tlb_en(1);
-	nlm_mmu_setup(0, 0, 0);
+	if (nlm_threadid() == 0) {
+		nlm_setup_extended_pagemask(0);
+		nlm_large_variable_tlb_en(1);
+		nlm_extended_tlb_en(1);
+		nlm_mmu_setup(0, 0, 0);
+	}
+
+	/* Enable no-read, no-exec, large-physical-address */
+	pagegrain = mips_rd_pagegrain();
+	pagegrain |= (1 << 31)	|	/* RIE */
+	    (1 << 30)		|	/* XIE */
+	    (1 << 29);			/* ELPA */
+	mips_wr_pagegrain(pagegrain);
 }
 
 static void
 xlp_parse_mmu_options(void)
 {
-	int i, j, k;
+	uint64_t sysbase;
 	uint32_t cpu_map = xlp_hw_thread_mask;
-	uint32_t core0_thr_mask, core_thr_mask;
+	uint32_t core0_thr_mask, core_thr_mask, cpu_rst_mask;
+	int i, j, k;
 
 #ifdef SMP
 	if (cpu_map == 0)
@@ -183,41 +196,39 @@ xlp_parse_mmu_options(void)
 		goto unsupp;
 	}
 
-	/* Verify other cores CPU masks */
+	/* Take out cores which do not exist on chip */
+	sysbase = nlm_get_sys_regbase(0);
+	cpu_rst_mask = nlm_read_sys_reg(sysbase, SYS_CPU_RESET) & 0xff;
 	for (i = 1; i < XLP_MAX_CORES; i++) {
-		core_thr_mask = (cpu_map >> (i*4)) & 0xf;
-		if (core_thr_mask) {
-			if (core_thr_mask != core0_thr_mask)
-				goto unsupp; 
-			xlp_ncores++;
-		}
+		if ((cpu_rst_mask & (1 << i)) == 0)
+			cpu_map &= ~(0xfu << (4 * i));
+	}
+
+ 	/* Verify other cores' CPU masks */
+	for (i = 1; i < XLP_MAX_CORES; i++) {
+		core_thr_mask = (cpu_map >> (4 * i)) & 0xf;
+		if (core_thr_mask == 0)
+	       		continue;
+		if (core_thr_mask != core0_thr_mask)
+			goto unsupp; 
+		xlp_ncores++;
 	}
 
 	xlp_hw_thread_mask = cpu_map;
 	/* setup hardware processor id to cpu id mapping */
 	for (i = 0; i< MAXCPU; i++)
 		xlp_cpuid_to_hwtid[i] = 
-		    xlp_hwtid_to_cpuid [i] = -1;
+		    xlp_hwtid_to_cpuid[i] = -1;
 	for (i = 0, k = 0; i < XLP_MAX_CORES; i++) {
-		if (((cpu_map >> (i*4)) & 0xf) == 0)
+		if (((cpu_map >> (i * 4)) & 0xf) == 0)
 			continue;
 		for (j = 0; j < xlp_threads_per_core; j++) {
-			xlp_cpuid_to_hwtid[k] = i*4 + j;
-			xlp_hwtid_to_cpuid[i*4 + j] = k;
+			xlp_cpuid_to_hwtid[k] = i * 4 + j;
+			xlp_hwtid_to_cpuid[i * 4 + j] = k;
 			k++;
 		}
 	}
 
-#ifdef SMP
-	/* 
-	 * We will enable the other threads in core 0 here
-	 * so that the TLB and cache info is correct when
-	 * mips_init runs
-	 */
-	xlp_enable_threads(xlp_mmuval);
-#endif
-	/* setup for the startup core */
-	xlp_setup_mmu();
 	return;
 
 unsupp:
@@ -285,10 +296,11 @@ xlp_bootargs_init(__register_t arg)
 	OF_interpret("perform-fixup", 0);
 
 	chosen = OF_finddevice("/chosen");
-	if (OF_getprop(chosen, "cpumask", &mask, sizeof(mask)) == 0)
+	if (OF_getprop(chosen, "cpumask", &mask, sizeof(mask)) != -1) {
 		xlp_hw_thread_mask = mask;
+	}
 
-	if (OF_getprop(chosen, "bootargs", buf, sizeof(buf)) == 0)
+	if (OF_getprop(chosen, "bootargs", buf, sizeof(buf)) != -1)
 		xlp_parse_bootargs(buf);
 }
 #else
@@ -303,6 +315,14 @@ xlp_bootargs_init(__register_t arg)
 	char	*p, *v, *n;
 	uint32_t mask;
 
+	/*
+	 * provide backward compat for passing cpu mask as arg
+	 */
+	if (arg & 1) {
+		xlp_hw_thread_mask = arg;
+		return;
+	}
+
 	p = (void *)(intptr_t)arg;
 	while (*p != '\0') {
 		strlcpy(buf, p, sizeof(buf));
@@ -368,14 +388,17 @@ xlp_pic_init(void)
 		2000,                   /* quality (adjusted in code) */
 	};
         int i;
+	int maxirt;
 
 	xlp_pic_base = nlm_get_pic_regbase(0);  /* TOOD: Add other nodes */
-        printf("Initializing PIC...@%jx\n", (uintmax_t)xlp_pic_base);
+	maxirt = nlm_read_reg(nlm_get_pic_pcibase(nlm_nodeid()),
+	    XLP_PCI_DEVINFO_REG0);
+        printf("Initializing PIC...@%jx %d IRTs\n", (uintmax_t)xlp_pic_base,
+	    maxirt);
 	/* Bind all PIC irqs to cpu 0 */
-        for(i = 0; i < PIC_NUM_IRTS; i++) {
-                nlm_pic_write_irt(xlp_pic_base, i, 0, 0, 1, 0,
-		    1, 0, 0x1);
-        }
+        for (i = 0; i < maxirt; i++)
+	    nlm_pic_write_irt(xlp_pic_base, i, 0, 0, 1, 0,
+	    1, 0, 0x1);
 
 	nlm_pic_set_timer(xlp_pic_base, PIC_CLOCK_TIMER, ~0ULL, 0, 0);
 	platform_timecounter = &pic_timecounter;
@@ -469,30 +492,12 @@ xlp_mem_init(void)
 	phys_avail[j] = phys_avail[j + 1] = 0;
 
 	/* copy phys_avail to dump_avail */
-	for(i = 0; i <= j + 1; i++) 
+	for (i = 0; i <= j + 1; i++) 
 		dump_avail[i] = phys_avail[i];
 
 	realmem = physmem = btoc(physsz);
 }
 
-static uint32_t
-xlp_get_cpu_frequency(void)
-{
-	uint64_t  sysbase = nlm_get_sys_regbase(0);
-	unsigned int pll_divf, pll_divr, dfs_div, num, denom;
-	uint32_t val;
-	       
-	val = nlm_read_sys_reg(sysbase, SYS_POWER_ON_RESET_CFG);
-	pll_divf = (val >> 10) & 0x7f;
-	pll_divr = (val >> 8)  & 0x3;
-	dfs_div  = (val >> 17) & 0x3;
-
-	num = pll_divf + 1;
-	denom = 3 * (pll_divr + 1) * (1<< (dfs_div + 1));
-	val = 800000000ULL * num / denom;
-	return (val);
-}
-
 void
 platform_start(__register_t a0 __unused,
     __register_t a1 __unused,
@@ -506,11 +511,12 @@ platform_start(__register_t a0 __unused,
 	/* initialize console so that we have printf */
 	boothowto |= (RB_SERIAL | RB_MULTIPLE);	/* Use multiple consoles */
 
+	nlm_pic_irt_init(); /* complete before interrupts or console init */
 	init_static_kenv(boot1_env, sizeof(boot1_env));
 	xlp_bootargs_init(a0);
 
 	/* clockrate used by delay, so initialize it here */
-	xlp_cpu_frequency = xlp_get_cpu_frequency();
+	xlp_cpu_frequency = xlp_get_cpu_frequency(0);
 	cpu_clock = xlp_cpu_frequency / 1000000;
 	mips_timer_early_init(xlp_cpu_frequency);
 
@@ -525,10 +531,18 @@ platform_start(__register_t a0 __unused,
 
 	bcopy(XLPResetEntry, (void *)MIPS_RESET_EXC_VEC,
               XLPResetEntryEnd - XLPResetEntry);
-
-	/*
-	 * MIPS generic init 
+#ifdef SMP
+	/* 
+	 * We will enable the other threads in core 0 here
+	 * so that the TLB and cache info is correct when
+	 * mips_init runs
 	 */
+	xlp_enable_threads(xlp_mmuval);
+#endif
+	/* setup for the startup core */
+	xlp_setup_mmu();
+
+	/* MIPS generic init */
 	mips_init();
 
 	/*
@@ -568,7 +582,7 @@ platform_reset(void)
 	uint64_t sysbase = nlm_get_sys_regbase(0);
 
 	nlm_write_sys_reg(sysbase, SYS_CHIP_RESET, 1);
-	for(;;)
+	for( ; ; )
 		__asm __volatile("wait");
 }
 
@@ -637,7 +651,6 @@ platform_init_ap(int cpuid)
 	if (thr == 0) {
 		xlp_setup_core();
 		xlp_enable_threads(xlp_mmuval);
-		xlp_setup_mmu();
 	} else {
 		/*
 		 * FIXME busy wait here eats too many cycles, especially 
@@ -648,6 +661,7 @@ platform_init_ap(int cpuid)
 		thr_unblock[thr] = 0;
 	}
 
+	xlp_setup_mmu();
 	stat = mips_rd_status();
 	KASSERT((stat & MIPS_SR_INT_IE) == 0,
 	    ("Interrupts enabled in %s!", __func__));
diff --git a/sys/mips/nlm/xlp_pci.c b/sys/mips/nlm/xlp_pci.c
index 09e214e..bb98c80 100644
--- a/sys/mips/nlm/xlp_pci.c
+++ b/sys/mips/nlm/xlp_pci.c
@@ -109,7 +109,6 @@ xlp_pci_init_resources(void)
 	if (rman_init(&emul_rman)
 	    || rman_manage_region(&emul_rman, 0x18000000ULL, 0x18ffffffULL))
 		panic("pci_init_resources emul_rman");
-
 }
 
 static int
@@ -117,7 +116,6 @@ xlp_pcib_probe(device_t dev)
 {
 
 	device_set_desc(dev, "XLP PCI bus");
-
 	xlp_pci_init_resources();
 	return (0);
 }
@@ -184,7 +182,6 @@ xlp_pcib_read_config(device_t dev, u_int b, u_int s, u_int f,
 		    (dev == 6 || dev == 2 || dev == 7))
 			data |= 0x1 << 8;	/* Fake int pin */
 	}
-
 	if (width == 1)
 		return ((data >> ((reg & 3) << 3)) & 0xff);
 	else if (width == 2)
@@ -220,7 +217,6 @@ xlp_pcib_write_config(device_t dev, u_int b, u_int s, u_int f,
 	}
 
 	nlm_write_pci_reg(cfgaddr, regindex, data);
-
 	return;
 }
 
@@ -253,8 +249,10 @@ xlp_pcie_link(device_t pcib, device_t dev)
 	device_t parent, tmp;
 
 	/* find the lane on which the slot is connected to */
+#if 0 /* Debug */
 	printf("xlp_pcie_link : bus %s dev %s\n", device_get_nameunit(pcib),
 		device_get_nameunit(dev));
+#endif
 	tmp = dev;
 	while (1) {
 		parent = device_get_parent(tmp);
@@ -269,20 +267,6 @@ xlp_pcie_link(device_t pcib, device_t dev)
 	return (pci_get_function(tmp));
 }
 
-/*
- * Find the IRQ for the link, each link has a different interrupt 
- * at the XLP pic
- */
-static int
-xlp_pcie_link_irt(int link)
-{
-
-	if( (link < 0) || (link > 3))
-		return (-1);
-
-	return PIC_IRT_PCIE_LINK_INDEX(link);
-}
-
 static int
 xlp_alloc_msi(device_t pcib, device_t dev, int count, int maxcount, int *irqs)
 {
@@ -346,7 +330,7 @@ bridge_pcie_ack(int irq)
 	node = nlm_nodeid();
 	reg = PCIE_MSI_STATUS;
 
-	switch(irq) {
+	switch (irq) {
 		case PIC_PCIE_0_IRQ:
 			base = nlm_pcicfg_base(XLP_IO_PCIE0_OFFSET(node));
 			break;
@@ -364,7 +348,6 @@ bridge_pcie_ack(int irq)
 	}
 
 	nlm_write_pci_reg(base, reg, 0xFFFFFFFF);
-
 	return;
 }
 
@@ -375,7 +358,6 @@ mips_platform_pci_setup_intr(device_t dev, device_t child,
 {
 	int error = 0;
 	int xlpirq;
-	int node,base,val,link;
 	void *extra_ack;
 
 	error = rman_activate_resource(irq);
@@ -399,6 +381,9 @@ mips_platform_pci_setup_intr(device_t dev, device_t child,
 	 * link, and assign the link interrupt to the device interrupt
 	 */
 	if (xlpirq >= 64) {
+		int node, val, link;
+		uint64_t base;
+
 		xlpirq -= 64;
 		if (xlpirq % 32 != 0)
 			return (0);
@@ -438,6 +423,7 @@ mips_platform_pci_setup_intr(device_t dev, device_t child,
 		xlpirq = xlp_irt_to_irq(xlpirq);
 	}
 	/* Set all irqs to CPU 0 for now */
+	printf("set up intr %d->%d(%d)\n", xlp_irq_to_irt(xlpirq), xlpirq, (int)rman_get_start(irq));
 	nlm_pic_write_irt_direct(xlp_pic_base, xlp_irq_to_irt(xlpirq), 1, 0,
 				 PIC_LOCAL_SCHEDULING, xlpirq, 0);
 	extra_ack = NULL;
@@ -468,41 +454,28 @@ assign_soc_resource(device_t child, int type, u_long *startp, u_long *endp,
 	int devid = pci_get_device(child);
 	int inst = pci_get_function(child);
 	int node = pci_get_slot(child) / 8;
+	int dev = pci_get_slot(child) % 8;
 
 	*rm = NULL;
 	*va = 0;
 	*bst = 0;
-	switch (devid) {
-	case PCI_DEVICE_ID_NLM_UART:
-		switch (type) {
-		case SYS_RES_IRQ:
-			*startp = *endp = PIC_UART_0_IRQ + inst;
-			*countp = 1;
-			break;
-		case SYS_RES_MEMORY: 
+	if (type == SYS_RES_IRQ) {
+		printf("%s: %d %d %d : start %d, end %d\n", __func__,
+				node, dev, inst, (int)*startp, (int)*endp);
+	} else if (type == SYS_RES_MEMORY) {
+		switch (devid) {
+		case PCI_DEVICE_ID_NLM_UART:
 			*va = nlm_get_uart_regbase(node, inst);
 			*startp = MIPS_KSEG1_TO_PHYS(va);
 			*countp = 0x100;
 			*rm = &emul_rman;
 			*bst = uart_bus_space_mem;
 			break;
-		};
-		break;
-
-	case PCI_DEVICE_ID_NLM_EHCI:
-		if (type == SYS_RES_IRQ) {
-			if (inst == 0)
-				*startp = *endp = PIC_EHCI_0_IRQ;
-			else if (inst == 3)
-				*startp = *endp = PIC_EHCI_1_IRQ;
-			else
-				device_printf(child, "bad instance %d\n", inst);
-
-			*countp = 1; 
 		}
-		break;
-	}
 
+	} else
+		printf("Unknown type %d in req for [%x%x]\n",
+			type, devid, inst);
 	/* default to rmi_bus_space for SoC resources */
 	if (type == SYS_RES_MEMORY && *bst == 0)
 		*bst = rmi_bus_space;
@@ -614,12 +587,40 @@ mips_pci_route_interrupt(device_t bus, device_t dev, int pin)
 	/*
 	 * Validate requested pin number.
 	 */
-	device_printf(bus, "route  %s %d", device_get_nameunit(dev), pin);
 	if ((pin < 1) || (pin > 4))
 		return (255);
 
-	link = xlp_pcie_link(bus, dev);
-	irt = xlp_pcie_link_irt(link);
+	device_printf(bus, "route  %s %d", device_get_nameunit(dev), pin);
+	if (pci_get_bus(dev) == 0 &&
+	    pci_get_vendor(dev) == PCI_VENDOR_NETLOGIC) {
+		/* SoC devices */
+		uint64_t pcibase;
+		int f, n, d, num;
+
+		f = pci_get_function(dev);
+		n = pci_get_slot(dev) / 8;
+		d = pci_get_slot(dev) % 8;
+
+		/*
+		 * For PCIe links, return link IRT, for other SoC devices
+		 * get the IRT from its PCIe header
+		 */
+		if (d == 1) {
+			irt = xlp_pcie_link_irt(f);
+		} else {
+			pcibase = nlm_pcicfg_base(XLP_HDR_OFFSET(n, 0, d, f));
+			irt = nlm_irtstart(pcibase);
+			num = nlm_irtnum(pcibase);
+			if (num != 1)
+				device_printf(bus, "[%d:%d:%d] Error %d IRQs\n",
+				    n, d, f, num);
+		}
+	} else {
+		/* Regular PCI devices */
+		link = xlp_pcie_link(bus, dev);
+		irt = xlp_pcie_link_irt(link);
+	}
+
 	if (irt != -1)
 		return (xlp_irt_to_irq(irt));