atomic: use <linux/atomic.h>
[linux-3.10.git] / arch / blackfin / mach-common / dpmc_modes.S
1 /*
2  * Copyright 2004-2008 Analog Devices Inc.
3  *
4  * Licensed under the GPL-2 or later.
5  */
6
7 #include <linux/linkage.h>
8 #include <asm/blackfin.h>
9 #include <mach/irq.h>
10 #include <asm/dpmc.h>
11
12 .section .l1.text
13
14 ENTRY(_sleep_mode)
15         [--SP] = (R7:4, P5:3);
16         [--SP] = RETS;
17
18         call _set_sic_iwr;
19
20         P0.H = hi(PLL_CTL);
21         P0.L = lo(PLL_CTL);
22         R1 = W[P0](z);
23         BITSET (R1, 3);
24         W[P0] = R1.L;
25
26         CLI R2;
27         SSYNC;
28         IDLE;
29         STI R2;
30
31         call _test_pll_locked;
32
33         R0 = IWR_ENABLE(0);
34         R1 = IWR_DISABLE_ALL;
35         R2 = IWR_DISABLE_ALL;
36
37         call _set_sic_iwr;
38
39         P0.H = hi(PLL_CTL);
40         P0.L = lo(PLL_CTL);
41         R7 = w[p0](z);
42         BITCLR (R7, 3);
43         BITCLR (R7, 5);
44         w[p0] = R7.L;
45         IDLE;
46         call _test_pll_locked;
47
48         RETS = [SP++];
49         (R7:4, P5:3) = [SP++];
50         RTS;
51 ENDPROC(_sleep_mode)
52
53 /*
54  * This func never returns as it puts the part into hibernate, and
55  * is only called from do_hibernate, so we don't bother saving or
56  * restoring any of the normal C runtime state.  When we wake up,
57  * the entry point will be in do_hibernate and not here.
58  *
59  * We accept just one argument -- the value to write to VR_CTL.
60  */
61 ENTRY(_hibernate_mode)
62         /* Save/setup the regs we need early for minor pipeline optimization */
63         R4 = R0;
64         P3.H = hi(VR_CTL);
65         P3.L = lo(VR_CTL);
66
67         /* Disable all wakeup sources */
68         R0 = IWR_DISABLE_ALL;
69         R1 = IWR_DISABLE_ALL;
70         R2 = IWR_DISABLE_ALL;
71         call _set_sic_iwr;
72         call _set_dram_srfs;
73         SSYNC;
74
75         /* Finally, we climb into our cave to hibernate */
76         W[P3] = R4.L;
77         CLI R2;
78         IDLE;
79 .Lforever:
80         jump .Lforever;
81 ENDPROC(_hibernate_mode)
82
83 ENTRY(_sleep_deeper)
84         [--SP] = (R7:4, P5:3);
85         [--SP] = RETS;
86
87         CLI R4;
88
89         P3 = R0;
90         P4 = R1;
91         P5 = R2;
92
93         R0 = IWR_ENABLE(0);
94         R1 = IWR_DISABLE_ALL;
95         R2 = IWR_DISABLE_ALL;
96
97         call _set_sic_iwr;
98         call _set_dram_srfs;    /* Set SDRAM Self Refresh */
99
100         P0.H = hi(PLL_DIV);
101         P0.L = lo(PLL_DIV);
102         R6 = W[P0](z);
103         R0.L = 0xF;
104         W[P0] = R0.l;           /* Set Max VCO to SCLK divider */
105
106         P0.H = hi(PLL_CTL);
107         P0.L = lo(PLL_CTL);
108         R5 = W[P0](z);
109         R0.L = (CONFIG_MIN_VCO_HZ/CONFIG_CLKIN_HZ) << 9;
110         W[P0] = R0.l;           /* Set Min CLKIN to VCO multiplier */
111
112         SSYNC;
113         IDLE;
114
115         call _test_pll_locked;
116
117         P0.H = hi(VR_CTL);
118         P0.L = lo(VR_CTL);
119         R7 = W[P0](z);
120         R1 = 0x6;
121         R1 <<= 16;
122         R2 = 0x0404(Z);
123         R1 = R1|R2;
124
125         R2 = DEPOSIT(R7, R1);
126         W[P0] = R2;             /* Set Min Core Voltage */
127
128         SSYNC;
129         IDLE;
130
131         call _test_pll_locked;
132
133         R0 = P3;
134         R1 = P4;
135         R3 = P5;
136         call _set_sic_iwr;      /* Set Awake from IDLE */
137
138         P0.H = hi(PLL_CTL);
139         P0.L = lo(PLL_CTL);
140         R0 = W[P0](z);
141         BITSET (R0, 3);
142         W[P0] = R0.L;           /* Turn CCLK OFF */
143         SSYNC;
144         IDLE;
145
146         call _test_pll_locked;
147
148         R0 = IWR_ENABLE(0);
149         R1 = IWR_DISABLE_ALL;
150         R2 = IWR_DISABLE_ALL;
151
152         call _set_sic_iwr;      /* Set Awake from IDLE PLL */
153
154         P0.H = hi(VR_CTL);
155         P0.L = lo(VR_CTL);
156         W[P0]= R7;
157
158         SSYNC;
159         IDLE;
160
161         call _test_pll_locked;
162
163         P0.H = hi(PLL_DIV);
164         P0.L = lo(PLL_DIV);
165         W[P0]= R6;              /* Restore CCLK and SCLK divider */
166
167         P0.H = hi(PLL_CTL);
168         P0.L = lo(PLL_CTL);
169         w[p0] = R5;             /* Restore VCO multiplier */
170         IDLE;
171         call _test_pll_locked;
172
173         call _unset_dram_srfs;  /* SDRAM Self Refresh Off */
174
175         STI R4;
176
177         RETS = [SP++];
178         (R7:4, P5:3) = [SP++];
179         RTS;
180 ENDPROC(_sleep_deeper)
181
182 ENTRY(_set_dram_srfs)
183         /*  set the dram to self refresh mode */
184         SSYNC;
185 #if defined(EBIU_RSTCTL)        /* DDR */
186         P0.H = hi(EBIU_RSTCTL);
187         P0.L = lo(EBIU_RSTCTL);
188         R2 = [P0];
189         BITSET(R2, 3); /* SRREQ enter self-refresh mode */
190         [P0] = R2;
191         SSYNC;
192 1:
193         R2 = [P0];
194         CC = BITTST(R2, 4);
195         if !CC JUMP 1b;
196 #else                           /* SDRAM */
197         P0.L = lo(EBIU_SDGCTL);
198         P0.H = hi(EBIU_SDGCTL);
199         P1.L = lo(EBIU_SDSTAT);
200         P1.H = hi(EBIU_SDSTAT);
201
202         R2 = [P0];
203         BITSET(R2, 24); /* SRFS enter self-refresh mode */
204         [P0] = R2;
205         SSYNC;
206
207 1:
208         R2 = w[P1];
209         SSYNC;
210         cc = BITTST(R2, 1); /* SDSRA poll self-refresh status */
211         if !cc jump 1b;
212
213         R2 = [P0];
214         BITCLR(R2, 0); /* SCTLE disable CLKOUT */
215         [P0] = R2;
216 #endif
217         RTS;
218 ENDPROC(_set_dram_srfs)
219
220 ENTRY(_unset_dram_srfs)
221         /*  set the dram out of self refresh mode */
222
223 #if defined(EBIU_RSTCTL)        /* DDR */
224         P0.H = hi(EBIU_RSTCTL);
225         P0.L = lo(EBIU_RSTCTL);
226         R2 = [P0];
227         BITCLR(R2, 3); /* clear SRREQ bit */
228         [P0] = R2;
229 #elif defined(EBIU_SDGCTL)      /* SDRAM */
230         /* release CLKOUT from self-refresh */
231         P0.L = lo(EBIU_SDGCTL);
232         P0.H = hi(EBIU_SDGCTL);
233
234         R2 = [P0];
235         BITSET(R2, 0); /* SCTLE enable CLKOUT */
236         [P0] = R2
237         SSYNC;
238
239         /* release SDRAM from self-refresh */
240         R2 = [P0];
241         BITCLR(R2, 24); /* clear SRFS bit */
242         [P0] = R2
243 #endif
244
245         SSYNC;
246         RTS;
247 ENDPROC(_unset_dram_srfs)
248
249 ENTRY(_set_sic_iwr)
250 #ifdef SIC_IWR0
251         P0.H = hi(SYSMMR_BASE);
252         P0.L = lo(SYSMMR_BASE);
253         [P0 + (SIC_IWR0 - SYSMMR_BASE)] = R0;
254         [P0 + (SIC_IWR1 - SYSMMR_BASE)] = R1;
255 # ifdef SIC_IWR2
256         [P0 + (SIC_IWR2 - SYSMMR_BASE)] = R2;
257 # endif
258 #else
259         P0.H = hi(SIC_IWR);
260         P0.L = lo(SIC_IWR);
261         [P0] = R0;
262 #endif
263
264         SSYNC;
265         RTS;
266 ENDPROC(_set_sic_iwr)
267
268 ENTRY(_test_pll_locked)
269         P0.H = hi(PLL_STAT);
270         P0.L = lo(PLL_STAT);
271 1:
272         R0 = W[P0] (Z);
273         CC = BITTST(R0,5);
274         IF !CC JUMP 1b;
275         RTS;
276 ENDPROC(_test_pll_locked)
277
278 .section .text
279
280 #define PM_REG0  R7
281 #define PM_REG1  R6
282 #define PM_REG2  R5
283 #define PM_REG3  R4
284 #define PM_REG4  R3
285 #define PM_REG5  R2
286 #define PM_REG6  R1
287 #define PM_REG7  R0
288 #define PM_REG8  P5
289 #define PM_REG9  P4
290 #define PM_REG10 P3
291 #define PM_REG11 P2
292 #define PM_REG12 P1
293 #define PM_REG13 P0
294
295 #define PM_REGSET0  R7:7
296 #define PM_REGSET1  R7:6
297 #define PM_REGSET2  R7:5
298 #define PM_REGSET3  R7:4
299 #define PM_REGSET4  R7:3
300 #define PM_REGSET5  R7:2
301 #define PM_REGSET6  R7:1
302 #define PM_REGSET7  R7:0
303 #define PM_REGSET8  R7:0, P5:5
304 #define PM_REGSET9  R7:0, P5:4
305 #define PM_REGSET10 R7:0, P5:3
306 #define PM_REGSET11 R7:0, P5:2
307 #define PM_REGSET12 R7:0, P5:1
308 #define PM_REGSET13 R7:0, P5:0
309
310 #define _PM_PUSH(n, x, w, base) PM_REG##n = w[FP + ((x) - (base))];
311 #define _PM_POP(n, x, w, base)  w[FP + ((x) - (base))] = PM_REG##n;
312 #define PM_PUSH_SYNC(n)         [--sp] = (PM_REGSET##n);
313 #define PM_POP_SYNC(n)          (PM_REGSET##n) = [sp++];
314 #define PM_PUSH(n, x)           PM_REG##n = [FP++];
315 #define PM_POP(n, x)            [FP--] = PM_REG##n;
316 #define PM_CORE_PUSH(n, x)      _PM_PUSH(n, x, , COREMMR_BASE)
317 #define PM_CORE_POP(n, x)       _PM_POP(n, x, , COREMMR_BASE)
318 #define PM_SYS_PUSH(n, x)       _PM_PUSH(n, x, , SYSMMR_BASE)
319 #define PM_SYS_POP(n, x)        _PM_POP(n, x, , SYSMMR_BASE)
320 #define PM_SYS_PUSH16(n, x)     _PM_PUSH(n, x, w, SYSMMR_BASE)
321 #define PM_SYS_POP16(n, x)      _PM_POP(n, x, w, SYSMMR_BASE)
322
323 ENTRY(_do_hibernate)
324         /*
325          * Save the core regs early so we can blow them away when
326          * saving/restoring MMR states
327          */
328         [--sp] = (R7:0, P5:0);
329         [--sp] = fp;
330         [--sp] = usp;
331
332         [--sp] = i0;
333         [--sp] = i1;
334         [--sp] = i2;
335         [--sp] = i3;
336
337         [--sp] = m0;
338         [--sp] = m1;
339         [--sp] = m2;
340         [--sp] = m3;
341
342         [--sp] = l0;
343         [--sp] = l1;
344         [--sp] = l2;
345         [--sp] = l3;
346
347         [--sp] = b0;
348         [--sp] = b1;
349         [--sp] = b2;
350         [--sp] = b3;
351         [--sp] = a0.x;
352         [--sp] = a0.w;
353         [--sp] = a1.x;
354         [--sp] = a1.w;
355
356         [--sp] = LC0;
357         [--sp] = LC1;
358         [--sp] = LT0;
359         [--sp] = LT1;
360         [--sp] = LB0;
361         [--sp] = LB1;
362
363         /* We can't push RETI directly as that'll change IPEND[4] */
364         r7 = RETI;
365         [--sp] = RETS;
366         [--sp] = ASTAT;
367         [--sp] = CYCLES;
368         [--sp] = CYCLES2;
369         [--sp] = SYSCFG;
370         [--sp] = RETX;
371         [--sp] = SEQSTAT;
372         [--sp] = r7;
373
374         /* Save first func arg in M3 */
375         M3 = R0;
376
377         /* Save system MMRs */
378         FP.H = hi(SYSMMR_BASE);
379         FP.L = lo(SYSMMR_BASE);
380
381 #ifdef SIC_IMASK0
382         PM_SYS_PUSH(0, SIC_IMASK0)
383         PM_SYS_PUSH(1, SIC_IMASK1)
384 # ifdef SIC_IMASK2
385         PM_SYS_PUSH(2, SIC_IMASK2)
386 # endif
387 #else
388         PM_SYS_PUSH(0, SIC_IMASK)
389 #endif
390 #ifdef SIC_IAR0
391         PM_SYS_PUSH(3, SIC_IAR0)
392         PM_SYS_PUSH(4, SIC_IAR1)
393         PM_SYS_PUSH(5, SIC_IAR2)
394 #endif
395 #ifdef SIC_IAR3
396         PM_SYS_PUSH(6, SIC_IAR3)
397 #endif
398 #ifdef SIC_IAR4
399         PM_SYS_PUSH(7, SIC_IAR4)
400         PM_SYS_PUSH(8, SIC_IAR5)
401         PM_SYS_PUSH(9, SIC_IAR6)
402 #endif
403 #ifdef SIC_IAR7
404         PM_SYS_PUSH(10, SIC_IAR7)
405 #endif
406 #ifdef SIC_IAR8
407         PM_SYS_PUSH(11, SIC_IAR8)
408         PM_SYS_PUSH(12, SIC_IAR9)
409         PM_SYS_PUSH(13, SIC_IAR10)
410 #endif
411         PM_PUSH_SYNC(13)
412 #ifdef SIC_IAR11
413         PM_SYS_PUSH(0, SIC_IAR11)
414 #endif
415
416 #ifdef SIC_IWR
417         PM_SYS_PUSH(1, SIC_IWR)
418 #endif
419 #ifdef SIC_IWR0
420         PM_SYS_PUSH(1, SIC_IWR0)
421 #endif
422 #ifdef SIC_IWR1
423         PM_SYS_PUSH(2, SIC_IWR1)
424 #endif
425 #ifdef SIC_IWR2
426         PM_SYS_PUSH(3, SIC_IWR2)
427 #endif
428
429 #ifdef PINT0_ASSIGN
430         PM_SYS_PUSH(4, PINT0_MASK_SET)
431         PM_SYS_PUSH(5, PINT1_MASK_SET)
432         PM_SYS_PUSH(6, PINT2_MASK_SET)
433         PM_SYS_PUSH(7, PINT3_MASK_SET)
434         PM_SYS_PUSH(8, PINT0_ASSIGN)
435         PM_SYS_PUSH(9, PINT1_ASSIGN)
436         PM_SYS_PUSH(10, PINT2_ASSIGN)
437         PM_SYS_PUSH(11, PINT3_ASSIGN)
438         PM_SYS_PUSH(12, PINT0_INVERT_SET)
439         PM_SYS_PUSH(13, PINT1_INVERT_SET)
440         PM_PUSH_SYNC(13)
441         PM_SYS_PUSH(0, PINT2_INVERT_SET)
442         PM_SYS_PUSH(1, PINT3_INVERT_SET)
443         PM_SYS_PUSH(2, PINT0_EDGE_SET)
444         PM_SYS_PUSH(3, PINT1_EDGE_SET)
445         PM_SYS_PUSH(4, PINT2_EDGE_SET)
446         PM_SYS_PUSH(5, PINT3_EDGE_SET)
447 #endif
448
449         PM_SYS_PUSH16(6, SYSCR)
450
451         PM_SYS_PUSH16(7, EBIU_AMGCTL)
452         PM_SYS_PUSH(8, EBIU_AMBCTL0)
453         PM_SYS_PUSH(9, EBIU_AMBCTL1)
454 #ifdef EBIU_FCTL
455         PM_SYS_PUSH(10, EBIU_MBSCTL)
456         PM_SYS_PUSH(11, EBIU_MODE)
457         PM_SYS_PUSH(12, EBIU_FCTL)
458         PM_PUSH_SYNC(12)
459 #else
460         PM_PUSH_SYNC(9)
461 #endif
462
463         /* Save Core MMRs */
464         I0.H = hi(COREMMR_BASE);
465         I0.L = lo(COREMMR_BASE);
466         I1 = I0;
467         I2 = I0;
468         I3 = I0;
469         B0 = I0;
470         B1 = I0;
471         B2 = I0;
472         B3 = I0;
473         I1.L = lo(DCPLB_ADDR0);
474         I2.L = lo(DCPLB_DATA0);
475         I3.L = lo(ICPLB_ADDR0);
476         B0.L = lo(ICPLB_DATA0);
477         B1.L = lo(EVT2);
478         B2.L = lo(IMASK);
479         B3.L = lo(TCNTL);
480
481         /* DCPLB Addr */
482         FP = I1;
483         PM_PUSH(0, DCPLB_ADDR0)
484         PM_PUSH(1, DCPLB_ADDR1)
485         PM_PUSH(2, DCPLB_ADDR2)
486         PM_PUSH(3, DCPLB_ADDR3)
487         PM_PUSH(4, DCPLB_ADDR4)
488         PM_PUSH(5, DCPLB_ADDR5)
489         PM_PUSH(6, DCPLB_ADDR6)
490         PM_PUSH(7, DCPLB_ADDR7)
491         PM_PUSH(8, DCPLB_ADDR8)
492         PM_PUSH(9, DCPLB_ADDR9)
493         PM_PUSH(10, DCPLB_ADDR10)
494         PM_PUSH(11, DCPLB_ADDR11)
495         PM_PUSH(12, DCPLB_ADDR12)
496         PM_PUSH(13, DCPLB_ADDR13)
497         PM_PUSH_SYNC(13)
498         PM_PUSH(0, DCPLB_ADDR14)
499         PM_PUSH(1, DCPLB_ADDR15)
500
501         /* DCPLB Data */
502         FP = I2;
503         PM_PUSH(2, DCPLB_DATA0)
504         PM_PUSH(3, DCPLB_DATA1)
505         PM_PUSH(4, DCPLB_DATA2)
506         PM_PUSH(5, DCPLB_DATA3)
507         PM_PUSH(6, DCPLB_DATA4)
508         PM_PUSH(7, DCPLB_DATA5)
509         PM_PUSH(8, DCPLB_DATA6)
510         PM_PUSH(9, DCPLB_DATA7)
511         PM_PUSH(10, DCPLB_DATA8)
512         PM_PUSH(11, DCPLB_DATA9)
513         PM_PUSH(12, DCPLB_DATA10)
514         PM_PUSH(13, DCPLB_DATA11)
515         PM_PUSH_SYNC(13)
516         PM_PUSH(0, DCPLB_DATA12)
517         PM_PUSH(1, DCPLB_DATA13)
518         PM_PUSH(2, DCPLB_DATA14)
519         PM_PUSH(3, DCPLB_DATA15)
520
521         /* ICPLB Addr */
522         FP = I3;
523         PM_PUSH(4, ICPLB_ADDR0)
524         PM_PUSH(5, ICPLB_ADDR1)
525         PM_PUSH(6, ICPLB_ADDR2)
526         PM_PUSH(7, ICPLB_ADDR3)
527         PM_PUSH(8, ICPLB_ADDR4)
528         PM_PUSH(9, ICPLB_ADDR5)
529         PM_PUSH(10, ICPLB_ADDR6)
530         PM_PUSH(11, ICPLB_ADDR7)
531         PM_PUSH(12, ICPLB_ADDR8)
532         PM_PUSH(13, ICPLB_ADDR9)
533         PM_PUSH_SYNC(13)
534         PM_PUSH(0, ICPLB_ADDR10)
535         PM_PUSH(1, ICPLB_ADDR11)
536         PM_PUSH(2, ICPLB_ADDR12)
537         PM_PUSH(3, ICPLB_ADDR13)
538         PM_PUSH(4, ICPLB_ADDR14)
539         PM_PUSH(5, ICPLB_ADDR15)
540
541         /* ICPLB Data */
542         FP = B0;
543         PM_PUSH(6, ICPLB_DATA0)
544         PM_PUSH(7, ICPLB_DATA1)
545         PM_PUSH(8, ICPLB_DATA2)
546         PM_PUSH(9, ICPLB_DATA3)
547         PM_PUSH(10, ICPLB_DATA4)
548         PM_PUSH(11, ICPLB_DATA5)
549         PM_PUSH(12, ICPLB_DATA6)
550         PM_PUSH(13, ICPLB_DATA7)
551         PM_PUSH_SYNC(13)
552         PM_PUSH(0, ICPLB_DATA8)
553         PM_PUSH(1, ICPLB_DATA9)
554         PM_PUSH(2, ICPLB_DATA10)
555         PM_PUSH(3, ICPLB_DATA11)
556         PM_PUSH(4, ICPLB_DATA12)
557         PM_PUSH(5, ICPLB_DATA13)
558         PM_PUSH(6, ICPLB_DATA14)
559         PM_PUSH(7, ICPLB_DATA15)
560
561         /* Event Vectors */
562         FP = B1;
563         PM_PUSH(8, EVT2)
564         PM_PUSH(9, EVT3)
565         FP += 4;        /* EVT4 */
566         PM_PUSH(10, EVT5)
567         PM_PUSH(11, EVT6)
568         PM_PUSH(12, EVT7)
569         PM_PUSH(13, EVT8)
570         PM_PUSH_SYNC(13)
571         PM_PUSH(0, EVT9)
572         PM_PUSH(1, EVT10)
573         PM_PUSH(2, EVT11)
574         PM_PUSH(3, EVT12)
575         PM_PUSH(4, EVT13)
576         PM_PUSH(5, EVT14)
577         PM_PUSH(6, EVT15)
578
579         /* CEC */
580         FP = B2;
581         PM_PUSH(7, IMASK)
582         FP += 4;        /* IPEND */
583         PM_PUSH(8, ILAT)
584         PM_PUSH(9, IPRIO)
585
586         /* Core Timer */
587         FP = B3;
588         PM_PUSH(10, TCNTL)
589         PM_PUSH(11, TPERIOD)
590         PM_PUSH(12, TSCALE)
591         PM_PUSH(13, TCOUNT)
592         PM_PUSH_SYNC(13)
593
594         /* Misc non-contiguous registers */
595         FP = I0;
596         PM_CORE_PUSH(0, DMEM_CONTROL);
597         PM_CORE_PUSH(1, IMEM_CONTROL);
598         PM_CORE_PUSH(2, TBUFCTL);
599         PM_PUSH_SYNC(2)
600
601         /* Setup args to hibernate mode early for pipeline optimization */
602         R0 = M3;
603         P1.H = _hibernate_mode;
604         P1.L = _hibernate_mode;
605
606         /* Save Magic, return address and Stack Pointer */
607         P0 = 0;
608         R1.H = 0xDEAD;  /* Hibernate Magic */
609         R1.L = 0xBEEF;
610         R2.H = .Lpm_resume_here;
611         R2.L = .Lpm_resume_here;
612         [P0++] = R1;    /* Store Hibernate Magic */
613         [P0++] = R2;    /* Save Return Address */
614         [P0++] = SP;    /* Save Stack Pointer */
615
616         /* Must use an indirect call as we need to jump to L1 */
617         call (P1); /* Goodbye */
618
619 .Lpm_resume_here:
620
621         /* Restore Core MMRs */
622         I0.H = hi(COREMMR_BASE);
623         I0.L = lo(COREMMR_BASE);
624         I1 = I0;
625         I2 = I0;
626         I3 = I0;
627         B0 = I0;
628         B1 = I0;
629         B2 = I0;
630         B3 = I0;
631         I1.L = lo(DCPLB_ADDR15);
632         I2.L = lo(DCPLB_DATA15);
633         I3.L = lo(ICPLB_ADDR15);
634         B0.L = lo(ICPLB_DATA15);
635         B1.L = lo(EVT15);
636         B2.L = lo(IPRIO);
637         B3.L = lo(TCOUNT);
638
639         /* Misc non-contiguous registers */
640         FP = I0;
641         PM_POP_SYNC(2)
642         PM_CORE_POP(2, TBUFCTL)
643         PM_CORE_POP(1, IMEM_CONTROL)
644         PM_CORE_POP(0, DMEM_CONTROL)
645
646         /* Core Timer */
647         PM_POP_SYNC(13)
648         FP = B3;
649         PM_POP(13, TCOUNT)
650         PM_POP(12, TSCALE)
651         PM_POP(11, TPERIOD)
652         PM_POP(10, TCNTL)
653
654         /* CEC */
655         FP = B2;
656         PM_POP(9, IPRIO)
657         PM_POP(8, ILAT)
658         FP += -4;       /* IPEND */
659         PM_POP(7, IMASK)
660
661         /* Event Vectors */
662         FP = B1;
663         PM_POP(6, EVT15)
664         PM_POP(5, EVT14)
665         PM_POP(4, EVT13)
666         PM_POP(3, EVT12)
667         PM_POP(2, EVT11)
668         PM_POP(1, EVT10)
669         PM_POP(0, EVT9)
670         PM_POP_SYNC(13)
671         PM_POP(13, EVT8)
672         PM_POP(12, EVT7)
673         PM_POP(11, EVT6)
674         PM_POP(10, EVT5)
675         FP += -4;       /* EVT4 */
676         PM_POP(9, EVT3)
677         PM_POP(8, EVT2)
678
679         /* ICPLB Data */
680         FP = B0;
681         PM_POP(7, ICPLB_DATA15)
682         PM_POP(6, ICPLB_DATA14)
683         PM_POP(5, ICPLB_DATA13)
684         PM_POP(4, ICPLB_DATA12)
685         PM_POP(3, ICPLB_DATA11)
686         PM_POP(2, ICPLB_DATA10)
687         PM_POP(1, ICPLB_DATA9)
688         PM_POP(0, ICPLB_DATA8)
689         PM_POP_SYNC(13)
690         PM_POP(13, ICPLB_DATA7)
691         PM_POP(12, ICPLB_DATA6)
692         PM_POP(11, ICPLB_DATA5)
693         PM_POP(10, ICPLB_DATA4)
694         PM_POP(9, ICPLB_DATA3)
695         PM_POP(8, ICPLB_DATA2)
696         PM_POP(7, ICPLB_DATA1)
697         PM_POP(6, ICPLB_DATA0)
698
699         /* ICPLB Addr */
700         FP = I3;
701         PM_POP(5, ICPLB_ADDR15)
702         PM_POP(4, ICPLB_ADDR14)
703         PM_POP(3, ICPLB_ADDR13)
704         PM_POP(2, ICPLB_ADDR12)
705         PM_POP(1, ICPLB_ADDR11)
706         PM_POP(0, ICPLB_ADDR10)
707         PM_POP_SYNC(13)
708         PM_POP(13, ICPLB_ADDR9)
709         PM_POP(12, ICPLB_ADDR8)
710         PM_POP(11, ICPLB_ADDR7)
711         PM_POP(10, ICPLB_ADDR6)
712         PM_POP(9, ICPLB_ADDR5)
713         PM_POP(8, ICPLB_ADDR4)
714         PM_POP(7, ICPLB_ADDR3)
715         PM_POP(6, ICPLB_ADDR2)
716         PM_POP(5, ICPLB_ADDR1)
717         PM_POP(4, ICPLB_ADDR0)
718
719         /* DCPLB Data */
720         FP = I2;
721         PM_POP(3, DCPLB_DATA15)
722         PM_POP(2, DCPLB_DATA14)
723         PM_POP(1, DCPLB_DATA13)
724         PM_POP(0, DCPLB_DATA12)
725         PM_POP_SYNC(13)
726         PM_POP(13, DCPLB_DATA11)
727         PM_POP(12, DCPLB_DATA10)
728         PM_POP(11, DCPLB_DATA9)
729         PM_POP(10, DCPLB_DATA8)
730         PM_POP(9, DCPLB_DATA7)
731         PM_POP(8, DCPLB_DATA6)
732         PM_POP(7, DCPLB_DATA5)
733         PM_POP(6, DCPLB_DATA4)
734         PM_POP(5, DCPLB_DATA3)
735         PM_POP(4, DCPLB_DATA2)
736         PM_POP(3, DCPLB_DATA1)
737         PM_POP(2, DCPLB_DATA0)
738
739         /* DCPLB Addr */
740         FP = I1;
741         PM_POP(1, DCPLB_ADDR15)
742         PM_POP(0, DCPLB_ADDR14)
743         PM_POP_SYNC(13)
744         PM_POP(13, DCPLB_ADDR13)
745         PM_POP(12, DCPLB_ADDR12)
746         PM_POP(11, DCPLB_ADDR11)
747         PM_POP(10, DCPLB_ADDR10)
748         PM_POP(9, DCPLB_ADDR9)
749         PM_POP(8, DCPLB_ADDR8)
750         PM_POP(7, DCPLB_ADDR7)
751         PM_POP(6, DCPLB_ADDR6)
752         PM_POP(5, DCPLB_ADDR5)
753         PM_POP(4, DCPLB_ADDR4)
754         PM_POP(3, DCPLB_ADDR3)
755         PM_POP(2, DCPLB_ADDR2)
756         PM_POP(1, DCPLB_ADDR1)
757         PM_POP(0, DCPLB_ADDR0)
758
759         /* Restore System MMRs */
760         FP.H = hi(SYSMMR_BASE);
761         FP.L = lo(SYSMMR_BASE);
762
763 #ifdef EBIU_FCTL
764         PM_POP_SYNC(12)
765         PM_SYS_POP(12, EBIU_FCTL)
766         PM_SYS_POP(11, EBIU_MODE)
767         PM_SYS_POP(10, EBIU_MBSCTL)
768 #else
769         PM_POP_SYNC(9)
770 #endif
771         PM_SYS_POP(9, EBIU_AMBCTL1)
772         PM_SYS_POP(8, EBIU_AMBCTL0)
773         PM_SYS_POP16(7, EBIU_AMGCTL)
774
775         PM_SYS_POP16(6, SYSCR)
776
777 #ifdef PINT0_ASSIGN
778         PM_SYS_POP(5, PINT3_EDGE_SET)
779         PM_SYS_POP(4, PINT2_EDGE_SET)
780         PM_SYS_POP(3, PINT1_EDGE_SET)
781         PM_SYS_POP(2, PINT0_EDGE_SET)
782         PM_SYS_POP(1, PINT3_INVERT_SET)
783         PM_SYS_POP(0, PINT2_INVERT_SET)
784         PM_POP_SYNC(13)
785         PM_SYS_POP(13, PINT1_INVERT_SET)
786         PM_SYS_POP(12, PINT0_INVERT_SET)
787         PM_SYS_POP(11, PINT3_ASSIGN)
788         PM_SYS_POP(10, PINT2_ASSIGN)
789         PM_SYS_POP(9, PINT1_ASSIGN)
790         PM_SYS_POP(8, PINT0_ASSIGN)
791         PM_SYS_POP(7, PINT3_MASK_SET)
792         PM_SYS_POP(6, PINT2_MASK_SET)
793         PM_SYS_POP(5, PINT1_MASK_SET)
794         PM_SYS_POP(4, PINT0_MASK_SET)
795 #endif
796
797 #ifdef SIC_IWR2
798         PM_SYS_POP(3, SIC_IWR2)
799 #endif
800 #ifdef SIC_IWR1
801         PM_SYS_POP(2, SIC_IWR1)
802 #endif
803 #ifdef SIC_IWR0
804         PM_SYS_POP(1, SIC_IWR0)
805 #endif
806 #ifdef SIC_IWR
807         PM_SYS_POP(1, SIC_IWR)
808 #endif
809
810 #ifdef SIC_IAR11
811         PM_SYS_POP(0, SIC_IAR11)
812 #endif
813         PM_POP_SYNC(13)
814 #ifdef SIC_IAR8
815         PM_SYS_POP(13, SIC_IAR10)
816         PM_SYS_POP(12, SIC_IAR9)
817         PM_SYS_POP(11, SIC_IAR8)
818 #endif
819 #ifdef SIC_IAR7
820         PM_SYS_POP(10, SIC_IAR7)
821 #endif
822 #ifdef SIC_IAR6
823         PM_SYS_POP(9, SIC_IAR6)
824         PM_SYS_POP(8, SIC_IAR5)
825         PM_SYS_POP(7, SIC_IAR4)
826 #endif
827 #ifdef SIC_IAR3
828         PM_SYS_POP(6, SIC_IAR3)
829 #endif
830 #ifdef SIC_IAR0
831         PM_SYS_POP(5, SIC_IAR2)
832         PM_SYS_POP(4, SIC_IAR1)
833         PM_SYS_POP(3, SIC_IAR0)
834 #endif
835 #ifdef SIC_IMASK0
836 # ifdef SIC_IMASK2
837         PM_SYS_POP(2, SIC_IMASK2)
838 # endif
839         PM_SYS_POP(1, SIC_IMASK1)
840         PM_SYS_POP(0, SIC_IMASK0)
841 #else
842         PM_SYS_POP(0, SIC_IMASK)
843 #endif
844
845         /* Restore Core Registers */
846         RETI = [sp++];
847         SEQSTAT = [sp++];
848         RETX = [sp++];
849         SYSCFG = [sp++];
850         CYCLES2 = [sp++];
851         CYCLES = [sp++];
852         ASTAT = [sp++];
853         RETS = [sp++];
854
855         LB1 = [sp++];
856         LB0 = [sp++];
857         LT1 = [sp++];
858         LT0 = [sp++];
859         LC1 = [sp++];
860         LC0 = [sp++];
861
862         a1.w = [sp++];
863         a1.x = [sp++];
864         a0.w = [sp++];
865         a0.x = [sp++];
866         b3 = [sp++];
867         b2 = [sp++];
868         b1 = [sp++];
869         b0 = [sp++];
870
871         l3 = [sp++];
872         l2 = [sp++];
873         l1 = [sp++];
874         l0 = [sp++];
875
876         m3 = [sp++];
877         m2 = [sp++];
878         m1 = [sp++];
879         m0 = [sp++];
880
881         i3 = [sp++];
882         i2 = [sp++];
883         i1 = [sp++];
884         i0 = [sp++];
885
886         usp = [sp++];
887         fp = [sp++];
888         (R7:0, P5:0) = [sp++];
889
890         [--sp] = RETI;  /* Clear Global Interrupt Disable */
891         SP += 4;
892
893         RTS;
894 ENDPROC(_do_hibernate)