[CPU] Move saturating add/sub to base/math.h

src/xenia/base/math.h

@@ -14,6 +14,7 @@
 #include <cmath>
 #include <cstdint>
 #include <cstring>
+#include <limits>
 #include <type_traits>
 #include "xenia/base/platform.h"
 
@@ -302,6 +303,38 @@ int64_t m128_i64(const __m128& v) {
 uint16_t float_to_half(float value);
 float half_to_float(uint16_t value);
 
+// http://locklessinc.com/articles/sat_arithmetic/
+template <typename T>
+inline T sat_add(T a, T b) {
+  using TU = std::make_unsigned<T>::type;
+  TU result = TU(a) + TU(b);
+  if (std::is_unsigned<T>::value) {
+    result |= TU(-static_cast<std::make_signed<T>::type>(result < TU(a)));
+  } else {
+    TU overflowed =
+        (TU(a) >> (sizeof(T) * 8 - 1)) + std::numeric_limits<T>::max();
+    if (T((overflowed ^ TU(b)) | ~(TU(b) ^ result)) >= 0) {
+      result = overflowed;
+    }
+  }
+  return T(result);
+}
+template <typename T>
+inline T sat_sub(T a, T b) {
+  using TU = std::make_unsigned<T>::type;
+  TU result = TU(a) - TU(b);
+  if (std::is_unsigned<T>::value) {
+    result &= TU(-static_cast<std::make_signed<T>::type>(result <= TU(a)));
+  } else {
+    TU overflowed =
+        (TU(a) >> (sizeof(T) * 8 - 1)) + std::numeric_limits<T>::max();
+    if (T((overflowed ^ TU(b)) & (overflowed ^ result)) < 0) {
+      result = overflowed;
+    }
+  }
+  return T(result);
+}
+
 }  // namespace xe
 
 #endif  // XENIA_BASE_MATH_H_