sparc,leon: Sparc-Leon SMP support

Support SMP for a Sparc-Leon multiprocessor system. Add Leon specific SMP code to arch/sparc/kernel/leon_smp.c. Signed-off-by: Konrad Eisele <konrad@gaisler.com> Signed-off-by: David S. Miller <davem@davemloft.net>

sparc,leon: Sparc-Leon SMP support
Support SMP for a Sparc-Leon multiprocessor system. Add Leon specific SMP code to arch/sparc/kernel/leon_smp.c. Signed-off-by: Konrad Eisele <konrad@gaisler.com> Signed-off-by: David S. Miller <davem@davemloft.net>
Konrad Eisele · David S. Miller
1 parent b6727b12dd
Showing 11 changed files with 732 additions and 3 deletions Side-by-side Diff
arch/sparc/include/asm/leon.h
arch/sparc/include/asm/smp_32.h
arch/sparc/kernel/Makefile
arch/sparc/kernel/entry.S
arch/sparc/kernel/head_32.S
arch/sparc/kernel/ioport.c
arch/sparc/kernel/leon_kernel.c
arch/sparc/kernel/leon_smp.c
arch/sparc/kernel/smp_32.c
arch/sparc/kernel/trampoline_32.S
arch/sparc/mm/srmmu.c
@@ -340,6 +340,30 @@
 extern void leon_switch_mm(void);
 extern int srmmu_swprobe_trace;
  
+#ifdef CONFIG_SMP
+extern int leon_smp_nrcpus(void);
+extern void leon_clear_profile_irq(int cpu);
+extern void leon_smp_done(void);
+extern void leon_boot_cpus(void);
+extern int leon_boot_one_cpu(int i);
+void leon_init_smp(void);
+extern void cpu_probe(void);
+extern void cpu_idle(void);
+extern void init_IRQ(void);
+extern void cpu_panic(void);
+extern int __leon_processor_id(void);
+void leon_enable_irq_cpu(unsigned int irq_nr, unsigned int cpu);
+
+extern unsigned int real_irq_entry[], smpleon_ticker[];
+extern unsigned int patchme_maybe_smp_msg[];
+extern unsigned long trapbase_cpu1[];
+extern unsigned long trapbase_cpu2[];
+extern unsigned long trapbase_cpu3[];
+extern unsigned int t_nmi[], linux_trap_ipi15_leon[];
+extern unsigned int linux_trap_ipi15_sun4m[];
+
+#endif /* CONFIG_SMP */
+
 #endif /* __KERNEL__ */
  
 #endif /* __ASSEMBLY__ */
@@ -356,6 +380,10 @@
 #define leon_switch_mm() do {} while (0)
 #define leon_init_IRQ() do {} while (0)
 #define init_leon() do {} while (0)
+#define leon_smp_done() do {} while (0)
+#define leon_boot_cpus() do {} while (0)
+#define leon_boot_one_cpu(i) 1
+#define leon_init_smp() do {} while (0)
  
 #endif /* !defined(CONFIG_SPARC_LEON) */
  
@@ -106,6 +106,15 @@
 	return cpuid;
 }
  
+extern inline int hard_smpleon_processor_id(void)
+{
+	int cpuid;
+	__asm__ __volatile__("rd     %%asr17,%0\n\t"
+			     "srl    %0,28,%0" :
+			     "=&r" (cpuid) : );
+	return cpuid;
+}
+
 #ifndef MODULE
 static inline int hard_smp_processor_id(void)
 {
@@ -72,7 +72,7 @@
 obj-$(CONFIG_SPARC32_PCI) += pcic.o
  
 obj-$(CONFIG_SMP)         += trampoline_$(BITS).o smp_$(BITS).o
-obj-$(CONFIG_SPARC32_SMP) += sun4m_smp.o sun4d_smp.o
+obj-$(CONFIG_SPARC32_SMP) += sun4m_smp.o sun4d_smp.o leon_smp.o
 obj-$(CONFIG_SPARC64_SMP) += hvtramp.o
  
 obj-y                     += auxio_$(BITS).o
@@ -400,6 +400,39 @@
 	/* FIXME */
 1:	b,a	1b
  
+#ifdef CONFIG_SPARC_LEON
+
+	.globl	smpleon_ticker
+	/* SMP per-cpu ticker interrupts are handled specially. */
+smpleon_ticker:
+        SAVE_ALL
+	or	%l0, PSR_PIL, %g2
+	wr	%g2, 0x0, %psr
+	WRITE_PAUSE
+	wr	%g2, PSR_ET, %psr
+	WRITE_PAUSE
+	call	leon_percpu_timer_interrupt
+	 add	%sp, STACKFRAME_SZ, %o0
+	wr	%l0, PSR_ET, %psr
+	WRITE_PAUSE
+	RESTORE_ALL
+
+	.align	4
+	.globl	linux_trap_ipi15_leon
+linux_trap_ipi15_leon:
+	SAVE_ALL
+	or	%l0, PSR_PIL, %l4
+	wr	%l4, 0x0, %psr
+	WRITE_PAUSE
+	wr	%l4, PSR_ET, %psr
+	WRITE_PAUSE
+	call	leon_cross_call_irq
+	 nop
+	b	ret_trap_lockless_ipi
+	 clr	%l6
+
+#endif /* CONFIG_SPARC_LEON */
+
 #endif /* CONFIG_SMP */
  
 	/* This routine handles illegal instructions and privileged
@@ -811,8 +811,30 @@
 got_prop:
 #ifdef CONFIG_SPARC_LEON
 	        /* no cpu-type check is needed, it is a SPARC-LEON */
+#ifdef CONFIG_SMP
+		ba leon_smp_init
+		 nop
+
+		.global leon_smp_init
+leon_smp_init:
+		sethi	%hi(boot_cpu_id), %g1    ! master always 0
+		stb	%g0, [%g1 + %lo(boot_cpu_id)]
+		sethi	%hi(boot_cpu_id4), %g1   ! master always 0
+		stb	%g0, [%g1 + %lo(boot_cpu_id4)]
+
+		rd     %asr17,%g1
+		srl    %g1,28,%g1
+
+		cmp %g0,%g1
+		 beq sun4c_continue_boot         !continue with master
+		nop
+
+		ba leon_smp_cpu_startup
+		 nop
+#else
 		ba sun4c_continue_boot
 		 nop
+#endif
 #endif
 		set	cputypval, %o2
 		ldub	[%o2 + 0x4], %l1
@@ -48,8 +48,13 @@
 #include <asm/dma.h>
 #include <asm/iommu.h>
 #include <asm/io-unit.h>
+#include <asm/leon.h>
  
+#ifdef CONFIG_SPARC_LEON
+#define mmu_inval_dma_area(p, l) leon_flush_dcache_all()
+#else
 #define mmu_inval_dma_area(p, l)	/* Anton pulled it out for 2.4.0-xx */
+#endif
  
 static struct resource *_sparc_find_resource(struct resource *r,
 					     unsigned long);
@@ -12,11 +12,14 @@
 #include <linux/of_platform.h>
 #include <linux/interrupt.h>
 #include <linux/of_device.h>
+
 #include <asm/oplib.h>
 #include <asm/timer.h>
 #include <asm/prom.h>
 #include <asm/leon.h>
 #include <asm/leon_amba.h>
+#include <asm/traps.h>
+#include <asm/cacheflush.h>
  
 #include "prom.h"
 #include "irq.h"
@@ -115,6 +118,21 @@
 				      (((1000000 / 100) - 1)));
 		LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[0].ctrl, 0);
  
+#ifdef CONFIG_SMP
+		leon_percpu_timer_dev[0].start = (int)leon3_gptimer_regs;
+		leon_percpu_timer_dev[0].irq = leon3_gptimer_irq+1;
+
+		if (!(LEON3_BYPASS_LOAD_PA(&leon3_gptimer_regs->config) &
+		      (1<<LEON3_GPTIMER_SEPIRQ))) {
+			prom_printf("irq timer not configured with seperate irqs \n");
+			BUG();
+		}
+
+		LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[1].val, 0);
+		LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[1].rld, (((1000000/100) - 1)));
+		LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[1].ctrl, 0);
+# endif
+
 	} else {
 		printk(KERN_ERR "No Timer/irqctrl found\n");
 		BUG();
  
@@ -130,11 +148,41 @@
 		prom_halt();
 	}
  
+# ifdef CONFIG_SMP
+	{
+		unsigned long flags;
+		struct tt_entry *trap_table = &sparc_ttable[SP_TRAP_IRQ1 + (leon_percpu_timer_dev[0].irq - 1)];
+
+		/* For SMP we use the level 14 ticker, however the bootup code
+		 * has copied the firmwares level 14 vector into boot cpu's
+		 * trap table, we must fix this now or we get squashed.
+		 */
+		local_irq_save(flags);
+
+		patchme_maybe_smp_msg[0] = 0x01000000; /* NOP out the branch */
+
+		/* Adjust so that we jump directly to smpleon_ticker */
+		trap_table->inst_three += smpleon_ticker - real_irq_entry;
+
+		local_flush_cache_all();
+		local_irq_restore(flags);
+	}
+# endif
+
 	if (leon3_gptimer_regs) {
 		LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[0].ctrl,
 				      LEON3_GPTIMER_EN |
 				      LEON3_GPTIMER_RL |
 				      LEON3_GPTIMER_LD | LEON3_GPTIMER_IRQEN);
+
+#ifdef CONFIG_SMP
+		LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[1].ctrl,
+				      LEON3_GPTIMER_EN |
+				      LEON3_GPTIMER_RL |
+				      LEON3_GPTIMER_LD |
+				      LEON3_GPTIMER_IRQEN);
+#endif
+
 	}
 }
  
@@ -174,6 +222,42 @@
 		prom_amba_init(dp, nextp);
 	}
 }
+
+#ifdef CONFIG_SMP
+
+void leon_set_cpu_int(int cpu, int level)
+{
+	unsigned long mask;
+	mask = get_irqmask(level);
+	LEON3_BYPASS_STORE_PA(&leon3_irqctrl_regs->force[cpu], mask);
+}
+
+static void leon_clear_ipi(int cpu, int level)
+{
+	unsigned long mask;
+	mask = get_irqmask(level);
+	LEON3_BYPASS_STORE_PA(&leon3_irqctrl_regs->force[cpu], mask<<16);
+}
+
+static void leon_set_udt(int cpu)
+{
+}
+
+void leon_clear_profile_irq(int cpu)
+{
+}
+
+void leon_enable_irq_cpu(unsigned int irq_nr, unsigned int cpu)
+{
+	unsigned long mask, flags, *addr;
+	mask = get_irqmask(irq_nr);
+	local_irq_save(flags);
+	addr = (unsigned long *)&(leon3_irqctrl_regs->mask[cpu]);
+	LEON3_BYPASS_STORE_PA(addr, (LEON3_BYPASS_LOAD_PA(addr) | (mask)));
+	local_irq_restore(flags);
+}
+
+#endif
  
 void __init leon_init_IRQ(void)
 {
+/* leon_smp.c: Sparc-Leon SMP support.
+ *
+ * based on sun4m_smp.c
+ * Copyright (C) 1996 David S. Miller (davem@caip.rutgers.edu)
+ * Copyright (C) 2009 Daniel Hellstrom (daniel@gaisler.com) Aeroflex Gaisler AB
+ * Copyright (C) 2009 Konrad Eisele (konrad@gaisler.com) Aeroflex Gaisler AB
+ */
+
+#include <asm/head.h>
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/threads.h>
+#include <linux/smp.h>
+#include <linux/smp_lock.h>
+#include <linux/interrupt.h>
+#include <linux/kernel_stat.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/mm.h>
+#include <linux/swap.h>
+#include <linux/profile.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+
+#include <asm/cacheflush.h>
+#include <asm/tlbflush.h>
+
+#include <asm/ptrace.h>
+#include <asm/atomic.h>
+#include <asm/irq_regs.h>
+
+#include <asm/delay.h>
+#include <asm/irq.h>
+#include <asm/page.h>
+#include <asm/pgalloc.h>
+#include <asm/pgtable.h>
+#include <asm/oplib.h>
+#include <asm/cpudata.h>
+#include <asm/asi.h>
+#include <asm/leon.h>
+#include <asm/leon_amba.h>
+
+#ifdef CONFIG_SPARC_LEON
+
+#include "irq.h"
+
+extern ctxd_t *srmmu_ctx_table_phys;
+static int smp_processors_ready;
+extern volatile unsigned long cpu_callin_map[NR_CPUS];
+extern unsigned char boot_cpu_id;
+extern cpumask_t smp_commenced_mask;
+void __init leon_configure_cache_smp(void);
+
+static inline unsigned long do_swap(volatile unsigned long *ptr,
+				    unsigned long val)
+{
+	__asm__ __volatile__("swapa [%1] %2, %0\n\t" : "=&r"(val)
+			     : "r"(ptr), "i"(ASI_LEON_DCACHE_MISS)
+			     : "memory");
+	return val;
+}
+
+static void smp_setup_percpu_timer(void);
+
+void __cpuinit leon_callin(void)
+{
+	int cpuid = hard_smpleon_processor_id();
+
+	local_flush_cache_all();
+	local_flush_tlb_all();
+	leon_configure_cache_smp();
+
+	/* Get our local ticker going. */
+	smp_setup_percpu_timer();
+
+	calibrate_delay();
+	smp_store_cpu_info(cpuid);
+
+	local_flush_cache_all();
+	local_flush_tlb_all();
+
+	/*
+	 * Unblock the master CPU _only_ when the scheduler state
+	 * of all secondary CPUs will be up-to-date, so after
+	 * the SMP initialization the master will be just allowed
+	 * to call the scheduler code.
+	 * Allow master to continue.
+	 */
+	do_swap(&cpu_callin_map[cpuid], 1);
+
+	local_flush_cache_all();
+	local_flush_tlb_all();
+
+	cpu_probe();
+
+	/* Fix idle thread fields. */
+	__asm__ __volatile__("ld [%0], %%g6\n\t" : : "r"(&current_set[cpuid])
+			     : "memory" /* paranoid */);
+
+	/* Attach to the address space of init_task. */
+	atomic_inc(&init_mm.mm_count);
+	current->active_mm = &init_mm;
+
+	while (!cpu_isset(cpuid, smp_commenced_mask))
+		mb();
+
+	local_irq_enable();
+	cpu_set(cpuid, cpu_online_map);
+}
+
+/*
+ *	Cycle through the processors asking the PROM to start each one.
+ */
+
+extern struct linux_prom_registers smp_penguin_ctable;
+
+void __init leon_configure_cache_smp(void)
+{
+	unsigned long cfg = sparc_leon3_get_dcachecfg();
+	int me = smp_processor_id();
+
+	if (ASI_LEON3_SYSCTRL_CFG_SSIZE(cfg) > 4) {
+		printk(KERN_INFO "Note: SMP with snooping only works on 4k cache, found %dk(0x%x) on cpu %d, disabling caches\n",
+		     (unsigned int)ASI_LEON3_SYSCTRL_CFG_SSIZE(cfg),
+		     (unsigned int)cfg, (unsigned int)me);
+		sparc_leon3_disable_cache();
+	} else {
+		if (cfg & ASI_LEON3_SYSCTRL_CFG_SNOOPING) {
+			sparc_leon3_enable_snooping();
+		} else {
+			printk(KERN_INFO "Note: You have to enable snooping in the vhdl model cpu %d, disabling caches\n",
+			     me);
+			sparc_leon3_disable_cache();
+		}
+	}
+
+	local_flush_cache_all();
+	local_flush_tlb_all();
+}
+
+void leon_smp_setbroadcast(unsigned int mask)
+{
+	int broadcast =
+	    ((LEON3_BYPASS_LOAD_PA(&(leon3_irqctrl_regs->mpstatus)) >>
+	      LEON3_IRQMPSTATUS_BROADCAST) & 1);
+	if (!broadcast) {
+		prom_printf("######## !!!! The irqmp-ctrl must have broadcast enabled, smp wont work !!!!! ####### nr cpus: %d\n",
+		     leon_smp_nrcpus());
+		if (leon_smp_nrcpus() > 1) {
+			BUG();
+		} else {
+			prom_printf("continue anyway\n");
+			return;
+		}
+	}
+	LEON_BYPASS_STORE_PA(&(leon3_irqctrl_regs->mpbroadcast), mask);
+}
+
+unsigned int leon_smp_getbroadcast(void)
+{
+	unsigned int mask;
+	mask = LEON_BYPASS_LOAD_PA(&(leon3_irqctrl_regs->mpbroadcast));
+	return mask;
+}
+
+int leon_smp_nrcpus(void)
+{
+	int nrcpu =
+	    ((LEON3_BYPASS_LOAD_PA(&(leon3_irqctrl_regs->mpstatus)) >>
+	      LEON3_IRQMPSTATUS_CPUNR) & 0xf) + 1;
+	return nrcpu;
+}
+
+void __init leon_boot_cpus(void)
+{
+	int nrcpu = leon_smp_nrcpus();
+	int me = smp_processor_id();
+
+	printk(KERN_INFO "%d:(%d:%d) cpus mpirq at 0x%x \n", (unsigned int)me,
+	       (unsigned int)nrcpu, (unsigned int)NR_CPUS,
+	       (unsigned int)&(leon3_irqctrl_regs->mpstatus));
+
+	leon_enable_irq_cpu(LEON3_IRQ_CROSS_CALL, me);
+	leon_enable_irq_cpu(LEON3_IRQ_TICKER, me);
+	leon_enable_irq_cpu(LEON3_IRQ_RESCHEDULE, me);
+
+	leon_smp_setbroadcast(1 << LEON3_IRQ_TICKER);
+
+	leon_configure_cache_smp();
+	smp_setup_percpu_timer();
+	local_flush_cache_all();
+
+}
+
+int __cpuinit leon_boot_one_cpu(int i)
+{
+
+	struct task_struct *p;
+	int timeout;
+
+	/* Cook up an idler for this guy. */
+	p = fork_idle(i);
+
+	current_set[i] = task_thread_info(p);
+
+	/* See trampoline.S:leon_smp_cpu_startup for details...
+	 * Initialize the contexts table
+	 * Since the call to prom_startcpu() trashes the structure,
+	 * we need to re-initialize it for each cpu
+	 */
+	smp_penguin_ctable.which_io = 0;
+	smp_penguin_ctable.phys_addr = (unsigned int)srmmu_ctx_table_phys;
+	smp_penguin_ctable.reg_size = 0;
+
+	/* whirrr, whirrr, whirrrrrrrrr... */
+	printk(KERN_INFO "Starting CPU %d : (irqmp: 0x%x)\n", (unsigned int)i,
+	       (unsigned int)&leon3_irqctrl_regs->mpstatus);
+	local_flush_cache_all();
+
+	LEON_BYPASS_STORE_PA(&(leon3_irqctrl_regs->mpstatus), 1 << i);
+
+	/* wheee... it's going... */
+	for (timeout = 0; timeout < 10000; timeout++) {
+		if (cpu_callin_map[i])
+			break;
+		udelay(200);
+	}
+	printk(KERN_INFO "Started CPU %d \n", (unsigned int)i);
+
+	if (!(cpu_callin_map[i])) {
+		printk(KERN_ERR "Processor %d is stuck.\n", i);
+		return -ENODEV;
+	} else {
+		leon_enable_irq_cpu(LEON3_IRQ_CROSS_CALL, i);
+		leon_enable_irq_cpu(LEON3_IRQ_TICKER, i);
+		leon_enable_irq_cpu(LEON3_IRQ_RESCHEDULE, i);
+	}
+
+	local_flush_cache_all();
+	return 0;
+}
+
+void __init leon_smp_done(void)
+{
+
+	int i, first;
+	int *prev;
+
+	/* setup cpu list for irq rotation */
+	first = 0;
+	prev = &first;
+	for (i = 0; i < NR_CPUS; i++) {
+		if (cpu_online(i)) {
+			*prev = i;
+			prev = &cpu_data(i).next;
+		}
+	}
+	*prev = first;
+	local_flush_cache_all();
+
+	/* Free unneeded trap tables */
+	if (!cpu_isset(1, cpu_present_map)) {
+		ClearPageReserved(virt_to_page(trapbase_cpu1));
+		init_page_count(virt_to_page(trapbase_cpu1));
+		free_page((unsigned long)trapbase_cpu1);
+		totalram_pages++;
+		num_physpages++;
+	}
+	if (!cpu_isset(2, cpu_present_map)) {
+		ClearPageReserved(virt_to_page(trapbase_cpu2));
+		init_page_count(virt_to_page(trapbase_cpu2));
+		free_page((unsigned long)trapbase_cpu2);
+		totalram_pages++;
+		num_physpages++;
+	}
+	if (!cpu_isset(3, cpu_present_map)) {
+		ClearPageReserved(virt_to_page(trapbase_cpu3));
+		init_page_count(virt_to_page(trapbase_cpu3));
+		free_page((unsigned long)trapbase_cpu3);
+		totalram_pages++;
+		num_physpages++;
+	}
+	/* Ok, they are spinning and ready to go. */
+	smp_processors_ready = 1;
+
+}
+
+void leon_irq_rotate(int cpu)
+{
+}
+
+static struct smp_funcall {
+	smpfunc_t func;
+	unsigned long arg1;
+	unsigned long arg2;
+	unsigned long arg3;
+	unsigned long arg4;
+	unsigned long arg5;
+	unsigned long processors_in[NR_CPUS];	/* Set when ipi entered. */
+	unsigned long processors_out[NR_CPUS];	/* Set when ipi exited. */
+} ccall_info;
+
+static DEFINE_SPINLOCK(cross_call_lock);
+
+/* Cross calls must be serialized, at least currently. */
+static void leon_cross_call(smpfunc_t func, cpumask_t mask, unsigned long arg1,
+			    unsigned long arg2, unsigned long arg3,
+			    unsigned long arg4)
+{
+	if (smp_processors_ready) {
+		register int high = NR_CPUS - 1;
+		unsigned long flags;
+
+		spin_lock_irqsave(&cross_call_lock, flags);
+
+		{
+			/* If you make changes here, make sure gcc generates proper code... */
+			register smpfunc_t f asm("i0") = func;
+			register unsigned long a1 asm("i1") = arg1;
+			register unsigned long a2 asm("i2") = arg2;
+			register unsigned long a3 asm("i3") = arg3;
+			register unsigned long a4 asm("i4") = arg4;
+			register unsigned long a5 asm("i5") = 0;
+
+			__asm__ __volatile__("std %0, [%6]\n\t"
+					     "std %2, [%6 + 8]\n\t"
+					     "std %4, [%6 + 16]\n\t" : :
+					     "r"(f), "r"(a1), "r"(a2), "r"(a3),
+					     "r"(a4), "r"(a5),
+					     "r"(&ccall_info.func));
+		}
+
+		/* Init receive/complete mapping, plus fire the IPI's off. */
+		{
+			register int i;
+
+			cpu_clear(smp_processor_id(), mask);
+			cpus_and(mask, cpu_online_map, mask);
+			for (i = 0; i <= high; i++) {
+				if (cpu_isset(i, mask)) {
+					ccall_info.processors_in[i] = 0;
+					ccall_info.processors_out[i] = 0;
+					set_cpu_int(i, LEON3_IRQ_CROSS_CALL);
+
+				}
+			}
+		}
+
+		{
+			register int i;
+
+			i = 0;
+			do {
+				if (!cpu_isset(i, mask))
+					continue;
+
+				while (!ccall_info.processors_in[i])
+					barrier();
+			} while (++i <= high);
+
+			i = 0;
+			do {
+				if (!cpu_isset(i, mask))
+					continue;
+
+				while (!ccall_info.processors_out[i])
+					barrier();
+			} while (++i <= high);
+		}
+
+		spin_unlock_irqrestore(&cross_call_lock, flags);
+	}
+}
+
+/* Running cross calls. */
+void leon_cross_call_irq(void)
+{
+	int i = smp_processor_id();
+
+	ccall_info.processors_in[i] = 1;
+	ccall_info.func(ccall_info.arg1, ccall_info.arg2, ccall_info.arg3,
+			ccall_info.arg4, ccall_info.arg5);
+	ccall_info.processors_out[i] = 1;
+}
+
+void leon_percpu_timer_interrupt(struct pt_regs *regs)
+{
+	struct pt_regs *old_regs;
+	int cpu = smp_processor_id();
+
+	old_regs = set_irq_regs(regs);
+
+	leon_clear_profile_irq(cpu);
+
+	profile_tick(CPU_PROFILING);
+
+	if (!--prof_counter(cpu)) {
+		int user = user_mode(regs);
+
+		irq_enter();
+		update_process_times(user);
+		irq_exit();
+
+		prof_counter(cpu) = prof_multiplier(cpu);
+	}
+	set_irq_regs(old_regs);
+}
+
+static void __init smp_setup_percpu_timer(void)
+{
+	int cpu = smp_processor_id();
+
+	prof_counter(cpu) = prof_multiplier(cpu) = 1;
+}
+
+void __init leon_blackbox_id(unsigned *addr)
+{
+	int rd = *addr & 0x3e000000;
+	int rs1 = rd >> 11;
+
+	/* patch places where ___b_hard_smp_processor_id appears */
+	addr[0] = 0x81444000 | rd;	/* rd %asr17, reg */
+	addr[1] = 0x8130201c | rd | rs1;	/* srl reg, 0x1c, reg */
+	addr[2] = 0x01000000;	/* nop */
+}
+
+void __init leon_blackbox_current(unsigned *addr)
+{
+	int rd = *addr & 0x3e000000;
+	int rs1 = rd >> 11;
+
+	/* patch LOAD_CURRENT macro where ___b_load_current appears */
+	addr[0] = 0x81444000 | rd;	/* rd %asr17, reg */
+	addr[2] = 0x8130201c | rd | rs1;	/* srl reg, 0x1c, reg */
+	addr[4] = 0x81282002 | rd | rs1;	/* sll reg, 0x2, reg */
+
+}
+
+/*
+ * CPU idle callback function
+ * See .../arch/sparc/kernel/process.c
+ */
+void pmc_leon_idle(void)
+{
+	__asm__ volatile ("mov %g0, %asr19");
+}
+
+void __init leon_init_smp(void)
+{
+	/* Patch ipi15 trap table */
+	t_nmi[1] = t_nmi[1] + (linux_trap_ipi15_leon - linux_trap_ipi15_sun4m);
+
+	BTFIXUPSET_BLACKBOX(hard_smp_processor_id, leon_blackbox_id);
+	BTFIXUPSET_BLACKBOX(load_current, leon_blackbox_current);
+	BTFIXUPSET_CALL(smp_cross_call, leon_cross_call, BTFIXUPCALL_NORM);
+	BTFIXUPSET_CALL(__hard_smp_processor_id, __leon_processor_id,
+			BTFIXUPCALL_NORM);
+
+#ifndef PMC_NO_IDLE
+	/* Assign power management IDLE handler */
+	pm_idle = pmc_leon_idle;
+	printk(KERN_INFO "leon: power management initialized\n");
+#endif
+
+}
+
+#endif /* CONFIG_SPARC_LEON */
@@ -32,6 +32,7 @@
 #include <asm/cacheflush.h>
 #include <asm/tlbflush.h>
 #include <asm/cpudata.h>
+#include <asm/leon.h>
  
 #include "irq.h"
  
@@ -96,6 +97,9 @@
 	case sun4d:
 		smp4d_smp_done();
 		break;
+	case sparc_leon:
+		leon_smp_done();
+		break;
 	case sun4e:
 		printk("SUN4E\n");
 		BUG();
@@ -306,6 +310,9 @@
 	case sun4d:
 		smp4d_boot_cpus();
 		break;
+	case sparc_leon:
+		leon_boot_cpus();
+		break;
 	case sun4e:
 		printk("SUN4E\n");
 		BUG();
@@ -375,6 +382,9 @@
 		break;
 	case sun4d:
 		ret = smp4d_boot_one_cpu(cpu);
+		break;
+	case sparc_leon:
+		ret = leon_boot_one_cpu(cpu);
 		break;
 	case sun4e:
 		printk("SUN4E\n");
@@ -15,7 +15,7 @@
 #include <asm/contregs.h>
 #include <asm/thread_info.h>
  
-	.globl sun4m_cpu_startup, __smp4m_processor_id
+	.globl sun4m_cpu_startup, __smp4m_processor_id, __leon_processor_id
 	.globl sun4d_cpu_startup, __smp4d_processor_id
  
 	__CPUINIT
@@ -106,6 +106,12 @@
 	retl
 	 mov	%g1, %o7
  
+__leon_processor_id:
+	rd     %asr17,%g2
+        srl    %g2,28,%g2
+	retl
+	 mov	%g1, %o7
+
 /* CPUID in bootbus can be found at PA 0xff0140000 */
 #define SUN4D_BOOTBUS_CPUID	0xf0140000
  
@@ -160,4 +166,65 @@
 	 nop
  
 	b,a	smp_do_cpu_idle
+
+#ifdef CONFIG_SPARC_LEON
+
+	__CPUINIT
+	.align	4
+        .global leon_smp_cpu_startup, smp_penguin_ctable
+
+leon_smp_cpu_startup:
+
+        set smp_penguin_ctable,%g1
+        ld [%g1+4],%g1
+        srl %g1,4,%g1
+        set 0x00000100,%g5 /* SRMMU_CTXTBL_PTR */
+	sta %g1, [%g5] ASI_M_MMUREGS
+
+	/* Set up a sane %psr -- PIL<0xf> S<0x1> PS<0x1> CWP<0x0> */
+	set	(PSR_PIL | PSR_S | PSR_PS), %g1
+	wr	%g1, 0x0, %psr		! traps off though
+	WRITE_PAUSE
+
+	/* Our %wim is one behind CWP */
+	mov	2, %g1
+	wr	%g1, 0x0, %wim
+	WRITE_PAUSE
+
+	/* Set tbr - we use just one trap table. */
+	set	trapbase, %g1
+	wr	%g1, 0x0, %tbr
+	WRITE_PAUSE
+
+	/* Get our CPU id */
+        rd     %asr17,%g3
+
+	/* Give ourselves a stack and curptr. */
+	set	current_set, %g5
+	srl	%g3, 28, %g4
+	sll	%g4, 2, %g4
+	ld	[%g5 + %g4], %g6
+
+	sethi	%hi(THREAD_SIZE - STACKFRAME_SZ), %sp
+	or	%sp, %lo(THREAD_SIZE - STACKFRAME_SZ), %sp
+	add	%g6, %sp, %sp
+
+	/* Turn on traps (PSR_ET). */
+	rd	%psr, %g1
+	wr	%g1, PSR_ET, %psr	! traps on
+	WRITE_PAUSE
+
+	/* Init our caches, etc. */
+	set	poke_srmmu, %g5
+	ld	[%g5], %g5
+	call	%g5
+	 nop
+
+	/* Start this processor. */
+	call	leon_callin
+	 nop
+
+	b,a	smp_do_cpu_idle
+
+#endif
@@ -2301,7 +2301,8 @@
 	BTFIXUPSET_CALL(flush_cache_mm, smp_flush_cache_mm, BTFIXUPCALL_NORM);
 	BTFIXUPSET_CALL(flush_cache_range, smp_flush_cache_range, BTFIXUPCALL_NORM);
 	BTFIXUPSET_CALL(flush_cache_page, smp_flush_cache_page, BTFIXUPCALL_NORM);
-	if (sparc_cpu_model != sun4d) {
+	if (sparc_cpu_model != sun4d &&
+	    sparc_cpu_model != sparc_leon) {
 		BTFIXUPSET_CALL(flush_tlb_all, smp_flush_tlb_all, BTFIXUPCALL_NORM);
 		BTFIXUPSET_CALL(flush_tlb_mm, smp_flush_tlb_mm, BTFIXUPCALL_NORM);
 		BTFIXUPSET_CALL(flush_tlb_range, smp_flush_tlb_range, BTFIXUPCALL_NORM);
@@ -2330,6 +2331,8 @@
 #ifdef CONFIG_SMP
 	if (sparc_cpu_model == sun4d)
 		sun4d_init_smp();
+	else if (sparc_cpu_model == sparc_leon)
+		leon_init_smp();
 	else
 		sun4m_init_smp();
 #endif
...	...	@@ -340,6 +340,30 @@
340	340	extern void leon_switch_mm(void);
341	341	extern int srmmu_swprobe_trace;
342	342
	343	+#ifdef CONFIG_SMP
	344	+extern int leon_smp_nrcpus(void);
	345	+extern void leon_clear_profile_irq(int cpu);
	346	+extern void leon_smp_done(void);
	347	+extern void leon_boot_cpus(void);
	348	+extern int leon_boot_one_cpu(int i);
	349	+void leon_init_smp(void);
	350	+extern void cpu_probe(void);
	351	+extern void cpu_idle(void);
	352	+extern void init_IRQ(void);
	353	+extern void cpu_panic(void);
	354	+extern int __leon_processor_id(void);
	355	+void leon_enable_irq_cpu(unsigned int irq_nr, unsigned int cpu);
	356	+
	357	+extern unsigned int real_irq_entry[], smpleon_ticker[];
	358	+extern unsigned int patchme_maybe_smp_msg[];
	359	+extern unsigned long trapbase_cpu1[];
	360	+extern unsigned long trapbase_cpu2[];
	361	+extern unsigned long trapbase_cpu3[];
	362	+extern unsigned int t_nmi[], linux_trap_ipi15_leon[];
	363	+extern unsigned int linux_trap_ipi15_sun4m[];
	364	+
	365	+#endif /* CONFIG_SMP */
	366	+
343	367	#endif /* __KERNEL__ */
344	368
345	369	#endif /* __ASSEMBLY__ */
...	...	@@ -356,6 +380,10 @@
356	380	#define leon_switch_mm() do {} while (0)
357	381	#define leon_init_IRQ() do {} while (0)
358	382	#define init_leon() do {} while (0)
	383	+#define leon_smp_done() do {} while (0)
	384	+#define leon_boot_cpus() do {} while (0)
	385	+#define leon_boot_one_cpu(i) 1
	386	+#define leon_init_smp() do {} while (0)
359	387
360	388	#endif /* !defined(CONFIG_SPARC_LEON) */
361	389
...	...	@@ -106,6 +106,15 @@
106	106	return cpuid;
107	107	}
108	108
	109	+extern inline int hard_smpleon_processor_id(void)
	110	+{
	111	+ int cpuid;
	112	+ __asm__ __volatile__("rd %%asr17,%0\n\t"
	113	+ "srl %0,28,%0" :
	114	+ "=&r" (cpuid) : );
	115	+ return cpuid;
	116	+}
	117	+
109	118	#ifndef MODULE
110	119	static inline int hard_smp_processor_id(void)
111	120	{
...	...	@@ -72,7 +72,7 @@
72	72	obj-$(CONFIG_SPARC32_PCI) += pcic.o
73	73
74	74	obj-$(CONFIG_SMP) += trampoline_$(BITS).o smp_$(BITS).o
75		-obj-$(CONFIG_SPARC32_SMP) += sun4m_smp.o sun4d_smp.o
	75	+obj-$(CONFIG_SPARC32_SMP) += sun4m_smp.o sun4d_smp.o leon_smp.o
76	76	obj-$(CONFIG_SPARC64_SMP) += hvtramp.o
77	77
78	78	obj-y += auxio_$(BITS).o
...	...	@@ -400,6 +400,39 @@
400	400	/* FIXME */
401	401	1: b,a 1b
402	402
	403	+#ifdef CONFIG_SPARC_LEON
	404	+
	405	+ .globl smpleon_ticker
	406	+ /* SMP per-cpu ticker interrupts are handled specially. */
	407	+smpleon_ticker:
	408	+ SAVE_ALL
	409	+ or %l0, PSR_PIL, %g2
	410	+ wr %g2, 0x0, %psr
	411	+ WRITE_PAUSE
	412	+ wr %g2, PSR_ET, %psr
	413	+ WRITE_PAUSE
	414	+ call leon_percpu_timer_interrupt
	415	+ add %sp, STACKFRAME_SZ, %o0
	416	+ wr %l0, PSR_ET, %psr
	417	+ WRITE_PAUSE
	418	+ RESTORE_ALL
	419	+
	420	+ .align 4
	421	+ .globl linux_trap_ipi15_leon
	422	+linux_trap_ipi15_leon:
	423	+ SAVE_ALL
	424	+ or %l0, PSR_PIL, %l4
	425	+ wr %l4, 0x0, %psr
	426	+ WRITE_PAUSE
	427	+ wr %l4, PSR_ET, %psr
	428	+ WRITE_PAUSE
	429	+ call leon_cross_call_irq
	430	+ nop
	431	+ b ret_trap_lockless_ipi
	432	+ clr %l6
	433	+
	434	+#endif /* CONFIG_SPARC_LEON */
	435	+
403	436	#endif /* CONFIG_SMP */
404	437
405	438	/* This routine handles illegal instructions and privileged
...	...	@@ -811,8 +811,30 @@
811	811	got_prop:
812	812	#ifdef CONFIG_SPARC_LEON
813	813	/* no cpu-type check is needed, it is a SPARC-LEON */
	814	+#ifdef CONFIG_SMP
	815	+ ba leon_smp_init
	816	+ nop
	817	+
	818	+ .global leon_smp_init
	819	+leon_smp_init:
	820	+ sethi %hi(boot_cpu_id), %g1 ! master always 0
	821	+ stb %g0, [%g1 + %lo(boot_cpu_id)]
	822	+ sethi %hi(boot_cpu_id4), %g1 ! master always 0
	823	+ stb %g0, [%g1 + %lo(boot_cpu_id4)]
	824	+
	825	+ rd %asr17,%g1
	826	+ srl %g1,28,%g1
	827	+
	828	+ cmp %g0,%g1
	829	+ beq sun4c_continue_boot !continue with master
	830	+ nop
	831	+
	832	+ ba leon_smp_cpu_startup
	833	+ nop
	834	+#else
814	835	ba sun4c_continue_boot
815	836	nop
	837	+#endif
816	838	#endif
817	839	set cputypval, %o2
818	840	ldub [%o2 + 0x4], %l1
...	...	@@ -48,8 +48,13 @@
48	48	#include <asm/dma.h>
49	49	#include <asm/iommu.h>
50	50	#include <asm/io-unit.h>
	51	+#include <asm/leon.h>
51	52
	53	+#ifdef CONFIG_SPARC_LEON
	54	+#define mmu_inval_dma_area(p, l) leon_flush_dcache_all()
	55	+#else
52	56	#define mmu_inval_dma_area(p, l) /* Anton pulled it out for 2.4.0-xx */
	57	+#endif
53	58
54	59	static struct resource _sparc_find_resource(struct resource r,
55	60	unsigned long);
...	...	@@ -12,11 +12,14 @@
12	12	#include <linux/of_platform.h>
13	13	#include <linux/interrupt.h>
14	14	#include <linux/of_device.h>
	15	+
15	16	#include <asm/oplib.h>
16	17	#include <asm/timer.h>
17	18	#include <asm/prom.h>
18	19	#include <asm/leon.h>
19	20	#include <asm/leon_amba.h>
	21	+#include <asm/traps.h>
	22	+#include <asm/cacheflush.h>
20	23
21	24	#include "prom.h"
22	25	#include "irq.h"
...	...	@@ -115,6 +118,21 @@
115	118	(((1000000 / 100) - 1)));
116	119	LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[0].ctrl, 0);
117	120
	121	+#ifdef CONFIG_SMP
	122	+ leon_percpu_timer_dev[0].start = (int)leon3_gptimer_regs;
	123	+ leon_percpu_timer_dev[0].irq = leon3_gptimer_irq+1;
	124	+
	125	+ if (!(LEON3_BYPASS_LOAD_PA(&leon3_gptimer_regs->config) &
	126	+ (1<<LEON3_GPTIMER_SEPIRQ))) {
	127	+ prom_printf("irq timer not configured with seperate irqs \n");
	128	+ BUG();
	129	+ }
	130	+
	131	+ LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[1].val, 0);
	132	+ LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[1].rld, (((1000000/100) - 1)));
	133	+ LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[1].ctrl, 0);
	134	+# endif
	135	+
118	136	} else {
119	137	printk(KERN_ERR "No Timer/irqctrl found\n");
120	138	BUG();
121	139
...	...	@@ -130,11 +148,41 @@
130	148	prom_halt();
131	149	}
132	150
	151	+# ifdef CONFIG_SMP
	152	+ {
	153	+ unsigned long flags;
	154	+ struct tt_entry *trap_table = &sparc_ttable[SP_TRAP_IRQ1 + (leon_percpu_timer_dev[0].irq - 1)];
	155	+
	156	+ /* For SMP we use the level 14 ticker, however the bootup code
	157	+ * has copied the firmwares level 14 vector into boot cpu's
	158	+ * trap table, we must fix this now or we get squashed.
	159	+ */
	160	+ local_irq_save(flags);
	161	+
	162	+ patchme_maybe_smp_msg[0] = 0x01000000; /* NOP out the branch */
	163	+
	164	+ /* Adjust so that we jump directly to smpleon_ticker */
	165	+ trap_table->inst_three += smpleon_ticker - real_irq_entry;
	166	+
	167	+ local_flush_cache_all();
	168	+ local_irq_restore(flags);
	169	+ }
	170	+# endif
	171	+
133	172	if (leon3_gptimer_regs) {
134	173	LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[0].ctrl,
135	174	LEON3_GPTIMER_EN \|
136	175	LEON3_GPTIMER_RL \|
137	176	LEON3_GPTIMER_LD \| LEON3_GPTIMER_IRQEN);
	177	+
	178	+#ifdef CONFIG_SMP
	179	+ LEON3_BYPASS_STORE_PA(&leon3_gptimer_regs->e[1].ctrl,
	180	+ LEON3_GPTIMER_EN \|
	181	+ LEON3_GPTIMER_RL \|
	182	+ LEON3_GPTIMER_LD \|
	183	+ LEON3_GPTIMER_IRQEN);
	184	+#endif
	185	+
138	186	}
139	187	}
140	188
...	...	@@ -174,6 +222,42 @@
174	222	prom_amba_init(dp, nextp);
175	223	}
176	224	}
	225	+
	226	+#ifdef CONFIG_SMP
	227	+
	228	+void leon_set_cpu_int(int cpu, int level)
	229	+{
	230	+ unsigned long mask;
	231	+ mask = get_irqmask(level);
	232	+ LEON3_BYPASS_STORE_PA(&leon3_irqctrl_regs->force[cpu], mask);
	233	+}
	234	+
	235	+static void leon_clear_ipi(int cpu, int level)
	236	+{
	237	+ unsigned long mask;
	238	+ mask = get_irqmask(level);
	239	+ LEON3_BYPASS_STORE_PA(&leon3_irqctrl_regs->force[cpu], mask<<16);
	240	+}
	241	+
	242	+static void leon_set_udt(int cpu)
	243	+{
	244	+}
	245	+
	246	+void leon_clear_profile_irq(int cpu)
	247	+{
	248	+}
	249	+
	250	+void leon_enable_irq_cpu(unsigned int irq_nr, unsigned int cpu)
	251	+{
	252	+ unsigned long mask, flags, *addr;
	253	+ mask = get_irqmask(irq_nr);
	254	+ local_irq_save(flags);
	255	+ addr = (unsigned long *)&(leon3_irqctrl_regs->mask[cpu]);
	256	+ LEON3_BYPASS_STORE_PA(addr, (LEON3_BYPASS_LOAD_PA(addr) \| (mask)));
	257	+ local_irq_restore(flags);
	258	+}
	259	+
	260	+#endif
177	261
178	262	void __init leon_init_IRQ(void)
179	263	{
	1	+/* leon_smp.c: Sparc-Leon SMP support.
	2	+ *
	3	+ * based on sun4m_smp.c
	4	+ * Copyright (C) 1996 David S. Miller (davem@caip.rutgers.edu)
	5	+ * Copyright (C) 2009 Daniel Hellstrom (daniel@gaisler.com) Aeroflex Gaisler AB
	6	+ * Copyright (C) 2009 Konrad Eisele (konrad@gaisler.com) Aeroflex Gaisler AB
	7	+ */
	8	+
	9	+#include <asm/head.h>
	10	+
	11	+#include <linux/kernel.h>
	12	+#include <linux/sched.h>
	13	+#include <linux/threads.h>
	14	+#include <linux/smp.h>
	15	+#include <linux/smp_lock.h>
	16	+#include <linux/interrupt.h>
	17	+#include <linux/kernel_stat.h>
	18	+#include <linux/init.h>
	19	+#include <linux/spinlock.h>
	20	+#include <linux/mm.h>
	21	+#include <linux/swap.h>
	22	+#include <linux/profile.h>
	23	+#include <linux/pm.h>
	24	+#include <linux/delay.h>
	25	+
	26	+#include <asm/cacheflush.h>
	27	+#include <asm/tlbflush.h>
	28	+
	29	+#include <asm/ptrace.h>
	30	+#include <asm/atomic.h>
	31	+#include <asm/irq_regs.h>
	32	+
	33	+#include <asm/delay.h>
	34	+#include <asm/irq.h>
	35	+#include <asm/page.h>
	36	+#include <asm/pgalloc.h>
	37	+#include <asm/pgtable.h>
	38	+#include <asm/oplib.h>
	39	+#include <asm/cpudata.h>
	40	+#include <asm/asi.h>
	41	+#include <asm/leon.h>
	42	+#include <asm/leon_amba.h>
	43	+
	44	+#ifdef CONFIG_SPARC_LEON
	45	+
	46	+#include "irq.h"
	47	+
	48	+extern ctxd_t *srmmu_ctx_table_phys;
	49	+static int smp_processors_ready;
	50	+extern volatile unsigned long cpu_callin_map[NR_CPUS];
	51	+extern unsigned char boot_cpu_id;
	52	+extern cpumask_t smp_commenced_mask;
	53	+void __init leon_configure_cache_smp(void);
	54	+
	55	+static inline unsigned long do_swap(volatile unsigned long *ptr,
	56	+ unsigned long val)
	57	+{
	58	+ __asm__ __volatile__("swapa [%1] %2, %0\n\t" : "=&r"(val)
	59	+ : "r"(ptr), "i"(ASI_LEON_DCACHE_MISS)
	60	+ : "memory");
	61	+ return val;
	62	+}
	63	+
	64	+static void smp_setup_percpu_timer(void);
	65	+
	66	+void __cpuinit leon_callin(void)
	67	+{
	68	+ int cpuid = hard_smpleon_processor_id();
	69	+
	70	+ local_flush_cache_all();
	71	+ local_flush_tlb_all();
	72	+ leon_configure_cache_smp();
	73	+
	74	+ /* Get our local ticker going. */
	75	+ smp_setup_percpu_timer();
	76	+
	77	+ calibrate_delay();
	78	+ smp_store_cpu_info(cpuid);
	79	+
	80	+ local_flush_cache_all();
	81	+ local_flush_tlb_all();
	82	+
	83	+ /*
	84	+ * Unblock the master CPU _only_ when the scheduler state
	85	+ * of all secondary CPUs will be up-to-date, so after
	86	+ * the SMP initialization the master will be just allowed
	87	+ * to call the scheduler code.
	88	+ * Allow master to continue.
	89	+ */
	90	+ do_swap(&cpu_callin_map[cpuid], 1);
	91	+
	92	+ local_flush_cache_all();
	93	+ local_flush_tlb_all();
	94	+
	95	+ cpu_probe();
	96	+
	97	+ /* Fix idle thread fields. */
	98	+ __asm__ __volatile__("ld [%0], %%g6\n\t" : : "r"(&current_set[cpuid])
	99	+ : "memory" /* paranoid */);
	100	+
	101	+ /* Attach to the address space of init_task. */
	102	+ atomic_inc(&init_mm.mm_count);
	103	+ current->active_mm = &init_mm;
	104	+
	105	+ while (!cpu_isset(cpuid, smp_commenced_mask))
	106	+ mb();
	107	+
	108	+ local_irq_enable();
	109	+ cpu_set(cpuid, cpu_online_map);
	110	+}
	111	+
	112	+/*
	113	+ * Cycle through the processors asking the PROM to start each one.
	114	+ */
	115	+
	116	+extern struct linux_prom_registers smp_penguin_ctable;
	117	+
	118	+void __init leon_configure_cache_smp(void)
	119	+{
	120	+ unsigned long cfg = sparc_leon3_get_dcachecfg();
	121	+ int me = smp_processor_id();
	122	+
	123	+ if (ASI_LEON3_SYSCTRL_CFG_SSIZE(cfg) > 4) {
	124	+ printk(KERN_INFO "Note: SMP with snooping only works on 4k cache, found %dk(0x%x) on cpu %d, disabling caches\n",
	125	+ (unsigned int)ASI_LEON3_SYSCTRL_CFG_SSIZE(cfg),
	126	+ (unsigned int)cfg, (unsigned int)me);
	127	+ sparc_leon3_disable_cache();
	128	+ } else {
	129	+ if (cfg & ASI_LEON3_SYSCTRL_CFG_SNOOPING) {
	130	+ sparc_leon3_enable_snooping();
	131	+ } else {
	132	+ printk(KERN_INFO "Note: You have to enable snooping in the vhdl model cpu %d, disabling caches\n",
	133	+ me);
	134	+ sparc_leon3_disable_cache();
	135	+ }
	136	+ }
	137	+
	138	+ local_flush_cache_all();
	139	+ local_flush_tlb_all();
	140	+}
	141	+
	142	+void leon_smp_setbroadcast(unsigned int mask)
	143	+{
	144	+ int broadcast =
	145	+ ((LEON3_BYPASS_LOAD_PA(&(leon3_irqctrl_regs->mpstatus)) >>
	146	+ LEON3_IRQMPSTATUS_BROADCAST) & 1);
	147	+ if (!broadcast) {
	148	+ prom_printf("######## !!!! The irqmp-ctrl must have broadcast enabled, smp wont work !!!!! ####### nr cpus: %d\n",
	149	+ leon_smp_nrcpus());
	150	+ if (leon_smp_nrcpus() > 1) {
	151	+ BUG();
	152	+ } else {
	153	+ prom_printf("continue anyway\n");
	154	+ return;
	155	+ }
	156	+ }
	157	+ LEON_BYPASS_STORE_PA(&(leon3_irqctrl_regs->mpbroadcast), mask);
	158	+}
	159	+
	160	+unsigned int leon_smp_getbroadcast(void)
	161	+{
	162	+ unsigned int mask;
	163	+ mask = LEON_BYPASS_LOAD_PA(&(leon3_irqctrl_regs->mpbroadcast));
	164	+ return mask;
	165	+}
	166	+
	167	+int leon_smp_nrcpus(void)
	168	+{
	169	+ int nrcpu =
	170	+ ((LEON3_BYPASS_LOAD_PA(&(leon3_irqctrl_regs->mpstatus)) >>
	171	+ LEON3_IRQMPSTATUS_CPUNR) & 0xf) + 1;
	172	+ return nrcpu;
	173	+}
	174	+
	175	+void __init leon_boot_cpus(void)
	176	+{
	177	+ int nrcpu = leon_smp_nrcpus();
	178	+ int me = smp_processor_id();
	179	+
	180	+ printk(KERN_INFO "%d:(%d:%d) cpus mpirq at 0x%x \n", (unsigned int)me,
	181	+ (unsigned int)nrcpu, (unsigned int)NR_CPUS,
	182	+ (unsigned int)&(leon3_irqctrl_regs->mpstatus));
	183	+
	184	+ leon_enable_irq_cpu(LEON3_IRQ_CROSS_CALL, me);
	185	+ leon_enable_irq_cpu(LEON3_IRQ_TICKER, me);
	186	+ leon_enable_irq_cpu(LEON3_IRQ_RESCHEDULE, me);
	187	+
	188	+ leon_smp_setbroadcast(1 << LEON3_IRQ_TICKER);
	189	+
	190	+ leon_configure_cache_smp();
	191	+ smp_setup_percpu_timer();
	192	+ local_flush_cache_all();
	193	+
	194	+}
	195	+
	196	+int __cpuinit leon_boot_one_cpu(int i)
	197	+{
	198	+
	199	+ struct task_struct *p;
	200	+ int timeout;
	201	+
	202	+ /* Cook up an idler for this guy. */
	203	+ p = fork_idle(i);
	204	+
	205	+ current_set[i] = task_thread_info(p);
	206	+
	207	+ /* See trampoline.S:leon_smp_cpu_startup for details...
	208	+ * Initialize the contexts table
	209	+ * Since the call to prom_startcpu() trashes the structure,
	210	+ * we need to re-initialize it for each cpu
	211	+ */
	212	+ smp_penguin_ctable.which_io = 0;
	213	+ smp_penguin_ctable.phys_addr = (unsigned int)srmmu_ctx_table_phys;
	214	+ smp_penguin_ctable.reg_size = 0;
	215	+
	216	+ /* whirrr, whirrr, whirrrrrrrrr... */
	217	+ printk(KERN_INFO "Starting CPU %d : (irqmp: 0x%x)\n", (unsigned int)i,
	218	+ (unsigned int)&leon3_irqctrl_regs->mpstatus);
	219	+ local_flush_cache_all();
	220	+
	221	+ LEON_BYPASS_STORE_PA(&(leon3_irqctrl_regs->mpstatus), 1 << i);
	222	+
	223	+ /* wheee... it's going... */
	224	+ for (timeout = 0; timeout < 10000; timeout++) {
	225	+ if (cpu_callin_map[i])
	226	+ break;
	227	+ udelay(200);
	228	+ }
	229	+ printk(KERN_INFO "Started CPU %d \n", (unsigned int)i);
	230	+
	231	+ if (!(cpu_callin_map[i])) {
	232	+ printk(KERN_ERR "Processor %d is stuck.\n", i);
	233	+ return -ENODEV;
	234	+ } else {
	235	+ leon_enable_irq_cpu(LEON3_IRQ_CROSS_CALL, i);
	236	+ leon_enable_irq_cpu(LEON3_IRQ_TICKER, i);
	237	+ leon_enable_irq_cpu(LEON3_IRQ_RESCHEDULE, i);
	238	+ }
	239	+
	240	+ local_flush_cache_all();
	241	+ return 0;
	242	+}
	243	+
	244	+void __init leon_smp_done(void)
	245	+{
	246	+
	247	+ int i, first;
	248	+ int *prev;
	249	+
	250	+ /* setup cpu list for irq rotation */
	251	+ first = 0;
	252	+ prev = &first;
	253	+ for (i = 0; i < NR_CPUS; i++) {
	254	+ if (cpu_online(i)) {
	255	+ *prev = i;
	256	+ prev = &cpu_data(i).next;
	257	+ }
	258	+ }
	259	+ *prev = first;
	260	+ local_flush_cache_all();
	261	+
	262	+ /* Free unneeded trap tables */
	263	+ if (!cpu_isset(1, cpu_present_map)) {
	264	+ ClearPageReserved(virt_to_page(trapbase_cpu1));
	265	+ init_page_count(virt_to_page(trapbase_cpu1));
	266	+ free_page((unsigned long)trapbase_cpu1);
	267	+ totalram_pages++;
	268	+ num_physpages++;
	269	+ }
	270	+ if (!cpu_isset(2, cpu_present_map)) {
	271	+ ClearPageReserved(virt_to_page(trapbase_cpu2));
	272	+ init_page_count(virt_to_page(trapbase_cpu2));
	273	+ free_page((unsigned long)trapbase_cpu2);
	274	+ totalram_pages++;
	275	+ num_physpages++;
	276	+ }
	277	+ if (!cpu_isset(3, cpu_present_map)) {
	278	+ ClearPageReserved(virt_to_page(trapbase_cpu3));
	279	+ init_page_count(virt_to_page(trapbase_cpu3));
	280	+ free_page((unsigned long)trapbase_cpu3);
	281	+ totalram_pages++;
	282	+ num_physpages++;
	283	+ }
	284	+ /* Ok, they are spinning and ready to go. */
	285	+ smp_processors_ready = 1;
	286	+
	287	+}
	288	+
	289	+void leon_irq_rotate(int cpu)
	290	+{
	291	+}
	292	+
	293	+static struct smp_funcall {
	294	+ smpfunc_t func;
	295	+ unsigned long arg1;
	296	+ unsigned long arg2;
	297	+ unsigned long arg3;
	298	+ unsigned long arg4;
	299	+ unsigned long arg5;
	300	+ unsigned long processors_in[NR_CPUS]; /* Set when ipi entered. */
	301	+ unsigned long processors_out[NR_CPUS]; /* Set when ipi exited. */
	302	+} ccall_info;
	303	+
	304	+static DEFINE_SPINLOCK(cross_call_lock);
	305	+
	306	+/* Cross calls must be serialized, at least currently. */
	307	+static void leon_cross_call(smpfunc_t func, cpumask_t mask, unsigned long arg1,
	308	+ unsigned long arg2, unsigned long arg3,
	309	+ unsigned long arg4)
	310	+{
	311	+ if (smp_processors_ready) {
	312	+ register int high = NR_CPUS - 1;
	313	+ unsigned long flags;
	314	+
	315	+ spin_lock_irqsave(&cross_call_lock, flags);
	316	+
	317	+ {
	318	+ /* If you make changes here, make sure gcc generates proper code... */
	319	+ register smpfunc_t f asm("i0") = func;
	320	+ register unsigned long a1 asm("i1") = arg1;
	321	+ register unsigned long a2 asm("i2") = arg2;
	322	+ register unsigned long a3 asm("i3") = arg3;
	323	+ register unsigned long a4 asm("i4") = arg4;
	324	+ register unsigned long a5 asm("i5") = 0;
	325	+
	326	+ __asm__ __volatile__("std %0, [%6]\n\t"
	327	+ "std %2, [%6 + 8]\n\t"
	328	+ "std %4, [%6 + 16]\n\t" : :
	329	+ "r"(f), "r"(a1), "r"(a2), "r"(a3),
	330	+ "r"(a4), "r"(a5),
	331	+ "r"(&ccall_info.func));
	332	+ }
	333	+
	334	+ /* Init receive/complete mapping, plus fire the IPI's off. */
	335	+ {
	336	+ register int i;
	337	+
	338	+ cpu_clear(smp_processor_id(), mask);
	339	+ cpus_and(mask, cpu_online_map, mask);
	340	+ for (i = 0; i <= high; i++) {
	341	+ if (cpu_isset(i, mask)) {
	342	+ ccall_info.processors_in[i] = 0;
	343	+ ccall_info.processors_out[i] = 0;
	344	+ set_cpu_int(i, LEON3_IRQ_CROSS_CALL);
	345	+
	346	+ }
	347	+ }
	348	+ }
	349	+
	350	+ {
	351	+ register int i;
	352	+
	353	+ i = 0;
	354	+ do {
	355	+ if (!cpu_isset(i, mask))
	356	+ continue;
	357	+
	358	+ while (!ccall_info.processors_in[i])
	359	+ barrier();
	360	+ } while (++i <= high);
	361	+
	362	+ i = 0;
	363	+ do {
	364	+ if (!cpu_isset(i, mask))
	365	+ continue;
	366	+
	367	+ while (!ccall_info.processors_out[i])
	368	+ barrier();
	369	+ } while (++i <= high);
	370	+ }
	371	+
	372	+ spin_unlock_irqrestore(&cross_call_lock, flags);
	373	+ }
	374	+}
	375	+
	376	+/* Running cross calls. */
	377	+void leon_cross_call_irq(void)
	378	+{
	379	+ int i = smp_processor_id();
	380	+
	381	+ ccall_info.processors_in[i] = 1;
	382	+ ccall_info.func(ccall_info.arg1, ccall_info.arg2, ccall_info.arg3,
	383	+ ccall_info.arg4, ccall_info.arg5);
	384	+ ccall_info.processors_out[i] = 1;
	385	+}
	386	+
	387	+void leon_percpu_timer_interrupt(struct pt_regs *regs)
	388	+{
	389	+ struct pt_regs *old_regs;
	390	+ int cpu = smp_processor_id();
	391	+
	392	+ old_regs = set_irq_regs(regs);
	393	+
	394	+ leon_clear_profile_irq(cpu);
	395	+
	396	+ profile_tick(CPU_PROFILING);
	397	+
	398	+ if (!--prof_counter(cpu)) {
	399	+ int user = user_mode(regs);
	400	+
	401	+ irq_enter();
	402	+ update_process_times(user);
	403	+ irq_exit();
	404	+
	405	+ prof_counter(cpu) = prof_multiplier(cpu);
	406	+ }
	407	+ set_irq_regs(old_regs);
	408	+}
	409	+
	410	+static void __init smp_setup_percpu_timer(void)
	411	+{
	412	+ int cpu = smp_processor_id();
	413	+
	414	+ prof_counter(cpu) = prof_multiplier(cpu) = 1;
	415	+}
	416	+
	417	+void __init leon_blackbox_id(unsigned *addr)
	418	+{
	419	+ int rd = *addr & 0x3e000000;
	420	+ int rs1 = rd >> 11;
	421	+
	422	+ /* patch places where ___b_hard_smp_processor_id appears */
	423	+ addr[0] = 0x81444000 \| rd; /* rd %asr17, reg */
	424	+ addr[1] = 0x8130201c \| rd \| rs1; /* srl reg, 0x1c, reg */
	425	+ addr[2] = 0x01000000; /* nop */
	426	+}
	427	+
	428	+void __init leon_blackbox_current(unsigned *addr)
	429	+{
	430	+ int rd = *addr & 0x3e000000;
	431	+ int rs1 = rd >> 11;
	432	+
	433	+ /* patch LOAD_CURRENT macro where ___b_load_current appears */
	434	+ addr[0] = 0x81444000 \| rd; /* rd %asr17, reg */
	435	+ addr[2] = 0x8130201c \| rd \| rs1; /* srl reg, 0x1c, reg */
	436	+ addr[4] = 0x81282002 \| rd \| rs1; /* sll reg, 0x2, reg */
	437	+
	438	+}
	439	+
	440	+/*
	441	+ * CPU idle callback function
	442	+ * See .../arch/sparc/kernel/process.c
	443	+ */
	444	+void pmc_leon_idle(void)
	445	+{
	446	+ __asm__ volatile ("mov %g0, %asr19");
	447	+}
	448	+
	449	+void __init leon_init_smp(void)
	450	+{
	451	+ /* Patch ipi15 trap table */
	452	+ t_nmi[1] = t_nmi[1] + (linux_trap_ipi15_leon - linux_trap_ipi15_sun4m);
	453	+
	454	+ BTFIXUPSET_BLACKBOX(hard_smp_processor_id, leon_blackbox_id);
	455	+ BTFIXUPSET_BLACKBOX(load_current, leon_blackbox_current);
	456	+ BTFIXUPSET_CALL(smp_cross_call, leon_cross_call, BTFIXUPCALL_NORM);
	457	+ BTFIXUPSET_CALL(__hard_smp_processor_id, __leon_processor_id,
	458	+ BTFIXUPCALL_NORM);
	459	+
	460	+#ifndef PMC_NO_IDLE
	461	+ /* Assign power management IDLE handler */
	462	+ pm_idle = pmc_leon_idle;
	463	+ printk(KERN_INFO "leon: power management initialized\n");
	464	+#endif
	465	+
	466	+}
	467	+
	468	+#endif /* CONFIG_SPARC_LEON */
...	...	@@ -32,6 +32,7 @@
32	32	#include <asm/cacheflush.h>
33	33	#include <asm/tlbflush.h>
34	34	#include <asm/cpudata.h>
	35	+#include <asm/leon.h>
35	36
36	37	#include "irq.h"
37	38
...	...	@@ -96,6 +97,9 @@
96	97	case sun4d:
97	98	smp4d_smp_done();
98	99	break;
	100	+ case sparc_leon:
	101	+ leon_smp_done();
	102	+ break;
99	103	case sun4e:
100	104	printk("SUN4E\n");
101	105	BUG();
...	...	@@ -306,6 +310,9 @@
306	310	case sun4d:
307	311	smp4d_boot_cpus();
308	312	break;
	313	+ case sparc_leon:
	314	+ leon_boot_cpus();
	315	+ break;
309	316	case sun4e:
310	317	printk("SUN4E\n");
311	318	BUG();
...	...	@@ -375,6 +382,9 @@
375	382	break;
376	383	case sun4d:
377	384	ret = smp4d_boot_one_cpu(cpu);
	385	+ break;
	386	+ case sparc_leon:
	387	+ ret = leon_boot_one_cpu(cpu);
378	388	break;
379	389	case sun4e:
380	390	printk("SUN4E\n");
...	...	@@ -15,7 +15,7 @@
15	15	#include <asm/contregs.h>
16	16	#include <asm/thread_info.h>
17	17
18		- .globl sun4m_cpu_startup, __smp4m_processor_id
	18	+ .globl sun4m_cpu_startup, __smp4m_processor_id, __leon_processor_id
19	19	.globl sun4d_cpu_startup, __smp4d_processor_id
20	20
21	21	__CPUINIT
...	...	@@ -106,6 +106,12 @@
106	106	retl
107	107	mov %g1, %o7
108	108
	109	+__leon_processor_id:
	110	+ rd %asr17,%g2
	111	+ srl %g2,28,%g2
	112	+ retl
	113	+ mov %g1, %o7
	114	+
109	115	/* CPUID in bootbus can be found at PA 0xff0140000 */
110	116	#define SUN4D_BOOTBUS_CPUID 0xf0140000
111	117
...	...	@@ -160,4 +166,65 @@
160	166	nop
161	167
162	168	b,a smp_do_cpu_idle
	169	+
	170	+#ifdef CONFIG_SPARC_LEON
	171	+
	172	+ __CPUINIT
	173	+ .align 4
	174	+ .global leon_smp_cpu_startup, smp_penguin_ctable
	175	+
	176	+leon_smp_cpu_startup:
	177	+
	178	+ set smp_penguin_ctable,%g1
	179	+ ld [%g1+4],%g1
	180	+ srl %g1,4,%g1
	181	+ set 0x00000100,%g5 /* SRMMU_CTXTBL_PTR */
	182	+ sta %g1, [%g5] ASI_M_MMUREGS
	183	+
	184	+ /* Set up a sane %psr -- PIL<0xf> S<0x1> PS<0x1> CWP<0x0> */
	185	+ set (PSR_PIL \| PSR_S \| PSR_PS), %g1
	186	+ wr %g1, 0x0, %psr ! traps off though
	187	+ WRITE_PAUSE
	188	+
	189	+ /* Our %wim is one behind CWP */
	190	+ mov 2, %g1
	191	+ wr %g1, 0x0, %wim
	192	+ WRITE_PAUSE
	193	+
	194	+ /* Set tbr - we use just one trap table. */
	195	+ set trapbase, %g1
	196	+ wr %g1, 0x0, %tbr
	197	+ WRITE_PAUSE
	198	+
	199	+ /* Get our CPU id */
	200	+ rd %asr17,%g3
	201	+
	202	+ /* Give ourselves a stack and curptr. */
	203	+ set current_set, %g5
	204	+ srl %g3, 28, %g4
	205	+ sll %g4, 2, %g4
	206	+ ld [%g5 + %g4], %g6
	207	+
	208	+ sethi %hi(THREAD_SIZE - STACKFRAME_SZ), %sp
	209	+ or %sp, %lo(THREAD_SIZE - STACKFRAME_SZ), %sp
	210	+ add %g6, %sp, %sp
	211	+
	212	+ /* Turn on traps (PSR_ET). */
	213	+ rd %psr, %g1
	214	+ wr %g1, PSR_ET, %psr ! traps on
	215	+ WRITE_PAUSE
	216	+
	217	+ /* Init our caches, etc. */
	218	+ set poke_srmmu, %g5
	219	+ ld [%g5], %g5
	220	+ call %g5
	221	+ nop
	222	+
	223	+ /* Start this processor. */
	224	+ call leon_callin
	225	+ nop
	226	+
	227	+ b,a smp_do_cpu_idle
	228	+
	229	+#endif