Date: Wed, 28 Dec 2011 15:26:38 +0000 (UTC) From: Grzegorz Bernacki <gber@FreeBSD.org> To: src-committers@freebsd.org, svn-src-projects@freebsd.org Subject: svn commit: r228932 - in projects/armv6/sys/arm/mv: . armadaxp Message-ID: <201112281526.pBSFQcuk012361@svn.freebsd.org>
next in thread | raw e-mail | index | archive | help
Author: gber Date: Wed Dec 28 15:26:38 2011 New Revision: 228932 URL: http://svn.freebsd.org/changeset/base/228932 Log: Implement ARM SMP related functions for Armada XP. - implement IPI - setup stacks for each core - low level bootstrap code Obtained from: Marvell, Semihalf Modified: projects/armv6/sys/arm/mv/armadaxp/armadaxp_mp.c projects/armv6/sys/arm/mv/mpic.c projects/armv6/sys/arm/mv/mv_machdep.c Modified: projects/armv6/sys/arm/mv/armadaxp/armadaxp_mp.c ============================================================================== --- projects/armv6/sys/arm/mv/armadaxp/armadaxp_mp.c Wed Dec 28 15:20:55 2011 (r228931) +++ projects/armv6/sys/arm/mv/armadaxp/armadaxp_mp.c Wed Dec 28 15:26:38 2011 (r228932) @@ -36,21 +36,24 @@ #include <arm/mv/mvwin.h> -/* XXX move to separate header files*/ -#define IPI_SELF 0 -#define IPI_ALL 1 -#define IPI_ALL_BUT_SELF 2 -void mpic_ipi_send(int cpus, u_int ipi); static int platform_get_ncpus(void); #define MV_AXP_CPU_DIVCLK_BASE (MV_BASE + 0x18700) -#define MV_AXP_CPU_DIVCLK_CTRL0 0x00 -#define MV_AXP_CPU_DIVCLK_CTRL2_RATIO_FULL0 0x08 -#define MV_AXP_CPU_DIVCLK_CTRL2_RATIO_FULL1 0x0c +#define CPU_DIVCLK_CTRL0 0x00 +#define CPU_DIVCLK_CTRL2_RATIO_FULL0 0x08 +#define CPU_DIVCLK_CTRL2_RATIO_FULL1 0x0c #define MV_COHERENCY_FABRIC_BASE (MV_MBUS_BRIDGE_BASE + 0x200) -#define MV_COHER_FABRIC_CTRL 0x00 -#define MV_COHER_FABRIC_CONF 0x04 +#define COHER_FABRIC_CTRL 0x00 +#define COHER_FABRIC_CONF 0x04 + +#define CPU_PMU(x) (MV_BASE + 0x22100 + (0x100 * (x))) +#define CPU_PMU_BOOT 0x24 + +#define MP (MV_BASE + 0x20800) +#define MP_SW_RESET(x) ((x) * 8) + +#define CPU_RESUME_CONTROL (0x20988) /* Coherency Fabric registers */ static uint32_t @@ -60,14 +63,12 @@ read_coher_fabric(uint32_t reg) return (bus_space_read_4(fdtbus_bs_tag, MV_COHERENCY_FABRIC_BASE, reg)); } -#ifdef not_yet static void write_coher_fabric(uint32_t reg, uint32_t val) { bus_space_write_4(fdtbus_bs_tag, MV_COHERENCY_FABRIC_BASE, reg, val); } -#endif /* Coherency Fabric registers */ static uint32_t @@ -84,17 +85,6 @@ write_cpu_clkdiv(uint32_t reg, uint32_t bus_space_write_4(fdtbus_bs_tag, MV_AXP_CPU_DIVCLK_BASE, reg, val); } -#if 0 -static void -hello_message(void) -{ - uint32_t cpuid; - - __asm __volatile("mrc p15, 0, %0, c0, c0, 5" : "=r" (cpuid)); - printf("CPU AP #%d is ready to serve you my sire.\n", cpuid); -} -#endif - void platform_mp_setmaxid(void) { @@ -111,45 +101,84 @@ platform_mp_probe(void) return (mp_ncpus > 1); } +void mpentry(void); +void mptramp(void); + +static void +initialize_coherency_fabric(void) +{ + uint32_t val, cpus, mask; + + cpus = platform_get_ncpus() - 1; + mask = (1 << cpus) - 1; + val = read_coher_fabric(COHER_FABRIC_CTRL); + val |= (mask << 24); + write_coher_fabric(COHER_FABRIC_CTRL, val); + + val = read_coher_fabric(COHER_FABRIC_CONF); + val |= (mask << 24); + write_coher_fabric(COHER_FABRIC_CONF, val); +} + + int platform_mp_start_ap(int cpuid) { - uint32_t reg; + uint32_t reg, *ptr; if (cpuid == 1) { + /* Copy boot code to SRAM */ + *((unsigned int*)(0xf1020240)) = 0xffff0101; + *((unsigned int*)(0xf1008500)) = 0xffff0003; + + pmap_kenter_nocache(0x880f0000, 0xffff0000); + reg = 0x880f0000; + + for (ptr = (uint32_t *)mptramp; ptr < (uint32_t *)mpentry; + ptr++, reg += 4) + *((uint32_t *)reg) = *ptr; + if (mp_ncpus > 1) { - reg = read_cpu_clkdiv(MV_AXP_CPU_DIVCLK_CTRL2_RATIO_FULL0); + reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL0); reg &= 0x00ffffff; reg |= 0x01000000; - write_cpu_clkdiv(MV_AXP_CPU_DIVCLK_CTRL2_RATIO_FULL0, reg); + write_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL0, reg); } if (mp_ncpus > 2) { - reg = read_cpu_clkdiv(MV_AXP_CPU_DIVCLK_CTRL2_RATIO_FULL1); + reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1); reg &= 0xff00ffff; reg |= 0x00010000; - write_cpu_clkdiv(MV_AXP_CPU_DIVCLK_CTRL2_RATIO_FULL1, reg); + write_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1, reg); } if (mp_ncpus > 3) { - reg = read_cpu_clkdiv(MV_AXP_CPU_DIVCLK_CTRL2_RATIO_FULL1); + reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1); reg &= 0x00ffffff; reg |= 0x01000000; - write_cpu_clkdiv(MV_AXP_CPU_DIVCLK_CTRL2_RATIO_FULL1, reg); + write_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1, reg); } - reg = read_cpu_clkdiv(MV_AXP_CPU_DIVCLK_CTRL0); + reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL0); reg |= ((0x1 << (mp_ncpus - 1)) - 1) << 21; - write_cpu_clkdiv(MV_AXP_CPU_DIVCLK_CTRL0, reg); - reg = read_cpu_clkdiv(MV_AXP_CPU_DIVCLK_CTRL0); + write_cpu_clkdiv(CPU_DIVCLK_CTRL0, reg); + reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL0); reg |= 0x01000000; - write_cpu_clkdiv(MV_AXP_CPU_DIVCLK_CTRL0, reg); + write_cpu_clkdiv(CPU_DIVCLK_CTRL0, reg); DELAY(100); reg &= ~(0xf << 21); - write_cpu_clkdiv(MV_AXP_CPU_DIVCLK_CTRL0, reg); + write_cpu_clkdiv(CPU_DIVCLK_CTRL0, reg); DELAY(100); + bus_space_write_4(fdtbus_bs_tag, MV_BASE, CPU_RESUME_CONTROL, 0); + + initialize_coherency_fabric(); + } + bus_space_write_4(fdtbus_bs_tag, CPU_PMU(cpuid), CPU_PMU_BOOT, + pmap_kextract(mpentry)); + bus_space_write_4(fdtbus_bs_tag, MP, MP_SW_RESET(cpuid), 0); + return (0); } @@ -157,14 +186,12 @@ static int platform_get_ncpus(void) { - return ((read_coher_fabric(MV_COHER_FABRIC_CONF) & 0xf) + 1); + return ((read_coher_fabric(COHER_FABRIC_CONF) & 0xf) + 1); } -#ifdef not_yet -static void -platform_ipi_send(int cpu, u_int ipi) +void +platform_ipi_send(cpuset_t cpus, u_int ipi) { - mpic_ipi_send(cpu, ipi); + pic_ipi_send(cpus, ipi); } -#endif Modified: projects/armv6/sys/arm/mv/mpic.c ============================================================================== --- projects/armv6/sys/arm/mv/mpic.c Wed Dec 28 15:20:55 2011 (r228931) +++ projects/armv6/sys/arm/mv/mpic.c Wed Dec 28 15:26:38 2011 (r228932) @@ -36,11 +36,15 @@ __FBSDID("$FreeBSD$"); #include <sys/systm.h> #include <sys/bus.h> #include <sys/kernel.h> +#include <sys/cpuset.h> #include <sys/ktr.h> #include <sys/module.h> #include <sys/rman.h> + #include <machine/bus.h> #include <machine/intr.h> +#include <machine/cpufunc.h> +#include <machine/smp.h> #include <dev/ofw/ofw_bus.h> #include <dev/ofw/ofw_bus_subr.h> @@ -56,22 +60,29 @@ __FBSDID("$FreeBSD$"); #define MPIC_ISE 0x30 #define MPIC_ICE 0x34 -#define MPIC_TASK_PRI 0xeb0 -#define MPIC_IIACK 0xeb4 -#define MPIC_ISM 0xeb8 -#define MPIC_ICM 0xebc + +#define MPIC_IN_DOORBELL 0x78 +#define MPIC_IN_DOORBELL_MASK 0x7c +#define MPIC_CTP 0xb0 +#define MPIC_CTP 0xb0 +#define MPIC_IIACK 0xb4 +#define MPIC_ISM 0xb8 +#define MPIC_ICM 0xbc #define MPIC_ERR_MASK 0xec0 struct mv_mpic_softc { - struct resource * mpic_res[1]; + struct resource * mpic_res[2]; bus_space_tag_t mpic_bst; bus_space_handle_t mpic_bsh; + bus_space_tag_t cpu_bst; + bus_space_handle_t cpu_bsh; int mpic_high_regs; int mpic_error_regs; }; static struct resource_spec mv_mpic_spec[] = { { SYS_RES_MEMORY, 0, RF_ACTIVE }, + { SYS_RES_MEMORY, 1, RF_ACTIVE }, { -1, 0 } }; @@ -86,6 +97,11 @@ uint32_t mv_mpic_get_cause_err(void); static void arm_mask_irq_err(uintptr_t); static void arm_unmask_irq_err(uintptr_t); +#define MPIC_CPU_WRITE(softc, reg, val) \ + bus_space_write_4((softc)->cpu_bst, (softc)->cpu_bsh, (reg), (val)) +#define MPIC_CPU_READ(softc, reg) \ + bus_space_read_4((softc)->cpu_bst, (softc)->cpu_bsh, (reg)) + static int mv_mpic_probe(device_t dev) { @@ -118,10 +134,12 @@ mv_mpic_attach(device_t dev) sc->mpic_bst = rman_get_bustag(sc->mpic_res[0]); sc->mpic_bsh = rman_get_bushandle(sc->mpic_res[0]); + sc->cpu_bst = rman_get_bustag(sc->mpic_res[1]); + sc->cpu_bsh = rman_get_bushandle(sc->mpic_res[1]); + bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, MPIC_CTRL, 1); - bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, - MPIC_TASK_PRI, 0); + MPIC_CPU_WRITE(mv_mpic_sc, MPIC_CTP, 0); return (0); } @@ -168,14 +186,12 @@ void arm_mask_irq(uintptr_t nb) { - bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, - MPIC_TASK_PRI, 1); + MPIC_CPU_WRITE(mv_mpic_sc, MPIC_CTP, 1); if (nb < MAIN_IRQS) { bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, MPIC_ICE, nb); - bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, - MPIC_ISM, nb); + MPIC_CPU_WRITE(mv_mpic_sc, MPIC_ISM, nb); } else arm_mask_irq_err(nb); } @@ -188,27 +204,26 @@ arm_mask_irq_err(uintptr_t nb) uint8_t bit_off; bit_off = nb - MAIN_IRQS; - mask = bus_space_read_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, - MPIC_ERR_MASK); + mask = MPIC_CPU_READ(mv_mpic_sc, MPIC_ERR_MASK); mask &= ~(1 << bit_off); - bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, - MPIC_ERR_MASK, mask); + MPIC_CPU_WRITE(mv_mpic_sc, MPIC_ERR_MASK, mask); } void arm_unmask_irq(uintptr_t nb) { - bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, - MPIC_TASK_PRI, 0); + MPIC_CPU_WRITE(mv_mpic_sc, MPIC_CTP, 0); if (nb < MAIN_IRQS) { bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, MPIC_ISE, nb); - bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, - MPIC_ICM, nb); + MPIC_CPU_WRITE(mv_mpic_sc, MPIC_ICM, nb); } else arm_unmask_irq_err(nb); + + if (nb == 0) + MPIC_CPU_WRITE(mv_mpic_sc, MPIC_IN_DOORBELL_MASK, 0xffffffff); } void @@ -219,23 +234,19 @@ arm_unmask_irq_err(uintptr_t nb) bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, MPIC_ISE, IRQ_ERR); - bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, - MPIC_ICM, IRQ_ERR); + MPIC_CPU_WRITE(mv_mpic_sc, MPIC_ICM, IRQ_ERR); bit_off = nb - MAIN_IRQS; - mask = bus_space_read_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, - MPIC_ERR_MASK); + mask = MPIC_CPU_READ(mv_mpic_sc, MPIC_ERR_MASK); mask |= (1 << bit_off); - bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, - MPIC_ERR_MASK, mask); + MPIC_CPU_WRITE(mv_mpic_sc, MPIC_ERR_MASK, mask); } uint32_t mv_mpic_get_cause(void) { - return (bus_space_read_4(mv_mpic_sc->mpic_bst, - mv_mpic_sc->mpic_bsh, MPIC_IIACK)); + return (MPIC_CPU_READ(mv_mpic_sc, MPIC_IIACK)); } uint32_t @@ -254,13 +265,41 @@ mv_mpic_get_cause_err(void) return (MAIN_IRQS + bit_off); } +#if defined(SMP) void -mpic_send_ipi(int cpus, u_int ipi) +pic_ipi_send(cpuset_t cpus, u_int ipi) { - uint32_t val; + uint32_t val, i; - val = cpus | ipi | 0xf00; + val = 0x01000000; + for (i = 0; i < MAXCPU; i++) + if (!CPU_ISSET(i, &cpus)) + val |= (1 << (8 + i)); + val |= ipi | 0xf00; bus_space_write_4(mv_mpic_sc->mpic_bst, mv_mpic_sc->mpic_bsh, MPIC_SOFT_INT, val); } + +int +pic_ipi_get(void) +{ + uint32_t val; + + val = MPIC_CPU_READ(mv_mpic_sc, MPIC_IN_DOORBELL); + if (val) + return (ffs(val) - 1); + + return (0x3ff); +} + +void +pic_ipi_clear(int ipi) +{ + uint32_t val; + + val = ~(1 << ipi); + MPIC_CPU_WRITE(mv_mpic_sc, MPIC_IN_DOORBELL, val); +} + +#endif Modified: projects/armv6/sys/arm/mv/mv_machdep.c ============================================================================== --- projects/armv6/sys/arm/mv/mv_machdep.c Wed Dec 28 15:20:55 2011 (r228931) +++ projects/armv6/sys/arm/mv/mv_machdep.c Wed Dec 28 15:26:38 2011 (r228932) @@ -133,6 +133,7 @@ vm_paddr_t phys_avail[10]; vm_paddr_t dump_avail[4]; vm_offset_t physical_pages; vm_offset_t pmap_bootstrap_lastaddr; +vm_paddr_t pmap_pa; const struct pmap_devmap *pmap_devmap_bootstrap_table; struct pv_addr systempage; @@ -142,6 +143,8 @@ struct pv_addr undstack; struct pv_addr abtstack; struct pv_addr kernelstack; +void set_stackptrs(int cpu); + static struct trapframe proc0_tf; static struct mem_region availmem_regions[FDT_MEM_REGIONS]; @@ -424,10 +427,10 @@ initarm(void *mdp, void *unused __unused dpcpu_init((void *)dpcpu.pv_va, 0); /* Allocate stacks for all modes */ - valloc_pages(irqstack, IRQ_STACK_SIZE); - valloc_pages(abtstack, ABT_STACK_SIZE); - valloc_pages(undstack, UND_STACK_SIZE); - valloc_pages(kernelstack, KSTACK_PAGES); + valloc_pages(irqstack, (IRQ_STACK_SIZE * MAXCPU)); + valloc_pages(abtstack, (ABT_STACK_SIZE * MAXCPU)); + valloc_pages(undstack, (UND_STACK_SIZE * MAXCPU)); + valloc_pages(kernelstack, (KSTACK_PAGES * MAXCPU)); init_param1(); @@ -488,6 +491,7 @@ initarm(void *mdp, void *unused __unused cpu_domains((DOMAIN_CLIENT << (PMAP_DOMAIN_KERNEL * 2)) | DOMAIN_CLIENT); + pmap_pa = kernel_l1pt.pv_pa; setttb(kernel_l1pt.pv_pa); cpu_tlb_flushID(); cpu_domains(DOMAIN_CLIENT << (PMAP_DOMAIN_KERNEL * 2)); @@ -536,12 +540,8 @@ initarm(void *mdp, void *unused __unused * of the stack memory. */ cpu_control(CPU_CONTROL_MMU_ENABLE, CPU_CONTROL_MMU_ENABLE); - set_stackptr(PSR_IRQ32_MODE, - irqstack.pv_va + IRQ_STACK_SIZE * PAGE_SIZE); - set_stackptr(PSR_ABT32_MODE, - abtstack.pv_va + ABT_STACK_SIZE * PAGE_SIZE); - set_stackptr(PSR_UND32_MODE, - undstack.pv_va + UND_STACK_SIZE * PAGE_SIZE); + + set_stackptrs(0); /* * We must now clean the cache again.... @@ -594,6 +594,18 @@ initarm(void *mdp, void *unused __unused sizeof(struct pcb))); } +void +set_stackptrs(int cpu) +{ + + set_stackptr(PSR_IRQ32_MODE, + irqstack.pv_va + ((IRQ_STACK_SIZE * PAGE_SIZE) * (cpu + 1))); + set_stackptr(PSR_ABT32_MODE, + abtstack.pv_va + ((ABT_STACK_SIZE * PAGE_SIZE) * (cpu + 1))); + set_stackptr(PSR_UND32_MODE, + undstack.pv_va + ((UND_STACK_SIZE * PAGE_SIZE) * (cpu + 1))); +} + #define MPP_PIN_MAX 68 #define MPP_PIN_CELLS 2 #define MPP_PINS_PER_REG 8 @@ -736,9 +748,9 @@ static struct pmap_devmap fdt_devmap[FDT static int platform_sram_devmap(struct pmap_devmap *map) { +#if !defined(SOC_MV_ARMADAXP) phandle_t child, root; u_long base, size; - /* * SRAM range. */ @@ -766,7 +778,9 @@ moveon: return (0); out: +#endif return (ENOENT); + } /*
Want to link to this message? Use this URL: <https://mail-archive.FreeBSD.org/cgi/mid.cgi?201112281526.pBSFQcuk012361>