Date: Wed, 28 Dec 2011 15:20:56 +0000 (UTC) From: Grzegorz Bernacki <gber@FreeBSD.org> To: src-committers@freebsd.org, svn-src-projects@freebsd.org Subject: svn commit: r228931 - in projects/armv6/sys/arm: arm include Message-ID: <201112281520.pBSFKu9I012147@svn.freebsd.org>
next in thread | raw e-mail | index | archive | help
Author: gber Date: Wed Dec 28 15:20:55 2011 New Revision: 228931 URL: http://svn.freebsd.org/changeset/base/228931 Log: Add architecture related SMP code. This work is based on MIPS approach mainly because it is very simple and works good as a initial implementation. We can consider more sophisticated schema like for example powerpc implementation which uses kobj. Obtained from: Marvell, Semihalf Modified: projects/armv6/sys/arm/arm/locore.S projects/armv6/sys/arm/arm/mp_machdep.c projects/armv6/sys/arm/include/smp.h Modified: projects/armv6/sys/arm/arm/locore.S ============================================================================== --- projects/armv6/sys/arm/arm/locore.S Wed Dec 28 15:15:00 2011 (r228930) +++ projects/armv6/sys/arm/arm/locore.S Wed Dec 28 15:20:55 2011 (r228931) @@ -1,6 +1,7 @@ /* $NetBSD: locore.S,v 1.14 2003/04/20 16:21:40 thorpej Exp $ */ /*- + * Copyright 2011 Semihalf * Copyright (C) 1994-1997 Mark Brinicombe * Copyright (C) 1994 Brini * All rights reserved. @@ -40,7 +41,7 @@ __FBSDID("$FreeBSD$"); /* What size should this really be ? It is only used by initarm() */ -#define INIT_ARM_STACK_SIZE 2048 +#define INIT_ARM_STACK_SIZE (2048 * 4) /* * This is for kvm_mkdb, and should be the address of the beginning @@ -228,10 +229,15 @@ Lstartup_pagetable: mmu_init_table: /* fill all table VA==PA */ /* map SDRAM VA==PA, WT cacheable */ +#if !defined(SMP) MMU_INIT(PHYSADDR, PHYSADDR , 64, L1_TYPE_S|L1_S_C|L1_S_AP(AP_KRW)) /* map VA 0xc0000000..0xc3ffffff to PA */ MMU_INIT(KERNBASE, PHYSADDR, 64, L1_TYPE_S|L1_S_C|L1_S_AP(AP_KRW)) - +#else + MMU_INIT(PHYSADDR, PHYSADDR , 64, L1_TYPE_S|L1_SHARED|L1_S_AP(AP_KRW)) + /* map VA 0xc0000000..0xc3ffffff to PA */ + MMU_INIT(KERNBASE, PHYSADDR, 64, L1_TYPE_S|L1_SHARED|L1_S_AP(AP_KRW)) +#endif .word 0 /* end of table */ #endif .Lstart: @@ -246,6 +252,11 @@ mmu_init_table: .Lvirt_done: .word virt_done +#if defined(SMP) +.Lmpvirt_done: + .word mpvirt_done +#endif + .Lmainreturned: .asciz "main() returned" .align 0 @@ -260,6 +271,118 @@ svcstk: .Lcpufuncs: .word _C_LABEL(cpufuncs) +#if defined(SMP) +Lsramaddr: + .word 0xffff0080 + +/* Use carefully!!! Changes r0, r1 */ +#define AP_DEBUG(tmp) \ + mrc p15, 0, r1, c0, c0, 5; \ + ldr r0, Lsramaddr; \ + add r0, r1, lsl #2; \ + mov r1, tmp; \ + str r1, [r0], #0x0000; + + +ASENTRY_NP(mptramp) + mov r0, #0 + mcr p15, 0, r0, c7, c7, 0 + + AP_DEBUG(#1) + + mrs r3, cpsr_all + bic r3, r3, #(PSR_MODE) + orr r3, r3, #(PSR_SVC32_MODE) + msr cpsr_all, r3 + + mrc p15, 0, r0, c0, c0, 5 + and r0, #0x0f /* Get CPU ID */ + + /* Read boot address for CPU */ + mov r1, #0x100 + mul r0, r0, r1 + ldr r1, Lpmureg + add r0, r0, r1 + ldr r1, [r0], #0x00 + + mov pc, r1 + +Lpmureg: + .word 0xd0022124 + +ASENTRY_NP(mpentry) + + AP_DEBUG(#2) + + /* Make sure interrupts are disabled. */ + mrs r7, cpsr + orr r7, r7, #(I32_bit|F32_bit) + msr cpsr_c, r7 + + /* Disable MMU for a while */ + mrc p15, 0, r2, c1, c0, 0 + bic r2, r2, #(CPU_CONTROL_MMU_ENABLE | CPU_CONTROL_DC_ENABLE |\ + CPU_CONTROL_WBUF_ENABLE) + bic r2, r2, #(CPU_CONTROL_IC_ENABLE) + bic r2, r2, #(CPU_CONTROL_BPRD_ENABLE) + mcr p15, 0, r2, c1, c0, 0 + + nop + nop + nop + + AP_DEBUG(#3) + +Ltag: + ldr r0, Lstartup_pagetable + mcr p15, 0, r0, c2, c0, 0 /* Set TTB */ + mcr p15, 0, r0, c8, c7, 0 /* Flush TLB */ + +#if defined(CPU_ARM11) || defined(CPU_MV_PJ4B) + mov r0, #0 + mcr p15, 0, r0, c13, c0, 1 /* Set ASID to 0 */ +#endif + + AP_DEBUG(#4) + + /* Set the Domain Access register. Very important! */ + mov r0, #((DOMAIN_CLIENT << (PMAP_DOMAIN_KERNEL*2)) | DOMAIN_CLIENT) + mcr p15, 0, r0, c3, c0, 0 + /* Enable MMU */ + mrc p15, 0, r0, c1, c0, 0 +#if defined(CPU_ARM11) || defined(CPU_MV_PJ4B) + orr r0, r0, #CPU_CONTROL_V6_EXTPAGE +#endif + orr r0, r0, #(CPU_CONTROL_MMU_ENABLE | CPU_CONTROL_DC_ENABLE) + mcr p15, 0, r0, c1, c0, 0 + nop + nop + nop + CPWAIT(r0) + + adr r1, .Lstart + ldmia r1, {r1, r2, sp} /* Set initial stack and */ + mrc p15, 0, r0, c0, c0, 5 + mov r1, #2048 + mul r2, r1, r0 + sub sp, sp, r2 + str r1, [sp] + ldr pc, .Lmpvirt_done + +mpvirt_done: + + mov fp, #0 /* trace back starts here */ + bl _C_LABEL(init_secondary) /* Off we go */ + + adr r0, .Lmpreturned + b _C_LABEL(panic) + /* NOTREACHED */ + +.Lmpreturned: + .asciz "main() returned" + .align 0 +#endif + ENTRY_NP(cpu_halt) mrs r2, cpsr bic r2, r2, #(PSR_MODE) Modified: projects/armv6/sys/arm/arm/mp_machdep.c ============================================================================== --- projects/armv6/sys/arm/arm/mp_machdep.c Wed Dec 28 15:15:00 2011 (r228930) +++ projects/armv6/sys/arm/arm/mp_machdep.c Wed Dec 28 15:20:55 2011 (r228931) @@ -28,34 +28,43 @@ __FBSDID("$FreeBSD$"); #include <sys/param.h> #include <sys/systm.h> #include <sys/bus.h> +#include <sys/kernel.h> #include <sys/lock.h> #include <sys/mutex.h> #include <sys/proc.h> #include <sys/pcpu.h> #include <sys/sched.h> #include <sys/smp.h> +#include <sys/ktr.h> #include <vm/vm.h> #include <vm/vm_extern.h> #include <vm/vm_kern.h> +#include <vm/pmap.h> +#include <machine/cpu.h> #include <machine/smp.h> +#include <machine/pcb.h> +#include <machine/pte.h> +#include <machine/intr.h> extern struct pcpu __pcpu[]; - /* used to hold the AP's until we are ready to release them */ -static struct mtx ap_boot_mtx; +struct mtx ap_boot_mtx; +struct pcb stoppcbs[MAXCPU]; /* # of Applications processors */ -int mp_naps; +volatile int mp_naps; /* Set to 1 once we're ready to let the APs out of the pen. */ -static volatile int aps_ready = 0; +volatile int aps_ready = 0; + +static int ipi_handler(void *arg); +void set_stackptrs(int cpu); /* Temporary variables for init_secondary() */ void *dpcpu; - /* Determine if we running MP machine */ int cpu_mp_probe(void) @@ -73,10 +82,12 @@ start_ap(int cpu) cpus = mp_naps; + dpcpu = (void *)kmem_alloc(kernel_map, DPCPU_SIZE); if (platform_mp_start_ap(cpu) != 0) return (-1); /* could not start AP */ for (ms = 0; ms < 5000; ++ms) { + cpu_dcache_wbinv_all(); if (mp_naps > cpus) return (0); /* success */ else @@ -86,7 +97,6 @@ start_ap(int cpu) return (-2); } - /* Initialize and fire up non-boot processors */ void cpu_mp_start(void) @@ -95,74 +105,89 @@ cpu_mp_start(void) mtx_init(&ap_boot_mtx, "ap boot", NULL, MTX_SPIN); - for (i = 1; i < mp_maxid; i++) { + for (i = 1; i < mp_ncpus; i++) { error = start_ap(i); if (error) { printf("AP #%d failed to start\n", i); continue; } + + printf("AP #%d started\n", i); CPU_SET(i, &all_cpus); } - - - return; } /* Introduce rest of cores to the world */ void cpu_mp_announce(void) { - return; + } +extern vm_paddr_t pmap_pa; void init_secondary(int cpu) { struct pcpu *pc; - void *dpcpu; - /* Per-cpu initialization */ + pj4b_config(); + + pj4bv6_setup(NULL); + + setttb(pmap_pa); + cpu_tlb_flushID(); + pc = &__pcpu[cpu]; + set_pcpu(pc); pcpu_init(pc, cpu, sizeof(struct pcpu)); - dpcpu = (void *)kmem_alloc(kernel_map, DPCPU_SIZE); dpcpu_init(dpcpu, cpu); + /* Provide stack pointers for other processor modes. */ + set_stackptrs(cpu); + /* Signal our startup to BSP */ mp_naps++; + cpu_dcache_wbinv_all(); /* Spin until the BSP releases the APs */ while (!aps_ready) - ; + cpu_dcache_wbinv_all(); /* Initialize curthread */ KASSERT(PCPU_GET(idlethread) != NULL, ("no idle thread")); - PCPU_SET(curthread, PCPU_GET(idlethread)); - + pc->pc_curthread = pc->pc_idlethread; + pc->pc_curpcb = pc->pc_idlethread->td_pcb; mtx_lock_spin(&ap_boot_mtx); - printf("SMP: AP CPU #%d Launched!\n", cpu); - + cpu_dcache_wbinv_all(); smp_cpus++; + cpu_dcache_wbinv_all(); if (smp_cpus == mp_ncpus) { /* enable IPI's, tlb shootdown, freezes etc */ + cpu_dcache_wbinv_all(); atomic_store_rel_int(&smp_started, 1); - /* - * XXX do we really need it - * smp_active = 1; - */ + cpu_dcache_wbinv_all(); + smp_active = 1; } mtx_unlock_spin(&ap_boot_mtx); + /* Enable ipi */ + arm_unmask_irq(0); + enable_interrupts(I32_bit); + + cpu_dcache_wbinv_all(); while (smp_started == 0) - ; + cpu_dcache_wbinv_all(); /* Start per-CPU event timers. */ cpu_initclocks_ap(); + CTR0(KTR_SMP, "go into scheduler"); + /* Enter the scheduler */ sched_throw(NULL); @@ -170,10 +195,102 @@ init_secondary(int cpu) /* NOTREACHED */ } +static int +ipi_handler(void *arg) +{ + u_int cpu, ipi; + + cpu = PCPU_GET(cpuid); + + ipi = pic_ipi_get(); + + while ((ipi != 0x3ff)) { + switch (ipi) { + case IPI_RENDEZVOUS: + CTR0(KTR_SMP, "IPI_RENDEZVOUS"); + smp_rendezvous_action(); + break; + + case IPI_AST: + CTR0(KTR_SMP, "IPI_AST"); + break; + + case IPI_STOP: + case IPI_STOP_HARD: + /* + * IPI_STOP_HARD is mapped to IPI_STOP so it is not + * necessary to add it in the switch. + */ + CTR0(KTR_SMP, "IPI_STOP or IPI_STOP_HARD"); + + savectx(&stoppcbs[cpu]); + + /* Indicate we are stopped */ + CPU_SET_ATOMIC(cpu, &stopped_cpus); + + /* Wait for restart */ + while (!CPU_ISSET(cpu, &started_cpus)) + cpu_spinwait(); + + CPU_CLR_ATOMIC(cpu, &started_cpus); + CPU_CLR_ATOMIC(cpu, &stopped_cpus); + CTR0(KTR_SMP, "IPI_STOP (restart)"); + break; + case IPI_PREEMPT: + CTR1(KTR_SMP, "%s: IPI_PREEMPT", __func__); + sched_preempt(curthread); + break; + case IPI_HARDCLOCK: + CTR1(KTR_SMP, "%s: IPI_HARDCLOCK", __func__); + hardclockintr(); + break; + case IPI_TLB: + CTR1(KTR_SMP, "%s: IPI_TLB", __func__); + cpufuncs.cf_tlb_flushID(); + break; + default: + panic("Unknown IPI 0x%0x on cpu %d", ipi, curcpu); + } + + pic_ipi_clear(ipi); + ipi = pic_ipi_get(); + } + + return (FILTER_HANDLED); +} + +static void +release_aps(void *dummy __unused) +{ + + if (mp_ncpus == 1) + return; + + /* + * IPI handler + */ + arm_setup_irqhandler("ipi", ipi_handler, NULL, NULL, 0, + INTR_TYPE_MISC | INTR_EXCL, NULL); + + /* Enable ipi */ + arm_unmask_irq(0); + + atomic_store_rel_int(&aps_ready, 1); + cpu_dcache_wbinv_all(); + printf("Release APs\n"); + while (smp_started == 0) { + cpu_dcache_wbinv_all(); + DELAY(5000); + } +} + +SYSINIT(start_aps, SI_SUB_SMP, SI_ORDER_FIRST, release_aps, NULL); + struct cpu_group * cpu_topo(void) { - return (smp_topo_1level(CG_SHARE_L2, 4, 0)); + + return (smp_topo_1level(CG_SHARE_L2, 1, 0)); } void @@ -187,21 +304,37 @@ cpu_mp_setmaxid(void) void ipi_all_but_self(u_int ipi) { + cpuset_t other_cpus; - return; + other_cpus = all_cpus; + CPU_CLR(PCPU_GET(cpuid), &other_cpus); + CTR2(KTR_SMP, "%s: ipi: %x", __func__, ipi); + platform_ipi_send(other_cpus, ipi); } void ipi_cpu(int cpu, u_int ipi) { + cpuset_t cpus; - return; + CPU_SET(cpu, &cpus); + + CTR3(KTR_SMP, "%s: cpu: %d, ipi: %x", __func__, cpu, ipi); + platform_ipi_send(cpus, ipi); } void ipi_selected(cpuset_t cpus, u_int ipi) { - return; + CTR2(KTR_SMP, "%s: ipi: %x", __func__, ipi); + platform_ipi_send(cpus, ipi); } +void +tlb_broadcast(int ipi) +{ + + if (smp_started) + ipi_all_but_self(ipi); +} Modified: projects/armv6/sys/arm/include/smp.h ============================================================================== --- projects/armv6/sys/arm/include/smp.h Wed Dec 28 15:15:00 2011 (r228930) +++ projects/armv6/sys/arm/include/smp.h Wed Dec 28 15:20:55 2011 (r228931) @@ -11,6 +11,7 @@ #define IPI_STOP 4 #define IPI_STOP_HARD 5 #define IPI_HARDCLOCK 6 +#define IPI_TLB 7 void init_secondary(int cpu); @@ -18,10 +19,16 @@ void ipi_all_but_self(u_int ipi); void ipi_cpu(int cpu, u_int ipi); void ipi_selected(cpuset_t cpus, u_int ipi); +/* PIC interface */ +void pic_ipi_send(cpuset_t cpus, u_int ipi); +void pic_ipi_clear(int ipi); +int pic_ipi_get(void); + /* Platform interface */ void platform_mp_setmaxid(void); int platform_mp_probe(void); int platform_mp_start_ap(int cpuid); +void platform_ipi_send(cpuset_t cpus, u_int ipi); #endif /* !_MACHINE_SMP_H_ */
Want to link to this message? Use this URL: <https://mail-archive.FreeBSD.org/cgi/mid.cgi?201112281520.pBSFKu9I012147>