1 2/* 3 * Copyright 2018-2021 NXP 4 * 5 * SPDX-License-Identifier: BSD-3-Clause 6 * 7 */ 8 9#include <asm_macros.S> 10#include <assert_macros.S> 11 12#include <lib/psci/psci.h> 13 14#include <bl31_data.h> 15#include <plat_psci.h> 16 17 18#define RESET_RETRY_CNT 800 19#define PSCI_ABORT_CNT 100 20 21#if (SOC_CORE_RELEASE) 22 23.global _psci_cpu_on 24 25/* 26 * int _psci_cpu_on(u_register_t core_mask) 27 * x0 = target cpu core mask 28 * 29 * Called from C, so save the non-volatile regs 30 * save these as pairs of registers to maintain the 31 * required 16-byte alignment on the stack 32 * 33 */ 34 35func _psci_cpu_on 36 stp x4, x5, [sp, #-16]! 37 stp x6, x7, [sp, #-16]! 38 stp x8, x9, [sp, #-16]! 39 stp x10, x11, [sp, #-16]! 40 stp x12, x13, [sp, #-16]! 41 stp x14, x15, [sp, #-16]! 42 stp x16, x17, [sp, #-16]! 43 stp x18, x30, [sp, #-16]! 44 45 mov x6, x0 46 47 /* x0 = core mask (lsb) 48 * x6 = core mask (lsb) 49 */ 50 51 /* check if core disabled */ 52 bl _soc_ck_disabled /* 0-2 */ 53 cbnz w0, psci_disabled 54 55 /* check core data area to see if core cannot be turned on 56 * read the core state 57 */ 58 mov x0, x6 59 bl _getCoreState /* 0-5 */ 60 mov x9, x0 61 62 /* x6 = core mask (lsb) 63 * x9 = core state (from data area) 64 */ 65 66 cmp x9, #CORE_DISABLED 67 mov x0, #PSCI_E_DISABLED 68 b.eq cpu_on_done 69 70 cmp x9, #CORE_PENDING 71 mov x0, #PSCI_E_ON_PENDING 72 b.eq cpu_on_done 73 74 cmp x9, #CORE_RELEASED 75 mov x0, #PSCI_E_ALREADY_ON 76 b.eq cpu_on_done 77 788: 79 /* x6 = core mask (lsb) 80 * x9 = core state (from data area) 81 */ 82 83 cmp x9, #CORE_WFE 84 b.eq core_in_wfe 85 cmp x9, #CORE_IN_RESET 86 b.eq core_in_reset 87 cmp x9, #CORE_OFF 88 b.eq core_is_off 89 cmp x9, #CORE_OFF_PENDING 90 91 /* if state == CORE_OFF_PENDING, set abort */ 92 mov x0, x6 93 mov x1, #ABORT_FLAG_DATA 94 mov x2, #CORE_ABORT_OP 95 bl _setCoreData /* 0-3, [13-15] */ 96 97 ldr x3, =PSCI_ABORT_CNT 987: 99 /* watch for abort to take effect */ 100 mov x0, x6 101 bl _getCoreState /* 0-5 */ 102 cmp x0, #CORE_OFF 103 b.eq core_is_off 104 cmp x0, #CORE_PENDING 105 mov x0, #PSCI_E_SUCCESS 106 b.eq cpu_on_done 107 108 /* loop til finished */ 109 sub x3, x3, #1 110 cbnz x3, 7b 111 112 /* if we didn't see either CORE_OFF or CORE_PENDING, then this 113 * core is in CORE_OFF_PENDING - exit with success, as the core will 114 * respond to the abort request 115 */ 116 mov x0, #PSCI_E_SUCCESS 117 b cpu_on_done 118 119/* this is where we start up a core out of reset */ 120core_in_reset: 121 /* see if the soc-specific module supports this op */ 122 ldr x7, =SOC_CORE_RELEASE 123 cbnz x7, 3f 124 125 mov x0, #PSCI_E_NOT_SUPPORTED 126 b cpu_on_done 127 128 /* x6 = core mask (lsb) */ 1293: 130 /* set core state in data area */ 131 mov x0, x6 132 mov x1, #CORE_PENDING 133 bl _setCoreState /* 0-3, [13-15] */ 134 135 /* release the core from reset */ 136 mov x0, x6 137 bl _soc_core_release /* 0-3 */ 138 mov x0, #PSCI_E_SUCCESS 139 b cpu_on_done 140 141 /* Start up the core that has been powered-down via CPU_OFF 142 */ 143core_is_off: 144 /* see if the soc-specific module supports this op 145 */ 146 ldr x7, =SOC_CORE_RESTART 147 cbnz x7, 2f 148 149 mov x0, #PSCI_E_NOT_SUPPORTED 150 b cpu_on_done 151 152 /* x6 = core mask (lsb) */ 1532: 154 /* set core state in data area */ 155 mov x0, x6 156 mov x1, #CORE_WAKEUP 157 bl _setCoreState /* 0-3, [13-15] */ 158 159 /* put the core back into service */ 160 mov x0, x6 161#if (SOC_CORE_RESTART) 162 bl _soc_core_restart /* 0-5 */ 163#endif 164 mov x0, #PSCI_E_SUCCESS 165 b cpu_on_done 166 167/* this is where we release a core that is being held in wfe */ 168core_in_wfe: 169 /* x6 = core mask (lsb) */ 170 171 /* set core state in data area */ 172 mov x0, x6 173 mov x1, #CORE_PENDING 174 bl _setCoreState /* 0-3, [13-15] */ 175 dsb sy 176 isb 177 178 /* put the core back into service */ 179 sev 180 sev 181 isb 182 mov x0, #PSCI_E_SUCCESS 183 184cpu_on_done: 185 /* restore the aarch32/64 non-volatile registers */ 186 ldp x18, x30, [sp], #16 187 ldp x16, x17, [sp], #16 188 ldp x14, x15, [sp], #16 189 ldp x12, x13, [sp], #16 190 ldp x10, x11, [sp], #16 191 ldp x8, x9, [sp], #16 192 ldp x6, x7, [sp], #16 193 ldp x4, x5, [sp], #16 194 b psci_completed 195endfunc _psci_cpu_on 196 197#endif 198 199 200#if (SOC_CORE_OFF) 201 202.global _psci_cpu_prep_off 203.global _psci_cpu_off_wfi 204 205/* 206 * void _psci_cpu_prep_off(u_register_t core_mask) 207 * this function performs the SoC-specific programming prior 208 * to shutting the core down 209 * x0 = core_mask 210 * 211 * called from C, so save the non-volatile regs 212 * save these as pairs of registers to maintain the 213 * required 16-byte alignment on the stack 214 */ 215 216func _psci_cpu_prep_off 217 218 stp x4, x5, [sp, #-16]! 219 stp x6, x7, [sp, #-16]! 220 stp x8, x9, [sp, #-16]! 221 stp x10, x11, [sp, #-16]! 222 stp x12, x13, [sp, #-16]! 223 stp x14, x15, [sp, #-16]! 224 stp x16, x17, [sp, #-16]! 225 stp x18, x30, [sp, #-16]! 226 227 mov x10, x0 /* x10 = core_mask */ 228 229 /* the core does not return from cpu_off, so no need 230 * to save/restore non-volatile registers 231 */ 232 233 /* mask interrupts by setting DAIF[7:4] to 'b1111 */ 234 msr DAIFSet, #0xF 235 236 /* read cpuectlr and save current value */ 237 mrs x4, CPUECTLR_EL1 238 mov x1, #CPUECTLR_DATA 239 mov x2, x4 240 mov x0, x10 241 bl _setCoreData 242 243 /* remove the core from coherency */ 244 bic x4, x4, #CPUECTLR_SMPEN_MASK 245 msr CPUECTLR_EL1, x4 246 247 /* save scr_el3 */ 248 mov x0, x10 249 mrs x4, SCR_EL3 250 mov x2, x4 251 mov x1, #SCR_EL3_DATA 252 bl _setCoreData 253 254 /* x4 = scr_el3 */ 255 256 /* secure SGI (FIQ) taken to EL3, set SCR_EL3[FIQ] */ 257 orr x4, x4, #SCR_FIQ_MASK 258 msr scr_el3, x4 259 260 /* x10 = core_mask */ 261 262 /* prep the core for shutdown */ 263 mov x0, x10 264 bl _soc_core_prep_off 265 266 /* restore the aarch32/64 non-volatile registers */ 267 ldp x18, x30, [sp], #16 268 ldp x16, x17, [sp], #16 269 ldp x14, x15, [sp], #16 270 ldp x12, x13, [sp], #16 271 ldp x10, x11, [sp], #16 272 ldp x8, x9, [sp], #16 273 ldp x6, x7, [sp], #16 274 ldp x4, x5, [sp], #16 275 b psci_completed 276endfunc _psci_cpu_prep_off 277 278/* 279 * void _psci_cpu_off_wfi(u_register_t core_mask, u_register_t resume_addr) 280 * - this function shuts down the core 281 * - this function does not return!! 282 */ 283 284func _psci_cpu_off_wfi 285 /* save the wakeup address */ 286 mov x29, x1 287 288 /* x0 = core_mask */ 289 290 /* shutdown the core */ 291 bl _soc_core_entr_off 292 293 /* branch to resume execution */ 294 br x29 295endfunc _psci_cpu_off_wfi 296 297#endif 298 299 300#if (SOC_CORE_RESTART) 301 302.global _psci_wakeup 303 304/* 305 * void _psci_wakeup(u_register_t core_mask) 306 * this function performs the SoC-specific programming 307 * after a core wakes up from OFF 308 * x0 = core mask 309 * 310 * called from C, so save the non-volatile regs 311 * save these as pairs of registers to maintain the 312 * required 16-byte alignment on the stack 313 */ 314 315func _psci_wakeup 316 317 stp x4, x5, [sp, #-16]! 318 stp x6, x7, [sp, #-16]! 319 stp x8, x9, [sp, #-16]! 320 stp x10, x11, [sp, #-16]! 321 stp x12, x13, [sp, #-16]! 322 stp x14, x15, [sp, #-16]! 323 stp x16, x17, [sp, #-16]! 324 stp x18, x30, [sp, #-16]! 325 326 mov x4, x0 /* x4 = core mask */ 327 328 /* restore scr_el3 */ 329 mov x0, x4 330 mov x1, #SCR_EL3_DATA 331 bl _getCoreData 332 /* x0 = saved scr_el3 */ 333 msr SCR_EL3, x0 334 335 /* x4 = core mask */ 336 337 /* restore CPUECTLR */ 338 mov x0, x4 339 mov x1, #CPUECTLR_DATA 340 bl _getCoreData 341 orr x0, x0, #CPUECTLR_SMPEN_MASK 342 msr CPUECTLR_EL1, x0 343 344 /* x4 = core mask */ 345 346 /* start the core back up */ 347 mov x0, x4 348 bl _soc_core_exit_off 349 350 /* restore the aarch32/64 non-volatile registers 351 */ 352 ldp x18, x30, [sp], #16 353 ldp x16, x17, [sp], #16 354 ldp x14, x15, [sp], #16 355 ldp x12, x13, [sp], #16 356 ldp x10, x11, [sp], #16 357 ldp x8, x9, [sp], #16 358 ldp x6, x7, [sp], #16 359 ldp x4, x5, [sp], #16 360 b psci_completed 361endfunc _psci_wakeup 362 363#endif 364 365 366#if (SOC_SYSTEM_RESET) 367 368.global _psci_system_reset 369 370func _psci_system_reset 371 372 /* system reset is mandatory 373 * system reset is soc-specific 374 * Note: under no circumstances do we return from this call 375 */ 376 bl _soc_sys_reset 377endfunc _psci_system_reset 378 379#endif 380 381 382#if (SOC_SYSTEM_OFF) 383 384.global _psci_system_off 385 386func _psci_system_off 387 388 /* system off is mandatory 389 * system off is soc-specific 390 * Note: under no circumstances do we return from this call */ 391 b _soc_sys_off 392endfunc _psci_system_off 393 394#endif 395 396 397#if (SOC_CORE_STANDBY) 398 399.global _psci_core_entr_stdby 400.global _psci_core_prep_stdby 401.global _psci_core_exit_stdby 402 403/* 404 * void _psci_core_entr_stdby(u_register_t core_mask) - this 405 * is the fast-path for simple core standby 406 */ 407 408func _psci_core_entr_stdby 409 stp x4, x5, [sp, #-16]! 410 stp x6, x30, [sp, #-16]! 411 412 mov x5, x0 /* x5 = core mask */ 413 414 /* save scr_el3 */ 415 mov x0, x5 416 mrs x4, SCR_EL3 417 mov x2, x4 418 mov x1, #SCR_EL3_DATA 419 bl _setCoreData 420 421 /* x4 = SCR_EL3 422 * x5 = core mask 423 */ 424 425 /* allow interrupts @ EL3 */ 426 orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK) 427 msr SCR_EL3, x4 428 429 /* x5 = core mask */ 430 431 /* put the core into standby */ 432 mov x0, x5 433 bl _soc_core_entr_stdby 434 435 /* restore scr_el3 */ 436 mov x0, x5 437 mov x1, #SCR_EL3_DATA 438 bl _getCoreData 439 /* x0 = saved scr_el3 */ 440 msr SCR_EL3, x0 441 442 ldp x6, x30, [sp], #16 443 ldp x4, x5, [sp], #16 444 isb 445 ret 446endfunc _psci_core_entr_stdby 447 448/* 449 * void _psci_core_prep_stdby(u_register_t core_mask) - this 450 * sets up the core to enter standby state thru the normal path 451 */ 452 453func _psci_core_prep_stdby 454 stp x4, x5, [sp, #-16]! 455 stp x6, x30, [sp, #-16]! 456 457 mov x5, x0 458 459 /* x5 = core mask */ 460 461 /* save scr_el3 */ 462 mov x0, x5 463 mrs x4, SCR_EL3 464 mov x2, x4 465 mov x1, #SCR_EL3_DATA 466 bl _setCoreData 467 468 /* allow interrupts @ EL3 */ 469 orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK) 470 msr SCR_EL3, x4 471 472 /* x5 = core mask */ 473 474 /* call for any SoC-specific programming */ 475 mov x0, x5 476 bl _soc_core_prep_stdby 477 478 ldp x6, x30, [sp], #16 479 ldp x4, x5, [sp], #16 480 isb 481 ret 482endfunc _psci_core_prep_stdby 483 484/* 485 * void _psci_core_exit_stdby(u_register_t core_mask) - this 486 * exits the core from standby state thru the normal path 487 */ 488 489func _psci_core_exit_stdby 490 stp x4, x5, [sp, #-16]! 491 stp x6, x30, [sp, #-16]! 492 493 mov x5, x0 494 495 /* x5 = core mask */ 496 497 /* restore scr_el3 */ 498 mov x0, x5 499 mov x1, #SCR_EL3_DATA 500 bl _getCoreData 501 /* x0 = saved scr_el3 */ 502 msr SCR_EL3, x0 503 504 /* x5 = core mask */ 505 506 /* perform any SoC-specific programming after standby state */ 507 mov x0, x5 508 bl _soc_core_exit_stdby 509 510 ldp x6, x30, [sp], #16 511 ldp x4, x5, [sp], #16 512 isb 513 ret 514endfunc _psci_core_exit_stdby 515 516#endif 517 518 519#if (SOC_CORE_PWR_DWN) 520 521.global _psci_core_prep_pwrdn 522.global _psci_cpu_pwrdn_wfi 523.global _psci_core_exit_pwrdn 524 525/* 526 * void _psci_core_prep_pwrdn_(u_register_t core_mask) 527 * this function prepares the core for power-down 528 * x0 = core mask 529 * 530 * called from C, so save the non-volatile regs 531 * save these as pairs of registers to maintain the 532 * required 16-byte alignment on the stack 533 */ 534 535func _psci_core_prep_pwrdn 536 stp x4, x5, [sp, #-16]! 537 stp x6, x7, [sp, #-16]! 538 stp x8, x9, [sp, #-16]! 539 stp x10, x11, [sp, #-16]! 540 stp x12, x13, [sp, #-16]! 541 stp x14, x15, [sp, #-16]! 542 stp x16, x17, [sp, #-16]! 543 stp x18, x30, [sp, #-16]! 544 545 mov x6, x0 546 547 /* x6 = core mask */ 548 549 /* mask interrupts by setting DAIF[7:4] to 'b1111 */ 550 msr DAIFSet, #0xF 551 552 /* save scr_el3 */ 553 mov x0, x6 554 mrs x4, SCR_EL3 555 mov x2, x4 556 mov x1, #SCR_EL3_DATA 557 bl _setCoreData 558 559 /* allow interrupts @ EL3 */ 560 orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK) 561 msr SCR_EL3, x4 562 563 /* save cpuectlr */ 564 mov x0, x6 565 mov x1, #CPUECTLR_DATA 566 mrs x2, CPUECTLR_EL1 567 bl _setCoreData 568 569 /* x6 = core mask */ 570 571 /* SoC-specific programming for power-down */ 572 mov x0, x6 573 bl _soc_core_prep_pwrdn 574 575 /* restore the aarch32/64 non-volatile registers 576 */ 577 ldp x18, x30, [sp], #16 578 ldp x16, x17, [sp], #16 579 ldp x14, x15, [sp], #16 580 ldp x12, x13, [sp], #16 581 ldp x10, x11, [sp], #16 582 ldp x8, x9, [sp], #16 583 ldp x6, x7, [sp], #16 584 ldp x4, x5, [sp], #16 585 b psci_completed 586endfunc _psci_core_prep_pwrdn 587 588/* 589 * void _psci_cpu_pwrdn_wfi(u_register_t core_mask, u_register_t resume_addr) 590 * this function powers down the core 591 */ 592 593func _psci_cpu_pwrdn_wfi 594 /* save the wakeup address */ 595 mov x29, x1 596 597 /* x0 = core mask */ 598 599 /* shutdown the core */ 600 bl _soc_core_entr_pwrdn 601 602 /* branch to resume execution */ 603 br x29 604endfunc _psci_cpu_pwrdn_wfi 605 606/* 607 * void _psci_core_exit_pwrdn_(u_register_t core_mask) 608 * this function cleans up after a core power-down 609 * x0 = core mask 610 * 611 * called from C, so save the non-volatile regs 612 * save these as pairs of registers to maintain the 613 * required 16-byte alignment on the stack 614 */ 615 616func _psci_core_exit_pwrdn 617 stp x4, x5, [sp, #-16]! 618 stp x6, x7, [sp, #-16]! 619 stp x8, x9, [sp, #-16]! 620 stp x10, x11, [sp, #-16]! 621 stp x12, x13, [sp, #-16]! 622 stp x14, x15, [sp, #-16]! 623 stp x16, x17, [sp, #-16]! 624 stp x18, x30, [sp, #-16]! 625 626 mov x5, x0 /* x5 = core mask */ 627 628 /* restore scr_el3 */ 629 mov x0, x5 630 mov x1, #SCR_EL3_DATA 631 bl _getCoreData 632 /* x0 = saved scr_el3 */ 633 msr SCR_EL3, x0 634 635 /* x5 = core mask */ 636 637 /* restore cpuectlr */ 638 mov x0, x5 639 mov x1, #CPUECTLR_DATA 640 bl _getCoreData 641 /* make sure smp is set */ 642 orr x0, x0, #CPUECTLR_SMPEN_MASK 643 msr CPUECTLR_EL1, x0 644 645 /* x5 = core mask */ 646 647 /* SoC-specific cleanup */ 648 mov x0, x5 649 bl _soc_core_exit_pwrdn 650 651 /* restore the aarch32/64 non-volatile registers 652 */ 653 ldp x18, x30, [sp], #16 654 ldp x16, x17, [sp], #16 655 ldp x14, x15, [sp], #16 656 ldp x12, x13, [sp], #16 657 ldp x10, x11, [sp], #16 658 ldp x8, x9, [sp], #16 659 ldp x6, x7, [sp], #16 660 ldp x4, x5, [sp], #16 661 b psci_completed 662endfunc _psci_core_exit_pwrdn 663 664#endif 665 666#if (SOC_CLUSTER_STANDBY) 667 668.global _psci_clstr_prep_stdby 669.global _psci_clstr_exit_stdby 670 671/* 672 * void _psci_clstr_prep_stdby(u_register_t core_mask) - this 673 * sets up the clstr to enter standby state thru the normal path 674 */ 675 676func _psci_clstr_prep_stdby 677 stp x4, x5, [sp, #-16]! 678 stp x6, x30, [sp, #-16]! 679 680 mov x5, x0 681 682 /* x5 = core mask */ 683 684 /* save scr_el3 */ 685 mov x0, x5 686 mrs x4, SCR_EL3 687 mov x2, x4 688 mov x1, #SCR_EL3_DATA 689 bl _setCoreData 690 691 /* allow interrupts @ EL3 */ 692 orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK) 693 msr SCR_EL3, x4 694 695 /* x5 = core mask */ 696 697 /* call for any SoC-specific programming */ 698 mov x0, x5 699 bl _soc_clstr_prep_stdby 700 701 ldp x6, x30, [sp], #16 702 ldp x4, x5, [sp], #16 703 isb 704 ret 705endfunc _psci_clstr_prep_stdby 706 707/* 708 * void _psci_clstr_exit_stdby(u_register_t core_mask) - this 709 * exits the clstr from standby state thru the normal path 710 */ 711 712func _psci_clstr_exit_stdby 713 stp x4, x5, [sp, #-16]! 714 stp x6, x30, [sp, #-16]! 715 716 mov x5, x0 /* x5 = core mask */ 717 718 /* restore scr_el3 */ 719 mov x0, x5 720 mov x1, #SCR_EL3_DATA 721 bl _getCoreData 722 /* x0 = saved scr_el3 */ 723 msr SCR_EL3, x0 724 725 /* x5 = core mask */ 726 727 /* perform any SoC-specific programming after standby state */ 728 mov x0, x5 729 bl _soc_clstr_exit_stdby 730 731 ldp x6, x30, [sp], #16 732 ldp x4, x5, [sp], #16 733 isb 734 ret 735endfunc _psci_clstr_exit_stdby 736 737#endif 738 739#if (SOC_CLUSTER_PWR_DWN) 740 741.global _psci_clstr_prep_pwrdn 742.global _psci_clstr_exit_pwrdn 743 744/* 745 * void _psci_clstr_prep_pwrdn_(u_register_t core_mask) 746 * this function prepares the cluster+core for power-down 747 * x0 = core mask 748 * 749 * called from C, so save the non-volatile regs 750 * save these as pairs of registers to maintain the 751 * required 16-byte alignment on the stack 752 */ 753 754func _psci_clstr_prep_pwrdn 755 stp x4, x5, [sp, #-16]! 756 stp x6, x7, [sp, #-16]! 757 stp x8, x9, [sp, #-16]! 758 stp x10, x11, [sp, #-16]! 759 stp x12, x13, [sp, #-16]! 760 stp x14, x15, [sp, #-16]! 761 stp x16, x17, [sp, #-16]! 762 stp x18, x30, [sp, #-16]! 763 764 mov x6, x0 /* x6 = core mask */ 765 766 /* mask interrupts by setting DAIF[7:4] to 'b1111 */ 767 msr DAIFSet, #0xF 768 769 /* save scr_el3 */ 770 mov x0, x6 771 mrs x4, SCR_EL3 772 mov x2, x4 773 mov x1, #SCR_EL3_DATA 774 bl _setCoreData 775 776 /* allow interrupts @ EL3 */ 777 orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK) 778 msr SCR_EL3, x4 779 780 /* save cpuectlr */ 781 mov x0, x6 782 mov x1, #CPUECTLR_DATA 783 mrs x2, CPUECTLR_EL1 784 mov x4, x2 785 bl _setCoreData 786 787 /* remove core from coherency */ 788 bic x4, x4, #CPUECTLR_SMPEN_MASK 789 msr CPUECTLR_EL1, x4 790 791 /* x6 = core mask */ 792 793 /* SoC-specific programming for power-down */ 794 mov x0, x6 795 bl _soc_clstr_prep_pwrdn 796 797 /* restore the aarch32/64 non-volatile registers 798 */ 799 ldp x18, x30, [sp], #16 800 ldp x16, x17, [sp], #16 801 ldp x14, x15, [sp], #16 802 ldp x12, x13, [sp], #16 803 ldp x10, x11, [sp], #16 804 ldp x8, x9, [sp], #16 805 ldp x6, x7, [sp], #16 806 ldp x4, x5, [sp], #16 807 b psci_completed 808endfunc _psci_clstr_prep_pwrdn 809 810/* 811 * void _psci_clstr_exit_pwrdn_(u_register_t core_mask) 812 * this function cleans up after a cluster power-down 813 * x0 = core mask 814 * 815 * called from C, so save the non-volatile regs 816 * save these as pairs of registers to maintain the 817 * required 16-byte alignment on the stack 818 */ 819 820func _psci_clstr_exit_pwrdn 821 stp x4, x5, [sp, #-16]! 822 stp x6, x7, [sp, #-16]! 823 stp x8, x9, [sp, #-16]! 824 stp x10, x11, [sp, #-16]! 825 stp x12, x13, [sp, #-16]! 826 stp x14, x15, [sp, #-16]! 827 stp x16, x17, [sp, #-16]! 828 stp x18, x30, [sp, #-16]! 829 830 mov x4, x0 /* x4 = core mask */ 831 832 /* restore scr_el3 */ 833 mov x0, x4 834 mov x1, #SCR_EL3_DATA 835 bl _getCoreData 836 /* x0 = saved scr_el3 */ 837 msr SCR_EL3, x0 838 839 /* x4 = core mask */ 840 841 /* restore cpuectlr */ 842 mov x0, x4 843 mov x1, #CPUECTLR_DATA 844 bl _getCoreData 845 /* make sure smp is set */ 846 orr x0, x0, #CPUECTLR_SMPEN_MASK 847 msr CPUECTLR_EL1, x0 848 849 /* x4 = core mask */ 850 851 /* SoC-specific cleanup */ 852 mov x0, x4 853 bl _soc_clstr_exit_pwrdn 854 855 /* restore the aarch32/64 non-volatile registers 856 */ 857 ldp x18, x30, [sp], #16 858 ldp x16, x17, [sp], #16 859 ldp x14, x15, [sp], #16 860 ldp x12, x13, [sp], #16 861 ldp x10, x11, [sp], #16 862 ldp x8, x9, [sp], #16 863 ldp x6, x7, [sp], #16 864 ldp x4, x5, [sp], #16 865 b psci_completed 866endfunc _psci_clstr_exit_pwrdn 867 868#endif 869 870#if (SOC_SYSTEM_STANDBY) 871 872.global _psci_sys_prep_stdby 873.global _psci_sys_exit_stdby 874 875/* 876 * void _psci_sys_prep_stdby(u_register_t core_mask) - this 877 * sets up the system to enter standby state thru the normal path 878 */ 879 880func _psci_sys_prep_stdby 881 stp x4, x5, [sp, #-16]! 882 stp x6, x30, [sp, #-16]! 883 884 mov x5, x0 /* x5 = core mask */ 885 886 /* save scr_el3 */ 887 mov x0, x5 888 mrs x4, SCR_EL3 889 mov x2, x4 890 mov x1, #SCR_EL3_DATA 891 bl _setCoreData 892 893 /* allow interrupts @ EL3 */ 894 orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK) 895 msr SCR_EL3, x4 896 897 /* x5 = core mask */ 898 899 /* call for any SoC-specific programming */ 900 mov x0, x5 901 bl _soc_sys_prep_stdby 902 903 ldp x6, x30, [sp], #16 904 ldp x4, x5, [sp], #16 905 isb 906 ret 907endfunc _psci_sys_prep_stdby 908 909/* 910 * void _psci_sys_exit_stdby(u_register_t core_mask) - this 911 * exits the system from standby state thru the normal path 912 */ 913 914func _psci_sys_exit_stdby 915 stp x4, x5, [sp, #-16]! 916 stp x6, x30, [sp, #-16]! 917 918 mov x5, x0 919 920 /* x5 = core mask */ 921 922 /* restore scr_el3 */ 923 mov x0, x5 924 mov x1, #SCR_EL3_DATA 925 bl _getCoreData 926 /* x0 = saved scr_el3 */ 927 msr SCR_EL3, x0 928 929 /* x5 = core mask */ 930 931 /* perform any SoC-specific programming after standby state */ 932 mov x0, x5 933 bl _soc_sys_exit_stdby 934 935 ldp x6, x30, [sp], #16 936 ldp x4, x5, [sp], #16 937 isb 938 ret 939endfunc _psci_sys_exit_stdby 940 941#endif 942 943#if (SOC_SYSTEM_PWR_DWN) 944 945.global _psci_sys_prep_pwrdn 946.global _psci_sys_pwrdn_wfi 947.global _psci_sys_exit_pwrdn 948 949/* 950 * void _psci_sys_prep_pwrdn_(u_register_t core_mask) 951 * this function prepares the system+core for power-down 952 * x0 = core mask 953 * 954 * called from C, so save the non-volatile regs 955 * save these as pairs of registers to maintain the 956 * required 16-byte alignment on the stack 957 */ 958 959func _psci_sys_prep_pwrdn 960 stp x4, x5, [sp, #-16]! 961 stp x6, x7, [sp, #-16]! 962 stp x8, x9, [sp, #-16]! 963 stp x10, x11, [sp, #-16]! 964 stp x12, x13, [sp, #-16]! 965 stp x14, x15, [sp, #-16]! 966 stp x16, x17, [sp, #-16]! 967 stp x18, x30, [sp, #-16]! 968 969 mov x6, x0 /* x6 = core mask */ 970 971 /* mask interrupts by setting DAIF[7:4] to 'b1111 */ 972 msr DAIFSet, #0xF 973 974 /* save scr_el3 */ 975 mov x0, x6 976 mrs x4, SCR_EL3 977 mov x2, x4 978 mov x1, #SCR_EL3_DATA 979 bl _setCoreData 980 981 /* allow interrupts @ EL3 */ 982 orr x4, x4, #(SCR_IRQ_MASK | SCR_FIQ_MASK) 983 msr SCR_EL3, x4 984 985 /* save cpuectlr */ 986 mov x0, x6 987 mov x1, #CPUECTLR_DATA 988 mrs x2, CPUECTLR_EL1 989 mov x4, x2 990 bl _setCoreData 991 992 /* remove core from coherency */ 993 bic x4, x4, #CPUECTLR_SMPEN_MASK 994 msr CPUECTLR_EL1, x4 995 996 /* x6 = core mask */ 997 998 /* SoC-specific programming for power-down */ 999 mov x0, x6 1000 bl _soc_sys_prep_pwrdn 1001 1002 /* restore the aarch32/64 non-volatile registers 1003 */ 1004 ldp x18, x30, [sp], #16 1005 ldp x16, x17, [sp], #16 1006 ldp x14, x15, [sp], #16 1007 ldp x12, x13, [sp], #16 1008 ldp x10, x11, [sp], #16 1009 ldp x8, x9, [sp], #16 1010 ldp x6, x7, [sp], #16 1011 ldp x4, x5, [sp], #16 1012 b psci_completed 1013endfunc _psci_sys_prep_pwrdn 1014 1015 1016/* 1017 * void _psci_sys_pwrdn_wfi(u_register_t core_mask, u_register_t resume_addr) 1018 * this function powers down the system 1019 */ 1020 1021func _psci_sys_pwrdn_wfi 1022 /* save the wakeup address */ 1023 mov x29, x1 1024 1025 /* x0 = core mask */ 1026 1027 /* shutdown the system */ 1028 bl _soc_sys_pwrdn_wfi 1029 1030 /* branch to resume execution */ 1031 br x29 1032endfunc _psci_sys_pwrdn_wfi 1033 1034/* 1035 * void _psci_sys_exit_pwrdn_(u_register_t core_mask) 1036 * this function cleans up after a system power-down 1037 * x0 = core mask 1038 * 1039 * Called from C, so save the non-volatile regs 1040 * save these as pairs of registers to maintain the 1041 * required 16-byte alignment on the stack 1042 */ 1043 1044func _psci_sys_exit_pwrdn 1045 1046 stp x4, x5, [sp, #-16]! 1047 stp x6, x7, [sp, #-16]! 1048 stp x8, x9, [sp, #-16]! 1049 stp x10, x11, [sp, #-16]! 1050 stp x12, x13, [sp, #-16]! 1051 stp x14, x15, [sp, #-16]! 1052 stp x16, x17, [sp, #-16]! 1053 stp x18, x30, [sp, #-16]! 1054 1055 mov x4, x0 /* x4 = core mask */ 1056 1057 /* restore scr_el3 */ 1058 mov x0, x4 1059 mov x1, #SCR_EL3_DATA 1060 bl _getCoreData 1061 1062 /* x0 = saved scr_el3 */ 1063 msr SCR_EL3, x0 1064 1065 /* x4 = core mask */ 1066 1067 /* restore cpuectlr */ 1068 mov x0, x4 1069 mov x1, #CPUECTLR_DATA 1070 bl _getCoreData 1071 1072 /* make sure smp is set */ 1073 orr x0, x0, #CPUECTLR_SMPEN_MASK 1074 msr CPUECTLR_EL1, x0 1075 1076 /* x4 = core mask */ 1077 1078 /* SoC-specific cleanup */ 1079 mov x0, x4 1080 bl _soc_sys_exit_pwrdn 1081 1082 /* restore the aarch32/64 non-volatile registers 1083 */ 1084 ldp x18, x30, [sp], #16 1085 ldp x16, x17, [sp], #16 1086 ldp x14, x15, [sp], #16 1087 ldp x12, x13, [sp], #16 1088 ldp x10, x11, [sp], #16 1089 ldp x8, x9, [sp], #16 1090 ldp x6, x7, [sp], #16 1091 ldp x4, x5, [sp], #16 1092 b psci_completed 1093endfunc _psci_sys_exit_pwrdn 1094 1095#endif 1096 1097 1098/* psci std returns */ 1099func psci_disabled 1100 ldr w0, =PSCI_E_DISABLED 1101 b psci_completed 1102endfunc psci_disabled 1103 1104 1105func psci_not_present 1106 ldr w0, =PSCI_E_NOT_PRESENT 1107 b psci_completed 1108endfunc psci_not_present 1109 1110 1111func psci_on_pending 1112 ldr w0, =PSCI_E_ON_PENDING 1113 b psci_completed 1114endfunc psci_on_pending 1115 1116 1117func psci_already_on 1118 ldr w0, =PSCI_E_ALREADY_ON 1119 b psci_completed 1120endfunc psci_already_on 1121 1122 1123func psci_failure 1124 ldr w0, =PSCI_E_INTERN_FAIL 1125 b psci_completed 1126endfunc psci_failure 1127 1128 1129func psci_unimplemented 1130 ldr w0, =PSCI_E_NOT_SUPPORTED 1131 b psci_completed 1132endfunc psci_unimplemented 1133 1134 1135func psci_denied 1136 ldr w0, =PSCI_E_DENIED 1137 b psci_completed 1138endfunc psci_denied 1139 1140 1141func psci_invalid 1142 ldr w0, =PSCI_E_INVALID_PARAMS 1143 b psci_completed 1144endfunc psci_invalid 1145 1146 1147func psci_success 1148 mov x0, #PSCI_E_SUCCESS 1149endfunc psci_success 1150 1151 1152func psci_completed 1153 /* x0 = status code */ 1154 ret 1155endfunc psci_completed 1156