From 9f91962baee1aade5618a66f436bb8f4039b2368 Mon Sep 17 00:00:00 2001
From: edolomb <none@example.com>
Date: Mon, 15 Jan 2018 20:11:08 +0000
Subject: Fixed Bugs

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11274 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 os/hal/ports/SAMA/SAMA5D2x/sama_matrix.c | 17 ++++++++++++-----
 os/hal/ports/SAMA/SAMA5D2x/sama_matrix.h |  2 +-
 2 files changed, 13 insertions(+), 6 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));
 }
 
 /**
diff --git a/os/hal/ports/SAMA/SAMA5D2x/sama_matrix.h b/os/hal/ports/SAMA/SAMA5D2x/sama_matrix.h
index a29be16da..11d6e8e85 100644
--- a/os/hal/ports/SAMA/SAMA5D2x/sama_matrix.h
+++ b/os/hal/ports/SAMA/SAMA5D2x/sama_matrix.h
@@ -320,7 +320,7 @@
 #ifdef __cplusplus
 extern "C" {
 #endif
- bool mtxConfigPeriphSecurity(Matrix *mtxp, uint8_t id, bool mode);
+ bool mtxConfigPeriphSecurity(Matrix *mtxp, uint32_t id, bool mode);
  void mtxConfigDefaultMaster(Matrix *mtxp, uint8_t slaveID,
                              uint8_t type, uint8_t masterID);
  void mtxConfigSlaveSec(Matrix *mtxp, uint8_t slaveID,
-- 
cgit v1.2.3