From 01699e00e0a3ca39a48e163775cfb9de986a45c6 Mon Sep 17 00:00:00 2001 From: Yuichi Nakamura Date: Thu, 25 Feb 2021 00:51:29 +0900 Subject: [PATCH] arm/rp2040: Raspberry Pi Pico SMP support --- arch/arm/Kconfig | 2 + arch/arm/src/rp2040/Make.defs | 8 + arch/arm/src/rp2040/hardware/rp2040_psm.h | 85 ++++ arch/arm/src/rp2040/hardware/rp2040_sio.h | 67 +-- arch/arm/src/rp2040/rp2040_cpuidlestack.c | 91 ++++ arch/arm/src/rp2040/rp2040_cpuindex.c | 60 +++ arch/arm/src/rp2040/rp2040_cpupause.c | 437 ++++++++++++++++++ arch/arm/src/rp2040/rp2040_cpustart.c | 257 ++++++++++ arch/arm/src/rp2040/rp2040_irq.c | 88 ++++ arch/arm/src/rp2040/rp2040_irq.h | 30 +- arch/arm/src/rp2040/rp2040_start.c | 22 +- arch/arm/src/rp2040/rp2040_testset.c | 85 ++++ .../raspberrypi-pico/configs/smp/defconfig | 55 +++ 13 files changed, 1219 insertions(+), 68 deletions(-) create mode 100644 arch/arm/src/rp2040/hardware/rp2040_psm.h create mode 100644 arch/arm/src/rp2040/rp2040_cpuidlestack.c create mode 100644 arch/arm/src/rp2040/rp2040_cpuindex.c create mode 100644 arch/arm/src/rp2040/rp2040_cpupause.c create mode 100644 arch/arm/src/rp2040/rp2040_cpustart.c create mode 100644 arch/arm/src/rp2040/rp2040_testset.c create mode 100644 boards/arm/rp2040/raspberrypi-pico/configs/smp/defconfig diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig index f378b426bf..d7fa1db3ea 100644 --- a/arch/arm/Kconfig +++ b/arch/arm/Kconfig @@ -229,6 +229,8 @@ config ARCH_CHIP_RP2040 bool "Raspberry Pi RP2040" select ARCH_CORTEXM0 select ARCH_HAVE_RAMVECTORS + select ARCH_HAVE_MULTICPU + select ARCH_HAVE_TESTSET ---help--- Raspberry Pi RP2040 architectures (ARM Cortex-M0+). diff --git a/arch/arm/src/rp2040/Make.defs b/arch/arm/src/rp2040/Make.defs index 8d36d9d67c..330c3c2b29 100644 --- a/arch/arm/src/rp2040/Make.defs +++ b/arch/arm/src/rp2040/Make.defs @@ -59,6 +59,14 @@ CHIP_CSRCS += rp2040_clock.c CHIP_CSRCS += rp2040_xosc.c CHIP_CSRCS += rp2040_pll.c +ifeq ($(CONFIG_SMP),y) +CHIP_CSRCS += rp2040_cpuindex.c +CHIP_CSRCS += rp2040_cpustart.c +CHIP_CSRCS += rp2040_cpupause.c +CHIP_CSRCS += rp2040_cpuidlestack.c +CHIP_CSRCS += rp2040_testset.c +endif + ifeq ($(CONFIG_RP2040_FLASH_BOOT),y) ifneq ($(PICO_SDK_PATH),) include chip/boot2/Make.defs diff --git a/arch/arm/src/rp2040/hardware/rp2040_psm.h b/arch/arm/src/rp2040/hardware/rp2040_psm.h new file mode 100644 index 0000000000..93dc950f07 --- /dev/null +++ b/arch/arm/src/rp2040/hardware/rp2040_psm.h @@ -0,0 +1,85 @@ +/**************************************************************************** + * arch/arm/src/rp2040/hardware/rp2040_psm.h + * + * Generated from rp2040.svd originally provided by + * Raspberry Pi (Trading) Ltd. + * + * Copyright 2020 (c) 2020 Raspberry Pi (Trading) Ltd. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef __ARCH_ARM_SRC_RP2040_HARDWARE_RP2040_PSM_H +#define __ARCH_ARM_SRC_RP2040_HARDWARE_RP2040_PSM_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "hardware/rp2040_memorymap.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Register offsets *********************************************************/ + +#define RP2040_PSM_FRCE_ON_OFFSET 0x000000 /* Force block out of reset (i.e. power it on) */ +#define RP2040_PSM_FRCE_OFF_OFFSET 0x000004 /* Force into reset (i.e. power it off) */ +#define RP2040_PSM_WDSEL_OFFSET 0x000008 /* Set to 1 if this peripheral should be reset when the watchdog fires. */ +#define RP2040_PSM_DONE_OFFSET 0x00000c /* Indicates the peripheral's registers are ready to access. */ + +/* Register definitions *****************************************************/ + +#define RP2040_PSM_FRCE_ON (RP2040_PSM_BASE + RP2040_PSM_FRCE_ON_OFFSET) +#define RP2040_PSM_FRCE_OFF (RP2040_PSM_BASE + RP2040_PSM_FRCE_OFF_OFFSET) +#define RP2040_PSM_WDSEL (RP2040_PSM_BASE + RP2040_PSM_WDSEL_OFFSET) +#define RP2040_PSM_DONE (RP2040_PSM_BASE + RP2040_PSM_DONE_OFFSET) + +/* Register bit definitions *************************************************/ + +#define RP2040_PSM_PROC1 (1 << 16) +#define RP2040_PSM_PROC0 (1 << 15) +#define RP2040_PSM_SIO (1 << 14) +#define RP2040_PSM_VREG_AND_CHIP_RESET (1 << 13) +#define RP2040_PSM_XIP (1 << 12) +#define RP2040_PSM_SRAM5 (1 << 11) +#define RP2040_PSM_SRAM4 (1 << 10) +#define RP2040_PSM_SRAM3 (1 << 9) +#define RP2040_PSM_SRAM2 (1 << 8) +#define RP2040_PSM_SRAM1 (1 << 7) +#define RP2040_PSM_SRAM0 (1 << 6) +#define RP2040_PSM_ROM (1 << 5) +#define RP2040_PSM_BUSFABRIC (1 << 4) +#define RP2040_PSM_RESETS (1 << 3) +#define RP2040_PSM_CLOCKS (1 << 2) +#define RP2040_PSM_XOSC (1 << 1) +#define RP2040_PSM_ROSC (1 << 0) + +#endif /* __ARCH_ARM_SRC_RP2040_HARDWARE_RP2040_PSM_H */ diff --git a/arch/arm/src/rp2040/hardware/rp2040_sio.h b/arch/arm/src/rp2040/hardware/rp2040_sio.h index ba28ec5f33..8f569f2217 100644 --- a/arch/arm/src/rp2040/hardware/rp2040_sio.h +++ b/arch/arm/src/rp2040/hardware/rp2040_sio.h @@ -112,38 +112,8 @@ #define RP2040_SIO_INTERP1_ACCUM0_ADD_OFFSET 0x0000f4 /* Values written here are atomically added to ACCUM0 Reading yields lane 0's raw shift and mask value (BASE0 not added). */ #define RP2040_SIO_INTERP1_ACCUM1_ADD_OFFSET 0x0000f8 /* Values written here are atomically added to ACCUM1 Reading yields lane 1's raw shift and mask value (BASE1 not added). */ #define RP2040_SIO_INTERP1_BASE_1AND0_OFFSET 0x0000fc /* On write, the lower 16 bits go to BASE0, upper bits to BASE1 simultaneously. Each half is sign-extended to 32 bits if that lane's SIGNED flag is set. */ -#define RP2040_SIO_SPINLOCK0_OFFSET 0x000100 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK1_OFFSET 0x000104 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK2_OFFSET 0x000108 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK3_OFFSET 0x00010c /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK4_OFFSET 0x000110 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK5_OFFSET 0x000114 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK6_OFFSET 0x000118 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK7_OFFSET 0x00011c /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK8_OFFSET 0x000120 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK9_OFFSET 0x000124 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK10_OFFSET 0x000128 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK11_OFFSET 0x00012c /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK12_OFFSET 0x000130 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK13_OFFSET 0x000134 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK14_OFFSET 0x000138 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK15_OFFSET 0x00013c /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK16_OFFSET 0x000140 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK17_OFFSET 0x000144 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK18_OFFSET 0x000148 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK19_OFFSET 0x00014c /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK20_OFFSET 0x000150 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK21_OFFSET 0x000154 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK22_OFFSET 0x000158 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK23_OFFSET 0x00015c /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK24_OFFSET 0x000160 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK25_OFFSET 0x000164 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK26_OFFSET 0x000168 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK27_OFFSET 0x00016c /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK28_OFFSET 0x000170 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK29_OFFSET 0x000174 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK30_OFFSET 0x000178 /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ -#define RP2040_SIO_SPINLOCK31_OFFSET 0x00017c /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ +#define RP2040_SIO_SPINLOCK_OFFSET(n) ((n) * 4 + 0x000100) + /* Reading from a spinlock address will: - Return 0 if lock is already locked - Otherwise return nonzero, and simultaneously claim the lock Writing (any value) releases the lock. If core 0 and core 1 attempt to claim the same lock simultaneously, core 0 wins. The value returned on success is 0x1 << lock number. */ /* Register definitions *****************************************************/ @@ -209,38 +179,7 @@ #define RP2040_SIO_INTERP1_ACCUM0_ADD (RP2040_SIO_BASE + RP2040_SIO_INTERP1_ACCUM0_ADD_OFFSET) #define RP2040_SIO_INTERP1_ACCUM1_ADD (RP2040_SIO_BASE + RP2040_SIO_INTERP1_ACCUM1_ADD_OFFSET) #define RP2040_SIO_INTERP1_BASE_1AND0 (RP2040_SIO_BASE + RP2040_SIO_INTERP1_BASE_1AND0_OFFSET) -#define RP2040_SIO_SPINLOCK0 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK0_OFFSET) -#define RP2040_SIO_SPINLOCK1 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK1_OFFSET) -#define RP2040_SIO_SPINLOCK2 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK2_OFFSET) -#define RP2040_SIO_SPINLOCK3 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK3_OFFSET) -#define RP2040_SIO_SPINLOCK4 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK4_OFFSET) -#define RP2040_SIO_SPINLOCK5 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK5_OFFSET) -#define RP2040_SIO_SPINLOCK6 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK6_OFFSET) -#define RP2040_SIO_SPINLOCK7 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK7_OFFSET) -#define RP2040_SIO_SPINLOCK8 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK8_OFFSET) -#define RP2040_SIO_SPINLOCK9 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK9_OFFSET) -#define RP2040_SIO_SPINLOCK10 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK10_OFFSET) -#define RP2040_SIO_SPINLOCK11 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK11_OFFSET) -#define RP2040_SIO_SPINLOCK12 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK12_OFFSET) -#define RP2040_SIO_SPINLOCK13 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK13_OFFSET) -#define RP2040_SIO_SPINLOCK14 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK14_OFFSET) -#define RP2040_SIO_SPINLOCK15 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK15_OFFSET) -#define RP2040_SIO_SPINLOCK16 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK16_OFFSET) -#define RP2040_SIO_SPINLOCK17 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK17_OFFSET) -#define RP2040_SIO_SPINLOCK18 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK18_OFFSET) -#define RP2040_SIO_SPINLOCK19 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK19_OFFSET) -#define RP2040_SIO_SPINLOCK20 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK20_OFFSET) -#define RP2040_SIO_SPINLOCK21 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK21_OFFSET) -#define RP2040_SIO_SPINLOCK22 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK22_OFFSET) -#define RP2040_SIO_SPINLOCK23 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK23_OFFSET) -#define RP2040_SIO_SPINLOCK24 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK24_OFFSET) -#define RP2040_SIO_SPINLOCK25 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK25_OFFSET) -#define RP2040_SIO_SPINLOCK26 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK26_OFFSET) -#define RP2040_SIO_SPINLOCK27 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK27_OFFSET) -#define RP2040_SIO_SPINLOCK28 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK28_OFFSET) -#define RP2040_SIO_SPINLOCK29 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK29_OFFSET) -#define RP2040_SIO_SPINLOCK30 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK30_OFFSET) -#define RP2040_SIO_SPINLOCK31 (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK31_OFFSET) +#define RP2040_SIO_SPINLOCK(n) (RP2040_SIO_BASE + RP2040_SIO_SPINLOCK_OFFSET(n)) /* Register bit definitions *************************************************/ diff --git a/arch/arm/src/rp2040/rp2040_cpuidlestack.c b/arch/arm/src/rp2040/rp2040_cpuidlestack.c new file mode 100644 index 0000000000..e675a2fed1 --- /dev/null +++ b/arch/arm/src/rp2040/rp2040_cpuidlestack.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * arch/arm/src/rp2040/rp2040_cpuidlestack.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include +#include + +#include "arm_internal.h" + +#ifdef CONFIG_SMP + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_cpu_idlestack + * + * Description: + * Allocate a stack for the CPU[n] IDLE task (n > 0) if appropriate and + * setup up stack-related information in the IDLE task's TCB. This + * function is always called before up_cpu_start(). This function is + * only called for the CPU's initial IDLE task; up_create_task is used for + * all normal tasks, pthreads, and kernel threads for all CPUs. + * + * The initial IDLE task is a special case because the CPUs can be started + * in different wans in different environments: + * + * 1. The CPU may already have been started and waiting in a low power + * state for up_cpu_start(). In this case, the IDLE thread's stack + * has already been allocated and is already in use. Here + * up_cpu_idlestack() only has to provide information about the + * already allocated stack. + * + * 2. The CPU may be disabled but started when up_cpu_start() is called. + * In this case, a new stack will need to be created for the IDLE + * thread and this function is then equivalent to: + * + * return up_create_stack(tcb, stack_size, TCB_FLAG_TTYPE_KERNEL); + * + * The following TCB fields must be initialized by this function: + * + * - adj_stack_size: Stack size after adjustment for hardware, processor, + * etc. This value is retained only for debug purposes. + * - stack_alloc_ptr: Pointer to allocated stack + * - adj_stack_ptr: Adjusted stack_alloc_ptr for HW. The initial value of + * the stack pointer. + * + * Input Parameters: + * - cpu: CPU index that indicates which CPU the IDLE task is + * being created for. + * - tcb: The TCB of new CPU IDLE task + * - stack_size: The requested stack size for the IDLE task. At least + * this much must be allocated. This should be + * CONFIG_SMP_STACK_SIZE. + * + ****************************************************************************/ + +int up_cpu_idlestack(int cpu, FAR struct tcb_s *tcb, size_t stack_size) +{ +#if CONFIG_SMP_NCPUS > 1 + up_create_stack(tcb, stack_size, TCB_FLAG_TTYPE_KERNEL); +#endif + return OK; +} + +#endif /* CONFIG_SMP */ diff --git a/arch/arm/src/rp2040/rp2040_cpuindex.c b/arch/arm/src/rp2040/rp2040_cpuindex.c new file mode 100644 index 0000000000..3d1f0ff4ab --- /dev/null +++ b/arch/arm/src/rp2040/rp2040_cpuindex.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * arch/arm/src/rp2040/rp2040_cpuindex.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include + +#include "arm_arch.h" + +#include "hardware/rp2040_sio.h" + +#ifdef CONFIG_SMP + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_cpu_index + * + * Description: + * Return an index in the range of 0 through (CONFIG_SMP_NCPUS-1) that + * corresponds to the currently executing CPU. + * + * Input Parameters: + * None + * + * Returned Value: + * An integer index in the range of 0 through (CONFIG_SMP_NCPUS-1) that + * corresponds to the currently executing CPU. + * + ****************************************************************************/ + +int up_cpu_index(void) +{ + return getreg32(RP2040_SIO_CPUID); +} + +#endif /* CONFIG_SMP */ diff --git a/arch/arm/src/rp2040/rp2040_cpupause.c b/arch/arm/src/rp2040/rp2040_cpupause.c new file mode 100644 index 0000000000..639bb59251 --- /dev/null +++ b/arch/arm/src/rp2040/rp2040_cpupause.c @@ -0,0 +1,437 @@ +/**************************************************************************** + * arch/arm/src/rp2040/rp2040_cpupause.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "arm_arch.h" +#include "sched/sched.h" +#include "arm_internal.h" +#include "hardware/rp2040_sio.h" + +#ifdef CONFIG_SMP + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#if 0 +#define DPRINTF(fmt, args...) llinfo(fmt, ##args) +#else +#define DPRINTF(fmt, args...) do {} while (0) +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* These spinlocks are used in the SMP configuration in order to implement + * up_cpu_pause(). The protocol for CPUn to pause CPUm is as follows + * + * 1. The up_cpu_pause() implementation on CPUn locks both g_cpu_wait[m] + * and g_cpu_paused[m]. CPUn then waits spinning on g_cpu_paused[m]. + * 2. CPUm receives the interrupt it (1) unlocks g_cpu_paused[m] and + * (2) locks g_cpu_wait[m]. The first unblocks CPUn and the second + * blocks CPUm in the interrupt handler. + * + * When CPUm resumes, CPUn unlocks g_cpu_wait[m] and the interrupt handler + * on CPUm continues. CPUm must, of course, also then unlock g_cpu_wait[m] + * so that it will be ready for the next pause operation. + */ + +static volatile spinlock_t g_cpu_wait[CONFIG_SMP_NCPUS]; +static volatile spinlock_t g_cpu_paused[CONFIG_SMP_NCPUS]; + +/**************************************************************************** + * Name: rp2040_handle_irqreq + * + * Description: + * If an irq handling request is found on cpu, call up_enable_irq() or + * up_disable_irq(). + * + * Input Parameters: + * irqreq - The IRQ number to be handled (>0 : enable / <0 : disable) + * + ****************************************************************************/ + +static void rp2040_handle_irqreq(int irqreq) +{ + DEBUGASSERT(up_cpu_index() == 0); + + /* Unlock the spinlock first */ + + spin_unlock(&g_cpu_paused[0]); + + /* Then wait for the spinlock to be released */ + + spin_lock(&g_cpu_wait[0]); + + if (irqreq > 0) + { + up_enable_irq(irqreq); + } + else + { + up_disable_irq(-irqreq); + } + + /* Finally unlock the spinlock */ + + spin_unlock(&g_cpu_wait[0]); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_cpu_pausereq + * + * Description: + * Return true if a pause request is pending for this CPU. + * + * Input Parameters: + * cpu - The index of the CPU to be queried + * + * Returned Value: + * true = a pause request is pending. + * false = no pasue request is pending. + * + ****************************************************************************/ + +bool up_cpu_pausereq(int cpu) +{ + return spin_islocked(&g_cpu_paused[cpu]); +} + +/**************************************************************************** + * Name: up_cpu_paused + * + * Description: + * Handle a pause request from another CPU. Normally, this logic is + * executed from interrupt handling logic within the architecture-specific + * However, it is sometimes necessary necessary to perform the pending + * pause operation in other contexts where the interrupt cannot be taken + * in order to avoid deadlocks. + * + * This function performs the following operations: + * + * 1. It saves the current task state at the head of the current assigned + * task list. + * 2. It waits on a spinlock, then + * 3. Returns from interrupt, restoring the state of the new task at the + * head of the ready to run list. + * + * Input Parameters: + * cpu - The index of the CPU to be paused + * + * Returned Value: + * On success, OK is returned. Otherwise, a negated errno value indicating + * the nature of the failure is returned. + * + ****************************************************************************/ + +int up_cpu_paused(int cpu) +{ + FAR struct tcb_s *tcb = this_task(); + + /* Update scheduler parameters */ + + nxsched_suspend_scheduler(tcb); + +#ifdef CONFIG_SCHED_INSTRUMENTATION + /* Notify that we are paused */ + + sched_note_cpu_paused(tcb); +#endif + + /* Save the current context at CURRENT_REGS into the TCB at the head + * of the assigned task list for this CPU. + */ + + arm_savestate(tcb->xcp.regs); + + /* Wait for the spinlock to be released */ + + spin_unlock(&g_cpu_paused[cpu]); + spin_lock(&g_cpu_wait[cpu]); + + /* Restore the exception context of the tcb at the (new) head of the + * assigned task list. + */ + + tcb = this_task(); + +#ifdef CONFIG_SCHED_INSTRUMENTATION + /* Notify that we have resumed */ + + sched_note_cpu_resumed(tcb); +#endif + + /* Reset scheduler parameters */ + + nxsched_resume_scheduler(tcb); + + /* Then switch contexts. Any necessary address environment changes + * will be made when the interrupt returns. + */ + + arm_restorestate(tcb->xcp.regs); + spin_unlock(&g_cpu_wait[cpu]); + + return OK; +} + +/**************************************************************************** + * Name: arm_pause_handler + * + * Description: + * Inter-CPU interrupt handler + * + * Input Parameters: + * Standard interrupt handler inputs + * + * Returned Value: + * Should always return OK + * + ****************************************************************************/ + +int arm_pause_handler(int irq, void *c, FAR void *arg) +{ + int cpu = up_cpu_index(); + int irqreq; + uint32_t stat; + + stat = getreg32(RP2040_SIO_FIFO_ST); + if (stat & (RP2040_SIO_FIFO_ST_ROE | RP2040_SIO_FIFO_ST_WOF)) + { + /* Clear sticky flag */ + + putreg32(0, RP2040_SIO_FIFO_ST); + } + + if (!(stat & RP2040_SIO_FIFO_ST_VLD)) + { + /* No data received */ + + return OK; + } + + irqreq = getreg32(RP2040_SIO_FIFO_RD); + + if (irqreq != 0) + { + /* Handle IRQ enable/disable request */ + + rp2040_handle_irqreq(irqreq); + return OK; + } + + DPRINTF("cpu%d will be paused \n", cpu); + + /* Check for false alarms. Such false could occur as a consequence of + * some deadlock breaking logic that might have already serviced the SG2 + * interrupt by calling up_cpu_paused. + */ + + if (up_cpu_pausereq(cpu)) + { + /* NOTE: The following enter_critical_section() will call + * up_cpu_paused() to process a pause request to break a deadlock + * because the caller held a critical section. Once up_cpu_paused() + * finished, the caller will proceed and release the g_cpu_irqlock. + * Then this CPU will acquire g_cpu_irqlock in the function. + */ + + irqstate_t flags = enter_critical_section(); + + /* NOTE: the pause request should not exist here */ + + DEBUGVERIFY(!up_cpu_pausereq(cpu)); + + leave_critical_section(flags); + } + + return OK; +} + +/**************************************************************************** + * Name: up_cpu_pause + * + * Description: + * Save the state of the current task at the head of the + * g_assignedtasks[cpu] task list and then pause task execution on the + * CPU. + * + * This function is called by the OS when the logic executing on one CPU + * needs to modify the state of the g_assignedtasks[cpu] list for another + * CPU. + * + * Input Parameters: + * cpu - The index of the CPU to be stopped/ + * + * Returned Value: + * Zero on success; a negated errno value on failure. + * + ****************************************************************************/ + +int up_cpu_pause(int cpu) +{ + DPRINTF("cpu=%d\n", cpu); + + DEBUGASSERT(cpu >= 0 && cpu < CONFIG_SMP_NCPUS && cpu != this_cpu()); + +#ifdef CONFIG_SCHED_INSTRUMENTATION + /* Notify of the pause event */ + + sched_note_cpu_pause(this_task(), cpu); +#endif + + /* Take the both spinlocks. The g_cpu_wait spinlock will prevent the SGI2 + * handler from returning until up_cpu_resume() is called; g_cpu_paused + * is a handshake that will prefent this function from returning until + * the CPU is actually paused. + */ + + DEBUGASSERT(!spin_islocked(&g_cpu_wait[cpu]) && + !spin_islocked(&g_cpu_paused[cpu])); + + spin_lock(&g_cpu_wait[cpu]); + spin_lock(&g_cpu_paused[cpu]); + + DEBUGASSERT(cpu != up_cpu_index()); + + /* Generate IRQ for CPU(cpu) */ + + while (!(getreg32(RP2040_SIO_FIFO_ST) & RP2040_SIO_FIFO_ST_RDY)) + ; + putreg32(0, RP2040_SIO_FIFO_WR); + + /* Wait for the other CPU to unlock g_cpu_paused meaning that + * it is fully paused and ready for up_cpu_resume(); + */ + + spin_lock(&g_cpu_paused[cpu]); + spin_unlock(&g_cpu_paused[cpu]); + + /* On successful return g_cpu_wait will be locked, the other CPU will be + * spinning on g_cpu_wait and will not continue until g_cpu_resume() is + * called. g_cpu_paused will be unlocked in any case. + */ + + return 0; +} + +/**************************************************************************** + * Name: up_cpu_resume + * + * Description: + * Restart the cpu after it was paused via up_cpu_pause(), restoring the + * state of the task at the head of the g_assignedtasks[cpu] list, and + * resume normal tasking. + * + * This function is called after up_cpu_pause in order resume operation of + * the CPU after modifying its g_assignedtasks[cpu] list. + * + * Input Parameters: + * cpu - The index of the CPU being re-started. + * + * Returned Value: + * Zero on success; a negated errno value on failure. + * + ****************************************************************************/ + +int up_cpu_resume(int cpu) +{ + DPRINTF("cpu=%d\n", cpu); + + DEBUGASSERT(cpu >= 0 && cpu < CONFIG_SMP_NCPUS && cpu != this_cpu()); + +#ifdef CONFIG_SCHED_INSTRUMENTATION + /* Notify of the resume event */ + + sched_note_cpu_resume(this_task(), cpu); +#endif + + /* Release the spinlock. Releasing the spinlock will cause the SGI2 + * handler on 'cpu' to continue and return from interrupt to the newly + * established thread. + */ + + DEBUGASSERT(spin_islocked(&g_cpu_wait[cpu]) && + !spin_islocked(&g_cpu_paused[cpu])); + + spin_unlock(&g_cpu_wait[cpu]); + return 0; +} + +/**************************************************************************** + * Name: rp2040_send_irqreq() + * + * Description: + * Send up_enable_irq() / up_disable_irq() request to the Core #0 + * + * This function is called from up_enable_irq() or up_disable_irq() + * to be handled on specified CPU. Locking protocol in the sequence is + * the same as up_pause_cpu() plus up_resume_cpu(). + * + * Input Parameters: + * irqreq - The IRQ number to be handled (>0 : enable / <0 : disable) + * + ****************************************************************************/ + +void rp2040_send_irqreq(int irqreq) +{ + /* Wait for the spinlocks to be released */ + + spin_lock(&g_cpu_wait[0]); + spin_lock(&g_cpu_paused[0]); + + /* Send IRQ number to Core #0 */ + + while (!(getreg32(RP2040_SIO_FIFO_ST) & RP2040_SIO_FIFO_ST_RDY)) + ; + putreg32(irqreq, RP2040_SIO_FIFO_WR); + + /* Wait for the handler is executed on cpu */ + + spin_lock(&g_cpu_paused[0]); + spin_unlock(&g_cpu_paused[0]); + + /* Finally unlock the spinlock to proceed the handler */ + + spin_unlock(&g_cpu_wait[0]); + return; +} + +#endif /* CONFIG_SMP */ diff --git a/arch/arm/src/rp2040/rp2040_cpustart.c b/arch/arm/src/rp2040/rp2040_cpustart.c new file mode 100644 index 0000000000..05e1fddf2e --- /dev/null +++ b/arch/arm/src/rp2040/rp2040_cpustart.c @@ -0,0 +1,257 @@ +/**************************************************************************** + * arch/arm/src/rp2040/rp2040_cpustart.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "nvic.h" +#include "arm_arch.h" +#include "sched/sched.h" +#include "init/init.h" +#include "arm_internal.h" + +#include "hardware/rp2040_memorymap.h" +#include "hardware/rp2040_sio.h" +#include "hardware/rp2040_psm.h" + +#ifdef CONFIG_SMP + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#if 0 +# define DPRINTF(fmt, args...) _err(fmt, ##args) +#else +# define DPRINTF(fmt, args...) do {} while (0) +#endif + +#ifdef CONFIG_DEBUG_FEATURES +# define showprogress(c) arm_lowputc(c) +#else +# define showprogress(c) +#endif + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +volatile static spinlock_t g_core1_boot; + +extern int arm_pause_handler(int irq, void *c, FAR void *arg); + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: fifo_drain + * + * Description: + * Drain all data in the inter-processor FIFO + ****************************************************************************/ + +static void fifo_drain(void) +{ + putreg32(0, RP2040_SIO_FIFO_ST); + + while (getreg32(RP2040_SIO_FIFO_ST) & RP2040_SIO_FIFO_ST_VLD) + { + getreg32(RP2040_SIO_FIFO_RD); + } + + __asm__ volatile ("sev"); +} + +/**************************************************************************** + * Name: fifo_comm + * + * Description: + * Communicate with CPU Core 1 using inter-processor FIFO for boot + * + * Input Parameters: + * msg - Data to be sent to Core 1 + * + * Returned Value: + * true on success; false on failure. + * + ****************************************************************************/ + +static int fifo_comm(uint32_t msg) +{ + uint32_t rcv; + + while (!(getreg32(RP2040_SIO_FIFO_ST) & RP2040_SIO_FIFO_ST_RDY)) + ; + putreg32(msg, RP2040_SIO_FIFO_WR); + __asm__ volatile ("sev"); + + while (!(getreg32(RP2040_SIO_FIFO_ST) & RP2040_SIO_FIFO_ST_VLD)) + __asm__ volatile ("wfe"); + + rcv = getreg32(RP2040_SIO_FIFO_RD); + + return msg == rcv; +} + +/**************************************************************************** + * Name: core1_boot + * + * Description: + * This is the boot vector for Core #1 + * + * Input Parameters: + * + * Returned Value: + * + ****************************************************************************/ + +static void core1_boot(void) +{ + fifo_drain(); + + /* Setup NVIC */ + + up_irqinitialize(); + + /* Enable inter-processor FIFO interrupt */ + + irq_attach(RP2040_SIO_IRQ_PROC1, arm_pause_handler, NULL); + up_enable_irq(RP2040_SIO_IRQ_PROC1); + + spin_unlock(&g_core1_boot); + +#ifdef CONFIG_SCHED_INSTRUMENTATION + /* Notify that this CPU has started */ + + sched_note_cpu_started(this_task()); +#endif + + /* Then transfer control to the IDLE task */ + + nx_idle_trampoline(); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_cpu_start + * + * Description: + * In an SMP configution, only one CPU is initially active (CPU 0). System + * initialization occurs on that single thread. At the completion of the + * initialization of the OS, just before beginning normal multitasking, + * the additional CPUs would be started by calling this function. + * + * Each CPU is provided the entry point to is IDLE task when started. A + * TCB for each CPU's IDLE task has been initialized and placed in the + * CPU's g_assignedtasks[cpu] list. Not stack has been allocated or + * initialized. + * + * The OS initialization logic calls this function repeatedly until each + * CPU has been started, 1 through (CONFIG_SMP_NCPUS-1). + * + * Input Parameters: + * cpu - The index of the CPU being started. This will be a numeric + * value in the range of from one to (CONFIG_SMP_NCPUS-1). (CPU + * 0 is already active) + * + * Returned Value: + * Zero on success; a negated errno value on failure. + * + ****************************************************************************/ + +int up_cpu_start(int cpu) +{ + int i; + struct tcb_s *tcb = current_task(cpu); + uint32_t core1_boot_msg[5]; + + DPRINTF("cpu=%d\n", cpu); + +#ifdef CONFIG_SCHED_INSTRUMENTATION + /* Notify of the start event */ + + sched_note_cpu_start(this_task(), cpu); +#endif + + /* Reset Core 1 */ + + setbits_reg32(RP2040_PSM_PROC1, RP2040_PSM_FRCE_OFF); + while (!(getreg32(RP2040_PSM_FRCE_OFF) & RP2040_PSM_PROC1)) + ; + clrbits_reg32(RP2040_PSM_PROC1, RP2040_PSM_FRCE_OFF); + + spin_lock(&g_core1_boot); + + /* Send initial VTOR, MSP, PC for Core 1 boot */ + + core1_boot_msg[0] = 0; + core1_boot_msg[1] = 1; + core1_boot_msg[2] = getreg32(ARMV6M_SYSCON_VECTAB); + core1_boot_msg[3] = (uint32_t)tcb->adj_stack_ptr; + core1_boot_msg[4] = (uint32_t)core1_boot; + + do + { + fifo_drain(); + for (i = 0; i < 5; i++) + { + if (!fifo_comm(core1_boot_msg[i])) + { + break; + } + } + } + while (i < 5); + + fifo_drain(); + + /* Enable inter-processor FIFO interrupt */ + + irq_attach(RP2040_SIO_IRQ_PROC0, arm_pause_handler, NULL); + up_enable_irq(RP2040_SIO_IRQ_PROC0); + + spin_lock(&g_core1_boot); + + /* CPU Core 1 boot done */ + + spin_unlock(&g_core1_boot); + + return 0; +} + +#endif /* CONFIG_SMP */ diff --git a/arch/arm/src/rp2040/rp2040_irq.c b/arch/arm/src/rp2040/rp2040_irq.c index a94705e1eb..45f37a0ce6 100644 --- a/arch/arm/src/rp2040/rp2040_irq.c +++ b/arch/arm/src/rp2040/rp2040_irq.c @@ -48,6 +48,10 @@ (NVIC_SYSH_PRIORITY_DEFAULT << 24 | NVIC_SYSH_PRIORITY_DEFAULT << 16 | \ NVIC_SYSH_PRIORITY_DEFAULT << 8 | NVIC_SYSH_PRIORITY_DEFAULT) +#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7 +# define INTSTACK_ALLOC (CONFIG_SMP_NCPUS * INTSTACK_SIZE) +#endif + /**************************************************************************** * Public Data ****************************************************************************/ @@ -58,7 +62,37 @@ * CURRENT_REGS for portability. */ +#ifdef CONFIG_SMP +/* For the case of configurations with multiple CPUs, then there must be one + * such value for each processor that can receive an interrupt. + */ + +volatile uint32_t *g_current_regs[CONFIG_SMP_NCPUS]; +#else volatile uint32_t *g_current_regs[1]; +#endif + +#ifdef CONFIG_SMP +extern void rp2040_send_irqreq(int irqreq); +#endif + +#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7 +/* In the SMP configuration, we will need custom interrupt stacks. + * These definitions provide the aligned stack allocations. + */ + +static uint64_t g_intstack_alloc[INTSTACK_ALLOC >> 3]; + +/* These definitions provide the "top" of the push-down stacks. */ + +const uint32_t g_cpu_intstack_top[CONFIG_SMP_NCPUS] = +{ + (uint32_t)g_intstack_alloc + INTSTACK_SIZE, +#if CONFIG_SMP_NCPUS > 1 + (uint32_t)g_intstack_alloc + (2 * INTSTACK_SIZE), +#endif /* CONFIG_SMP_NCPUS > 1 */ +}; +#endif /* defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7 */ /* This is the address of the exception vector table (determined by the * linker script). @@ -273,6 +307,17 @@ void up_disable_irq(int irq) { DEBUGASSERT((unsigned)irq < NR_IRQS); +#ifdef CONFIG_SMP + if (irq >= RP2040_IRQ_EXTINT && irq != RP2040_SIO_IRQ_PROC1 && + up_cpu_index() != 0) + { + /* Must be handled by Core 0 */ + + rp2040_send_irqreq(-irq); + return; + } +#endif + /* Check for an external interrupt */ if (irq >= RP2040_IRQ_EXTINT && irq < RP2040_IRQ_EXTINT + 32) @@ -310,6 +355,17 @@ void up_enable_irq(int irq) DEBUGASSERT((unsigned)irq < NR_IRQS); +#ifdef CONFIG_SMP + if (irq >= RP2040_IRQ_EXTINT && irq != RP2040_SIO_IRQ_PROC1 && + up_cpu_index() != 0) + { + /* Must be handled by Core 0 */ + + rp2040_send_irqreq(irq); + return; + } +#endif + /* Check for external interrupt */ if (irq >= RP2040_IRQ_EXTINT && irq < RP2040_IRQ_EXTINT + 32) @@ -412,3 +468,35 @@ int up_prioritize_irq(int irq, int priority) return OK; } #endif + +/**************************************************************************** + * Name: arm_intstack_base + * + * Description: + * Return a pointer to the "base" the correct interrupt stack allocation + * for the current CPU. NOTE: Here, the base means "top" of the stack + * + ****************************************************************************/ + +#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7 +uintptr_t arm_intstack_base(void) +{ + return g_cpu_intstack_top[up_cpu_index()]; +} +#endif + +/**************************************************************************** + * Name: arm_intstack_alloc + * + * Description: + * Return a pointer to the "alloc" the correct interrupt stack allocation + * for the current CPU. + * + ****************************************************************************/ + +#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7 +uintptr_t arm_intstack_alloc(void) +{ + return g_cpu_intstack_top[up_cpu_index()] - INTSTACK_SIZE; +} +#endif diff --git a/arch/arm/src/rp2040/rp2040_irq.h b/arch/arm/src/rp2040/rp2040_irq.h index a6e0842580..d989ca5006 100644 --- a/arch/arm/src/rp2040/rp2040_irq.h +++ b/arch/arm/src/rp2040/rp2040_irq.h @@ -31,6 +31,12 @@ * Pre-processor Definitions ****************************************************************************/ +/* The size of one interrupt stack. This is the configured value aligned + * the 8-bytes as required by the ARM EABI. + */ + +#define INTSTACK_SIZE (CONFIG_ARCH_INTERRUPTSTACK & ~7) + /**************************************************************************** * Public Types ****************************************************************************/ @@ -39,12 +45,30 @@ * Public Data ****************************************************************************/ -/**************************************************************************** - * Inline Functions - ****************************************************************************/ +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif /**************************************************************************** * Public Function Prototypes ****************************************************************************/ +#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7 +EXTERN uintptr_t arm_intstack_base(void); +EXTERN uintptr_t arm_intstack_alloc(void); +#endif + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ #endif /* __ARCH_ARM_SRC_RP2040_RP2040_IRQ_H */ diff --git a/arch/arm/src/rp2040/rp2040_start.c b/arch/arm/src/rp2040/rp2040_start.c index 87423c2d9b..8c87ed403e 100644 --- a/arch/arm/src/rp2040/rp2040_start.c +++ b/arch/arm/src/rp2040/rp2040_start.c @@ -37,6 +37,7 @@ #include "rp2040_config.h" #include "rp2040_clock.h" #include "rp2040_uart.h" +#include "hardware/rp2040_sio.h" /**************************************************************************** * Pre-processor Definitions @@ -87,6 +88,15 @@ void __start(void) const uint32_t *src; #endif uint32_t *dest; + int i; + + if (up_cpu_index() != 0) + { + while (1) + { + __asm__ volatile ("wfe"); + } + } /* Clear .bss. We'll do this inline (vs. calling memset) just to be * certain that there are no issues with the state of global variables. @@ -97,10 +107,20 @@ void __start(void) *dest++ = 0; } - /* Configure the uart so that we can get debug output as soon as possible */ + /* Set up clock */ rp2040_clockconfig(); rp2040_boardearlyinitialize(); + + /* Initialize all spinlock states */ + + for (i = 0; i < 32; i++) + { + putreg32(0, RP2040_SIO_SPINLOCK(i)); + } + + /* Configure the uart so that we can get debug output as soon as possible */ + rp2040_lowsetup(); showprogress('A'); diff --git a/arch/arm/src/rp2040/rp2040_testset.c b/arch/arm/src/rp2040/rp2040_testset.c new file mode 100644 index 0000000000..df693bf38c --- /dev/null +++ b/arch/arm/src/rp2040/rp2040_testset.c @@ -0,0 +1,85 @@ +/**************************************************************************** + * arch/arm/src/rp2040/rp2040_testset.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include "hardware/rp2040_sio.h" + +#include "arm_arch.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define RP2040_TESTSET_SPINLOCK 0 /* Spinlock used for test and set */ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_testset + * + * Description: + * Perform and atomic test and set operation on the provided spinlock. + * This function must be provided via the architecture-specific logic. + * + * Input Parameters: + * lock - The address of spinlock object. + * + * Returned Value: + * The spinlock is always locked upon return. The value of previous value + * of the spinlock variable is returned, either SP_LOCKED if the spinlock + * as previously locked (meaning that the test-and-set operation failed to + * obtain the lock) or SP_UNLOCKED if the spinlock was previously unlocked + * (meaning that we successfully obtained the lock) + * + ****************************************************************************/ + +spinlock_t up_testset(volatile FAR spinlock_t *lock) +{ + spinlock_t ret; + + /* Lock hardware spinlock */ + + while (getreg32(RP2040_SIO_SPINLOCK(RP2040_TESTSET_SPINLOCK)) == 0) + ; + + ret = *lock; + + if (ret == SP_UNLOCKED) + { + *lock = SP_LOCKED; + SP_DMB(); + } + + /* Unlock hardware spinlock */ + + putreg32(0, RP2040_SIO_SPINLOCK(RP2040_TESTSET_SPINLOCK)); + + return ret; +} diff --git a/boards/arm/rp2040/raspberrypi-pico/configs/smp/defconfig b/boards/arm/rp2040/raspberrypi-pico/configs/smp/defconfig new file mode 100644 index 0000000000..f53d4bcc80 --- /dev/null +++ b/boards/arm/rp2040/raspberrypi-pico/configs/smp/defconfig @@ -0,0 +1,55 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON is not set +# CONFIG_LIBC_LONG_LONG is not set +# CONFIG_NSH_ARGCAT is not set +# CONFIG_NSH_CMDOPT_HEXDUMP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_LOSMART is not set +# CONFIG_NSH_DISABLE_PRINTF is not set +# CONFIG_NSH_DISABLE_TRUNCATE is not set +# CONFIG_STANDARD_SERIAL is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD="raspberrypi-pico" +CONFIG_ARCH_BOARD_RASPBERRYPI_PICO=y +CONFIG_ARCH_CHIP="rp2040" +CONFIG_ARCH_CHIP_RP2040=y +CONFIG_ARCH_RAMVECTORS=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=2988 +CONFIG_BUILTIN=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_EXAMPLES_HELLO=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_MAX_TASKS=16 +CONFIG_NFILE_DESCRIPTORS=6 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_READLINE=y +CONFIG_RAM_SIZE=270336 +CONFIG_RAM_START=0x20000000 +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SMP=y +CONFIG_SMP_NCPUS=2 +CONFIG_START_DAY=9 +CONFIG_START_MONTH=2 +CONFIG_START_YEAR=2021 +CONFIG_SYSLOG_CONSOLE=y +CONFIG_SYSTEM_NSH=y +CONFIG_TESTING_GETPRIME=y +CONFIG_TESTING_OSTEST=y +CONFIG_TESTING_SMP=y +CONFIG_UART0_SERIAL_CONSOLE=y +CONFIG_USER_ENTRYPOINT="nsh_main"