summaryrefslogtreecommitdiff
path: root/bootwrapper/c_start.c
blob: 148c0a3a3fdfc176f77ce0907dacd20a5d3a2a22 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
/*
 * Copyright (c) 2012, ARM Limited. All rights reserved.
 *       
 * Redistribution and use in source and binary forms, with
 * or without modification, are permitted provided that the
 * following conditions are met:
 *     
 * Redistributions of source code must retain the above
 * copyright notice, this list of conditions and the 
 * following disclaimer.
 *
 * 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.
 *      
 * Neither the name of ARM nor the names of its
 * contributors may be used to endorse or promote products
 * derived from this software without specific prior written
 * permission.                        
 */ 

#include <stdio.h>
#include <string.h>
#include "helpers.h"
#include "bootwrapper.h"
#include "vgic.h"

extern void *vector_table, *bl_sec_image;
extern void bl_image(unsigned, unsigned, unsigned, unsigned);
extern void kernel_start(int, int, int, int);

/* Reserve some stack space - 512 bytes per CPU -> 2048 bytes total */
unsigned stack[NUM_CPUS * STACK_SIZE];
unsigned stack_size = STACK_SIZE * sizeof(unsigned);
unsigned char cpus_ready[NUM_CLUSTERS][NUM_CPUS];
unsigned thumb = T2;
void (*kernel_start_address) (int, int, int, int);
volatile unsigned model_pen = 0;

unsigned gic_int_num(void)
{
        unsigned intcount = 0;

        intcount = read32(GIC_ID_PHY_BASE + GICD_CTR);
        intcount = ((intcount & 0x1F) + 1) * 32;

        return intcount;
}

/*
 * Function to configure the GIC ready for use in Non-Secure State
 */
void setup_gic_nonsecure(unsigned cluster_id, unsigned cpu_id)
{
        unsigned ctr = 0, num_ints = gic_int_num();

        /* Ensure all GIC interrupts are Non-Secure */
        write32(GIC_ID_PHY_BASE + GICD_SEC + (ctr << 2), 0xffffffff);   /* IRQs  0-31 are Non-Secure */
        if (cpu_id == 0 && cluster_id == 0) {
                for (ctr = 1; ctr <= (num_ints >> 5); ctr++)
                        write32(GIC_ID_PHY_BASE + GICD_SEC + (ctr << 2), 0xffffffff);   /* Set all SPIs as non-secure */
        }

        /* Ensure all interrupts can get through the priority mask */
        write32(GIC_IC_PHY_BASE + GICC_PRIMASK, 0xff);
}

/*
 * Function to send wakeup IPI to the secondary CPUs
 */
void kick(unsigned cpu_id, int secondary_cpus)
{
        int cpu_mask = ((1 << (secondary_cpus + 1)) - 1) & ~(1 << cpu_id);

        write32(VE_SYS_BASE + FLAGS_CLR, 0xffffffff);   // clear the flags register
        write32(VE_SYS_BASE + FLAGS_SET, (unsigned)start);      // set the start address in the flags register
        write32(GIC_ID_PHY_BASE, 0x1);  // turn on the GIC distributor
        write32(GIC_IC_PHY_BASE, 0x1);  // turn on the GIC CPU interface
        write32(GIC_ID_PHY_BASE + GICD_SW, cpu_mask << 16);     // send an interrupt to everyone else
}

/*
 * This function doesn't retun - it waits for an address to be 
 * written into the FLAGs register, then jumps to that address.
 */
void secondary_main(unsigned cluster_id, unsigned cpu_id)
{
        unsigned val;
        void (*secondary_start) (void);

        /* Ensure I cache is on and interrupts are masked */
        inv_icache_all();
        write_sctlr(read_sctlr() | (1 << 12));
        write_cpsr(read_cpsr() | 0x80);

        /* tell CPU0 that we are ready */
        cpus_ready[cluster_id][cpu_id] = 1;

        /*
         * We're not the primary core, so we need to wait for the primary
         * core to tell us what to do. While doing that, go into WFI so we
         * don't just sit here consuming system resources (i.e. bus
         * badwidth); make sure a soft IRQ gets through to the core, but
         * don't actually take the interrupt - that way we'll come out of
         * WFI without worrying about interrupt vectors (which may have gone
         * away, since the primary core is playing with our memory).
         */
        write32(GIC_IC_PHY_BASE + GICC_CTL, 0x1);       /* Enable GIC CPU Interface */
        write32(GIC_IC_PHY_BASE + GICC_PRIMASK, 0x000000f0);    /* Set Priority Mask to allow interrupts */

        /* If the start address isn't already set, go to sleep */
        while (val = read32(VE_SYS_BASE + FLAGS_SET), val == 0
               || val == (unsigned)start) {
                wfi();
                /* Acknowledge the interrupt that woke us */
                /* Read the Acknowledge register, write End Of Interrupt */
                write32(GIC_IC_PHY_BASE + GICC_EOI,
                        read32(GIC_IC_PHY_BASE + GICC_INTACK));
        }

        /* TODO: If MMU is enabled, then synchronise caches here */
        secondary_start = (void (*)())val;
        secondary_start();      /* No return from here */
}

void wait_for_secondaries(unsigned active_clusters, unsigned secondary_cpus)
{
        int i, j, ready;

        printf("Waiting for %d secondary CPUs\n", secondary_cpus);

        while (TRUE) {
                ready = 0;

		for (i = 0; i < active_clusters; i++)
		{
			for (j = 0; j < NUM_CPUS; j++) {
				if (cpus_ready[i][j]) {
					++ready;
				}
			}
		}

                if (ready == secondary_cpus) {
                        break;
                }

                /* Don't thrash the memory system, give the secondaries some time */
                for (j = 0; j < 1000; ++j) {
                        __nop();
                }
        }
}

/*
 * Function to determine number of clusters in the system.
 * The way to do this will differ on different systems.
 * Add support for the system you are running on.
 * At the moment, it supports Kingfisher dual cluster.
 */
int get_cluster_count(void)
{
    unsigned int kfs_id, active_clusters;
    int num_clusters = 0;

    kfs_id = read32(VE_KFSCB_BASE + KFS_ID_OFFSET);

    switch (((kfs_id & KFS_ID_ARCH_MASK) >> KFS_ID_ARCH_SHIFT))
    {
    case 0:
    case 1:
    case 2:
    case 3:
	    active_clusters = read32(VE_KFSCB_BASE + KFS_CFG_R_OFFSET) & ACTIVE_CLUSTER_MASK;
	    num_clusters = (active_clusters == 0x3) ? 2 : 1;
	    break;
    }

    return num_clusters;
}

void c_start(void)
{
        unsigned cpu_id = read_cpuid();
        unsigned cluster_id = read_clusterid();
	unsigned secondary_cpus = 0;
	unsigned platform = VERSATILE_EXPRESS;
	unsigned active_clusters = get_cluster_count();

	secondary_cpus = CLUSTER_CPU_COUNT(cluster_id) - 1;
	if (active_clusters > 1) {
		secondary_cpus += CLUSTER_CPU_COUNT(!cluster_id);
	}

        write_vbar((unsigned)&vector_table);
        write_mvbar((unsigned)&bl_sec_image);
        if (cpu_id == 0 && cluster_id == 0)
                config_uart();

        enable_user_perfmon_access();
        enable_perfmon();
        enable_swp();
        write_cpacr(read_cpacr() | 0xfffffff);
        write_nsacr(0x00073fff);
        enable_coherency();

        /* Also grant NS access to CCI registers */
        if (cpu_id == 0 && cluster_id == 0)
                write32(CCI_BASE + SECURE_ACCESS_REG, 0x1);

        /*
         * Secondaries wait here while initialisation of global peripherals is done
         */
        if (model_pen == 0 && (cpu_id || cluster_id)) {
                do {
                        wfe();
                } while (model_pen == 0);
        }

        setup_gic_nonsecure(cluster_id, cpu_id);

        if (cpu_id == 0 && cluster_id == 0) {
                model_pen = 1;
                dsb();
                sev();
        }

        enter_monitor_mode();
        write_scr(0x131);       /* HVC, NS, FW and AW bits  */
        write_cnthctl(PL1PCTEN | PL1PCEN);
        write_cntkctl(PL0PCTEN | PL0VCTEN | PL0VTEN | PL0PTEN);
        write_cntfrq(CP15_TIMER_FREQ);

        /* Start secondary CPUs, if any */
        if (cpu_id == 0 && cluster_id == 0 && secondary_cpus > 0) {
                printf("Kicking %d secondary CPU(s)\n", secondary_cpus);
                drain_uart_fifo();
                kick(cpu_id, secondary_cpus);
        }

        enter_nonsecure_world((unsigned)bl_image);

        /* Secondary CPUs go off to secondary_main() */
        if (cpu_id || cluster_id) {
                secondary_main(cluster_id, cpu_id); /* no return */
        }

        /* Primary waits for the secondaries to get ready before loading the payload */
        wait_for_secondaries(active_clusters, secondary_cpus);

        /* Load the payload kernel */
        printf("Kernel at 0x%x\n", kernel_start);

        if (thumb)
                kernel_start_address =
                        (void (*)(int, int, int, int))((unsigned)kernel_start | 1);
        else
                kernel_start_address = kernel_start;

        printf("Kernel entry point 0x%x (%s)\n", kernel_start_address,
               ((unsigned)kernel_start_address & 1) ? "thumb" : "arm");

        drain_uart_fifo();

        /* Clear FLAGS register, as this is what Linux expects to find */
        write32(VE_SYS_BASE + FLAGS_CLR, 0xffffffff);

        /* TODO: If MMU is enabled then caches need to be cleaned here */

        /* Start the kernel */
        kernel_start_address(0, platform & PLATFORM_MASK, 0, 0);        /* No return from here */

        return;
}