diff options
Diffstat (limited to 'os/hal/ports/SAMA/SAMA5D2x/sama_matrix.c')
-rw-r--r-- | os/hal/ports/SAMA/SAMA5D2x/sama_matrix.c | 17 |
1 files changed, 12 insertions, 5 deletions
diff --git a/os/hal/ports/SAMA/SAMA5D2x/sama_matrix.c b/os/hal/ports/SAMA/SAMA5D2x/sama_matrix.c index 27a3544b5..dc187a5ff 100644 --- a/os/hal/ports/SAMA/SAMA5D2x/sama_matrix.c +++ b/os/hal/ports/SAMA/SAMA5D2x/sama_matrix.c @@ -88,19 +88,26 @@ * @retval false Peripheral is secured.
*
*/
-bool mtxConfigPeriphSecurity(Matrix *mtxp, uint8_t id, bool mode) {
+bool mtxConfigPeriphSecurity(Matrix *mtxp, uint32_t id, bool mode) {
+ uint32_t mask;
+ if (id < 74) {
+ mask = id & 0x1F;
+ }
+ else {
+ mask = (id & 0x1F) - 1;
+ }
mtxDisableWP(mtxp);
if (mode) {
- mtxp->MATRIX_SPSELR[id / 32] |= (MATRIX_SPSELR_NSECP0 << id);
+ mtxp->MATRIX_SPSELR[id / 32] |= (MATRIX_SPSELR_NSECP0 << mask);
}
else {
- mtxp->MATRIX_SPSELR[id / 32] &= ~(MATRIX_SPSELR_NSECP0 << id);
+ mtxp->MATRIX_SPSELR[id / 32] &= ~(MATRIX_SPSELR_NSECP0 << mask);
}
mtxEnableWP(mtxp);
- return (MATRIX0->MATRIX_SPSELR[id / 32] & (MATRIX_SPSELR_NSECP0 << id)) &
- (MATRIX1->MATRIX_SPSELR[id / 32] & (MATRIX_SPSELR_NSECP0 << id));
+ return (MATRIX0->MATRIX_SPSELR[id / 32] & (MATRIX_SPSELR_NSECP0 << mask)) &
+ (MATRIX1->MATRIX_SPSELR[id / 32] & (MATRIX_SPSELR_NSECP0 << mask));
}
/**
|