From ab9c48576c984d67cf3bc66e29e9e154e8c934ce Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Fri, 11 May 2012 18:14:39 +0000
Subject: Small optimization.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@4186 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 os/hal/src/mmcsd.c | 48 +++++++++++++++++++++---------------------------
 1 file changed, 21 insertions(+), 27 deletions(-)

(limited to 'os/hal/src')

diff --git a/os/hal/src/mmcsd.c b/os/hal/src/mmcsd.c
index 44a552cbd..62a3d368d 100644
--- a/os/hal/src/mmcsd.c
+++ b/os/hal/src/mmcsd.c
@@ -48,39 +48,33 @@
 /*===========================================================================*/
 
 /**
- * @brief   Get slice with data from uint32_t[4] array.
+ * @brief   Gets a bit field from a words array.
+ * @note    The bit zero is the LSb of the first word.
+ *
+ * @param[in] data      pointer to the words array
+ * @param[in] end       bit offset of the last bit of the field, inclusive
+ * @param[in] start     bit offset of the first bit of the field, inclusive
+ *
+ * @return              The bits field value, left aligned.
  *
  * @notapi
  */
 static uint32_t mmcsd_get_slice(uint32_t *data, uint32_t end, uint32_t start) {
-  uint32_t word = 0;
-  uint32_t mask = 0;
-
-  chDbgCheck(end >= start, "sdc_get_slice");
+  unsigned startidx, endidx, startoff;
+  uint32_t endmask;
 
-  while ((start - 32 * word) > 31) {
-    word++;
-    data++;
-  }
+  chDbgCheck((end >= start) && ((end - start) < 32), "mmcsd_get_slice");
 
-  end   -= 32 * word;
-  start -= 32 * word;
+  startidx = start / 32;
+  startoff = start % 32;
+  endidx   = end / 32;
+  endmask  = (1 << ((end % 32) + 1)) - 1;
 
-  if (end < 31) {
-    /* Value lays in one word.*/
-    mask = (1 << (end - start + 1)) - 1;
-    return (*data >> start) & mask;
-  }
-  else {
-    /* Value spread on separate words.*/
-    uint32_t lsb, msb;
-    lsb = *data >> start;
-    data++;
-    mask = (1 << (end - 32 + 1)) - 1;
-    msb = *data & mask;
-    msb = msb << (32 - start);
-    return msb | lsb;
-  }
+  /* One or two pieces?*/
+  if (startidx < endidx)
+    return (data[startidx] >> startoff) |               /* Two pieces case. */
+           ((data[endidx] & endmask) << (32 - startoff));
+  return (data[startidx] & endmask) >> startoff;        /* One piece case.  */
 }
 
 /*===========================================================================*/
@@ -105,7 +99,7 @@ uint32_t mmcsdGetCapacity(uint32_t csd[4]) {
     a = mmcsd_get_slice(csd, MMCSD_CSD_10_C_SIZE_SLICE);
     b = mmcsd_get_slice(csd, MMCSD_CSD_10_C_SIZE_MULT_SLICE);
     c = mmcsd_get_slice(csd, MMCSD_CSD_10_READ_BL_LEN_SLICE);
-    return (a + 1) << (b + 2) << (c - 9);   /* 2^9 == MMCSD_BLOCK_SIZE.     */
+    return (a + 1) << (b + 2) << (c - 9);       /* 2^9 == MMCSD_BLOCK_SIZE. */
   case 1:
     /* CSD version 2.0.*/
     return 1024 * (mmcsd_get_slice(csd, MMCSD_CSD_20_C_SIZE_SLICE) + 1);
-- 
cgit v1.2.3