svn commit: r228931 - in projects/armv6/sys/arm: arm include
Grzegorz Bernacki
gber at FreeBSD.org
Wed Dec 28 15:20:56 UTC 2011
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_ */
More information about the svn-src-projects
mailing list