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 SDRAM_WaitIpCmdDone()
29{
30    unsigned int reg;
31    do
32    {
33        reg = MEM_ReadU32(0x400D403C);
34    }while((reg & 0x3) == 0);
35
36    MEM_WriteU32(0x400D403C, 0x00000003);    // clear IPCMDERR and IPCMDDONE bits
37}
38
39void EnablePllLdo()
40{
41    unsigned int val;
42
43    // ANADIG_MISC_VDDSOC_AI_CTRL
44    val = MEM_ReadU32(0x40C84820);
45    val &= ~(0x10000 | 0xFF);
46    MEM_WriteU32(0x40C84820, val);
47
48    // ANADIG_MISC_VDDSOC_AI_WDATA
49    MEM_WriteU32(0x40C84830, 0x105);
50
51    // ANADIG_PMU_PMU_LDO_PLL
52    val = MEM_ReadU32(0x40C84500);
53    val ^= 0x10000;
54    MEM_WriteU32(0x40C84500, val);
55}
56
57void InitSysPll2Pfd1()
58{
59    unsigned int val;
60    unsigned int stable;
61
62    // ANADIG_PLL_PLL_528_PFD
63    val = MEM_ReadU32(0x40C84270);
64    if (((val & 0x8000) != 0) || (((val & 0x3F00) >> 8) != 16))
65    {
66        stable = val & 0x4000;
67
68        val |= 0x8000;
69        MEM_WriteU32(0x40C84270, val);
70
71        val = MEM_ReadU32(0x40C84270);
72        val &= ~0x3F00;
73        val |= 16 << 8;
74        MEM_WriteU32(0x40C84270, val);
75
76        val = MEM_ReadU32(0x40C84250);
77        val ^= 0x4;
78        MEM_WriteU32(0x40C84250, val);
79
80        val = MEM_ReadU32(0x40C84270);
81        val &= ~0x8000;
82        MEM_WriteU32(0x40C84270, val);
83        do
84        {
85            val = MEM_ReadU32(0x40C84270) & 0x4000;
86        } while (val == stable);
87    }
88    else
89    {
90        Report("syspll2 pfd1 has been initialized already");
91        val &= ~0x8000;
92        MEM_WriteU32(0x40C84270, val);
93    }
94}
95
96void InitSysPll2()
97{
98    unsigned int val;
99
100    // ANADIG_PLL_PLL_528_CTRL
101    val = MEM_ReadU32(0x40C84240);
102    if (val & 0x800000)
103    {
104        // SysPll2 has been initialized
105        val &= ~0x40000000;
106        MEM_WriteU32(0x40C84240, val);
107        Report("syspll2 has been initialized already");
108        return;
109    }
110
111    val = MEM_ReadU32(0x40C84270);
112    val |= 0x80808080;
113    MEM_WriteU32(0x40C84270, val);
114
115    val = MEM_ReadU32(0x40C84240);
116    val &= ~(0x802000);
117    val |= 0x40000000;
118    MEM_WriteU32(0x40C84240, val);
119
120    // ANADIG_PLL_PLL_528_MFN
121    MEM_WriteU32(0x40C84280, 0);
122    // ANADIG_PLL_PLL_528_MFI
123    MEM_WriteU32(0x40C84290, 22);
124    // ANADIG_PLL_PLL_528_MFD
125    MEM_WriteU32(0x40C842A0, 0x0FFFFFFF);
126
127    // ANADIG_PLL_PLL_528_CTRL
128    MEM_WriteU32(0x40C84240, 0x8 | 0x40000000);
129    SYS_Sleep(30);
130
131    // ANADIG_PLL_PLL_528_CTRL
132    val = MEM_ReadU32(0x40C84240);
133    val |= 0x800000 | 0x800;
134    MEM_WriteU32(0x40C84240, val);
135    SYS_Sleep(250);
136
137    val = MEM_ReadU32(0x40C84240);
138    val &= ~0x800;
139    MEM_WriteU32(0x40C84240, val);
140
141    do
142    {
143        val = MEM_ReadU32(0x40C84240);
144    } while ((val & 0x20000000) == 0);
145
146    val |= 0x2000;
147    MEM_WriteU32(0x40C84240, val);
148
149    val &= ~0x40000000;
150    MEM_WriteU32(0x40C84240, val);
151}
152
153
154void SetSemcClock()
155{
156    unsigned int reg;
157    unsigned int val;
158
159    EnablePllLdo();
160    InitSysPll2();
161    InitSysPll2Pfd1();
162
163    // Set SEMC root clock
164    reg = 0x40CC0200;
165    // Use sys pll2 pfd1 divided by 3: 198Mhz
166    val = 0x602;
167    MEM_WriteU32(reg, val);
168}
169
170void SDRAMInit()
171{
172    // Config IOMUX
173    MEM_WriteU32(0x400E8010, 0x00000000);
174    MEM_WriteU32(0x400E8014, 0x00000000);
175    MEM_WriteU32(0x400E8018, 0x00000000);
176    MEM_WriteU32(0x400E801C, 0x00000000);
177    MEM_WriteU32(0x400E8020, 0x00000000);
178    MEM_WriteU32(0x400E8024, 0x00000000);
179    MEM_WriteU32(0x400E8028, 0x00000000);
180    MEM_WriteU32(0x400E802C, 0x00000000);
181    MEM_WriteU32(0x400E8030, 0x00000000);
182    MEM_WriteU32(0x400E8034, 0x00000000);
183    MEM_WriteU32(0x400E8038, 0x00000000);
184    MEM_WriteU32(0x400E803C, 0x00000000);
185    MEM_WriteU32(0x400E8040, 0x00000000);
186    MEM_WriteU32(0x400E8044, 0x00000000);
187    MEM_WriteU32(0x400E8048, 0x00000000);
188    MEM_WriteU32(0x400E804C, 0x00000000);
189    MEM_WriteU32(0x400E8050, 0x00000000);
190    MEM_WriteU32(0x400E8054, 0x00000000);
191    MEM_WriteU32(0x400E8058, 0x00000000);
192    MEM_WriteU32(0x400E805C, 0x00000000);
193    MEM_WriteU32(0x400E8060, 0x00000000);
194    MEM_WriteU32(0x400E8064, 0x00000000);
195    MEM_WriteU32(0x400E8068, 0x00000000);
196    MEM_WriteU32(0x400E806C, 0x00000000);
197    MEM_WriteU32(0x400E8070, 0x00000000);
198    MEM_WriteU32(0x400E8074, 0x00000000);
199    MEM_WriteU32(0x400E8078, 0x00000000);
200    MEM_WriteU32(0x400E807C, 0x00000000);
201    MEM_WriteU32(0x400E8080, 0x00000000);
202    MEM_WriteU32(0x400E8084, 0x00000000);
203    MEM_WriteU32(0x400E8088, 0x00000000);
204    MEM_WriteU32(0x400E808C, 0x00000000);
205    MEM_WriteU32(0x400E8090, 0x00000000);
206    MEM_WriteU32(0x400E8094, 0x00000000);
207    MEM_WriteU32(0x400E8098, 0x00000000);
208    MEM_WriteU32(0x400E809C, 0x00000000);
209    MEM_WriteU32(0x400E80A0, 0x00000000);
210    MEM_WriteU32(0x400E80A4, 0x00000000);
211    MEM_WriteU32(0x400E80A8, 0x00000000);
212    MEM_WriteU32(0x400E80AC, 0x00000010); // EMC_39, DQS PIN, enable SION
213    MEM_WriteU32(0x400E80B0, 0x00000000);
214    MEM_WriteU32(0x400E80B4, 0x00000000);
215    MEM_WriteU32(0x400E80B8, 0x00000000);
216    MEM_WriteU32(0x400E80BC, 0x00000000);
217    MEM_WriteU32(0x400E80C0, 0x00000000);
218    MEM_WriteU32(0x400E80C4, 0x00000000);
219    MEM_WriteU32(0x400E80C8, 0x00000000);
220    MEM_WriteU32(0x400E80CC, 0x00000000);
221    MEM_WriteU32(0x400E80D0, 0x00000000);
222    MEM_WriteU32(0x400E80D4, 0x00000000);
223    MEM_WriteU32(0x400E80D8, 0x00000000);
224    MEM_WriteU32(0x400E80DC, 0x00000000);
225    MEM_WriteU32(0x400E80E0, 0x00000000);
226    MEM_WriteU32(0x400E80E4, 0x00000000);
227    MEM_WriteU32(0x400E80E8, 0x00000000);
228    MEM_WriteU32(0x400E80EC, 0x00000000);
229    MEM_WriteU32(0x400E80F0, 0x00000000);
230    MEM_WriteU32(0x400E80F4, 0x00000000);
231    MEM_WriteU32(0x400E80F8, 0x00000000);
232    MEM_WriteU32(0x400E80FC, 0x00000000);
233    MEM_WriteU32(0x400E8100, 0x00000000);
234    MEM_WriteU32(0x400E8104, 0x00000000);
235    MEM_WriteU32(0x400E8108, 0x00000000);
236
237    // PAD ctrl
238    // PDRV = 1b (normal); PULL = 10b (PD)
239    MEM_WriteU32(0x400E8254, 0x00000008);
240    MEM_WriteU32(0x400E8258, 0x00000008);
241    MEM_WriteU32(0x400E825C, 0x00000008);
242    MEM_WriteU32(0x400E8260, 0x00000008);
243    MEM_WriteU32(0x400E8264, 0x00000008);
244    MEM_WriteU32(0x400E8268, 0x00000008);
245    MEM_WriteU32(0x400E826C, 0x00000008);
246    MEM_WriteU32(0x400E8270, 0x00000008);
247    MEM_WriteU32(0x400E8274, 0x00000008);
248    MEM_WriteU32(0x400E8278, 0x00000008);
249    MEM_WriteU32(0x400E827C, 0x00000008);
250    MEM_WriteU32(0x400E8280, 0x00000008);
251    MEM_WriteU32(0x400E8284, 0x00000008);
252    MEM_WriteU32(0x400E8288, 0x00000008);
253    MEM_WriteU32(0x400E828C, 0x00000008);
254    MEM_WriteU32(0x400E8290, 0x00000008);
255    MEM_WriteU32(0x400E8294, 0x00000008);
256    MEM_WriteU32(0x400E8298, 0x00000008);
257    MEM_WriteU32(0x400E829C, 0x00000008);
258    MEM_WriteU32(0x400E82A0, 0x00000008);
259    MEM_WriteU32(0x400E82A4, 0x00000008);
260    MEM_WriteU32(0x400E82A8, 0x00000008);
261    MEM_WriteU32(0x400E82AC, 0x00000008);
262    MEM_WriteU32(0x400E82B0, 0x00000008);
263    MEM_WriteU32(0x400E82B4, 0x00000008);
264    MEM_WriteU32(0x400E82B8, 0x00000008);
265    MEM_WriteU32(0x400E82BC, 0x00000008);
266    MEM_WriteU32(0x400E82C0, 0x00000008);
267    MEM_WriteU32(0x400E82C4, 0x00000008);
268    MEM_WriteU32(0x400E82C8, 0x00000008);
269    MEM_WriteU32(0x400E82CC, 0x00000008);
270    MEM_WriteU32(0x400E82D0, 0x00000008);
271    MEM_WriteU32(0x400E82D4, 0x00000008);
272    MEM_WriteU32(0x400E82D8, 0x00000008);
273    MEM_WriteU32(0x400E82DC, 0x00000008);
274    MEM_WriteU32(0x400E82E0, 0x00000008);
275    MEM_WriteU32(0x400E82E4, 0x00000008);
276    MEM_WriteU32(0x400E82E8, 0x00000008);
277    MEM_WriteU32(0x400E82EC, 0x00000008);
278    MEM_WriteU32(0x400E82F0, 0x00000008);
279    MEM_WriteU32(0x400E82F4, 0x00000008);
280    MEM_WriteU32(0x400E82F8, 0x00000008);
281    MEM_WriteU32(0x400E82FC, 0x00000008);
282    MEM_WriteU32(0x400E8300, 0x00000008);
283    MEM_WriteU32(0x400E8304, 0x00000008);
284    MEM_WriteU32(0x400E8308, 0x00000008);
285    MEM_WriteU32(0x400E830C, 0x00000008);
286    MEM_WriteU32(0x400E8310, 0x00000008);
287    MEM_WriteU32(0x400E8314, 0x00000008);
288    MEM_WriteU32(0x400E8318, 0x00000008);
289    MEM_WriteU32(0x400E831C, 0x00000008);
290    MEM_WriteU32(0x400E8320, 0x00000008);
291    MEM_WriteU32(0x400E8324, 0x00000008);
292    MEM_WriteU32(0x400E8328, 0x00000008);
293    MEM_WriteU32(0x400E832C, 0x00000008);
294    MEM_WriteU32(0x400E8330, 0x00000008);
295    MEM_WriteU32(0x400E8334, 0x00000008);
296    MEM_WriteU32(0x400E8338, 0x00000008);
297    MEM_WriteU32(0x400E833C, 0x00000008);
298    MEM_WriteU32(0x400E8400, 0x00000008);
299    MEM_WriteU32(0x400E8404, 0x00000008);
300    MEM_WriteU32(0x400E8408, 0x00000008);
301    MEM_WriteU32(0x400E840C, 0x00000008);
302
303    // Config SDR Controller Registers/
304    MEM_WriteU32(0x400d4000, 0x10000004); // MCR
305    MEM_WriteU32(0x400d4008, 0x00000081); // BMCR0
306    MEM_WriteU32(0x400d400C, 0x00000081); // BMCR1
307    MEM_WriteU32(0x400d4010, 0x8000001D); // BR0, 64MB
308
309    MEM_WriteU32(0x400d4040, 0x00000F32); // SDRAMCR0, 32bit
310    MEM_WriteU32(0x400d4044, 0x00772A22); // SDRAMCR1
311    MEM_WriteU32(0x400d4048, 0x00010A0D); // SDRAMCR2
312    MEM_WriteU32(0x400d404C, 0x21210408); // SDRAMCR3
313
314    MEM_WriteU32(0x400d4090, 0x80000000); // IPCR0
315    MEM_WriteU32(0x400d4094, 0x00000002); // IPCR1
316    MEM_WriteU32(0x400d4098, 0x00000000); // IPCR2
317
318    MEM_WriteU32(0x400d409C, 0xA55A000F); // IPCMD, SD_CC_IPREA
319    SDRAM_WaitIpCmdDone();
320    MEM_WriteU32(0x400d409C, 0xA55A000C); // SD_CC_IAF
321    SDRAM_WaitIpCmdDone();
322    MEM_WriteU32(0x400d409C, 0xA55A000C); // SD_CC_IAF
323    SDRAM_WaitIpCmdDone();
324    MEM_WriteU32(0x400d40A0, 0x00000033); // IPTXDAT
325    MEM_WriteU32(0x400d409C, 0xA55A000A); // SD_CC_IMS
326    SDRAM_WaitIpCmdDone();
327
328    MEM_WriteU32(0x400d4150, 0x00000017); // DCCR
329    MEM_WriteU32(0x400d404C, 0x21210409); // enable sdram self refresh after initialization done.
330
331    Report("SDRAM init done");
332}
333
334void InitTarget(void) {
335    CPU = CORTEX_M7;
336    JLINK_CORESIGHT_AddAP(0, CORESIGHT_AHB_AP);
337    JLINK_CORESIGHT_AddAP(1, CORESIGHT_AHB_AP);
338    JLINK_CORESIGHT_AddAP(2, CORESIGHT_APB_AP);
339
340    // Dummy read
341    JLINK_CORESIGHT_ReadAP(JLINK_CORESIGHT_AP_REG_IDR);
342    SYS_Sleep(10);
343
344    PrepareTrapCode();
345    // Release CM4 from SRC
346    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, 0x40C04000);
347    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_DATA, 1);
348
349    // Disable system reset caused by sysrstreq from each core
350    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, 0x40C04004);
351    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_DATA, 0xF << 10);
352
353    CPU = CORTEX_M4;
354    CORESIGHT_IndexAHBAPToUse = 1;
355
356    // SetSkipRestoreRAMCode command is used to skip the restoring of the RAMCode
357    JLINK_ExecCommand("SetSkipRestoreRAMCode = 1");
358}
359
360void SetupTarget(void) {
361    SetSemcClock();
362    SDRAMInit();
363}
364
365void CM4VectReset()
366{
367    MEM_WriteU32(0xE000ED0C, 0x5FA0001);
368    SYS_Sleep(10);
369    Report("CM4 Vector Reset");
370}
371
372void CM4SrcReset()
373{
374    unsigned int t;
375
376    /* Issue M4 reset */
377    MEM_WriteU32(0x40c04284, 1);
378
379    /* Check M4 reset status */
380    t = MEM_ReadU32(0x40c04290);
381    t &= 0x1;
382    while (t)
383    {
384        t = MEM_ReadU32(0x40c04290);
385        t &= 0x1;
386    }
387    SYS_Sleep(10);
388    Report("CM4 SRC reset");
389
390}
391
392void ResetCM4()
393{
394    CM4SrcReset();
395}
396
397void ResetTarget()
398{
399    CORESIGHT_IndexAHBAPToUse = 0;
400    ResetCM4();
401    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_ADDR, 0x40C04000);
402    JLINK_CORESIGHT_WriteAP(JLINK_CORESIGHT_AP_REG_DATA, 1);
403    CORESIGHT_IndexAHBAPToUse = 1;
404}
405
406void AfterResetTarget(void) {
407    SetSemcClock();
408    SDRAMInit();
409}
410
411