git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@3188 35acf78f-673a-0410-8e92-d51de3d6d3f4

1 files changed, 45 insertions, 34 deletions

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