Index: sys/powerpc/booke/locore.S =================================================================== --- sys/powerpc/booke/locore.S +++ sys/powerpc/booke/locore.S @@ -58,6 +58,20 @@ .text .globl __start __start: +#ifdef SMP + b 1f + + .globl bp_ident +bp_ident: + .long 0 + +bp_tlb1_end: +1: + mflr %r30 + lwz %r30,0(%r30) + or. %r30,%r30,%r30 + bne 1f +#endif /* * Assumptions on the boot loader: @@ -97,6 +111,7 @@ mr %r30, %r3 mr %r31, %r4 +1: /* * Initial cleanup */ @@ -144,7 +159,14 @@ 1: /* Invalidate all entries in TLB0 */ li %r3, 0 - bl tlb_inval_all + rlwinm %r3, %r3, 3, (1 << 3) /* TLBSEL */ + ori %r3, %r3, (1 << 2) /* INVALL */ + tlbivax 0, %r3 + isync + msync + + tlbsync + msync cmpwi %r30, 0 beq done_mapping @@ -154,15 +176,78 @@ */ bl 1f 1: mflr %r3 - bl tlb1_find_current /* the entry found is returned in r29 */ + mfspr %r17, SPR_PID0 + slwi %r17, %r17, MAS6_SPID0_SHIFT + mtspr SPR_MAS6, %r17 + isync + tlbsx 0, %r3 + mfspr %r17, SPR_MAS0 + rlwinm %r29, %r17, 16, 20, 31 /* MAS0[ESEL] -> r29 */ - bl tlb1_inval_all_but_current + /* Make sure we have IPROT set on the entry */ + mfspr %r17, SPR_MAS1 + oris %r17, %r17, MAS1_IPROT@h + mtspr SPR_MAS1, %r17 + isync + tlbwe + isync + msync + + mr %r6, %r3 + mfspr %r3, SPR_TLB1CFG /* Get number of entries */ + andi. %r3, %r3, TLBCFG_NENTRY_MASK@l + li %r4, 0 /* Start from Entry 0 */ +1: lis %r5, MAS0_TLBSEL1@h + rlwimi %r5, %r4, 16, 10, 15 + mtspr SPR_MAS0, %r5 + isync + tlbre + mfspr %r5, SPR_MAS1 + cmpw %r4, %r29 /* our current entry? */ + beq 2f + rlwinm %r5, %r5, 0, 2, 31 /* clear VALID and IPROT bits */ + mtspr SPR_MAS1, %r5 + isync + tlbwe + isync + msync +2: addi %r4, %r4, 1 + cmpw %r4, %r3 /* Check if this is the last entry */ + bne 1b /* * Create temporary mapping in AS=1 and switch to it */ addi %r3, %r29, 1 - bl tlb1_temp_mapping_as1 + mr %r28, %r3 + + /* Read our current translation */ + lis %r3, MAS0_TLBSEL1@h /* Select TLB1 */ + rlwimi %r3, %r29, 16, 10, 15 /* Select our current entry */ + mtspr SPR_MAS0, %r3 + isync + tlbre + + /* Prepare and write temp entry */ + lis %r3, MAS0_TLBSEL1@h /* Select TLB1 */ + rlwimi %r3, %r28, 16, 10, 15 /* Select temp entry */ + mtspr SPR_MAS0, %r3 + isync + mfspr %r5, SPR_MAS1 + li %r4, 1 /* AS=1 */ + rlwimi %r5, %r4, 12, 19, 19 + li %r4, 0 /* Global mapping, TID=0 */ + rlwimi %r5, %r4, 16, 8, 15 + oris %r5, %r5, (MAS1_VALID | MAS1_IPROT)@h + mtspr SPR_MAS1, %r5 + isync + mflr %r3 + bl zero_mas7 + bl zero_mas8 + mtlr %r3 + tlbwe + isync + msync mfmsr %r3 ori %r3, %r3, (PSL_IS | PSL_DS) @@ -196,9 +281,7 @@ lis %r3, KERNBASE@h ori %r3, %r3, KERNBASE@l /* EPN = KERNBASE */ -#ifdef SMP - ori %r3, %r3, MAS2_M@l /* WIMGE = 0b00100 */ -#endif + ori %r3, %r3, _TLB_ENTRY_MEM@l /* WIMGE = 0b00100 */ mtspr SPR_MAS2, %r3 isync @@ -244,6 +327,17 @@ lwz %r2,0(%r1) add %r1,%r1,%r2 addi %r1, %r1, (TMPSTACKSZ - 16) +#ifdef SMP + bl 1f + .long bp_ident-. +1: + mflr %r5 + lwz %r3,0(%r5) + add %r3,%r3,%r5 + lwz %r3,0(%r3) + or. %r3,%r3,%r3 + bne ap_startup +#endif /* * Relocate kernel @@ -261,7 +355,7 @@ bl elf_reloc_self /* - * Initialise exception vector offsets + * Initialize exception vector offsets */ bl ivor_setup @@ -286,173 +380,9 @@ #ifdef SMP -/************************************************************************/ -/* AP Boot page */ -/************************************************************************/ - .text - .globl __boot_page - .align 12 -__boot_page: - bl 1f - - .globl bp_ntlb1s -bp_ntlb1s: - .long 0 - - .globl bp_tlb1 -bp_tlb1: - .space 4 * 3 * 64 - - .globl bp_tlb1_end -bp_tlb1_end: - +ap_startup: /* - * Initial configuration - */ -1: mflr %r31 /* r31 hold the address of bp_ntlb1s */ - - /* Set HIDs */ - mfpvr %r3 - rlwinm %r3, %r3, 16, 16, 31 - - /* HID0 for E500 is default */ - lis %r4, HID0_E500_DEFAULT_SET@h - ori %r4, %r4, HID0_E500_DEFAULT_SET@l - - cmpli 0, 0, %r3, FSL_E500mc - bne 2f - lis %r4, HID0_E500MC_DEFAULT_SET@h - ori %r4, %r4, HID0_E500MC_DEFAULT_SET@l - b 3f -2: - cmpli 0, 0, %r3, FSL_E5500 - bne 3f - lis %r4, HID0_E5500_DEFAULT_SET@h - ori %r4, %r4, HID0_E5500_DEFAULT_SET@l -3: - mtspr SPR_HID0, %r4 - isync -/* - * E500mc and E5500 do not have HID1 register, so skip HID1 setup on - * this core. - */ - cmpli 0, 0, %r3, FSL_E500mc - beq 1f - cmpli 0, 0, %r3, FSL_E5500 - beq 1f - - lis %r3, HID1_E500_DEFAULT_SET@h - ori %r3, %r3, HID1_E500_DEFAULT_SET@l - mtspr SPR_HID1, %r3 - isync -1: - /* Enable branch prediction */ - li %r3, BUCSR_BPEN - mtspr SPR_BUCSR, %r3 - isync - - /* Invalidate all entries in TLB0 */ - li %r3, 0 - bl tlb_inval_all - -/* - * Find TLB1 entry which is translating us now - */ - bl 2f -2: mflr %r3 - bl tlb1_find_current /* the entry number found is in r29 */ - - bl tlb1_inval_all_but_current - -/* - * Create temporary translation in AS=1 and switch to it - */ - lwz %r3, 0(%r31) - bl tlb1_temp_mapping_as1 - - mfmsr %r3 - ori %r3, %r3, (PSL_IS | PSL_DS) - bl 3f -3: mflr %r4 - addi %r4, %r4, 20 - mtspr SPR_SRR0, %r4 - mtspr SPR_SRR1, %r3 - rfi /* Switch context */ - -/* - * Invalidate initial entry - */ - mr %r3, %r29 - bl tlb1_inval_entry - -/* - * Setup final mapping in TLB1[1] and switch to it - */ - lwz %r6, 0(%r31) - addi %r5, %r31, 4 - li %r4, 0 - -4: lis %r3, MAS0_TLBSEL1@h - rlwimi %r3, %r4, 16, 12, 15 - mtspr SPR_MAS0, %r3 - isync - lwz %r3, 0(%r5) - mtspr SPR_MAS1, %r3 - isync - lwz %r3, 4(%r5) - mtspr SPR_MAS2, %r3 - isync - lwz %r3, 8(%r5) - mtspr SPR_MAS3, %r3 - isync - tlbwe - isync - msync - addi %r5, %r5, 12 - addi %r4, %r4, 1 - cmpw %r4, %r6 - blt 4b - - /* Switch to the final mapping */ - bl 5f - .long __boot_page-. -5: mflr %r5 - lwz %r3,0(%r3) - add %r5,%r5,%r3 /* __boot_page in r5 */ - bl 6f -6: mflr %r3 - rlwinm %r3, %r3, 0, 0xfff /* Offset from boot page start */ - add %r3, %r3, %r5 /* Make this virtual address */ - addi %r3, %r3, 32 - li %r4, 0 /* Note AS=0 */ - mtspr SPR_SRR0, %r3 - mtspr SPR_SRR1, %r4 - rfi - -/* - * At this point we're running at virtual addresses KERNBASE and beyond so - * it's allowed to directly access all locations the kernel was linked - * against. - */ - -/* - * Invalidate temp mapping - */ - mr %r3, %r28 - bl tlb1_inval_entry - -/* - * Setup a temporary stack - */ - bl 1f - .long tmpstack-. -1: mflr %r1 - lwz %r2,0(%r1) - add %r1,%r1,%r2 - addi %r1, %r1, (TMPSTACKSZ - 16) - -/* - * Initialise exception vector offsets + * Initialize exception vector offsets */ bl ivor_setup @@ -476,51 +406,10 @@ bl machdep_ap_bootstrap /* NOT REACHED */ -6: b 6b +2: b 2b #endif /* SMP */ /* - * Invalidate all entries in the given TLB. - * - * r3 TLBSEL - */ -tlb_inval_all: - rlwinm %r3, %r3, 3, (1 << 3) /* TLBSEL */ - ori %r3, %r3, (1 << 2) /* INVALL */ - tlbivax 0, %r3 - isync - msync - - tlbsync - msync - blr - -/* - * expects address to look up in r3, returns entry number in r29 - * - * FIXME: the hidden assumption is we are now running in AS=0, but we should - * retrieve actual AS from MSR[IS|DS] and put it in MAS6[SAS] - */ -tlb1_find_current: - mfspr %r17, SPR_PID0 - slwi %r17, %r17, MAS6_SPID0_SHIFT - mtspr SPR_MAS6, %r17 - isync - tlbsx 0, %r3 - mfspr %r17, SPR_MAS0 - rlwinm %r29, %r17, 16, 20, 31 /* MAS0[ESEL] -> r29 */ - - /* Make sure we have IPROT set on the entry */ - mfspr %r17, SPR_MAS1 - oris %r17, %r17, MAS1_IPROT@h - mtspr SPR_MAS1, %r17 - isync - tlbwe - isync - msync - blr - -/* * Invalidates a single entry in TLB1. * * r3 ESEL @@ -541,75 +430,6 @@ blr /* - * r3 entry of temp translation - * r29 entry of current translation - * r28 returns temp entry passed in r3 - * r4-r5 scratched - */ -tlb1_temp_mapping_as1: - mr %r28, %r3 - - /* Read our current translation */ - lis %r3, MAS0_TLBSEL1@h /* Select TLB1 */ - rlwimi %r3, %r29, 16, 10, 15 /* Select our current entry */ - mtspr SPR_MAS0, %r3 - isync - tlbre - - /* Prepare and write temp entry */ - lis %r3, MAS0_TLBSEL1@h /* Select TLB1 */ - rlwimi %r3, %r28, 16, 10, 15 /* Select temp entry */ - mtspr SPR_MAS0, %r3 - isync - mfspr %r5, SPR_MAS1 - li %r4, 1 /* AS=1 */ - rlwimi %r5, %r4, 12, 19, 19 - li %r4, 0 /* Global mapping, TID=0 */ - rlwimi %r5, %r4, 16, 8, 15 - oris %r5, %r5, (MAS1_VALID | MAS1_IPROT)@h - mtspr SPR_MAS1, %r5 - isync - mflr %r3 - bl zero_mas7 - bl zero_mas8 - mtlr %r3 - tlbwe - isync - msync - blr - -/* - * Loops over TLB1, invalidates all entries skipping the one which currently - * maps this code. - * - * r29 current entry - * r3-r5 scratched - */ -tlb1_inval_all_but_current: - mr %r6, %r3 - mfspr %r3, SPR_TLB1CFG /* Get number of entries */ - andi. %r3, %r3, TLBCFG_NENTRY_MASK@l - li %r4, 0 /* Start from Entry 0 */ -1: lis %r5, MAS0_TLBSEL1@h - rlwimi %r5, %r4, 16, 10, 15 - mtspr SPR_MAS0, %r5 - isync - tlbre - mfspr %r5, SPR_MAS1 - cmpw %r4, %r29 /* our current entry? */ - beq 2f - rlwinm %r5, %r5, 0, 2, 31 /* clear VALID and IPROT bits */ - mtspr SPR_MAS1, %r5 - isync - tlbwe - isync - msync -2: addi %r4, %r4, 1 - cmpw %r4, %r3 /* Check if this is the last entry */ - bne 1b - blr - -/* * MAS7 and MAS8 conditional zeroing. */ .globl zero_mas7 @@ -649,8 +469,8 @@ * Upon release from holdoff CPU fetches the last word of the boot * page. */ - .space 4092 - (__boot_page_padding - __boot_page) - b __boot_page + .space 4092 - (__boot_page_padding - __start) + b __start #endif /* SMP */ /************************************************************************/ Index: sys/powerpc/booke/pmap.c =================================================================== --- sys/powerpc/booke/pmap.c +++ sys/powerpc/booke/pmap.c @@ -1377,8 +1377,10 @@ * Finish TLB1 configuration: the BSP already set up its TLB1 and we * have the snapshot of its contents in the s/w tlb1[] table, so use * these values directly to (re)program AP's TLB1 hardware. + * + * TLB1[0] is already set with the kernel map, so skip it. */ - for (i = bp_ntlb1s; i < tlb1_idx; i++) { + for (i = 1; i < tlb1_idx; i++) { /* Skip invalid entries */ if (!(tlb1[i].mas1 & MAS1_VALID)) continue; Index: sys/powerpc/mpc85xx/platform_mpc85xx.c =================================================================== --- sys/powerpc/mpc85xx/platform_mpc85xx.c +++ sys/powerpc/mpc85xx/platform_mpc85xx.c @@ -61,10 +61,11 @@ #ifdef SMP extern void *ap_pcpu; extern vm_paddr_t kernload; /* Kernel physical load address */ -extern uint8_t __boot_page[]; /* Boot page body */ +extern uint8_t __start[]; /* Boot page body */ extern uint32_t bp_ntlb1s; extern uint32_t bp_tlb1[]; extern uint32_t bp_tlb1_end[]; +extern int bp_ident; #endif extern uint32_t *bootinfo; @@ -321,10 +322,9 @@ mpc85xx_smp_start_cpu(platform_t plat, struct pcpu *pc) { #ifdef SMP - uint32_t *tlb1; vm_paddr_t bptr; uint32_t reg; - int i, timeout; + int timeout; uintptr_t brr; int cpuid; @@ -336,7 +336,7 @@ if ((reg & cpuid) != 0) { printf("%s: CPU %d is disabled!\n", __func__, pc->pc_cpuid); - return (-1); + return (ENXIO); } brr = OCP85XX_BRR; @@ -352,26 +352,10 @@ } ap_pcpu = pc; + bp_ident = pc->pc_cpuid; __asm __volatile("msync; isync"); - i = 0; - tlb1 = bp_tlb1; - while (i < bp_ntlb1s && tlb1 < bp_tlb1_end) { - mtspr(SPR_MAS0, MAS0_TLBSEL(1) | MAS0_ESEL(i)); - __asm __volatile("isync; tlbre"); - tlb1[0] = mfspr(SPR_MAS1); - tlb1[1] = mfspr(SPR_MAS2); - tlb1[2] = mfspr(SPR_MAS3); - i++; - tlb1 += 3; - } - if (i < bp_ntlb1s) - bp_ntlb1s = i; - - /* Flush caches to have our changes hit DRAM. */ - cpu_flush_dcache(__boot_page, 4096); - - bptr = ((vm_paddr_t)(uintptr_t)__boot_page - KERNBASE) + kernload; + bptr = ((vm_paddr_t)(uintptr_t)__start - KERNBASE) + kernload; KASSERT((bptr & 0xfff) == 0, ("%s: boot page is not aligned (%#jx)", __func__, (uintmax_t)bptr)); #ifdef QORIQ_DPAA