move to a byte swap method in the toolkit

Author: gigapod

src/sfeTk/sfeToolkit.cpp

@@ -23,7 +23,6 @@ ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 #include "sfeToolkit.h"
-#include <cstdint>
 
 /**
  * @brief C function - Runtime check for system byte order

src/sfeTk/sfeToolkit.h

@@ -26,6 +26,9 @@ CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 
 #pragma once
 
+#include <climits>
+#include <cstdint>
+#include <type_traits>
 /**
     @brief Common include file for the core of the SparkFun Electronics Toolkit
 */
@@ -41,8 +44,19 @@ enum class sfeTKByteOrder : uint8_t
 // Use a namespace for the toolkit "utilities and helpers"
 namespace sfeToolkit
 {
-
 // Function to determine the byte order of the system
 sfeTKByteOrder systemByteOrder(void);
 
+// Method to swap the byte order of any unsigned integer - you pick the size. Uses constexpr so it's a compile time
+// operation/inline/optimized
+//
+// from
+// https://stackoverflow.com/questions/36936584/how-to-write-constexpr-swap-function-to-change-endianess-of-an-integer
+//
+template <class T>
+constexpr typename std::enable_if<std::is_unsigned<T>::value, T>::type byte_swap(T i, T j = 0u, std::size_t n = 0u)
+{
+    return n == sizeof(T) ? j : byte_swap<T>(i >> CHAR_BIT, (j << CHAR_BIT) | (i & (T)(unsigned char)(-1)), n + 1);
+}
+
 }; // namespace sfeToolkit

src/sfeTkArdI2C.cpp

@@ -209,7 +209,8 @@ sfeTkError_t sfeTkArdI2C::writeRegisterRegion(uint8_t devReg, const uint8_t *dat
 //
 sfeTkError_t sfeTkArdI2C::writeRegister16Region(uint16_t devReg, const uint8_t *data, size_t length)
 {
-    devReg = ((devReg << 8) & 0xff00) | ((devReg >> 8) & 0x00ff);
+    // devReg = ((devReg << 8) & 0xff00) | ((devReg >> 8) & 0x00ff);
+    devReg = sfeToolkit::byte_swap(devReg);
     return writeRegisterRegionAddress((uint8_t *)&devReg, 2, data, length);
 }
 
@@ -227,7 +228,8 @@ sfeTkError_t sfeTkArdI2C::writeRegister16Region16(uint16_t devReg, const uint16_
         return writeRegisterRegionAddress((uint8_t *)&devReg, 2, (uint8_t *)data, length * 2);
 
     // okay, we need to swap
-    devReg = ((devReg << 8) & 0xff00) | ((devReg >> 8) & 0x00ff);
+    devReg = sfeToolkit::byte_swap(devReg);
+    // devReg = ((devReg << 8) & 0xff00) | ((devReg >> 8) & 0x00ff);
     uint16_t data16[length];
     for (size_t i = 0; i < length; i++)
         data16[i] = ((data[i] << 8) & 0xff00) | ((data[i] >> 8) & 0x00ff);