arm/mach-shmobile/headsmp.S

0001 /* SPDX-License-Identifier: GPL-2.0
0002  *
0003  * SMP support for R-Mobile / SH-Mobile
0004  *
0005  * Copyright (C) 2010  Magnus Damm
0006  * Copyright (C) 2010  Takashi Yoshii
0007  *
0008  * Based on vexpress, Copyright (c) 2003 ARM Limited, All Rights Reserved
0009  */
0010 #include <linux/init.h>
0011 #include <linux/linkage.h>
0012 #include <linux/threads.h>
0013 #include <asm/assembler.h>
0014 #include <asm/memory.h>
0015
0016 #define SCTLR_MMU   0x01
0017 #define BOOTROM_ADDRESS 0xE6340000
0018 #define RWTCSRA_ADDRESS 0xE6020004
0019 #define RWTCSRA_WOVF    0x10
0020
0021 /*
0022  * Reset vector for secondary CPUs.
0023  * This will be mapped at address 0 by SBAR register.
0024  * We need _long_ jump to the physical address.
0025  */
0026     .arm
0027     .align  12
0028 ENTRY(shmobile_boot_vector)
0029     ldr     r1, 1f
0030     bx  r1
0031
0032 ENDPROC(shmobile_boot_vector)
0033
0034     .align  2
0035     .globl  shmobile_boot_fn
0036 shmobile_boot_fn:
0037 1:  .space  4
0038     .globl  shmobile_boot_size
0039 shmobile_boot_size:
0040     .long   . - shmobile_boot_vector
0041
0042 #ifdef CONFIG_ARCH_RCAR_GEN2
0043 /*
0044  * Reset vector for R-Car Gen2 and RZ/G1 secondary CPUs.
0045  * This will be mapped at address 0 by SBAR register.
0046  */
0047 ENTRY(shmobile_boot_vector_gen2)
0048     mrc p15, 0, r0, c0, c0, 5       @ r0 = MPIDR
0049     ldr r1, shmobile_boot_cpu_gen2
0050     cmp r0, r1
0051     bne shmobile_smp_continue_gen2
0052
0053     mrc p15, 0, r1, c1, c0, 0       @ r1 = SCTLR
0054     and r0, r1, #SCTLR_MMU
0055     cmp r0, #SCTLR_MMU
0056     beq shmobile_smp_continue_gen2
0057
0058     ldr r0, rwtcsra
0059     mov r1, #0
0060     ldrb    r1, [r0]
0061     and r0, r1, #RWTCSRA_WOVF
0062     cmp r0, #RWTCSRA_WOVF
0063     bne shmobile_smp_continue_gen2
0064
0065     ldr r0, bootrom
0066     bx  r0
0067
0068 shmobile_smp_continue_gen2:
0069     ldr     r1, shmobile_boot_fn_gen2
0070     bx  r1
0071
0072 ENDPROC(shmobile_boot_vector_gen2)
0073
0074     .align  4
0075 rwtcsra:
0076     .word   RWTCSRA_ADDRESS
0077 bootrom:
0078     .word   BOOTROM_ADDRESS
0079     .globl  shmobile_boot_cpu_gen2
0080 shmobile_boot_cpu_gen2:
0081     .word   0x00000000
0082
0083     .align  2
0084     .globl  shmobile_boot_fn_gen2
0085 shmobile_boot_fn_gen2:
0086     .space  4
0087     .globl  shmobile_boot_size_gen2
0088 shmobile_boot_size_gen2:
0089     .long   . - shmobile_boot_vector_gen2
0090 #endif /* CONFIG_ARCH_RCAR_GEN2 */
0091
0092 /*
0093  * Per-CPU SMP boot function/argument selection code based on MPIDR
0094  */
0095
0096 ENTRY(shmobile_smp_boot)
0097     mrc p15, 0, r1, c0, c0, 5       @ r1 = MPIDR
0098     and r0, r1, #0xffffff       @ MPIDR_HWID_BITMASK
0099                         @ r0 = cpu_logical_map() value
0100     mov r1, #0              @ r1 = CPU index
0101     adr r2, 1f
0102     ldmia   r2, {r5, r6, r7}
0103     add r5, r5, r2          @ array of per-cpu mpidr values
0104     add r6, r6, r2          @ array of per-cpu functions
0105     add r7, r7, r2          @ array of per-cpu arguments
0106
0107 shmobile_smp_boot_find_mpidr:
0108     ldr r8, [r5, r1, lsl #2]
0109     cmp r8, r0
0110     bne shmobile_smp_boot_next
0111
0112     ldr r9, [r6, r1, lsl #2]
0113     cmp r9, #0
0114     bne shmobile_smp_boot_found
0115
0116 shmobile_smp_boot_next:
0117     add r1, r1, #1
0118     cmp r1, #NR_CPUS
0119     blo shmobile_smp_boot_find_mpidr
0120
0121     b   shmobile_smp_sleep
0122
0123 shmobile_smp_boot_found:
0124     ldr r0, [r7, r1, lsl #2]
0125     ret r9
0126 ENDPROC(shmobile_smp_boot)
0127
0128 ENTRY(shmobile_smp_sleep)
0129     wfi
0130     b   shmobile_smp_boot
0131 ENDPROC(shmobile_smp_sleep)
0132
0133     .align  2
0134 1:  .long   shmobile_smp_mpidr - .
0135     .long   shmobile_smp_fn - 1b
0136     .long   shmobile_smp_arg - 1b
0137
0138     .bss
0139     .globl  shmobile_smp_mpidr
0140 shmobile_smp_mpidr:
0141     .space  NR_CPUS * 4
0142     .globl  shmobile_smp_fn
0143 shmobile_smp_fn:
0144     .space  NR_CPUS * 4
0145     .globl  shmobile_smp_arg
0146 shmobile_smp_arg:
0147     .space  NR_CPUS * 4