1 // SPDX-License-Identifier: BSD-Source-Code 2 /* 3 * Copyright (c) 2013, Atmel Corporation 4 * Copyright (c) 2017, Timesys Corporation 5 * 6 * All rights reserved. 7 * 8 * Redistribution and use in source and binary forms, with or without 9 * modification, are permitted provided that the following conditions are met: 10 * 11 * - Redistributions of source code must retain the above copyright notice, 12 * this list of conditions and the disclaimer below. 13 * 14 * Atmel's name may not be used to endorse or promote products derived from 15 * this software without specific prior written permission. 16 * 17 * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR 18 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 19 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE 20 * DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, INDIRECT, 21 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 22 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 23 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 24 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 26 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 */ 28 #include <arm32.h> 29 #include <initcall.h> 30 #include <io.h> 31 #include <kernel/dt.h> 32 #include <kernel/pm.h> 33 #include <kernel/panic.h> 34 #include <matrix.h> 35 #include <platform_config.h> 36 #include <sama5d2.h> 37 #include <stdint.h> 38 #include <tz_matrix.h> 39 #include <trace.h> 40 41 #define MATRIX_AXIMX 1 42 #define MATRIX_H64MX 2 43 #define MATRIX_H32MX 3 44 45 #define SECURITY_TYPE_AS 1 46 #define SECURITY_TYPE_NS 2 47 #define SECURITY_TYPE_PS 3 48 49 #define WORLD_NON_SECURE 0 50 #define WORLD_SECURE 1 51 52 #define MATRIX_SPSELR_COUNT 3 53 #define MATRIX_SLAVE_COUNT 15 54 55 struct peri_security { 56 unsigned int peri_id; 57 unsigned int matrix; 58 unsigned int security_type; 59 paddr_t addr; 60 }; 61 62 static const struct peri_security peri_security_array[] = { 63 { 64 .peri_id = AT91C_ID_PMC, 65 .matrix = MATRIX_H64MX, 66 .security_type = SECURITY_TYPE_PS, 67 .addr = AT91C_BASE_PMC, 68 }, 69 { 70 .peri_id = AT91C_ID_ARM, 71 .matrix = MATRIX_H64MX, 72 .security_type = SECURITY_TYPE_PS, 73 }, 74 { 75 .peri_id = AT91C_ID_PIT, 76 .matrix = MATRIX_H32MX, 77 .security_type = SECURITY_TYPE_PS, 78 .addr = AT91C_BASE_PITC, 79 }, 80 { 81 .peri_id = AT91C_ID_WDT, 82 .matrix = MATRIX_H32MX, 83 .security_type = SECURITY_TYPE_PS, 84 .addr = AT91C_BASE_WDT, 85 }, 86 { 87 .peri_id = AT91C_ID_GMAC, 88 .matrix = MATRIX_H32MX, 89 .security_type = SECURITY_TYPE_PS, 90 .addr = AT91C_BASE_GMAC, 91 }, 92 { 93 .peri_id = AT91C_ID_XDMAC0, 94 .matrix = MATRIX_H64MX, 95 .security_type = SECURITY_TYPE_PS, 96 .addr = AT91C_BASE_XDMAC0, 97 }, 98 { 99 .peri_id = AT91C_ID_XDMAC1, 100 .matrix = MATRIX_H64MX, 101 .security_type = SECURITY_TYPE_PS, 102 .addr = AT91C_BASE_XDMAC1, 103 }, 104 { 105 .peri_id = AT91C_ID_ICM, 106 .matrix = MATRIX_H32MX, 107 .security_type = SECURITY_TYPE_PS, 108 .addr = AT91C_BASE_ICM, 109 }, 110 { 111 .peri_id = AT91C_ID_AES, 112 .matrix = MATRIX_H64MX, 113 .security_type = SECURITY_TYPE_PS, 114 .addr = AT91C_BASE_AES, 115 }, 116 { 117 .peri_id = AT91C_ID_AESB, 118 .matrix = MATRIX_H64MX, 119 .security_type = SECURITY_TYPE_PS, 120 .addr = AT91C_BASE_AESB, 121 }, 122 { 123 .peri_id = AT91C_ID_TDES, 124 .matrix = MATRIX_H32MX, 125 .security_type = SECURITY_TYPE_PS, 126 .addr = AT91C_BASE_TDES, 127 }, 128 { 129 .peri_id = AT91C_ID_SHA, 130 .matrix = MATRIX_H64MX, 131 .security_type = SECURITY_TYPE_PS, 132 .addr = AT91C_BASE_SHA, 133 }, 134 { 135 .peri_id = AT91C_ID_MPDDRC, 136 .matrix = MATRIX_H64MX, 137 .security_type = SECURITY_TYPE_PS, 138 .addr = AT91C_BASE_MPDDRC, 139 }, 140 { 141 .peri_id = AT91C_ID_MATRIX1, 142 .matrix = MATRIX_H32MX, 143 .security_type = SECURITY_TYPE_AS, 144 .addr = AT91C_BASE_MATRIX32, 145 }, 146 { 147 .peri_id = AT91C_ID_MATRIX0, 148 .matrix = MATRIX_H64MX, 149 .security_type = SECURITY_TYPE_AS, 150 .addr = AT91C_BASE_MATRIX64, 151 }, 152 { 153 .peri_id = AT91C_ID_SECUMOD, 154 .matrix = MATRIX_H32MX, 155 .security_type = SECURITY_TYPE_AS, 156 .addr = AT91C_BASE_SECUMOD, 157 }, 158 { 159 .peri_id = AT91C_ID_HSMC, 160 .matrix = MATRIX_H32MX, 161 .security_type = SECURITY_TYPE_PS, 162 .addr = AT91C_BASE_HSMC, 163 }, 164 { 165 .peri_id = AT91C_ID_PIOA, 166 .matrix = MATRIX_H32MX, 167 .security_type = SECURITY_TYPE_AS, 168 .addr = AT91C_BASE_PIOA, 169 }, 170 { 171 .peri_id = AT91C_ID_FLEXCOM0, 172 .matrix = MATRIX_H32MX, 173 .security_type = SECURITY_TYPE_PS, 174 .addr = AT91C_BASE_FLEXCOM0, 175 }, 176 { 177 .peri_id = AT91C_ID_FLEXCOM1, 178 .matrix = MATRIX_H32MX, 179 .security_type = SECURITY_TYPE_PS, 180 .addr = AT91C_BASE_FLEXCOM1, 181 }, 182 { 183 .peri_id = AT91C_ID_FLEXCOM2, 184 .matrix = MATRIX_H32MX, 185 .security_type = SECURITY_TYPE_PS, 186 .addr = AT91C_BASE_FLEXCOM2, 187 }, 188 { 189 .peri_id = AT91C_ID_FLEXCOM3, 190 .matrix = MATRIX_H32MX, 191 .security_type = SECURITY_TYPE_PS, 192 .addr = AT91C_BASE_FLEXCOM3, 193 }, 194 { 195 .peri_id = AT91C_ID_FLEXCOM4, 196 .matrix = MATRIX_H32MX, 197 .security_type = SECURITY_TYPE_PS, 198 .addr = AT91C_BASE_FLEXCOM4, 199 }, 200 { 201 .peri_id = AT91C_ID_UART0, 202 .matrix = MATRIX_H32MX, 203 .security_type = SECURITY_TYPE_PS, 204 .addr = AT91C_BASE_UART0, 205 }, 206 { 207 .peri_id = AT91C_ID_UART1, 208 .matrix = MATRIX_H32MX, 209 .security_type = SECURITY_TYPE_PS, 210 .addr = AT91C_BASE_UART1, 211 }, 212 { 213 .peri_id = AT91C_ID_UART2, 214 .matrix = MATRIX_H32MX, 215 .security_type = SECURITY_TYPE_PS, 216 .addr = AT91C_BASE_UART2, 217 }, 218 { 219 .peri_id = AT91C_ID_UART3, 220 .matrix = MATRIX_H32MX, 221 .security_type = SECURITY_TYPE_PS, 222 .addr = AT91C_BASE_UART3, 223 }, 224 { 225 .peri_id = AT91C_ID_UART4, 226 .matrix = MATRIX_H32MX, 227 .security_type = SECURITY_TYPE_PS, 228 .addr = AT91C_BASE_UART4, 229 }, 230 { 231 .peri_id = AT91C_ID_TWI0, 232 .matrix = MATRIX_H32MX, 233 .security_type = SECURITY_TYPE_PS, 234 .addr = AT91C_BASE_TWI0, 235 }, 236 { 237 .peri_id = AT91C_ID_TWI1, 238 .matrix = MATRIX_H32MX, 239 .security_type = SECURITY_TYPE_PS, 240 .addr = AT91C_BASE_TWI1, 241 }, 242 { 243 .peri_id = AT91C_ID_SDMMC0, 244 .matrix = MATRIX_H64MX, 245 .security_type = SECURITY_TYPE_PS, 246 .addr = AT91C_BASE_SDHC0, 247 }, 248 { 249 .peri_id = AT91C_ID_SDMMC1, 250 .matrix = MATRIX_H64MX, 251 .security_type = SECURITY_TYPE_PS, 252 .addr = AT91C_BASE_SDHC1, 253 }, 254 { 255 .peri_id = AT91C_ID_SPI0, 256 .matrix = MATRIX_H32MX, 257 .security_type = SECURITY_TYPE_PS, 258 .addr = AT91C_BASE_SPI0, 259 }, 260 { 261 .peri_id = AT91C_ID_SPI1, 262 .matrix = MATRIX_H32MX, 263 .security_type = SECURITY_TYPE_PS, 264 .addr = AT91C_BASE_SPI1, 265 }, 266 { 267 .peri_id = AT91C_ID_TC0, 268 .matrix = MATRIX_H32MX, 269 .security_type = SECURITY_TYPE_PS, 270 .addr = AT91C_BASE_TC0, 271 }, 272 { 273 .peri_id = AT91C_ID_TC1, 274 .matrix = MATRIX_H32MX, 275 .security_type = SECURITY_TYPE_PS, 276 .addr = AT91C_BASE_TC1, 277 }, 278 { 279 .peri_id = AT91C_ID_PWM, 280 .matrix = MATRIX_H32MX, 281 .security_type = SECURITY_TYPE_PS, 282 .addr = AT91C_BASE_PWMC, 283 }, 284 { 285 .peri_id = AT91C_ID_ADC, 286 .matrix = MATRIX_H32MX, 287 .security_type = SECURITY_TYPE_PS, 288 .addr = AT91C_BASE_ADC, 289 }, 290 { 291 .peri_id = AT91C_ID_UHPHS, 292 .matrix = MATRIX_H32MX, 293 .security_type = SECURITY_TYPE_PS, 294 }, 295 { 296 .peri_id = AT91C_ID_UDPHS, 297 .matrix = MATRIX_H32MX, 298 .security_type = SECURITY_TYPE_PS, 299 .addr = AT91C_BASE_UDPHS, 300 }, 301 { 302 .peri_id = AT91C_ID_SSC0, 303 .matrix = MATRIX_H32MX, 304 .security_type = SECURITY_TYPE_PS, 305 .addr = AT91C_BASE_SSC0, 306 }, 307 { 308 .peri_id = AT91C_ID_SSC1, 309 .matrix = MATRIX_H32MX, 310 .security_type = SECURITY_TYPE_PS, 311 .addr = AT91C_BASE_SSC1, 312 }, 313 { 314 .peri_id = AT91C_ID_LCDC, 315 .matrix = MATRIX_H64MX, 316 .security_type = SECURITY_TYPE_PS, 317 .addr = AT91C_BASE_LCDC, 318 }, 319 { 320 .peri_id = AT91C_ID_ISI, 321 .matrix = MATRIX_H64MX, 322 .security_type = SECURITY_TYPE_PS, 323 .addr = AT91C_BASE_HXISI, 324 }, 325 { 326 .peri_id = AT91C_ID_TRNG, 327 .matrix = MATRIX_H32MX, 328 .security_type = SECURITY_TYPE_PS, 329 .addr = AT91C_BASE_TRNG, 330 }, 331 { 332 .peri_id = AT91C_ID_PDMIC, 333 .matrix = MATRIX_H32MX, 334 .security_type = SECURITY_TYPE_PS, 335 .addr = AT91C_BASE_PDMIC, 336 }, 337 { 338 .peri_id = AT91C_ID_IRQ, 339 .matrix = MATRIX_H32MX, 340 .security_type = SECURITY_TYPE_NS, 341 }, 342 { 343 .peri_id = AT91C_ID_SFC, 344 .matrix = MATRIX_H32MX, 345 .security_type = SECURITY_TYPE_PS, 346 .addr = AT91C_BASE_SFC, 347 }, 348 { 349 .peri_id = AT91C_ID_SECURAM, 350 .matrix = MATRIX_H32MX, 351 .security_type = SECURITY_TYPE_AS, 352 .addr = AT91C_BASE_SECURAM, 353 }, 354 { 355 .peri_id = AT91C_ID_QSPI0, 356 .matrix = MATRIX_H64MX, 357 .security_type = SECURITY_TYPE_PS, 358 .addr = AT91C_BASE_QSPI0, 359 }, 360 { 361 .peri_id = AT91C_ID_QSPI1, 362 .matrix = MATRIX_H64MX, 363 .security_type = SECURITY_TYPE_PS, 364 .addr = AT91C_BASE_QSPI1, 365 }, 366 { 367 .peri_id = AT91C_ID_I2SC0, 368 .matrix = MATRIX_H32MX, 369 .security_type = SECURITY_TYPE_PS, 370 .addr = AT91C_BASE_I2SC0, 371 }, 372 { 373 .peri_id = AT91C_ID_I2SC1, 374 .matrix = MATRIX_H32MX, 375 .security_type = SECURITY_TYPE_PS, 376 .addr = AT91C_BASE_I2SC1, 377 }, 378 { 379 .peri_id = AT91C_ID_CAN0_INT0, 380 .matrix = MATRIX_H32MX, 381 .security_type = SECURITY_TYPE_PS, 382 }, 383 { 384 .peri_id = AT91C_ID_CAN1_INT0, 385 .matrix = MATRIX_H32MX, 386 .security_type = SECURITY_TYPE_PS, 387 }, 388 { 389 .peri_id = AT91C_ID_CLASSD, 390 .matrix = MATRIX_H32MX, 391 .security_type = SECURITY_TYPE_PS, 392 .addr = AT91C_BASE_CLASSD, 393 }, 394 { 395 .peri_id = AT91C_ID_SFR, 396 .matrix = MATRIX_H32MX, 397 .security_type = SECURITY_TYPE_PS, 398 .addr = AT91C_BASE_SFR, 399 }, 400 { 401 .peri_id = AT91C_ID_SAIC, 402 .matrix = MATRIX_H32MX, 403 .security_type = SECURITY_TYPE_AS, 404 .addr = AT91C_BASE_SAIC, 405 }, 406 { 407 .peri_id = AT91C_ID_AIC, 408 .matrix = MATRIX_H32MX, 409 .security_type = SECURITY_TYPE_NS, 410 .addr = AT91C_BASE_AIC, 411 }, 412 { 413 .peri_id = AT91C_ID_L2CC, 414 .matrix = MATRIX_H64MX, 415 .security_type = SECURITY_TYPE_PS, 416 .addr = AT91C_BASE_L2CC, 417 }, 418 { 419 .peri_id = AT91C_ID_CAN0_INT1, 420 .matrix = MATRIX_H32MX, 421 .security_type = SECURITY_TYPE_PS, 422 }, 423 { 424 .peri_id = AT91C_ID_CAN1_INT1, 425 .matrix = MATRIX_H32MX, 426 .security_type = SECURITY_TYPE_PS, 427 }, 428 { 429 .peri_id = AT91C_ID_GMAC_Q1, 430 .matrix = MATRIX_H32MX, 431 .security_type = SECURITY_TYPE_PS, 432 }, 433 { 434 .peri_id = AT91C_ID_GMAC_Q2, 435 .matrix = MATRIX_H32MX, 436 .security_type = SECURITY_TYPE_PS, 437 }, 438 { 439 .peri_id = AT91C_ID_PIOB, 440 .matrix = MATRIX_H32MX, 441 .security_type = SECURITY_TYPE_AS, 442 .addr = AT91C_BASE_PIOB, 443 }, 444 { 445 .peri_id = AT91C_ID_PIOC, 446 .matrix = MATRIX_H32MX, 447 .security_type = SECURITY_TYPE_AS, 448 .addr = AT91C_BASE_PIOC, 449 }, 450 { 451 .peri_id = AT91C_ID_PIOD, 452 .matrix = MATRIX_H32MX, 453 .security_type = SECURITY_TYPE_AS, 454 .addr = AT91C_BASE_PIOD, 455 }, 456 { 457 .peri_id = AT91C_ID_SDMMC0_TIMER, 458 .matrix = MATRIX_H32MX, 459 .security_type = SECURITY_TYPE_PS, 460 }, 461 { 462 .peri_id = AT91C_ID_SDMMC1_TIMER, 463 .matrix = MATRIX_H32MX, 464 .security_type = SECURITY_TYPE_PS, 465 }, 466 { 467 .peri_id = AT91C_ID_SYS, 468 .matrix = MATRIX_H32MX, 469 .security_type = SECURITY_TYPE_PS, 470 .addr = AT91C_BASE_SYSC, 471 }, 472 { 473 .peri_id = AT91C_ID_ACC, 474 .matrix = MATRIX_H32MX, 475 .security_type = SECURITY_TYPE_PS, 476 .addr = AT91C_BASE_ACC, 477 }, 478 { 479 .peri_id = AT91C_ID_RXLP, 480 .matrix = MATRIX_H32MX, 481 .security_type = SECURITY_TYPE_PS, 482 .addr = AT91C_BASE_RXLP, 483 }, 484 { 485 .peri_id = AT91C_ID_SFRBU, 486 .matrix = MATRIX_H32MX, 487 .security_type = SECURITY_TYPE_PS, 488 .addr = AT91C_BASE_SFRBU, 489 }, 490 { 491 .peri_id = AT91C_ID_CHIPID, 492 .matrix = MATRIX_H32MX, 493 .security_type = SECURITY_TYPE_PS, 494 .addr = AT91C_BASE_CHIPID, 495 }, 496 }; 497 498 static void matrix_write(unsigned int base, 499 unsigned int offset, 500 const unsigned int value) 501 { 502 io_write32(offset + base, value); 503 } 504 505 static unsigned int matrix_read(int base, unsigned int offset) 506 { 507 return io_read32(offset + base); 508 } 509 510 void matrix_write_protect_enable(unsigned int matrix_base) 511 { 512 matrix_write(matrix_base, MATRIX_WPMR, 513 MATRIX_WPMR_WPKEY_PASSWD | MATRIX_WPMR_WPEN_ENABLE); 514 } 515 516 void matrix_write_protect_disable(unsigned int matrix_base) 517 { 518 matrix_write(matrix_base, MATRIX_WPMR, MATRIX_WPMR_WPKEY_PASSWD); 519 } 520 521 void matrix_configure_slave_security(unsigned int matrix_base, 522 unsigned int slave, 523 unsigned int srtop_setting, 524 unsigned int srsplit_setting, 525 unsigned int ssr_setting) 526 { 527 matrix_write(matrix_base, MATRIX_SSR(slave), ssr_setting); 528 matrix_write(matrix_base, MATRIX_SRTSR(slave), srtop_setting); 529 matrix_write(matrix_base, MATRIX_SASSR(slave), srsplit_setting); 530 } 531 532 static const struct peri_security *get_peri_security(unsigned int peri_id) 533 { 534 unsigned int i; 535 536 for (i = 0; i < ARRAY_SIZE(peri_security_array); i++) { 537 if (peri_id == peri_security_array[i].peri_id) 538 return &peri_security_array[i]; 539 } 540 541 return NULL; 542 } 543 544 static int matrix_set_periph_world(unsigned int matrix, unsigned int peri_id, 545 unsigned int world) 546 { 547 unsigned int base; 548 unsigned int spselr; 549 unsigned int idx; 550 unsigned int bit; 551 552 idx = peri_id / 32; 553 if (idx > 3) 554 return -1; 555 556 bit = (0x01 << (peri_id % 32)); 557 558 if (matrix == MATRIX_H32MX) 559 base = matrix32_base(); 560 else if (matrix == MATRIX_H64MX) 561 base = matrix64_base(); 562 else 563 return -1; 564 565 spselr = matrix_read(base, MATRIX_SPSELR(idx)); 566 if (world == WORLD_SECURE) 567 spselr &= ~bit; 568 else 569 spselr |= bit; 570 matrix_write(base, MATRIX_SPSELR(idx), spselr); 571 572 return 0; 573 } 574 575 TEE_Result matrix_dt_get_id(const void *fdt, int node, unsigned int *id) 576 { 577 unsigned int i = 0; 578 paddr_t pbase = 0; 579 580 pbase = fdt_reg_base_address(fdt, node); 581 if (pbase == DT_INFO_INVALID_REG) 582 return TEE_ERROR_BAD_PARAMETERS; 583 584 for (i = 0; i < ARRAY_SIZE(peri_security_array); i++) { 585 if (peri_security_array[i].addr == pbase) { 586 *id = peri_security_array[i].peri_id; 587 return TEE_SUCCESS; 588 } 589 } 590 591 return TEE_ERROR_ITEM_NOT_FOUND; 592 } 593 594 int matrix_configure_periph_secure(unsigned int peri_id) 595 { 596 const struct peri_security *psec = NULL; 597 598 psec = get_peri_security(peri_id); 599 if (!psec) 600 return -1; 601 602 return matrix_set_periph_world(psec->matrix, peri_id, WORLD_SECURE); 603 } 604 605 int matrix_configure_periph_non_secure(unsigned int *peri_id_array, 606 unsigned int size) 607 { 608 unsigned int i; 609 unsigned int *peri_id_p; 610 unsigned int matrix; 611 unsigned int peri_id; 612 const struct peri_security *peripheral_sec; 613 int ret; 614 615 if (!peri_id_array || !size) 616 return -1; 617 618 peri_id_p = peri_id_array; 619 for (i = 0; i < size; i++) { 620 peripheral_sec = get_peri_security(*peri_id_p); 621 if (!peripheral_sec) 622 return -1; 623 624 if (peripheral_sec->security_type != SECURITY_TYPE_PS) 625 return -1; 626 627 matrix = peripheral_sec->matrix; 628 peri_id = *peri_id_p; 629 ret = matrix_set_periph_world(matrix, peri_id, 630 WORLD_NON_SECURE); 631 if (ret) 632 return -1; 633 634 peri_id_p++; 635 } 636 637 return 0; 638 } 639 640 #ifdef CFG_PM_ARM32 641 struct matrix_state { 642 uint32_t spselr[MATRIX_SPSELR_COUNT]; 643 uint32_t ssr[MATRIX_SLAVE_COUNT]; 644 uint32_t srtsr[MATRIX_SLAVE_COUNT]; 645 uint32_t sassr[MATRIX_SLAVE_COUNT]; 646 uint32_t meier; 647 uint32_t meimr; 648 }; 649 650 static struct matrix_state matrix32_state; 651 static struct matrix_state matrix64_state; 652 653 static void matrix_save_regs(vaddr_t base, struct matrix_state *state) 654 { 655 int idx = 0; 656 657 for (idx = 0; idx < MATRIX_SPSELR_COUNT; idx++) 658 state->spselr[idx] = matrix_read(base, MATRIX_SPSELR(idx)); 659 660 for (idx = 0; idx < MATRIX_SLAVE_COUNT; idx++) { 661 state->ssr[idx] = matrix_read(base, MATRIX_SSR(idx)); 662 state->srtsr[idx] = matrix_read(base, MATRIX_SRTSR(idx)); 663 state->sassr[idx] = matrix_read(base, MATRIX_SASSR(idx)); 664 } 665 666 state->meier = matrix_read(base, MATRIX_MEIER); 667 state->meimr = matrix_read(base, MATRIX_MEIMR); 668 } 669 670 static void matrix_suspend(void) 671 { 672 matrix_save_regs(matrix32_base(), &matrix32_state); 673 matrix_save_regs(matrix64_base(), &matrix64_state); 674 } 675 676 static void matrix_restore_regs(vaddr_t base, struct matrix_state *state) 677 { 678 int idx = 0; 679 680 matrix_write_protect_disable(base); 681 682 for (idx = 0; idx < MATRIX_SPSELR_COUNT; idx++) 683 matrix_write(base, MATRIX_SPSELR(idx), state->spselr[idx]); 684 685 for (idx = 0; idx < MATRIX_SLAVE_COUNT; idx++) { 686 matrix_write(base, MATRIX_SSR(idx), state->ssr[idx]); 687 matrix_write(base, MATRIX_SRTSR(idx), state->srtsr[idx]); 688 matrix_write(base, MATRIX_SASSR(idx), state->sassr[idx]); 689 } 690 691 matrix_write(base, MATRIX_MEIER, state->meier); 692 matrix_write(base, MATRIX_MEIMR, state->meimr); 693 } 694 695 static void matrix_resume(void) 696 { 697 matrix_restore_regs(matrix32_base(), &matrix32_state); 698 matrix_restore_regs(matrix64_base(), &matrix64_state); 699 } 700 701 static TEE_Result matrix_pm(enum pm_op op, uint32_t pm_hint __unused, 702 const struct pm_callback_handle *hdl __unused) 703 { 704 switch (op) { 705 case PM_OP_RESUME: 706 matrix_resume(); 707 break; 708 case PM_OP_SUSPEND: 709 matrix_suspend(); 710 break; 711 default: 712 panic("Invalid PM operation"); 713 } 714 715 return TEE_SUCCESS; 716 } 717 718 static TEE_Result matrix_pm_init(void) 719 { 720 /* 721 * We can't call matrix_register_pm in matrix_init since allocator is 722 * not ready yet so we just call it later in this driver init callback. 723 */ 724 register_pm_driver_cb(matrix_pm, NULL, "sam-matrix"); 725 726 return TEE_SUCCESS; 727 } 728 driver_init(matrix_pm_init); 729 730 #endif 731