+/*
+ * TLB load/store/modify handlers.
+ *
+ * Only the fastpath gets synthesized at runtime, the slowpath for
+ * do_page_fault remains normal asm.
+ */
+extern void tlb_do_page_fault_0(void);
+extern void tlb_do_page_fault_1(void);
+
+#define __tlb_handler_align \
+ __attribute__((__aligned__(1 << CONFIG_MIPS_L1_CACHE_SHIFT)))
+
+/*
+ * 128 instructions for the fastpath handler is generous and should
+ * never be exceeded.
+ */
+#define FASTPATH_SIZE 128
+
+u32 __tlb_handler_align handle_tlbl[FASTPATH_SIZE];
+u32 __tlb_handler_align handle_tlbs[FASTPATH_SIZE];
+u32 __tlb_handler_align handle_tlbm[FASTPATH_SIZE];
+
+static void __init
+iPTE_LW(u32 **p, struct label **l, unsigned int pte, int offset,
+ unsigned int ptr)
+{
+#ifdef CONFIG_SMP
+# ifdef CONFIG_64BIT_PHYS_ADDR
+ if (cpu_has_64bits)
+ i_lld(p, pte, offset, ptr);
+ else
+# endif
+ i_LL(p, pte, offset, ptr);
+#else
+# ifdef CONFIG_64BIT_PHYS_ADDR
+ if (cpu_has_64bits)
+ i_ld(p, pte, offset, ptr);
+ else
+# endif
+ i_LW(p, pte, offset, ptr);
+#endif
+}
+
+static void __init
+iPTE_SW(u32 **p, struct reloc **r, unsigned int pte, int offset,
+ unsigned int ptr)
+{
+#ifdef CONFIG_SMP
+# ifdef CONFIG_64BIT_PHYS_ADDR
+ if (cpu_has_64bits)
+ i_scd(p, pte, offset, ptr);
+ else
+# endif
+ i_SC(p, pte, offset, ptr);
+
+ if (r10000_llsc_war())
+ il_beqzl(p, r, pte, label_smp_pgtable_change);
+ else
+ il_beqz(p, r, pte, label_smp_pgtable_change);
+
+# ifdef CONFIG_64BIT_PHYS_ADDR
+ if (!cpu_has_64bits) {
+ /* no i_nop needed */
+ i_ll(p, pte, sizeof(pte_t) / 2, ptr);
+ i_ori(p, pte, pte, _PAGE_VALID);
+ i_sc(p, pte, sizeof(pte_t) / 2, ptr);
+ il_beqz(p, r, pte, label_smp_pgtable_change);
+ /* no i_nop needed */
+ i_lw(p, pte, 0, ptr);
+ } else
+ i_nop(p);
+# else
+ i_nop(p);
+# endif
+#else
+# ifdef CONFIG_64BIT_PHYS_ADDR
+ if (cpu_has_64bits)
+ i_sd(p, pte, offset, ptr);
+ else
+# endif
+ i_SW(p, pte, offset, ptr);
+
+# ifdef CONFIG_64BIT_PHYS_ADDR
+ if (!cpu_has_64bits) {
+ i_lw(p, pte, sizeof(pte_t) / 2, ptr);
+ i_ori(p, pte, pte, _PAGE_VALID);
+ i_sw(p, pte, sizeof(pte_t) / 2, ptr);
+ i_lw(p, pte, 0, ptr);
+ }
+# endif
+#endif
+}
+
+/*
+ * Check if PTE is present, if not then jump to LABEL. PTR points to
+ * the page table where this PTE is located, PTE will be re-loaded
+ * with it's original value.
+ */
+static void __init
+build_pte_present(u32 **p, struct label **l, struct reloc **r,
+ unsigned int pte, unsigned int ptr, enum label_id lid)
+{
+ i_andi(p, pte, pte, _PAGE_PRESENT | _PAGE_READ);
+ i_xori(p, pte, pte, _PAGE_PRESENT | _PAGE_READ);
+ il_bnez(p, r, pte, lid);
+ iPTE_LW(p, l, pte, 0, ptr);
+}
+
+/* Make PTE valid, store result in PTR. */
+static void __init
+build_make_valid(u32 **p, struct reloc **r, unsigned int pte,
+ unsigned int ptr)
+{
+ i_ori(p, pte, pte, _PAGE_VALID | _PAGE_ACCESSED);
+ iPTE_SW(p, r, pte, 0, ptr);
+}
+
+/*
+ * Check if PTE can be written to, if not branch to LABEL. Regardless
+ * restore PTE with value from PTR when done.
+ */
+static void __init
+build_pte_writable(u32 **p, struct label **l, struct reloc **r,
+ unsigned int pte, unsigned int ptr, enum label_id lid)
+{
+ i_andi(p, pte, pte, _PAGE_PRESENT | _PAGE_WRITE);
+ i_xori(p, pte, pte, _PAGE_PRESENT | _PAGE_WRITE);
+ il_bnez(p, r, pte, lid);
+ iPTE_LW(p, l, pte, 0, ptr);
+}
+
+/* Make PTE writable, update software status bits as well, then store
+ * at PTR.
+ */
+static void __init
+build_make_write(u32 **p, struct reloc **r, unsigned int pte,
+ unsigned int ptr)
+{
+ i_ori(p, pte, pte,
+ _PAGE_ACCESSED | _PAGE_MODIFIED | _PAGE_VALID | _PAGE_DIRTY);
+ iPTE_SW(p, r, pte, 0, ptr);
+}
+
+/*
+ * Check if PTE can be modified, if not branch to LABEL. Regardless
+ * restore PTE with value from PTR when done.
+ */
+static void __init
+build_pte_modifiable(u32 **p, struct label **l, struct reloc **r,
+ unsigned int pte, unsigned int ptr, enum label_id lid)
+{
+ i_andi(p, pte, pte, _PAGE_WRITE);
+ il_beqz(p, r, pte, lid);
+ iPTE_LW(p, l, pte, 0, ptr);
+}
+
+/*
+ * R3000 style TLB load/store/modify handlers.
+ */
+
+/* This places the pte in the page table at PTR into ENTRYLO0. */
+static void __init
+build_r3000_pte_reload(u32 **p, unsigned int ptr)
+{
+ i_lw(p, ptr, 0, ptr);
+ i_nop(p); /* load delay */
+ i_mtc0(p, ptr, C0_ENTRYLO0);
+ i_nop(p); /* cp0 delay */
+}
+
+/*
+ * The index register may have the probe fail bit set,
+ * because we would trap on access kseg2, i.e. without refill.
+ */
+static void __init
+build_r3000_tlb_write(u32 **p, struct label **l, struct reloc **r,
+ unsigned int tmp)
+{
+ i_mfc0(p, tmp, C0_INDEX);
+ i_nop(p); /* cp0 delay */
+ il_bltz(p, r, tmp, label_r3000_write_probe_fail);
+ i_nop(p); /* branch delay */
+ i_tlbwi(p);
+ il_b(p, r, label_r3000_write_probe_ok);
+ i_nop(p); /* branch delay */
+ l_r3000_write_probe_fail(l, *p);
+ i_tlbwr(p);
+ l_r3000_write_probe_ok(l, *p);
+}
+
+static void __init
+build_r3000_tlbchange_handler_head(u32 **p, unsigned int pte,
+ unsigned int ptr)
+{
+ long pgdc = (long)pgd_current;
+
+ i_mfc0(p, pte, C0_BADVADDR);
+ i_lui(p, ptr, rel_hi(pgdc)); /* cp0 delay */
+ i_lw(p, ptr, rel_lo(pgdc), ptr);
+ i_srl(p, pte, pte, 22); /* load delay */
+ i_sll(p, pte, pte, 2);
+ i_addu(p, ptr, ptr, pte);
+ i_mfc0(p, pte, C0_CONTEXT);
+ i_lw(p, ptr, 0, ptr); /* cp0 delay */
+ i_andi(p, pte, pte, 0xffc); /* load delay */
+ i_addu(p, ptr, ptr, pte);
+ i_lw(p, pte, 0, ptr);
+ i_nop(p); /* load delay */
+ i_tlbp(p);
+}
+
+static void __init
+build_r3000_tlbchange_handler_tail(u32 **p, unsigned int tmp)
+{
+ i_mfc0(p, tmp, C0_EPC);
+ i_nop(p); /* cp0 delay */
+ i_jr(p, tmp);
+ i_rfe(p); /* branch delay */
+}
+
+static void __init build_r3000_tlb_load_handler(void)
+{
+ u32 *p = handle_tlbl;
+ struct label *l = labels;
+ struct reloc *r = relocs;
+
+ memset(handle_tlbl, 0, sizeof(handle_tlbl));
+ memset(labels, 0, sizeof(labels));
+ memset(relocs, 0, sizeof(relocs));
+
+ build_r3000_tlbchange_handler_head(&p, K0, K1);
+ build_pte_present(&p, &l, &r, K0, K1, label_nopage_tlbl);
+ build_make_valid(&p, &r, K0, K1);
+ build_r3000_pte_reload(&p, K1);
+ build_r3000_tlb_write(&p, &l, &r, K0);
+ build_r3000_tlbchange_handler_tail(&p, K0);
+
+ l_nopage_tlbl(&l, p);
+ i_j(&p, (unsigned long)tlb_do_page_fault_0 & 0x0fffffff);
+ i_nop(&p);
+
+ if ((p - handle_tlbl) > FASTPATH_SIZE)
+ panic("TLB load handler fastpath space exceeded");
+
+ resolve_relocs(relocs, labels);
+ printk("Synthesized TLB load handler fastpath (%u instructions).\n",
+ (unsigned int)(p - handle_tlbl));
+
+#ifdef DEBUG_TLB
+ {
+ int i;
+
+ for (i = 0; i < FASTPATH_SIZE; i++)
+ printk("%08x\n", handle_tlbl[i]);
+ }
+#endif
+
+ flush_icache_range((unsigned long)handle_tlbl,
+ (unsigned long)handle_tlbl + FASTPATH_SIZE * sizeof(u32));
+}
+
+static void __init build_r3000_tlb_store_handler(void)
+{
+ u32 *p = handle_tlbs;
+ struct label *l = labels;
+ struct reloc *r = relocs;
+
+ memset(handle_tlbs, 0, sizeof(handle_tlbs));
+ memset(labels, 0, sizeof(labels));
+ memset(relocs, 0, sizeof(relocs));
+
+ build_r3000_tlbchange_handler_head(&p, K0, K1);
+ build_pte_writable(&p, &l, &r, K0, K1, label_nopage_tlbs);
+ build_make_write(&p, &r, K0, K1);
+ build_r3000_pte_reload(&p, K1);
+ build_r3000_tlb_write(&p, &l, &r, K0);
+ build_r3000_tlbchange_handler_tail(&p, K0);
+
+ l_nopage_tlbs(&l, p);
+ i_j(&p, (unsigned long)tlb_do_page_fault_1 & 0x0fffffff);
+ i_nop(&p);
+
+ if ((p - handle_tlbs) > FASTPATH_SIZE)
+ panic("TLB store handler fastpath space exceeded");
+
+ resolve_relocs(relocs, labels);
+ printk("Synthesized TLB store handler fastpath (%u instructions).\n",
+ (unsigned int)(p - handle_tlbs));
+
+#ifdef DEBUG_TLB
+ {
+ int i;
+
+ for (i = 0; i < FASTPATH_SIZE; i++)
+ printk("%08x\n", handle_tlbs[i]);
+ }
+#endif
+
+ flush_icache_range((unsigned long)handle_tlbs,
+ (unsigned long)handle_tlbs + FASTPATH_SIZE * sizeof(u32));
+}
+
+static void __init build_r3000_tlb_modify_handler(void)
+{
+ u32 *p = handle_tlbm;
+ struct label *l = labels;
+ struct reloc *r = relocs;
+
+ memset(handle_tlbm, 0, sizeof(handle_tlbm));
+ memset(labels, 0, sizeof(labels));
+ memset(relocs, 0, sizeof(relocs));
+
+ build_r3000_tlbchange_handler_head(&p, K0, K1);
+ build_pte_modifiable(&p, &l, &r, K0, K1, label_nopage_tlbm);
+ build_make_write(&p, &r, K0, K1);
+ build_r3000_pte_reload(&p, K1);
+ i_tlbwi(&p);
+ build_r3000_tlbchange_handler_tail(&p, K0);
+
+ l_nopage_tlbm(&l, p);
+ i_j(&p, (unsigned long)tlb_do_page_fault_1 & 0x0fffffff);
+ i_nop(&p);
+
+ if ((p - handle_tlbm) > FASTPATH_SIZE)
+ panic("TLB modify handler fastpath space exceeded");
+
+ resolve_relocs(relocs, labels);
+ printk("Synthesized TLB modify handler fastpath (%u instructions).\n",
+ (unsigned int)(p - handle_tlbm));
+
+#ifdef DEBUG_TLB
+ {
+ int i;
+
+ for (i = 0; i < FASTPATH_SIZE; i++)
+ printk("%08x\n", handle_tlbm[i]);
+ }
+#endif
+
+ flush_icache_range((unsigned long)handle_tlbm,
+ (unsigned long)handle_tlbm + FASTPATH_SIZE * sizeof(u32));
+}
+
+/*
+ * R4000 style TLB load/store/modify handlers.
+ */
+static void __init
+build_r4000_tlbchange_handler_head(u32 **p, struct label **l,
+ struct reloc **r, unsigned int pte,
+ unsigned int ptr)
+{
+#ifdef CONFIG_MIPS64
+ build_get_pmde64(p, l, r, pte, ptr); /* get pmd in ptr */
+#else
+ build_get_pgde32(p, pte, ptr); /* get pgd in ptr */
+#endif
+
+ i_MFC0(p, pte, C0_BADVADDR);
+ i_LW(p, ptr, 0, ptr);
+ i_SRL(p, pte, pte, PAGE_SHIFT + PTE_ORDER - PTE_T_LOG2);
+ i_andi(p, pte, pte, (PTRS_PER_PTE - 1) << PTE_T_LOG2);
+ i_ADDU(p, ptr, ptr, pte);
+
+#ifdef CONFIG_SMP
+ l_smp_pgtable_change(l, *p);
+# endif
+ iPTE_LW(p, l, pte, 0, ptr); /* get even pte */
+ build_tlb_probe_entry(p);
+}
+
+static void __init
+build_r4000_tlbchange_handler_tail(u32 **p, struct label **l,
+ struct reloc **r, unsigned int tmp,
+ unsigned int ptr)
+{
+ i_ori(p, ptr, ptr, sizeof(pte_t));
+ i_xori(p, ptr, ptr, sizeof(pte_t));
+ build_update_entries(p, tmp, ptr);
+ build_tlb_write_entry(p, l, r, tlb_indexed);
+ l_leave(l, *p);
+ i_eret(p); /* return from trap */
+
+#ifdef CONFIG_MIPS64
+ build_get_pgd_vmalloc64(p, l, r, tmp, ptr);
+#endif
+}
+
+static void __init build_r4000_tlb_load_handler(void)
+{
+ u32 *p = handle_tlbl;
+ struct label *l = labels;
+ struct reloc *r = relocs;
+
+ memset(handle_tlbl, 0, sizeof(handle_tlbl));
+ memset(labels, 0, sizeof(labels));
+ memset(relocs, 0, sizeof(relocs));
+
+ if (bcm1250_m3_war()) {
+ i_MFC0(&p, K0, C0_BADVADDR);
+ i_MFC0(&p, K1, C0_ENTRYHI);
+ i_xor(&p, K0, K0, K1);
+ i_SRL(&p, K0, K0, PAGE_SHIFT + 1);
+ il_bnez(&p, &r, K0, label_leave);
+ /* No need for i_nop */
+ }
+
+ build_r4000_tlbchange_handler_head(&p, &l, &r, K0, K1);
+ build_pte_present(&p, &l, &r, K0, K1, label_nopage_tlbl);
+ build_make_valid(&p, &r, K0, K1);
+ build_r4000_tlbchange_handler_tail(&p, &l, &r, K0, K1);
+
+ l_nopage_tlbl(&l, p);
+ i_j(&p, (unsigned long)tlb_do_page_fault_0 & 0x0fffffff);
+ i_nop(&p);
+
+ if ((p - handle_tlbl) > FASTPATH_SIZE)
+ panic("TLB load handler fastpath space exceeded");
+
+ resolve_relocs(relocs, labels);
+ printk("Synthesized TLB load handler fastpath (%u instructions).\n",
+ (unsigned int)(p - handle_tlbl));
+
+#ifdef DEBUG_TLB
+ {
+ int i;
+
+ for (i = 0; i < FASTPATH_SIZE; i++)
+ printk("%08x\n", handle_tlbl[i]);
+ }
+#endif
+
+ flush_icache_range((unsigned long)handle_tlbl,
+ (unsigned long)handle_tlbl + FASTPATH_SIZE * sizeof(u32));
+}
+
+static void __init build_r4000_tlb_store_handler(void)
+{
+ u32 *p = handle_tlbs;
+ struct label *l = labels;
+ struct reloc *r = relocs;
+
+ memset(handle_tlbs, 0, sizeof(handle_tlbs));
+ memset(labels, 0, sizeof(labels));
+ memset(relocs, 0, sizeof(relocs));
+
+ build_r4000_tlbchange_handler_head(&p, &l, &r, K0, K1);
+ build_pte_writable(&p, &l, &r, K0, K1, label_nopage_tlbs);
+ build_make_write(&p, &r, K0, K1);
+ build_r4000_tlbchange_handler_tail(&p, &l, &r, K0, K1);
+
+ l_nopage_tlbs(&l, p);
+ i_j(&p, (unsigned long)tlb_do_page_fault_1 & 0x0fffffff);
+ i_nop(&p);
+
+ if ((p - handle_tlbs) > FASTPATH_SIZE)
+ panic("TLB store handler fastpath space exceeded");
+
+ resolve_relocs(relocs, labels);
+ printk("Synthesized TLB store handler fastpath (%u instructions).\n",
+ (unsigned int)(p - handle_tlbs));
+
+#ifdef DEBUG_TLB
+ {
+ int i;
+
+ for (i = 0; i < FASTPATH_SIZE; i++)
+ printk("%08x\n", handle_tlbs[i]);
+ }
+#endif
+
+ flush_icache_range((unsigned long)handle_tlbs,
+ (unsigned long)handle_tlbs + FASTPATH_SIZE * sizeof(u32));
+}
+
+static void __init build_r4000_tlb_modify_handler(void)
+{
+ u32 *p = handle_tlbm;
+ struct label *l = labels;
+ struct reloc *r = relocs;
+
+ memset(handle_tlbm, 0, sizeof(handle_tlbm));
+ memset(labels, 0, sizeof(labels));
+ memset(relocs, 0, sizeof(relocs));
+
+ build_r4000_tlbchange_handler_head(&p, &l, &r, K0, K1);
+ build_pte_modifiable(&p, &l, &r, K0, K1, label_nopage_tlbm);
+ /* Present and writable bits set, set accessed and dirty bits. */
+ build_make_write(&p, &r, K0, K1);
+ build_r4000_tlbchange_handler_tail(&p, &l, &r, K0, K1);
+
+ l_nopage_tlbm(&l, p);
+ i_j(&p, (unsigned long)tlb_do_page_fault_1 & 0x0fffffff);
+ i_nop(&p);
+
+ if ((p - handle_tlbm) > FASTPATH_SIZE)
+ panic("TLB modify handler fastpath space exceeded");
+
+ resolve_relocs(relocs, labels);
+ printk("Synthesized TLB modify handler fastpath (%u instructions).\n",
+ (unsigned int)(p - handle_tlbm));
+
+#ifdef DEBUG_TLB
+ {
+ int i;
+
+ for (i = 0; i < FASTPATH_SIZE; i++)
+ printk("%08x\n", handle_tlbm[i]);
+ }
+#endif
+
+ flush_icache_range((unsigned long)handle_tlbm,
+ (unsigned long)handle_tlbm + FASTPATH_SIZE * sizeof(u32));
+}
+