Derived from Peter Watkins <treestem@gmail.com>'s work.
Signed-off-by: Ralf Baechle <ralf@linux-mips.org>
 #define offset(string, ptr, member) \
        __asm__("\n@@@" string "%0" : : "i" (_offset(ptr, member)))
 #define constant(string, member) \
-       __asm__("\n@@@" string "%x0" : : "ri" (member))
+       __asm__("\n@@@" string "%X0" : : "ri" (member))
 #define size(string, size) \
        __asm__("\n@@@" string "%0" : : "i" (sizeof(size)))
 #define linefeed text("")
 
 
        MTC0            zero, CP0_CONTEXT       # clear context register
        PTR_LA          $28, init_thread_union
-       PTR_ADDIU       sp, $28, _THREAD_SIZE - 32
+       PTR_LI          sp, _THREAD_SIZE - 32
+       PTR_ADDU        sp, $28
        set_saved_sp    sp, t0, t1
        PTR_SUBU        sp, 4 * SZREG           # init stack pointer
 
 
        move    $28, a2
        cpu_restore_nonscratch a1
 
+#if (_THREAD_SIZE - 32) < 0x10000
        PTR_ADDIU       t0, $28, _THREAD_SIZE - 32
+#else
+       PTR_LI          t0, _THREAD_SIZE - 32
+       PTR_ADDU        t0, $28
+#endif
        set_saved_sp    t0, t1, t2
 #ifdef CONFIG_MIPS_MT_SMTC
        /* Read-modify-writes of Status must be atomic on a VPE */
 
   /* writeable */
   .data : {                    /* Data */
     . = . + DATAOFFSET;                /* for CONFIG_MAPPED_KERNEL */
+    /*
+     * This ALIGN is needed as a workaround for a bug a gcc bug upto 4.1 which
+     * limits the maximum alignment to at most 32kB and results in the following
+     * warning:
+     *
+     *  CC      arch/mips/kernel/init_task.o
+     * arch/mips/kernel/init_task.c:30: warning: alignment of ‘init_thread_union’
+     * is greater than maximum object file alignment.  Using 32768
+     */
+    . = ALIGN(_PAGE_SIZE);
     *(.data.init_task)
 
     *(.data)
 
        printk("Addr                 == %08lx\n", addr);
        printk("tasks->mm.pgd        == %08lx\n", (unsigned long) t->mm->pgd);
 
-       page_dir = pgd_offset(t->mm, 0);
+       page_dir = pgd_offset(t->mm, 0UL);
        printk("page_dir == %016lx\n", (unsigned long) page_dir);
 
        pgd = pgd_offset(t->mm, addr);
        dump_list_process(current, address);
 }
 
-unsigned int vtop(void *address)
+unsigned long vtop(void *address)
 {
        pgd_t   *pgd;
        pud_t   *pud;
        pmd_t   *pmd;
        pte_t   *pte;
-       unsigned int addr, paddr;
+       unsigned long addr, paddr;
 
        addr = (unsigned long) address;
        pgd = pgd_offset(current->mm, addr);
 
                if (boot_mem_map.map[i].type != BOOT_MEM_ROM_DATA)
                        continue;
 
-               addr = boot_mem_map.map[i].addr;
+               addr = PAGE_ALIGN(boot_mem_map.map[i].addr);
                while (addr < boot_mem_map.map[i].addr
                              + boot_mem_map.map[i].size) {
                        ClearPageReserved(virt_to_page(__va(addr)));
 
        emit_instruction(mi);
 }
 
+static inline void build_addiu_a2(unsigned long offset)
+{
+       union mips_instruction mi;
+
+       BUG_ON(offset > 0x7fff);
+
+       mi.i_format.opcode     = cpu_has_64bit_gp_regs ? daddiu_op : addiu_op;
+       mi.i_format.rs         = 6;             /* $a2 */
+       mi.i_format.rt         = 6;             /* $a2 */
+       mi.i_format.simmediate = offset;
+
+       emit_instruction(mi);
+}
+
 static inline void build_addiu_a1(unsigned long offset)
 {
        union mips_instruction mi;
 void __init build_clear_page(void)
 {
        unsigned int loop_start;
+       unsigned long off;
 
        epc = (unsigned int *) &clear_page_array;
        instruction_pending = 0;
                }
        }
 
-       build_addiu_a2_a0(PAGE_SIZE - (cpu_has_prefetch ? pref_offset_clear : 0));
+        off = PAGE_SIZE - (cpu_has_prefetch ? pref_offset_clear : 0);
+       if (off > 0x7fff) {
+               build_addiu_a2_a0(off >> 1);
+               build_addiu_a2(off >> 1);
+       } else
+               build_addiu_a2_a0(off);
 
        if (R4600_V2_HIT_CACHEOP_WAR && cpu_is_r4600_v2_x())
                build_insn_word(0x3c01a000);    /* lui     $at, 0xa000  */
 void __init build_copy_page(void)
 {
        unsigned int loop_start;
+       unsigned long off;
 
        epc = (unsigned int *) ©_page_array;
        store_offset = load_offset = 0;
        instruction_pending = 0;
 
-       build_addiu_a2_a0(PAGE_SIZE - (cpu_has_prefetch ? pref_offset_copy : 0));
+       off = PAGE_SIZE - (cpu_has_prefetch ? pref_offset_copy : 0);
+       if (off > 0x7fff) {
+               build_addiu_a2_a0(off >> 1);
+               build_addiu_a2(off >> 1);
+       } else
+               build_addiu_a2_a0(off);
 
        if (R4600_V2_HIT_CACHEOP_WAR && cpu_is_r4600_v2_x())
                build_insn_word(0x3c01a000);    /* lui     $at, 0xa000  */
 
        insn_addu, insn_addiu, insn_and, insn_andi, insn_beq,
        insn_beql, insn_bgez, insn_bgezl, insn_bltz, insn_bltzl,
        insn_bne, insn_daddu, insn_daddiu, insn_dmfc0, insn_dmtc0,
-       insn_dsll, insn_dsll32, insn_dsra, insn_dsrl,
+       insn_dsll, insn_dsll32, insn_dsra, insn_dsrl, insn_dsrl32,
        insn_dsubu, insn_eret, insn_j, insn_jal, insn_jr, insn_ld,
        insn_ll, insn_lld, insn_lui, insn_lw, insn_mfc0, insn_mtc0,
        insn_ori, insn_rfe, insn_sc, insn_scd, insn_sd, insn_sll,
        { insn_dsll32, M(spec_op,0,0,0,0,dsll32_op), RT | RD | RE },
        { insn_dsra, M(spec_op,0,0,0,0,dsra_op), RT | RD | RE },
        { insn_dsrl, M(spec_op,0,0,0,0,dsrl_op), RT | RD | RE },
+       { insn_dsrl32, M(spec_op,0,0,0,0,dsrl32_op), RT | RD | RE },
        { insn_dsubu, M(spec_op,0,0,0,0,dsubu_op), RS | RT | RD },
        { insn_eret, M(cop0_op,cop_op,0,0,0,eret_op), 0 },
        { insn_j, M(j_op,0,0,0,0,0), JIMM },
 I_u2u1u3(_dsll32);
 I_u2u1u3(_dsra);
 I_u2u1u3(_dsrl);
+I_u2u1u3(_dsrl32);
 I_u3u1u2(_dsubu);
 I_0(_eret);
 I_u1(_j);
 #endif
 
        l_vmalloc_done(l, *p);
-       i_dsrl(p, tmp, tmp, PGDIR_SHIFT-3); /* get pgd offset in bytes */
+
+       if (PGDIR_SHIFT - 3 < 32)               /* get pgd offset in bytes */
+               i_dsrl(p, tmp, tmp, PGDIR_SHIFT-3);
+       else
+               i_dsrl32(p, tmp, tmp, PGDIR_SHIFT - 3 - 32);
+
        i_andi(p, tmp, tmp, (PTRS_PER_PGD - 1)<<3);
        i_daddu(p, ptr, ptr, tmp); /* add in pgd offset */
        i_dmfc0(p, tmp, C0_BADVADDR); /* get faulting address */
 
 static __init void build_adjust_context(u32 **p, unsigned int ctx)
 {
-       unsigned int shift = 4 - (PTE_T_LOG2 + 1);
+       unsigned int shift = 4 - (PTE_T_LOG2 + 1) + PAGE_SHIFT - 12;
        unsigned int mask = (PTRS_PER_PTE / 2 - 1) << (PTE_T_LOG2 + 1);
 
        switch (current_cpu_data.cputype) {
 
 #define PTR_L          lw
 #define PTR_S          sw
 #define PTR_LA         la
+#define PTR_LI         li
 #define PTR_SLL                sll
 #define PTR_SLLV       sllv
 #define PTR_SRL                srl
 #define PTR_L          ld
 #define PTR_S          sd
 #define PTR_LA         dla
+#define PTR_LI         dli
 #define PTR_SLL                dsll
 #define PTR_SLLV       dsllv
 #define PTR_SRL                dsrl
 
 
        ret = (pgd_t *) __get_free_pages(GFP_KERNEL, PGD_ORDER);
        if (ret) {
-               init = pgd_offset(&init_mm, 0);
+               init = pgd_offset(&init_mm, 0UL);
                pgd_init((unsigned long)ret);
                memcpy(ret + USER_PTRS_PER_PGD, init + USER_PTRS_PER_PGD,
                       (PTRS_PER_PGD - USER_PTRS_PER_PGD) * sizeof(pgd_t));
 
 #define __pmd_offset(address)  pmd_index(address)
 
 /* to find an entry in a kernel page-table-directory */
-#define pgd_offset_k(address) pgd_offset(&init_mm, 0)
+#define pgd_offset_k(address) pgd_offset(&init_mm, 0UL)
 
 #define pgd_index(address)     (((address) >> PGDIR_SHIFT) & (PTRS_PER_PGD-1))
 #define pmd_index(address)     (((address) >> PMD_SHIFT) & (PTRS_PER_PMD-1))