From b077c0f2cc2da6ec873af595d5ca1a8135c11738 Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Sun, 31 Jul 2011 18:14:49 +0000
Subject: [PATCH] git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@3186
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 os/hal/platforms/STM32/GPIOv2/pal_lld.c | 76 +++++++++++--------------
 1 file changed, 33 insertions(+), 43 deletions(-)

diff --git a/os/hal/platforms/STM32/GPIOv2/pal_lld.c b/os/hal/platforms/STM32/GPIOv2/pal_lld.c
index 3eba6cf95..5d33adafc 100644
--- a/os/hal/platforms/STM32/GPIOv2/pal_lld.c
+++ b/os/hal/platforms/STM32/GPIOv2/pal_lld.c
@@ -137,51 +137,8 @@ void _pal_lld_init(const PALConfig *config) {
  * @param[in] mode      the mode
  *
  * @notapi
- *
- * n * 1 +
- * n * 4 +
- * n * 16 +
- * n * 64 +
- * n * 256 +
- * n * 1024 +
- * n * 4096 +
- * n * 16384
  */
 #if 1
-void _pal_lld_setgroupmode(ioportid_t port,
-                           ioportmask_t mask,
-                           iomode_t mode) {
-
-  uint32_t m1 = (uint32_t)mask;
-  uint32_t m2 = 0;
-  uint32_t m4l = 0;
-  uint32_t m4h = 0;
-  uint32_t moder = (((mode & PAL_STM32_MODE_MASK) >> 0) & 3) * 0x5555;
-  uint32_t otyper = (((mode & PAL_STM32_OTYPE_MASK) >> 2) & 1) * 0xffff;
-  uint32_t ospeedr = (((mode & PAL_STM32_OSPEED_MASK) >> 3) & 3) * 0x5555;
-  uint32_t pupdr = (((mode & PAL_STM32_PUDR_MASK) >> 5) & 3) * 0x5555;
-  uint32_t afr = (((mode & PAL_STM32_ALTERNATE_MASK) >> 7) & 15) * 0x0f0f;
-  uint32_t bit = 0;
-  while (mask) {
-    if ((mask & 1) != 0) {
-      m2 |= 3 << (bit * 2);
-      if (bit < 8)
-        m4l |= 15 << ((bit & 7) * 4);
-      else
-        m4h |= 15 << ((bit & 7) * 4);
-    }
-    bit++;
-    mask >>= 1;
-  }
-  port->AFRL    = (port->AFRL & ~m4l) | afr;
-  port->AFRH    = (port->AFRH & ~m4h) | afr;
-  port->OSPEEDR = (port->OSPEEDR & ~m2) | ospeedr;
-  port->OTYPER  = (port->OTYPER & ~m1) | otyper;
-  port->PUPDR   = (port->PUPDR & ~m2) | pupdr;
-  port->MODER   = (port->MODER & ~m2) | moder;
-}
-
-#else
 void _pal_lld_setgroupmode(ioportid_t port,
                            ioportmask_t mask,
                            iomode_t mode) {
@@ -213,6 +170,39 @@ void _pal_lld_setgroupmode(ioportid_t port,
     mask >>= 1;
   }
 }
+#else
+void _pal_lld_setgroupmode(ioportid_t port,
+                           ioportmask_t mask,
+                           iomode_t mode) {
+
+  uint32_t m1 = (uint32_t)mask;
+  uint32_t m2 = 0;
+  uint32_t m4l = 0;
+  uint32_t m4h = 0;
+  uint32_t moder = (((mode & PAL_STM32_MODE_MASK) >> 0) & 3) * 0x5555;
+  uint32_t otyper = (((mode & PAL_STM32_OTYPE_MASK) >> 2) & 1) * 0xffff;
+  uint32_t ospeedr = (((mode & PAL_STM32_OSPEED_MASK) >> 3) & 3) * 0x5555;
+  uint32_t pupdr = (((mode & PAL_STM32_PUDR_MASK) >> 5) & 3) * 0x5555;
+  uint32_t afr = (((mode & PAL_STM32_ALTERNATE_MASK) >> 7) & 15) * 0x1111;
+  uint32_t bit = 0;
+  while (mask) {
+    if ((mask & 1) != 0) {
+      m2 |= 3 << bit;
+      if (bit < 16)
+        m4l |= 15 << ((bit & 14) * 2);
+      else
+        m4h |= 15 << ((bit & 14) * 2);
+    }
+    bit += 2;
+    mask >>= 1;
+  }
+  port->AFRL    = (port->AFRL & ~m4l) | (afr & m4l);
+  port->AFRH    = (port->AFRH & ~m4h) | (afr & m4h);
+  port->OSPEEDR = (port->OSPEEDR & ~m2) | (ospeedr & m2);
+  port->OTYPER  = (port->OTYPER & ~m1) | (otyper & m1);
+  port->PUPDR   = (port->PUPDR & ~m2) | (pupdr & m2);
+  port->MODER   = (port->MODER & ~m2) | (moder & m2);
+}
 #endif
 
 #endif /* HAL_USE_PAL */