From 404be633c1b8f584c86aa4031516cf6a72374058 Mon Sep 17 00:00:00 2001
From: Gwilym Kuiper <gw@ilym.me>
Date: Thu, 15 Apr 2021 23:05:53 +0100
Subject: [PATCH] Add implementation of set bits to save bit twiddling

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

diff --git a/src/memory_mapped.rs b/src/memory_mapped.rs
index 0c0cf6f2..730de380 100644
--- a/src/memory_mapped.rs
+++ b/src/memory_mapped.rs
@@ -1,3 +1,5 @@
+use core::ops;
+
 pub struct MemoryMapped<T> {
     address: *mut T,
 }
@@ -18,6 +20,24 @@ impl<T> MemoryMapped<T> {
     }
 }
 
+impl<T> MemoryMapped<T>
+where
+    T: From<u8>
+        + Copy
+        + ops::Shl<Output = T>
+        + ops::BitAnd<Output = T>
+        + ops::Sub<Output = T>
+        + ops::BitOr<Output = T>
+        + ops::Not<Output = T>,
+{
+    pub fn set_bits(&self, value: T, length: T, shift: T) {
+        let one: T = 1u8.into();
+        let mask: T = (one << length) - one;
+        let current_val = self.get();
+        self.set((current_val & !(mask << shift)) | ((value & mask) << shift));
+    }
+}
+
 pub struct MemoryMapped1DArray<T, const N: usize> {
     array: *mut [T; N],
 }