1 files changed, 99 insertions, 44 deletions

diff --git a/bsps/arm/tms570/start/pinmux.c b/bsps/arm/tms570/start/pinmux.c
index 6aec5f7c32..16eb41a129 100644
--- a/bsps/arm/tms570/start/pinmux.c
+++ b/bsps/arm/tms570/start/pinmux.c
@@ -1,13 +1,16 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+
 /**
  * @file
  *
  * @ingroup RTEMSBSPsARMTMS570
  *
- * @brief I/O Multiplexing Module (IOMM) basic support
+ * @brief This source file contains the I/O Multiplexing Module (IOMM) support
+ *   implementation.
  */
 
 /*
- * Copyright (c) 2015 Premysl Houdek <kom541000@gmail.com>
+ * Copyright (C) 2015 Premysl Houdek <kom541000@gmail.com>
  *
  * Google Summer of Code 2014 at
  * Czech Technical University in Prague
@@ -15,17 +18,45 @@
  * 166 36 Praha 6
  * Czech Republic
  *
- * The license and distribution terms for this file may be
- * found in the file LICENSE in this distribution or at
- * http://www.rtems.org/license/LICENSE.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
  */
 
 #include <bsp/tms570.h>
 #include <bsp/tms570-pinmux.h>
 #include <bsp/irq.h>
 
-uint32_t tms570_bsp_pinmmr_kick_key0 = 0x83E70B13U;
-uint32_t tms570_bsp_pinmmr_kick_key1 = 0x95A4F1E0U;
+RTEMS_STATIC_ASSERT(
+  TMS570_PIN_CLEAR_RQ_MASK == TMS570_PIN_FNC_CLEAR << TMS570_PIN_FNC_SHIFT,
+  TMS570_PIN_CONFIG
+);
+
+static inline void
+tms570_bsp_pin_to_pinmmrx(volatile uint32_t **pinmmrx, uint32_t *pin_shift,
+                          uint32_t config)
+{
+  uint32_t pin_num = (config & TMS570_PIN_NUM_MASK) >> TMS570_PIN_NUM_SHIFT;
+  *pinmmrx = TMS570_PINMUX + (pin_num >> 2);
+  *pin_shift = (pin_num & 0x3)*8;
+}
 
 /**
  * @brief select desired function of pin/ball
@@ -110,33 +141,11 @@ void
 tms570_bsp_pin_config_one(uint32_t pin_num_and_fnc)
 {
   rtems_interrupt_level intlev;
-  uint32_t pin_in_alt;
 
   rtems_interrupt_disable(intlev);
-
-  TMS570_IOMM.KICK_REG0 = tms570_bsp_pinmmr_kick_key0;
-  TMS570_IOMM.KICK_REG1 = tms570_bsp_pinmmr_kick_key1;
-
-  pin_in_alt = pin_num_and_fnc & TMS570_PIN_IN_ALT_MASK;
-  if ( pin_in_alt ) {
-    pin_in_alt >>= TMS570_PIN_IN_ALT_SHIFT;
-    if ( pin_in_alt & TMS570_PIN_CLEAR_RQ_MASK ) {
-      tms570_bsp_pin_clear_function(pin_in_alt, TMS570_PIN_FNC_AUTO);
-    } else {
-      tms570_bsp_pin_set_function(pin_in_alt, TMS570_PIN_FNC_AUTO);
-    }
-  }
-
-  pin_num_and_fnc &= TMS570_PIN_NUM_FNC_MASK;
-  if ( pin_num_and_fnc & TMS570_PIN_CLEAR_RQ_MASK ) {
-    tms570_bsp_pin_clear_function(pin_num_and_fnc, TMS570_PIN_FNC_AUTO);
-  } else {
-    tms570_bsp_pin_set_function(pin_num_and_fnc, TMS570_PIN_FNC_AUTO);
-  }
-
-  TMS570_IOMM.KICK_REG0 = 0;
-  TMS570_IOMM.KICK_REG1 = 0;
-
+  tms570_pin_config_prepare();
+  tms570_pin_config_apply(pin_num_and_fnc);
+  tms570_pin_config_complete();
   rtems_interrupt_enable(intlev);
 }
 
@@ -167,29 +176,75 @@ tms570_bsp_pinmmr_config(const uint32_t *pinmmr_values, int reg_start, int reg_c
   if ( reg_count <= 0)
     return;
 
-  TMS570_IOMM.KICK_REG0 = tms570_bsp_pinmmr_kick_key0;
-  TMS570_IOMM.KICK_REG1 = tms570_bsp_pinmmr_kick_key1;
+  tms570_pin_config_prepare();
 
-  pinmmrx = (&TMS570_IOMM.PINMUX.PINMMR0) + reg_start;
+  pinmmrx = TMS570_PINMUX + reg_start;
   pval = pinmmr_values;
   cnt = reg_count;
 
   do {
-    *pinmmrx = *pinmmrx & *pval;
+    *pinmmrx = *pval;
     pinmmrx++;
     pval++;
   } while( --cnt );
 
-  pinmmrx = (&TMS570_IOMM.PINMUX.PINMMR0) + reg_start;
-  pval = pinmmr_values;
-  cnt = reg_count;
+  tms570_pin_config_complete();
+}
 
-  do {
-    *pinmmrx = *pval;
-    pinmmrx++;
-    pval++;
-  } while( --cnt );
+void tms570_pin_config_prepare(void)
+{
+  TMS570_IOMM.KICK_REG0 = 0x83E70B13U;
+  TMS570_IOMM.KICK_REG1 = 0x95A4F1E0U;
+}
+
+static void
+tms570_pin_set_function(uint32_t config)
+{
+  volatile uint32_t *pinmmrx;
+  uint32_t pin_shift;
+  uint32_t pin_fnc;
+  uint32_t bit;
+  uint32_t val;
+
+  tms570_bsp_pin_to_pinmmrx(&pinmmrx, &pin_shift, config);
+  pin_fnc = (config & TMS570_PIN_FNC_MASK) >> TMS570_PIN_FNC_SHIFT;
+  bit = 1U << (pin_fnc + pin_shift);
+  val = *pinmmrx;
+  val &= ~(0xffU << pin_shift);
+
+  if ((config & TMS570_PIN_CLEAR_RQ_MASK) == 0) {
+    val |= bit;
+  }
+
+  *pinmmrx = val;
+}
+
+void tms570_pin_config_apply(uint32_t config)
+{
+  uint32_t pin_in_alt;
+  uint32_t pin_num_and_fnc;
 
+  pin_in_alt = config & TMS570_PIN_IN_ALT_MASK;
+  if (pin_in_alt != 0) {
+    pin_in_alt >>= TMS570_PIN_IN_ALT_SHIFT;
+    tms570_pin_set_function(pin_in_alt);
+  }
+
+  pin_num_and_fnc = config & TMS570_PIN_NUM_FNC_MASK;
+  tms570_pin_set_function(pin_num_and_fnc);
+}
+
+void tms570_pin_config_array_apply(const uint32_t *config, size_t count)
+{
+  size_t i;
+
+  for (i = 0; i < count; ++i) {
+    tms570_pin_config_apply(config[i]);
+  }
+}
+
+void tms570_pin_config_complete(void)
+{
   TMS570_IOMM.KICK_REG0 = 0;
   TMS570_IOMM.KICK_REG1 = 0;
 }