traps.c

		/* SRSMap is only defined if shadow sets are implemented */
		if (srssets > 1)
			change_c0_srsmap(0xf << n*4, srs << n*4);
	}

	if (srs == 0) {
		/*
		 * If no shadow set is selected then use the default handler
		 * that does normal register saving and standard interrupt exit
		 */
		extern char except_vec_vi, except_vec_vi_lui;
		extern char except_vec_vi_ori, except_vec_vi_end;
		extern char rollback_except_vec_vi;
		char *vec_start = using_rollback_handler() ?
			&rollback_except_vec_vi : &except_vec_vi;
#if defined(CONFIG_CPU_MICROMIPS) || defined(CONFIG_CPU_BIG_ENDIAN)
		const int lui_offset = &except_vec_vi_lui - vec_start + 2;
		const int ori_offset = &except_vec_vi_ori - vec_start + 2;
#else
		const int lui_offset = &except_vec_vi_lui - vec_start;
		const int ori_offset = &except_vec_vi_ori - vec_start;
#endif
		const int handler_len = &except_vec_vi_end - vec_start;

		if (handler_len > VECTORSPACING) {
			/*
			 * Sigh... panicing won't help as the console
			 * is probably not configured :(
			 */
			panic("VECTORSPACING too small");
		}

		set_handler(((unsigned long)b - ebase), vec_start,
#ifdef CONFIG_CPU_MICROMIPS
				(handler_len - 1));
#else
				handler_len);
#endif
		h = (u16 *)(b + lui_offset);
		*h = (handler >> 16) & 0xffff;
		h = (u16 *)(b + ori_offset);
		*h = (handler & 0xffff);
		local_flush_icache_range((unsigned long)b,
					 (unsigned long)(b+handler_len));
	}
	else {
		/*
		 * In other cases jump directly to the interrupt handler. It
		 * is the handler's responsibility to save registers if required
		 * (eg hi/lo) and return from the exception using "eret".
		 */
		u32 insn;

		h = (u16 *)b;
		/* j handler */
#ifdef CONFIG_CPU_MICROMIPS
		insn = 0xd4000000 | (((u32)handler & 0x07ffffff) >> 1);
#else
		insn = 0x08000000 | (((u32)handler & 0x0fffffff) >> 2);
#endif
		h[0] = (insn >> 16) & 0xffff;
		h[1] = insn & 0xffff;
		h[2] = 0;
		h[3] = 0;
		local_flush_icache_range((unsigned long)b,
					 (unsigned long)(b+8));
	}

	return (void *)old_handler;
}

void *set_vi_handler(int n, vi_handler_t addr)
{
	return set_vi_srs_handler(n, addr, 0);
}

extern void tlb_init(void);

/*
 * Timer interrupt
 */
int cp0_compare_irq;
EXPORT_SYMBOL_GPL(cp0_compare_irq);
int cp0_compare_irq_shift;

/*
 * Performance counter IRQ or -1 if shared with timer
 */
int cp0_perfcount_irq;
EXPORT_SYMBOL_GPL(cp0_perfcount_irq);

/*
 * Fast debug channel IRQ or -1 if not present
 */
int cp0_fdc_irq;
EXPORT_SYMBOL_GPL(cp0_fdc_irq);

static int noulri;

static int __init ulri_disable(char *s)
{
	pr_info("Disabling ulri\n");
	noulri = 1;

	return 1;
}
__setup("noulri", ulri_disable);

/* configure STATUS register */
static void configure_status(void)
{
	/*
	 * Disable coprocessors and select 32-bit or 64-bit addressing
	 * and the 16/32 or 32/32 FPR register model.  Reset the BEV
	 * flag that some firmware may have left set and the TS bit (for
	 * IP27).  Set XX for ISA IV code to work.
	 */
	unsigned int status_set = ST0_CU0;
#ifdef CONFIG_64BIT
	status_set |= ST0_FR|ST0_KX|ST0_SX|ST0_UX;
#endif
	if (current_cpu_data.isa_level & MIPS_CPU_ISA_IV)
		status_set |= ST0_XX;
	if (cpu_has_dsp)
		status_set |= ST0_MX;

	change_c0_status(ST0_CU|ST0_MX|ST0_RE|ST0_FR|ST0_BEV|ST0_TS|ST0_KX|ST0_SX|ST0_UX,
			 status_set);
}

unsigned int hwrena;
EXPORT_SYMBOL_GPL(hwrena);

/* configure HWRENA register */
static void configure_hwrena(void)
{
	hwrena = cpu_hwrena_impl_bits;

	if (cpu_has_mips_r2_r6)
		hwrena |= MIPS_HWRENA_CPUNUM |
			  MIPS_HWRENA_SYNCISTEP |
			  MIPS_HWRENA_CC |
			  MIPS_HWRENA_CCRES;

	if (!noulri && cpu_has_userlocal)
		hwrena |= MIPS_HWRENA_ULR;

	if (hwrena)
		write_c0_hwrena(hwrena);
}

static void configure_exception_vector(void)
{
	if (cpu_has_veic || cpu_has_vint) {
		unsigned long sr = set_c0_status(ST0_BEV);
		/* If available, use WG to set top bits of EBASE */
		if (cpu_has_ebase_wg) {
#ifdef CONFIG_64BIT
			write_c0_ebase_64(ebase | MIPS_EBASE_WG);
#else
			write_c0_ebase(ebase | MIPS_EBASE_WG);
#endif
		}
		write_c0_ebase(ebase);
		write_c0_status(sr);
		/* Setting vector spacing enables EI/VI mode  */
		change_c0_intctl(0x3e0, VECTORSPACING);
	}
	if (cpu_has_divec) {
		if (cpu_has_mipsmt) {
			unsigned int vpflags = dvpe();
			set_c0_cause(CAUSEF_IV);
			evpe(vpflags);
		} else
			set_c0_cause(CAUSEF_IV);
	}
}

void per_cpu_trap_init(bool is_boot_cpu)
{
	unsigned int cpu = smp_processor_id();

	configure_status();
	configure_hwrena();

	configure_exception_vector();

	/*
	 * Before R2 both interrupt numbers were fixed to 7, so on R2 only:
	 *
	 *  o read IntCtl.IPTI to determine the timer interrupt
	 *  o read IntCtl.IPPCI to determine the performance counter interrupt
	 *  o read IntCtl.IPFDC to determine the fast debug channel interrupt
	 */
	if (cpu_has_mips_r2_r6) {
		/*
		 * We shouldn't trust a secondary core has a sane EBASE register
		 * so use the one calculated by the boot CPU.
		 */
		if (!is_boot_cpu) {
			/* If available, use WG to set top bits of EBASE */
			if (cpu_has_ebase_wg) {
#ifdef CONFIG_64BIT
				write_c0_ebase_64(ebase | MIPS_EBASE_WG);
#else
				write_c0_ebase(ebase | MIPS_EBASE_WG);
#endif
			}
			write_c0_ebase(ebase);
		}

		cp0_compare_irq_shift = CAUSEB_TI - CAUSEB_IP;
		cp0_compare_irq = (read_c0_intctl() >> INTCTLB_IPTI) & 7;
		cp0_perfcount_irq = (read_c0_intctl() >> INTCTLB_IPPCI) & 7;
		cp0_fdc_irq = (read_c0_intctl() >> INTCTLB_IPFDC) & 7;
		if (!cp0_fdc_irq)
			cp0_fdc_irq = -1;

	} else {
		cp0_compare_irq = CP0_LEGACY_COMPARE_IRQ;
		cp0_compare_irq_shift = CP0_LEGACY_PERFCNT_IRQ;
		cp0_perfcount_irq = -1;
		cp0_fdc_irq = -1;
	}

	if (cpu_has_mmid)
		cpu_data[cpu].asid_cache = 0;
	else if (!cpu_data[cpu].asid_cache)
		cpu_data[cpu].asid_cache = asid_first_version(cpu);

	mmgrab(&init_mm);
	current->active_mm = &init_mm;
	BUG_ON(current->mm);
	enter_lazy_tlb(&init_mm, current);

	/* Boot CPU's cache setup in setup_arch(). */
	if (!is_boot_cpu)
		cpu_cache_init();
	tlb_init();
	TLBMISS_HANDLER_SETUP();
}

/* Install CPU exception handler */
void set_handler(unsigned long offset, void *addr, unsigned long size)
{
#ifdef CONFIG_CPU_MICROMIPS
	memcpy((void *)(ebase + offset), ((unsigned char *)addr - 1), size);
#else
	memcpy((void *)(ebase + offset), addr, size);
#endif
	local_flush_icache_range(ebase + offset, ebase + offset + size);
}

static const char panic_null_cerr[] =
	"Trying to set NULL cache error exception handler\n";

/*
 * Install uncached CPU exception handler.
 * This is suitable only for the cache error exception which is the only
 * exception handler that is being run uncached.
 */
void set_uncached_handler(unsigned long offset, void *addr,
	unsigned long size)
{
	unsigned long uncached_ebase = CKSEG1ADDR(ebase);

	if (!addr)
		panic(panic_null_cerr);

	memcpy((void *)(uncached_ebase + offset), addr, size);
}

static int __initdata rdhwr_noopt;
static int __init set_rdhwr_noopt(char *str)
{
	rdhwr_noopt = 1;
	return 1;
}

__setup("rdhwr_noopt", set_rdhwr_noopt);

void __init trap_init(void)
{
	extern char except_vec3_generic;
	extern char except_vec4;
	extern char except_vec3_r4000;
	unsigned long i, vec_size;
	phys_addr_t ebase_pa;

	check_wait();

	if (!cpu_has_mips_r2_r6) {
		ebase = CAC_BASE;
		ebase_pa = virt_to_phys((void *)ebase);
		vec_size = 0x400;

		memblock_reserve(ebase_pa, vec_size);
	} else {
		if (cpu_has_veic || cpu_has_vint)
			vec_size = 0x200 + VECTORSPACING*64;
		else
			vec_size = PAGE_SIZE;

		ebase_pa = memblock_phys_alloc(vec_size, 1 << fls(vec_size));
		if (!ebase_pa)
			panic("%s: Failed to allocate %lu bytes align=0x%x\n",
			      __func__, vec_size, 1 << fls(vec_size));

		/*
		 * Try to ensure ebase resides in KSeg0 if possible.
		 *
		 * It shouldn't generally be in XKPhys on MIPS64 to avoid
		 * hitting a poorly defined exception base for Cache Errors.
		 * The allocation is likely to be in the low 512MB of physical,
		 * in which case we should be able to convert to KSeg0.
		 *
		 * EVA is special though as it allows segments to be rearranged
		 * and to become uncached during cache error handling.
		 */
		if (!IS_ENABLED(CONFIG_EVA) && !WARN_ON(ebase_pa >= 0x20000000))
			ebase = CKSEG0ADDR(ebase_pa);
		else
			ebase = (unsigned long)phys_to_virt(ebase_pa);
	}

	if (cpu_has_mmips) {
		unsigned int config3 = read_c0_config3();

		if (IS_ENABLED(CONFIG_CPU_MICROMIPS))
			write_c0_config3(config3 | MIPS_CONF3_ISA_OE);
		else
			write_c0_config3(config3 & ~MIPS_CONF3_ISA_OE);
	}

	if (board_ebase_setup)
		board_ebase_setup();
	per_cpu_trap_init(true);
	memblock_set_bottom_up(false);

	/*
	 * Copy the generic exception handlers to their final destination.
	 * This will be overridden later as suitable for a particular
	 * configuration.
	 */
	set_handler(0x180, &except_vec3_generic, 0x80);

	/*
	 * Setup default vectors
	 */
	for (i = 0; i <= 31; i++)
		set_except_vector(i, handle_reserved);

	/*
	 * Copy the EJTAG debug exception vector handler code to it's final
	 * destination.
	 */
	if (cpu_has_ejtag && board_ejtag_handler_setup)
		board_ejtag_handler_setup();

	/*
	 * Only some CPUs have the watch exceptions.
	 */
	if (cpu_has_watch)
		set_except_vector(EXCCODE_WATCH, handle_watch);

	/*
	 * Initialise interrupt handlers
	 */
	if (cpu_has_veic || cpu_has_vint) {
		int nvec = cpu_has_veic ? 64 : 8;
		for (i = 0; i < nvec; i++)
			set_vi_handler(i, NULL);
	}
	else if (cpu_has_divec)
		set_handler(0x200, &except_vec4, 0x8);

	/*
	 * Some CPUs can enable/disable for cache parity detection, but does
	 * it different ways.
	 */
	parity_protection_init();

	/*
	 * The Data Bus Errors / Instruction Bus Errors are signaled
	 * by external hardware.  Therefore these two exceptions
	 * may have board specific handlers.
	 */
	if (board_be_init)
		board_be_init();

	set_except_vector(EXCCODE_INT, using_rollback_handler() ?
					rollback_handle_int : handle_int);
	set_except_vector(EXCCODE_MOD, handle_tlbm);
	set_except_vector(EXCCODE_TLBL, handle_tlbl);
	set_except_vector(EXCCODE_TLBS, handle_tlbs);

	set_except_vector(EXCCODE_ADEL, handle_adel);
	set_except_vector(EXCCODE_ADES, handle_ades);

	set_except_vector(EXCCODE_IBE, handle_ibe);
	set_except_vector(EXCCODE_DBE, handle_dbe);

	set_except_vector(EXCCODE_SYS, handle_sys);
	set_except_vector(EXCCODE_BP, handle_bp);

	if (rdhwr_noopt)
		set_except_vector(EXCCODE_RI, handle_ri);
	else {
		if (cpu_has_vtag_icache)
			set_except_vector(EXCCODE_RI, handle_ri_rdhwr_tlbp);
		else if (current_cpu_type() == CPU_LOONGSON3)
			set_except_vector(EXCCODE_RI, handle_ri_rdhwr_tlbp);
		else
			set_except_vector(EXCCODE_RI, handle_ri_rdhwr);
	}

	set_except_vector(EXCCODE_CPU, handle_cpu);
	set_except_vector(EXCCODE_OV, handle_ov);
	set_except_vector(EXCCODE_TR, handle_tr);
	set_except_vector(EXCCODE_MSAFPE, handle_msa_fpe);

	if (board_nmi_handler_setup)
		board_nmi_handler_setup();

	if (cpu_has_fpu && !cpu_has_nofpuex)
		set_except_vector(EXCCODE_FPE, handle_fpe);

	set_except_vector(MIPS_EXCCODE_TLBPAR, handle_ftlb);

	if (cpu_has_rixiex) {
		set_except_vector(EXCCODE_TLBRI, tlb_do_page_fault_0);
		set_except_vector(EXCCODE_TLBXI, tlb_do_page_fault_0);
	}

	set_except_vector(EXCCODE_MSADIS, handle_msa);
	set_except_vector(EXCCODE_MDMX, handle_mdmx);

	if (cpu_has_mcheck)
		set_except_vector(EXCCODE_MCHECK, handle_mcheck);

	if (cpu_has_mipsmt)
		set_except_vector(EXCCODE_THREAD, handle_mt);

	set_except_vector(EXCCODE_DSPDIS, handle_dsp);

	if (board_cache_error_setup)
		board_cache_error_setup();

	if (cpu_has_vce)
		/* Special exception: R4[04]00 uses also the divec space. */
		set_handler(0x180, &except_vec3_r4000, 0x100);
	else if (cpu_has_4kex)
		set_handler(0x180, &except_vec3_generic, 0x80);
	else
		set_handler(0x080, &except_vec3_generic, 0x80);

	local_flush_icache_range(ebase, ebase + 0x400);

	sort_extable(__start___dbe_table, __stop___dbe_table);

	cu2_notifier(default_cu2_call, 0x80000000);	/* Run last  */
}

static int trap_pm_notifier(struct notifier_block *self, unsigned long cmd,
			    void *v)
{
	switch (cmd) {
	case CPU_PM_ENTER_FAILED:
	case CPU_PM_EXIT:
		configure_status();
		configure_hwrena();
		configure_exception_vector();

		/* Restore register with CPU number for TLB handlers */
		TLBMISS_HANDLER_RESTORE();

		break;
	}

	return NOTIFY_OK;
}

static struct notifier_block trap_pm_notifier_block = {
	.notifier_call = trap_pm_notifier,
};

static int __init trap_pm_init(void)
{
	return cpu_pm_register_notifier(&trap_pm_notifier_block);
}
arch_initcall(trap_pm_init);