bool unpack_payload(uint8_t *message, int lenMes, uint8_t *payload, int lenPay) {
uint16_t crcMessage = 0;
uint16_t crcPayload = 0;
//Rebuild src:
crcMessage = message[lenMes - 3] << 8;
crcMessage &= 0xFF00;
crcMessage += message[lenMes - 2];
#ifdef DEBUG
DEBUGSERIAL.print("SRC received: "); DEBUGSERIAL.println(crcMessage);
#endif // DEBUG
//Extract payload:
memcpy(payload, &message[2], message[1]);
crcPayload = crc16(payload, message[1]);
#ifdef DEBUG
DEBUGSERIAL.print("SRC calc: "); DEBUGSERIAL.println(crcPayload);
#endif
if (crcPayload == crcMessage)
{
#ifdef DEBUG
DEBUGSERIAL.print("Received: "); SerialPrint(message, lenMes); DEBUGSERIAL.println();
DEBUGSERIAL.print("Payload : "); SerialPrint(payload, message[1] - 1); DEBUGSERIAL.println();
#endif // DEBUG
return true;
}
else
{
return false;
}
}
u16 MATCH(u16 measured, u16 desired) {
SerialPrint(UART,"Testing: ");
SerialPrint(UART,TICKS_LOW(desired), DEC);
SerialPrint(UART," <= ");
SerialPrint(UART,measured, DEC);
SerialPrint(UART," <= ");
SerialPrintln(TICKS_HIGH(desired), DEC);
return measured >= TICKS_LOW(desired) && measured <= TICKS_HIGH(desired);
}
int send_payload(uint8_t* payload, int lenPay) {
uint16_t crcPayload = crc16(payload, lenPay);
int count = 0;
uint8_t messageSend[256];
if (lenPay <= 256)
{
messageSend[count++] = 2;
messageSend[count++] = lenPay;
}
else
{
messageSend[count++] = 3;
messageSend[count++] = (uint8_t)(lenPay >> 8);
messageSend[count++] = (uint8_t)(lenPay & 0xFF);
}
memcpy(&messageSend[count], payload, lenPay);
count += lenPay;
messageSend[count++] = (uint8_t)(crcPayload >> 8);
messageSend[count++] = (uint8_t)(crcPayload & 0xFF);
messageSend[count++] = 3;
messageSend[count] = NULL;
//Sending package
SERIALIO.write(messageSend, count);
#ifdef DEBUG
DEBUGSERIAL.print("UART package send: "); SerialPrint(messageSend, count);
#endif // DEBUG
//Returns number of send bytes
return count;
}
void Delayus(UINT32 us)
{
UINT32 stop = ReadCoreTimer() + us * (UINT32)(FCP0 / 1000000UL);
#if 0 && defined(DEBUG)
SerialPrint("start = ");
SerialPrintNumber(ReadCoreTimer(), 10);
SerialPrint("\r\n");
SerialPrint("stop = ");
SerialPrintNumber(stop, 10);
SerialPrint("\r\n");
#endif
// valid only when using a signed type
while ((INT32) (ReadCoreTimer() - stop) < 0);
}
u16 MATCH_SPACE(u16 measured_ticks, u16 desired_us) {
SerialPrint(UART,"Testing space ");
SerialPrint(UART,measured_ticks * USECPERTICK, DEC);
SerialPrint(UART," vs ");
SerialPrint(UART,desired_us, DEC);
SerialPrint(UART,": ");
SerialPrint(UART,TICKS_LOW(desired_us - MARK_EXCESS), DEC);
SerialPrint(UART," <= ");
SerialPrint(UART,measured_ticks, DEC);
SerialPrint(UART," <= ");
SerialPrintln(TICKS_HIGH(desired_us - MARK_EXCESS), DEC);
return measured_ticks >= TICKS_LOW(desired_us - MARK_EXCESS) && measured_ticks <= TICKS_HIGH(desired_us - MARK_EXCESS);
}
void FlashOperation(UINT32 op, void* addr, UINT32 data32)
{
//UINT32 status;
// NVMADDR only accept Physical Address
NVMADDR = ConvertToPhysicalAddress(addr);
#if 0 && defined(DEBUG)
SerialPrint("FlashOperation / ");
if (op == FLASH_WORD_WRITE)
{
SerialPrint("Write Address 0x");
SerialPrintNumber(NVMADDR,16);
SerialPrint("\r\n");
mLED_1_Toggle();
}
if (op == FLASH_PAGE_ERASE)
{
SerialPrint("Erase Address 0x");
SerialPrintNumber(NVMADDR,16);
SerialPrint("\r\n");
mLED_2_Toggle();
}
#endif
// Load data into NVMDATA register
NVMDATA = data32;
// Suspend or Disable all Interrupts
//status = DisableInterrupt();
// Enable Flash Write/Erase Operations
// 1-Select Flash operation to perform
// Enable writes to WR bit and LVD circuit
NVMCON = _NVMCON_WREN_MASK | op;
// 2-Wait for LVD to become stable (at least 6us).
Delayus(7);
// 3-Write unlock sequence before the WR bit is set
NVMKEY = 0xAA996655;
NVMKEY = 0x556699AA;
// 4-Start the operation (WR=1)
NVMCONSET = _NVMCON_WR_MASK;
// 5-Wait for operation to complete (WR=0)
while (NVMCON & _NVMCON_WR_MASK);
// 6-Disable Flash Write/Erase operations
NVMCONCLR = _NVMCON_WREN_MASK;
// Restore Interrupts if necessary
#if 0
if (status & 1)
{
EnableInterrupt();
}
else
DisableInterrupt();
#endif
#if 0
// Return NVMERR and LVDERR Error Status Bits
if (NVMCON & (_NVMCON_WRERR_MASK | _NVMCON_LVDERR_MASK))
{
while (1)
{
mLED_2_Toggle();
DelayUs(500);
}
}
#endif
}
VOID
CEntryPoint (
IN UINTN MpId,
IN UINTN SecBootMode
)
{
CHAR8 Buffer[100];
UINTN CharCount;
UINTN JumpAddress;
// Invalidate the data cache. Doesn't have to do the Data cache clean.
ArmInvalidateDataCache ();
// Invalidate Instruction Cache
ArmInvalidateInstructionCache ();
// Invalidate I & D TLBs
ArmInvalidateInstructionAndDataTlb ();
// CPU specific settings
ArmCpuSetup (MpId);
// Enable Floating Point Coprocessor if supported by the platform
if (FixedPcdGet32 (PcdVFPEnabled)) {
ArmEnableVFP ();
}
// Initialize peripherals that must be done at the early stage
// Example: Some L2 controller, interconnect, clock, DMC, etc
ArmPlatformSecInitialize (MpId);
// Primary CPU clears out the SCU tag RAMs, secondaries wait
if (ArmPlatformIsPrimaryCore (MpId) && (SecBootMode == ARM_SEC_COLD_BOOT)) {
if (ArmIsMpCore()) {
// Signal for the initial memory is configured (event: BOOT_MEM_INIT)
ArmCallSEV ();
}
// SEC phase needs to run library constructors by hand. This assumes we are linked against the SerialLib
// In non SEC modules the init call is in autogenerated code.
SerialPortInitialize ();
// Start talking
if (FixedPcdGetBool (PcdTrustzoneSupport)) {
CharCount = AsciiSPrint (Buffer,sizeof (Buffer),"Secure firmware (version %s built at %a on %a)\n\r",
(CHAR16*)PcdGetPtr(PcdFirmwareVersionString), __TIME__, __DATE__);
} else {
CharCount = AsciiSPrint (Buffer,sizeof (Buffer),"Boot firmware (version %s built at %a on %a)\n\r",
(CHAR16*)PcdGetPtr(PcdFirmwareVersionString), __TIME__, __DATE__);
}
SerialPortWrite ((UINT8 *) Buffer, CharCount);
// Initialize the Debug Agent for Source Level Debugging
InitializeDebugAgent (DEBUG_AGENT_INIT_PREMEM_SEC, NULL, NULL);
SaveAndSetDebugTimerInterrupt (TRUE);
// Enable the GIC distributor and CPU Interface
// - no other Interrupts are enabled, doesn't have to worry about the priority.
// - all the cores are in secure state, use secure SGI's
ArmGicEnableDistributor (PcdGet32(PcdGicDistributorBase));
ArmGicEnableInterruptInterface (PcdGet32(PcdGicInterruptInterfaceBase));
} else {
// Enable the GIC CPU Interface
ArmGicEnableInterruptInterface (PcdGet32(PcdGicInterruptInterfaceBase));
}
// Enable Full Access to CoProcessors
ArmWriteCpacr (CPACR_CP_FULL_ACCESS);
// Test if Trustzone is supported on this platform
if (FixedPcdGetBool (PcdTrustzoneSupport)) {
if (ArmIsMpCore ()) {
// Setup SMP in Non Secure world
ArmCpuSetupSmpNonSecure (GET_CORE_ID(MpId));
}
// Either we use the Secure Stacks for Secure Monitor (in this case (Base == 0) && (Size == 0))
// Or we use separate Secure Monitor stacks (but (Base != 0) && (Size != 0))
ASSERT (((PcdGet32(PcdCPUCoresSecMonStackBase) == 0) && (PcdGet32(PcdCPUCoreSecMonStackSize) == 0)) ||
((PcdGet32(PcdCPUCoresSecMonStackBase) != 0) && (PcdGet32(PcdCPUCoreSecMonStackSize) != 0)));
// Enter Monitor Mode
enter_monitor_mode (
(UINTN)TrustedWorldInitialization, MpId, SecBootMode,
(VOID*) (PcdGet32 (PcdCPUCoresSecMonStackBase) +
(PcdGet32 (PcdCPUCoreSecMonStackSize) * (ArmPlatformGetCorePosition (MpId) + 1)))
);
} else {
if (ArmPlatformIsPrimaryCore (MpId)) {
SerialPrint ("Trust Zone Configuration is disabled\n\r");
}
// With Trustzone support the transition from Sec to Normal world is done by return_from_exception().
// If we want to keep this function call we need to ensure the SVC's SPSR point to the same Program
// Status Register as the the current one (CPSR).
copy_cpsr_into_spsr ();
// Call the Platform specific function to execute additional actions if required
JumpAddress = PcdGet32 (PcdFvBaseAddress);
ArmPlatformSecExtraAction (MpId, &JumpAddress);
NonTrustedWorldTransition (MpId, JumpAddress);
}
ASSERT (0); // We must never return from the above function
}
