add log2_of and use this function in get_period_head

2 files changed, 51 insertions, 44 deletions

diff --git a/tinyusb/common/common.h b/tinyusb/common/common.h
index e92f07ca6..1e78539b4 100644
--- a/tinyusb/common/common.h
+++ b/tinyusb/common/common.h
@@ -1,45 +1,40 @@
-/*
- * common.h
- *
- *  Created on: Nov 26, 2012
- *      Author: hathach
- */
+/**************************************************************************/
+/*!
+    @file     common.h
+    @author   hathach (tinyusb.org)
 
-/*
- * Software License Agreement (BSD License)
- * Copyright (c) 2013, hathach (tinyusb.org)
- * All rights reserved.
- *
- * 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.
- * 3. The name of the author may not be used to endorse or promote products
- *    derived from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
- *
- * This file is part of the tinyUSB stack.
- */
+    @section LICENSE
 
-/** \file
- *  \brief Common Header File
- *
- *  \note TBD
- */
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2013, hathach (tinyusb.org)
+    All rights reserved.
+
+    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.
+    3. Neither the name of the copyright holders nor the
+    names of its contributors may be used to endorse or promote products
+    derived from this software without specific prior written permission.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''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 HOLDER 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.
+
+    This file is part of the tinyusb stack.
+*/
+/**************************************************************************/
 
 /** \defgroup Group_Common Common Files
  * @{
@@ -171,6 +166,20 @@ static inline uint32_t offset4k(uint32_t value)
 	return (value & 0xFFFUL);
 }
 
+//------------- Mathematics -------------//
+static inline uint8_t log2_of(uint32_t value) ATTR_ALWAYS_INLINE ATTR_CONST;
+static inline uint8_t log2_of(uint32_t value)
+{
+  uint8_t result = 0; // log2 of value is its MSB's position
+
+  while (value >>= 1)
+  {
+    result++;
+  }
+  return result;
+}
+
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/tinyusb/host/ehci/ehci.c b/tinyusb/host/ehci/ehci.c
index 127b9bb0b..43dae3235 100644
--- a/tinyusb/host/ehci/ehci.c
+++ b/tinyusb/host/ehci/ehci.c
@@ -711,10 +711,8 @@ STATIC_ INLINE_ ehci_qhd_t* get_async_head(uint8_t hostid)
 
 STATIC_ INLINE_ ehci_link_t* get_period_head(uint8_t hostid, uint8_t interval_ms)
 {
-  return (ehci_link_t*) (ehci_data.period_head_arr[ hostid_to_data_idx(hostid) ] +
-                                (interval_ms < 2 ? 0 :
-                                 interval_ms < 4 ? 1 :
-                                 interval_ms < EHCI_FRAMELIST_SIZE ? 2 : 3));
+  return (ehci_link_t*) (&ehci_data.period_head_arr[ hostid_to_data_idx(hostid) ]
+                                                    [ log2_of( min8_of(EHCI_FRAMELIST_SIZE, interval_ms) ) ] );
 }
 
 STATIC_ INLINE_ ehci_qhd_t* get_control_qhd(uint8_t dev_addr)