| 1 | --- a/arch/arm/mach-cns3xxx/platsmp.c |
| 2 | +++ b/arch/arm/mach-cns3xxx/platsmp.c |
| 3 | @@ -1,18 +1,17 @@ |
| 4 | -/* linux/arch/arm/mach-cns3xxx/platsmp.c |
| 5 | +/* |
| 6 | + * linux/arch/arm/mach-cns3xxx/platsmp.c |
| 7 | * |
| 8 | - * Copyright 2011 Gateworks Corporation |
| 9 | + * Copyright (C) 2002 ARM Ltd. |
| 10 | + * Copyright 2012 Gateworks Corporation |
| 11 | * Chris Lang <clang@gateworks.com> |
| 12 | + * Tim Harvey <tharvey@gateworks.com> |
| 13 | * |
| 14 | - * Cloned from linux/arch/arm/mach-vexpress/platsmp.c |
| 15 | - * |
| 16 | - * Copyright (C) 2002 ARM Ltd. |
| 17 | * All Rights Reserved |
| 18 | * |
| 19 | * This program is free software; you can redistribute it and/or modify |
| 20 | * it under the terms of the GNU General Public License version 2 as |
| 21 | * published by the Free Software Foundation. |
| 22 | -*/ |
| 23 | - |
| 24 | + */ |
| 25 | #include <linux/init.h> |
| 26 | #include <linux/errno.h> |
| 27 | #include <linux/delay.h> |
| 28 | @@ -30,11 +29,13 @@ |
| 29 | |
| 30 | extern void cns3xxx_secondary_startup(void); |
| 31 | |
| 32 | +#define SCU_CPU_STATUS 0x08 |
| 33 | +static void __iomem *scu_base; |
| 34 | + |
| 35 | /* |
| 36 | * control for which core is the next to come out of the secondary |
| 37 | * boot "holding pen" |
| 38 | */ |
| 39 | - |
| 40 | volatile int __cpuinitdata pen_release = -1; |
| 41 | |
| 42 | /* |
| 43 | @@ -50,11 +51,6 @@ static void write_pen_release(int val) |
| 44 | outer_clean_range(__pa(&pen_release), __pa(&pen_release + 1)); |
| 45 | } |
| 46 | |
| 47 | -static void __iomem *scu_base_addr(void) |
| 48 | -{ |
| 49 | - return (void __iomem *)(CNS3XXX_TC11MP_SCU_BASE_VIRT); |
| 50 | -} |
| 51 | - |
| 52 | static DEFINE_SPINLOCK(boot_lock); |
| 53 | |
| 54 | void __cpuinit platform_secondary_init(unsigned int cpu) |
| 55 | @@ -128,25 +124,24 @@ int __cpuinit boot_secondary(unsigned in |
| 56 | * Initialise the CPU possible map early - this describes the CPUs |
| 57 | * which may be present or become present in the system. |
| 58 | */ |
| 59 | - |
| 60 | void __init smp_init_cpus(void) |
| 61 | { |
| 62 | - void __iomem *scu_base = scu_base_addr(); |
| 63 | unsigned int i, ncores; |
| 64 | + unsigned int status; |
| 65 | |
| 66 | - ncores = scu_base ? scu_get_core_count(scu_base) : 1; |
| 67 | + scu_base = (void __iomem *) CNS3XXX_TC11MP_SCU_BASE_VIRT; |
| 68 | |
| 69 | - /* sanity check */ |
| 70 | - if (ncores > NR_CPUS) { |
| 71 | - printk(KERN_WARNING |
| 72 | - "cns3xxx: no. of cores (%d) greater than configured " |
| 73 | - "maximum of %d - clipping\n", |
| 74 | - ncores, NR_CPUS); |
| 75 | - ncores = NR_CPUS; |
| 76 | + /* for CNS3xxx SCU_CPU_STATUS must be examined instead of SCU_CONFIGURATION |
| 77 | + * used in scu_get_core_count |
| 78 | + */ |
| 79 | + status = __raw_readl(scu_base + SCU_CPU_STATUS); |
| 80 | + for (i = 0; i < NR_CPUS+1; i++) { |
| 81 | + if (((status >> (i*2)) & 0x3) == 0) |
| 82 | + set_cpu_possible(i, true); |
| 83 | + else |
| 84 | + break; |
| 85 | } |
| 86 | - |
| 87 | - for (i = 0; i < ncores; i++) |
| 88 | - set_cpu_possible(i, true); |
| 89 | + ncores = i; |
| 90 | |
| 91 | set_smp_cross_call(gic_raise_softirq); |
| 92 | } |
| 93 | @@ -159,10 +154,14 @@ void __init platform_smp_prepare_cpus(un |
| 94 | * Initialise the present map, which describes the set of CPUs |
| 95 | * actually populated at the present time. |
| 96 | */ |
| 97 | - for (i = 0; i < max_cpus; i++) |
| 98 | + for (i = 0; i < max_cpus; i++) { |
| 99 | set_cpu_present(i, true); |
| 100 | + } |
| 101 | |
| 102 | - scu_enable(scu_base_addr()); |
| 103 | + /* |
| 104 | + * enable SCU |
| 105 | + */ |
| 106 | + scu_enable(scu_base); |
| 107 | |
| 108 | /* |
| 109 | * Write the address of secondary startup into the |
| 110 | |
