qoriq/start/bspsmp.c

0001 /* SPDX-License-Identifier: BSD-2-Clause */
0002
0003 /*
0004  * Copyright (C) 2013, 2017 embedded brains GmbH & Co. KG
0005  *
0006  * Redistribution and use in source and binary forms, with or without
0007  * modification, are permitted provided that the following conditions
0008  * are met:
0009  * 1. Redistributions of source code must retain the above copyright
0010  *    notice, this list of conditions and the following disclaimer.
0011  * 2. Redistributions in binary form must reproduce the above copyright
0012  *    notice, this list of conditions and the following disclaimer in the
0013  *    documentation and/or other materials provided with the distribution.
0014  *
0015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
0016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
0017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
0018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
0019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
0020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
0021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
0022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
0023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
0024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
0025  * POSSIBILITY OF SUCH DAMAGE.
0026  */
0027
0028 #include <rtems/score/smpimpl.h>
0029
0030 #include <libfdt.h>
0031
0032 #include <libcpu/powerpc-utility.h>
0033
0034 #include <bsp.h>
0035 #include <bsp/fdt.h>
0036 #include <bsp/mmu.h>
0037 #include <bsp/fatal.h>
0038 #include <bsp/qoriq.h>
0039 #include <bsp/vectors.h>
0040 #include <bsp/bootcard.h>
0041 #include <bsp/irq-generic.h>
0042 #include <bsp/linker-symbols.h>
0043
0044 LINKER_SYMBOL(bsp_exc_vector_base);
0045
0046 #if QORIQ_THREAD_COUNT > 1
0047 void _start_thread(void);
0048 #endif
0049
0050 void _start_secondary_processor(void);
0051
0052 #define IPI_INDEX 0
0053
0054 #if QORIQ_THREAD_COUNT > 1
0055 static bool is_started_by_u_boot(uint32_t cpu_index)
0056 {
0057   return cpu_index % QORIQ_THREAD_COUNT == 0;
0058 }
0059
0060 void qoriq_start_thread(Per_CPU_Control *cpu_self)
0061 {
0062   ppc_exc_initialize_interrupt_stack(
0063     (uintptr_t) cpu_self->interrupt_stack_low
0064   );
0065
0066   bsp_interrupt_facility_initialize();
0067
0068   _SMP_Start_multitasking_on_secondary_processor(cpu_self);
0069 }
0070 #endif
0071
0072 static void start_thread_if_necessary(uint32_t cpu_index_self)
0073 {
0074 #if QORIQ_THREAD_COUNT > 1
0075   uint32_t i;
0076
0077   for (i = 1; i < QORIQ_THREAD_COUNT; ++i) {
0078     uint32_t cpu_index_next = cpu_index_self + i;
0079
0080     if (
0081       is_started_by_u_boot(cpu_index_self)
0082         && cpu_index_next < rtems_configuration_get_maximum_processors()
0083         && _SMP_Should_start_processor(cpu_index_next)
0084     ) {
0085       /* Thread Initial Next Instruction Address (INIA) */
0086       PPC_SET_THREAD_MGMT_REGISTER(321, (uintptr_t) _start_thread);
0087
0088       /* Thread Initial Machine State (IMSR) */
0089       PPC_SET_THREAD_MGMT_REGISTER(289, QORIQ_INITIAL_MSR);
0090
0091       /* Thread Enable Set (TENS) */
0092       PPC_SET_SPECIAL_PURPOSE_REGISTER(FSL_EIS_TENS, 1U << i);
0093     }
0094   }
0095 #endif
0096 }
0097
0098 void bsp_start_on_secondary_processor(Per_CPU_Control *cpu_self)
0099 {
0100   uint32_t cpu_index_self;
0101
0102   qoriq_initialize_exceptions(cpu_self->interrupt_stack_low);
0103   bsp_interrupt_facility_initialize();
0104
0105   cpu_index_self = _Per_CPU_Get_index(cpu_self);
0106   start_thread_if_necessary(cpu_index_self);
0107
0108   _SMP_Start_multitasking_on_secondary_processor(cpu_self);
0109 }
0110
0111 #ifndef QORIQ_IS_HYPERVISOR_GUEST
0112 static void bsp_inter_processor_interrupt(void *arg)
0113 {
0114   _SMP_Inter_processor_interrupt_handler(_Per_CPU_Get());
0115 }
0116 #endif
0117
0118 static void setup_boot_page(void)
0119 {
0120 #ifdef QORIQ_IS_HYPERVISOR_GUEST
0121   qoriq_mmu_adjust_and_write_to_tlb1(
0122     QORIQ_TLB1_ENTRY_COUNT - 1,
0123     0xfffff000,
0124     0xffffffff,
0125     0,
0126     0,
0127     FSL_EIS_MAS3_SR | FSL_EIS_MAS3_SW,
0128     0
0129   );
0130 #endif
0131 }
0132
0133 static uint32_t discover_processors(void)
0134 {
0135   const void *fdt = bsp_fdt_get();
0136   int cpus = fdt_path_offset(fdt, "/cpus");
0137   int node = fdt_first_subnode(fdt, cpus);
0138   uint32_t cpu = 0;
0139
0140   while (node >= 0 && cpu < RTEMS_ARRAY_SIZE(qoriq_start_spin_table_addr)) {
0141     int len;
0142     fdt64_t *addr_fdt = (fdt64_t *)
0143       fdt_getprop(fdt, node, "cpu-release-addr", &len);
0144
0145     if (addr_fdt != NULL) {
0146       uintptr_t addr = (uintptr_t) fdt64_to_cpu(*addr_fdt);
0147
0148       qoriq_start_spin_table_addr[cpu] = (qoriq_start_spin_table *) addr;
0149     }
0150
0151     ++cpu;
0152     node = fdt_next_subnode(fdt, node);
0153   }
0154
0155   return cpu * QORIQ_THREAD_COUNT;
0156 }
0157
0158 uint32_t _CPU_SMP_Initialize(void)
0159 {
0160   uint32_t cpu_count = 1;
0161
0162   if (rtems_configuration_get_maximum_processors() > 0) {
0163     setup_boot_page();
0164     cpu_count = discover_processors();
0165   }
0166
0167   start_thread_if_necessary(0);
0168
0169   return cpu_count;
0170 }
0171
0172 static bool release_processor(
0173   qoriq_start_spin_table *spin_table,
0174   uint32_t cpu_index
0175 )
0176 {
0177   bool spin_table_present = (spin_table != NULL);
0178
0179   if (spin_table_present) {
0180     const Per_CPU_Control *cpu = _Per_CPU_Get_by_index(cpu_index);
0181
0182     spin_table->pir = cpu_index;
0183     spin_table->r3 = (uintptr_t) cpu->interrupt_stack_high;
0184     rtems_cache_flush_multiple_data_lines(spin_table, sizeof(*spin_table));
0185     ppc_synchronize_data();
0186     spin_table->addr = (uintptr_t) _start_secondary_processor;
0187     rtems_cache_flush_multiple_data_lines(spin_table, sizeof(*spin_table));
0188   }
0189
0190   return spin_table_present;
0191 }
0192
0193 static qoriq_start_spin_table *get_spin_table(uint32_t cpu_index)
0194 {
0195   qoriq_start_spin_table *spin_table;
0196
0197   spin_table = qoriq_start_spin_table_addr[cpu_index / QORIQ_THREAD_COUNT];
0198
0199   return spin_table;
0200 }
0201
0202 bool _CPU_SMP_Start_processor(uint32_t cpu_index)
0203 {
0204 #if QORIQ_THREAD_COUNT > 1
0205   if (is_started_by_u_boot(cpu_index)) {
0206     qoriq_start_spin_table *spin_table = get_spin_table(cpu_index);
0207
0208     return release_processor(spin_table, cpu_index);
0209   } else {
0210     return _SMP_Should_start_processor(cpu_index - 1);
0211   }
0212 #else
0213   qoriq_start_spin_table *spin_table = get_spin_table(cpu_index);
0214
0215   return release_processor(spin_table, cpu_index);
0216 #endif
0217 }
0218
0219 static rtems_interrupt_entry qoriq_ipi_entry;
0220
0221 void _CPU_SMP_Finalize_initialization(uint32_t cpu_count)
0222 {
0223 #ifndef QORIQ_IS_HYPERVISOR_GUEST
0224   rtems_status_code sc;
0225
0226   rtems_interrupt_entry_initialize(
0227     &qoriq_ipi_entry,
0228     bsp_inter_processor_interrupt,
0229     NULL,
0230     "IPI"
0231   );
0232   sc = rtems_interrupt_entry_install(
0233     QORIQ_IRQ_IPI_0 + IPI_INDEX,
0234     RTEMS_INTERRUPT_UNIQUE,
0235     &qoriq_ipi_entry
0236   );
0237   if (sc != RTEMS_SUCCESSFUL) {
0238     bsp_fatal(QORIQ_FATAL_SMP_IPI_HANDLER_INSTALL);
0239   }
0240 #endif
0241
0242   (void) cpu_count;
0243 }
0244
0245 void _CPU_SMP_Prepare_start_multitasking(void)
0246 {
0247   /* Do nothing */
0248 }
0249
0250 void _CPU_SMP_Send_interrupt(uint32_t target_processor_index)
0251 {
0252 #ifdef QORIQ_IS_HYPERVISOR_GUEST
0253   uint32_t msg;
0254
0255   /* DBELL message type */
0256   msg = (0U << (63 - 36)) | target_processor_index;
0257   __asm__ volatile ("msgsnd %0" : : "r" (msg));
0258 #else
0259   qoriq.pic.ipidr [IPI_INDEX].reg = 1U << target_processor_index;
0260 #endif
0261 }