From ca9c898c148acc4f30fcaccb0a481062bc063d7b Mon Sep 17 00:00:00 2001
From: Corwin <corwin@kuiper.dev>
Date: Sat, 6 Aug 2022 18:34:03 +0100
Subject: [PATCH] add general set bits

---
 agb/src/memory_mapped.rs | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/agb/src/memory_mapped.rs b/agb/src/memory_mapped.rs
index 57b5bb51..290d865f 100644
--- a/agb/src/memory_mapped.rs
+++ b/agb/src/memory_mapped.rs
@@ -38,6 +38,21 @@ where
     }
 }
 
+pub fn set_bits<T>(current_value: T, value: T, length: usize, shift: usize) -> T
+where
+    T: From<u8>
+        + Copy
+        + ops::Shl<usize, Output = T>
+        + ops::BitAnd<Output = T>
+        + ops::Sub<Output = T>
+        + ops::BitOr<Output = T>
+        + ops::Not<Output = T>,
+{
+    let one: T = 1u8.into();
+    let mask: T = (one << length) - one;
+    (current_value & !(mask << shift)) | ((value & mask) << shift)
+}
+
 pub struct MemoryMapped1DArray<T, const N: usize> {
     array: *mut [T; N],
 }