From 0c276798fd9ed7c2d91abd54d8fb2f3a20147130 Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Mon, 1 Aug 2011 14:33:04 +0000
Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@3188
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 os/hal/platforms/STM32/GPIOv2/pal_lld.c | 79 +++++++++++++++++++--------------
 1 file changed, 45 insertions(+), 34 deletions(-)

(limited to 'os/hal')

diff --git a/os/hal/platforms/STM32/GPIOv2/pal_lld.c b/os/hal/platforms/STM32/GPIOv2/pal_lld.c
index 5d33adafc..c84df64f6 100644
--- a/os/hal/platforms/STM32/GPIOv2/pal_lld.c
+++ b/os/hal/platforms/STM32/GPIOv2/pal_lld.c
@@ -143,49 +143,50 @@ void _pal_lld_setgroupmode(ioportid_t port,
                            ioportmask_t mask,
                            iomode_t mode) {
 
-  uint32_t modemask = ((mode & PAL_STM32_MODE_MASK) >> 0) & 3;
-  uint32_t otypemask = ((mode & PAL_STM32_OTYPE_MASK) >> 2) & 1;
-  uint32_t ospeedmask = ((mode & PAL_STM32_OSPEED_MASK) >> 3) & 3;
-  uint32_t pupdrmask = ((mode & PAL_STM32_PUDR_MASK) >> 5) & 15;
-  uint32_t bit = 0;
-  while (mask) {
+  uint32_t moder   = (mode & PAL_STM32_MODE_MASK) >> 0;
+  uint32_t otyper  = (mode & PAL_STM32_OTYPE_MASK) >> 2;
+  uint32_t ospeedr = (mode & PAL_STM32_OSPEED_MASK) >> 3;
+  uint32_t pupdr   = (mode & PAL_STM32_PUDR_MASK) >> 5;
+  uint32_t altr    = (mode & PAL_STM32_ALTERNATE_MASK) >> 7;
+  uint32_t bit     = 0;
+  while (TRUE) {
     if ((mask & 1) != 0) {
-      uint32_t m4 = 15 < ((bit & 7) * 4);
-      uint32_t altmask = ((mode & PAL_STM32_ALTERNATE_MASK) >> 7) <<
-                         ((bit & 7) * 4);
+      uint32_t altrmask, m1, m2, m4;
+
+      altrmask = altr << ((bit & 7) * 4);
+      m4 = 15 << ((bit & 7) * 4);
       if (bit < 8)
-        port->AFRL = (port->AFRL & ~m4) | altmask;
+        port->AFRL = (port->AFRL & ~m4) | altrmask;
       else
-        port->AFRH = (port->AFRH & ~m4) | altmask;
-      port->OTYPER  = (port->OTYPER & ~(1 << bit)) | otypemask;
-      port->OSPEEDR = (port->OSPEEDR & ~(3 << (bit * 2))) | ospeedmask;
-      port->PUPDR   = (port->PUPDR & ~(3 << (bit * 2))) | pupdrmask;
-      port->MODER   = (port->MODER & ~(3 << (bit * 2))) | modemask;
+        port->AFRH = (port->AFRH & ~m4) | altrmask;
+      m1 = 1 << bit;
+      port->OTYPER  = (port->OTYPER & ~m1) | otyper;
+      m2 = 3 << (bit * 2);
+      port->OSPEEDR = (port->OSPEEDR & ~m2) | ospeedr;
+      port->PUPDR   = (port->PUPDR & ~m2) | pupdr;
+      port->MODER   = (port->MODER & ~m2) | moder;
     }
-    modemask <<= 2;
-    otypemask <<= 1;
-    ospeedmask <<= 2;
-    pupdrmask <<= 2;
-    bit++;
     mask >>= 1;
+    if (!mask)
+      return;
+    otyper <<= 1;
+    ospeedr <<= 2;
+    pupdr <<= 2;
+    moder <<= 2;
+    bit++;
   }
 }
 #else
 void _pal_lld_setgroupmode(ioportid_t port,
                            ioportmask_t mask,
                            iomode_t mode) {
-
+  uint32_t afrm, moderm, pupdrm, otyperm, ospeedrm;
   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) {
+  do {
     if ((mask & 1) != 0) {
       m2 |= 3 << bit;
       if (bit < 16)
@@ -195,13 +196,23 @@ void _pal_lld_setgroupmode(ioportid_t port,
     }
     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);
+  } while (mask);
+
+  afrm = ((mode & PAL_STM32_ALTERNATE_MASK) >> 7) * 0x1111;
+  port->AFRL = (port->AFRL & ~m4l) | (afrm & m4l);
+  port->AFRH = (port->AFRH & ~m4h) | (afrm & m4h);
+
+  ospeedrm = ((mode & PAL_STM32_OSPEED_MASK) >> 3) * 0x5555;
+  port->OSPEEDR = (port->OSPEEDR & ~m2) | (ospeedrm & m2);
+
+  otyperm = ((mode & PAL_STM32_OTYPE_MASK) >> 2) * 0xffff;
+  port->OTYPER = (port->OTYPER & ~m1) | (otyperm & m1);
+
+  pupdrm = ((mode & PAL_STM32_PUDR_MASK) >> 5) * 0x5555;
+  port->PUPDR = (port->PUPDR & ~m2) | (pupdrm & m2);
+
+  moderm = ((mode & PAL_STM32_MODE_MASK) >> 0) * 0x5555;
+  port->MODER = (port->MODER & ~m2) | (moderm & m2);
 }
 #endif
 
-- 
cgit v1.2.3