#include "stdint.h"

extern void __ghs_mpc5748g_cpu1_entry(void);
extern void __ghs_mpc5748g_cpu2_entry(void);

#define ME_32(off) ((volatile uint32_t*) 0xfffb8000)[off/4]
#define ME_16(off) ((volatile uint16_t*) 0xfffb8000)[off/2]

void boot_core_1(void) {
    if (ME_32(0x1c0) & 0x4) { // ME_CS
	return;
    }
    ME_16(0x1c8) = 0x00fe; // ME_CCTL2 (z4b)
    ME_32(0x1e8) = ((uint32_t)__ghs_mpc5748g_cpu1_entry | 0x1); // ME_CADDR2 (z4b)
    ME_32(0x4) = 0x30005af0; // ME_MCTL
    ME_32(0x4) = 0x3000a50f; // ME_MCTL
    while (ME_32(0x4) & 0x08000000) {}; // wait for ME_MCTL[MTRANS]
}

void boot_core_2(void) {
    if (ME_32(0x1c0) & 0x8) { // ME_CS
	return;
    }
    ME_16(0x1ca) = 0x00fe; // ME_CCTL3 (z2)
    ME_32(0x1ec) = ((uint32_t)__ghs_mpc5748g_cpu2_entry | 0x1); // ME_CADDR3 (z2)
    ME_32(0x4) = 0x30005af0; // ME_MCTL
    ME_32(0x4) = 0x3000a50f; // ME_MCTL
    while (ME_32(0x4) & 0x08000000) {}; // wait for ME_MCTL[MTRANS]
}
