control.c 5.39 KB
Newer Older
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
#include "epicardium.h"
#include "api/dispatcher.h"
#include "api/interrupt-sender.h"
#include "modules/log.h"

#include "card10.h"

#include "max32665.h"
#include "sema.h"
#include "tmr.h"

static void __core1_init(void);

struct core1_info {
	/* Location of core1's interrupt vector table */
	volatile uintptr_t ivt_addr;
	/* Whether core 1 is ready for a new IVT */
	volatile bool ready;
};

/*
 * Information passing structure for controlling core 1.
 */
static volatile struct core1_info core1_info = {
	.ivt_addr = 0x00,
	.ready    = false,
};

/*
 * Minimal IVT needed for initial startup.  This IVT only contains the initial
 * stack pointer and reset-handler and is used to startup core 1.  Afterwards,
 * the payload's IVT is loaded into VTOR and used from then on.
 */
static uintptr_t core1_initial_ivt[] = {
	/* Initial Stack Pointer */
	0x20080000,
	/* Reset Handler */
	(uintptr_t)__core1_reset,
};

/*
 * Reset Handler
 *
 * Calls __core1_init() to reset & prepare the core for loading a new payload.
 */
__attribute__((naked)) void __core1_reset(void)
{
48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63
	/* Reset stack to MSP and set it to 0x20080000 */
	__asm volatile(
		"mov	r0, #0\n\t"
		"msr	control, r0\n\t"
		"mov	sp, %0\n\t"
		: /* No Outputs */
		: "r"(core1_initial_ivt[0])
		: "r0");

	/* Reset FPU */
	SCB->CPACR  = 0x00000000;
	FPU->FPDSCR = 0x00000000;
	FPU->FPCCR  = 0x00000000;
	__DSB();
	__ISB();

64 65 66 67 68 69 70 71 72 73 74 75 76
	__core1_init();
}

/*
 * Init core 1.  This function will reset the core and wait for a new IVT
 * address from Epicardium.  Once this address is received, it will start
 * execution with the supplied reset handler.
 */
void __core1_init(void)
{
	/*
	 * Clear any pending API interrupts.
	 */
77
	TMR_IntClear(MXC_TMR3);
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

	/*
	 * Reset Interrupts
	 *
	 * To ensure proper operation of the new payload, disable all interrupts
	 * and clear all pending ones.
	 */
	for (int i = 0; i < MXC_IRQ_EXT_COUNT; i++) {
		NVIC_DisableIRQ(i);
		NVIC_ClearPendingIRQ(i);
		NVIC_SetPriority(i, 0);
	}

	/*
	 * Check whether we catched the core during an interrupt.  If this is
	 * the case, try returning from the exception handler first and call
	 * __core1_reset() again in thread context.
	 */
	if ((SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) != 0) {
		/*
		 * Construct an exception frame so the CPU will jump back to our
		 * __core1_reset() function once we exit from the exception
		 * handler.
		 *
		 * To exit the exception, a special "EXC_RETURN" value is loaded
		 * into the link register and then branched to.
		 */
		__asm volatile(
			"ldr	r0, =0x41000000\n\t"
			"ldr	r1, =0\n\t"
			"push	{ r0 }\n\t" /* xPSR */
			"push	{ %0 }\n\t" /* PC */
			"push	{ %0 }\n\t" /* LR */
			"push	{ r1 }\n\t" /* R12 */
			"push	{ r1 }\n\t" /* R3 */
			"push	{ r1 }\n\t" /* R2 */
			"push	{ r1 }\n\t" /* R1 */
			"push	{ r1 }\n\t" /* R0 */

			"ldr	lr, =0xFFFFFFF9\n\t"
			"bx	lr\n\t"
			: /* No Outputs */
			: "r"((uintptr_t)__core1_reset)
			: "pc", "lr");

		/* unreachable */
		while (1)
			;
	}

	/* Wait for the IVT address */
	while (1) {
		while (SEMA_GetSema(_CONTROL_SEMAPHORE) == E_BUSY) {
		}

		__DMB();
		__ISB();

		/*
		 * The IVT address is reset to 0 by Epicardium before execution
		 * gets here.  Once a new address has been set, core 1 can use
		 * the new IVT.
		 */
		if (core1_info.ivt_addr != 0x00) {
			break;
		}

		/* Signal that we are ready for an IVT address */
		core1_info.ready = true;

148 149 150 151 152 153
		/*
		 * Reset the API interrupt so we never block Epicardium when it
		 * attempts to trigger an interrupt.
		 */
		API_CALL_MEM->int_id = (-1);

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
		SEMA_FreeSema(_CONTROL_SEMAPHORE);

		__WFE();
	}

	uintptr_t *ivt      = (uintptr_t *)core1_info.ivt_addr;
	core1_info.ivt_addr = 0x00;

	SEMA_FreeSema(_CONTROL_SEMAPHORE);

	/*
	 * Reset the call-flag before entering the payload so API calls behave
	 * properly.  This is necessary because epic_exec() will set the flag
	 * to "returning" on exit.
	 */
	API_CALL_MEM->call_flag = _API_FLAG_IDLE;

	/*
	 * Set the IVT
	 */
	SCB->VTOR = (uintptr_t)ivt;

	/*
	 * Clear any pending API interrupts.
	 */
179 180
	TMR_IntClear(MXC_TMR3);
	NVIC_ClearPendingIRQ(TMR3_IRQn);
181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201

	/*
	 * Jump to payload's reset handler
	 */
	__asm volatile(
		"ldr r0, %0\n\t"
		"blx r0\n\r"
		: /* No Outputs */
		: "m"(*(ivt + 1))
		: "r0");
}

void core1_boot(void)
{
	/*
	 * Boot using the initial IVT.  This will place core 1 into a loop,
	 * waiting for a payload.
	 */
	core1_start(&core1_initial_ivt);
}

202
void core1_trigger_reset(void)
203 204 205
{
	/* Signal core 1 that we intend to load a new payload. */
	api_interrupt_trigger(EPIC_INT_RESET);
206
}
207

208 209
void core1_wait_ready(void)
{
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
	/* Wait for the core to accept */
	while (1) {
		while (SEMA_GetSema(_CONTROL_SEMAPHORE) == E_BUSY) {
		}

		/*
		 * core 1 will set the ready flag once it is spinning in the
		 * above loop, waiting for a new IVT.
		 */
		if (core1_info.ready) {
			break;
		}

		SEMA_FreeSema(_CONTROL_SEMAPHORE);

		for (int i = 0; i < 10000; i++) {
		}
	}

	/*
	 * TODO: If the other core does not respond within a certain grace
	 * period, we need to force it into our desired state by overwriting
	 * all of its memory.  Yes, I don't like this method either ...
	 */
}

void core1_load(void *ivt, char *args)
{
	/* If the core is currently in an API call, reset it. */
	API_CALL_MEM->call_flag = _API_FLAG_IDLE;
	API_CALL_MEM->id        = 0;
	API_CALL_MEM->int_id    = (-1);

	api_prepare_args(args);

	core1_info.ivt_addr = (uintptr_t)ivt;
	core1_info.ready    = false;

	__DMB();
	__ISB();

	SEMA_FreeSema(_CONTROL_SEMAPHORE);

	__SEV();
	__WFE();
}