1/*
2 * Copyright 2019-2020 NXP
3 * All rights reserved.
4 *
5 * SPDX-License-Identifier: BSD-3-Clause
6 */
7
8void PrepareTrapCode(void) {
9    unsigned int start;
10    start = 0x20200000;
11
12    // Prepare stack
13    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, start);
14    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_DATA, start + 0x20);
15
16    // Prepare spin code provided by ROM
17    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, start + 0x4);
18    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_DATA, 0x23F041);
19
20    // Configure LPSR_GPR0 and LPSR_GPR1 for CM4 init vector
21    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, 0x40c0c000);
22    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_DATA, start & 0xFFFF);
23
24    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, 0x40C0c004);
25    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_DATA, (start & 0xFFFF0000) >> 16);
26}
27
28void InitTarget(void) {
29    CPU = CORTEX_M7;
30    // Manually configure AP
31    JLINK_CORESIGHT_AddAP(0, CORESIGHT_AHB_AP);
32    JLINK_CORESIGHT_AddAP(1, CORESIGHT_AHB_AP);
33    JLINK_CORESIGHT_AddAP(2, CORESIGHT_APB_AP);
34
35    // Dummy read
36    JLINK_CORESIGHT_ReadAP(JLINK_CORESIGHT_AP_REG_IDR);
37    SYS_Sleep(10);
38
39    PrepareTrapCode();
40    // Release CM4 from SRC
41    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, 0x40C04000);
42    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_DATA, 1);
43
44    // Disable system reset caused by sysrstreq from each core
45    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, 0x40C04004);
46    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_DATA, 0xF << 10);
47
48    // Switch to AP1
49    CPU = CORTEX_M4;
50    CORESIGHT_IndexAHBAPToUse = 1;
51
52    // SetSkipRestoreRAMCode command is used to skip the restoring of the RAMCode
53    JLINK_ExecCommand("SetSkipRestoreRAMCode = 1");
54}
55
56void CM4VectReset()
57{
58    MEM_WriteU32(0xE000ED0C, 0x5FA0001);
59    SYS_Sleep(10);
60    Report("CM4 Vector Reset");
61}
62
63void CM4SrcReset()
64{
65    unsigned int t;
66
67    /* Issue M4 reset */
68    MEM_WriteU32(0x40c04284, 1);
69
70    /* Check M4 reset status */
71    t = MEM_ReadU32(0x40c04290);
72    t &= 0x1;
73    while (t)
74    {
75        t = MEM_ReadU32(0x40c04290);
76        t &= 0x1;
77    }
78    SYS_Sleep(10);
79    Report("CM4 SRC reset");
80
81}
82
83void ResetCM4()
84{
85    CM4SrcReset();
86}
87
88void ResetTarget()
89{
90    CORESIGHT_IndexAHBAPToUse = 0;
91    ResetCM4();
92    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, 0x40C04000);
93    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_DATA, 1);
94    CORESIGHT_IndexAHBAPToUse = 1;
95}
96
97