From 2f79114217798c4ca45994d4cb76587a2a549f28 Mon Sep 17 00:00:00 2001
From: Jaby <fenir50@web.de>
Date: Thu, 16 Mar 2023 22:20:43 +0100
Subject: [PATCH] Support IOPortValues

---
 include/PSX/Auxiliary/bits.hpp           |  4 +--
 include/PSX/System/IOPorts/ioport.hpp    | 45 +++++++++++++++++++++---
 include/PSX/System/IOPorts/memory_io.hpp |  2 ++
 3 files changed, 44 insertions(+), 7 deletions(-)

diff --git a/include/PSX/Auxiliary/bits.hpp b/include/PSX/Auxiliary/bits.hpp
index 935d2700..8ef3a671 100644
--- a/include/PSX/Auxiliary/bits.hpp
+++ b/include/PSX/Auxiliary/bits.hpp
@@ -37,12 +37,12 @@ namespace JabyEngine {
 
             template<typename T>
             static constexpr T set_normalized(T raw_value, T value, size_t start_bit, size_t length) {
-                return (clear_normalized(raw_value, start_bit, length) | (value << start_bit));
+                return (clear_normalized(raw_value, start_bit, length) | (crop_value(value, length) << start_bit));
             }
 
             template<typename T>
             static constexpr T get_normalized(T raw_value, size_t start_bit, size_t length) {
-                return (raw_value & range_mask<T>(start_bit, length)) >> start_bit;
+                return crop_value((raw_value & range_mask<T>(start_bit, length)) >> start_bit, length);
             }
         }
 
diff --git a/include/PSX/System/IOPorts/ioport.hpp b/include/PSX/System/IOPorts/ioport.hpp
index 97f1fbdd..7253482d 100644
--- a/include/PSX/System/IOPorts/ioport.hpp
+++ b/include/PSX/System/IOPorts/ioport.hpp
@@ -1,19 +1,20 @@
 #ifndef __JABYENGINE_IOPORT_HPP__
 #define __JABYENGINE_IOPORT_HPP__
 #include "../../Auxiliary/complex_bitmap.hpp"
+#include "../../Auxiliary/types.hpp"
 
 namespace JabyEngine {
     struct IOBitUnset {
-        size_t pos;
+        uint16_t pos;
 
-        constexpr IOBitUnset(size_t bit_pos) : pos(bit_pos) {
+        constexpr IOBitUnset(uint16_t bit_pos) : pos(bit_pos) {
         }
     };
 
     struct IOBitSet {
-        size_t pos;
+        uint16_t pos;
 
-        constexpr IOBitSet(size_t bit_pos) : pos(bit_pos) {
+        constexpr IOBitSet(uint16_t bit_pos) : pos(bit_pos) {
         }
 
         constexpr IOBitUnset operator!() const {
@@ -21,6 +22,26 @@ namespace JabyEngine {
         }
     };
 
+    struct IOValueSet {
+        template<typename T>
+        using IOValueSetPair = pair<IOValueSet, T>;
+
+        uint16_t pos;
+        uint16_t length;
+
+        constexpr IOValueSet(uint16_t pos, uint16_t length) : pos(pos), length(length) {
+        }
+
+        static constexpr IOValueSet from_to(uint16_t first_bit, uint16_t last_bit) {
+            return IOValueSet(first_bit, (last_bit - first_bit) + 1);
+        }
+
+        template<typename T>
+        constexpr IOValueSetPair<T> with(T value) const {
+            return {*this, value};
+        }
+    };
+
     template<typename T>
     struct NormalValue {
         typedef T Value;
@@ -47,10 +68,23 @@ namespace JabyEngine {
             void set(IOBitSet bit) { \
                 this->raw_value = bit::set(this->raw_value, bit.pos); \
             } \
+            \
             void set(IOBitUnset bit) { \
                 this->raw_value = bit::clear(this->raw_value, bit.pos); \
             } \
             \
+            void set(IOValueSet bits, type value) { \
+                this->raw_value = bit::value::set_normalized(this->raw_value, value, bits.pos, bits.length); \
+            } \
+            \
+            void set(const IOValueSet::IOValueSetPair<type>& value) { \
+                this->set(value.first, value.second); \
+            } \
+            \
+            type get(IOValueSet bits) const { \
+                return bit::value::get_normalized(this->raw_value, bits.pos, bits.length); \
+            } \
+            \
             \
             void clear(IOBitSet bit) { \
                 this->raw_value = bit::clear(this->raw_value, bit.pos); \
@@ -58,13 +92,14 @@ namespace JabyEngine {
             \
             \
             bool is_set(IOBitSet bit) const { \
-                return bit::is_set(this->raw_value, bit.pos);\
+                return bit::is_set(this->raw_value, bit.pos); \
             } \
             \
             \
             void operator=(type value) { \
                 this->raw_value = value; \
             } \
+            \
             type operator*() const { \
                 return this->raw_value; \
             } \
diff --git a/include/PSX/System/IOPorts/memory_io.hpp b/include/PSX/System/IOPorts/memory_io.hpp
index dd884d23..bf142144 100644
--- a/include/PSX/System/IOPorts/memory_io.hpp
+++ b/include/PSX/System/IOPorts/memory_io.hpp
@@ -5,6 +5,8 @@
 namespace JabyEngine {
     namespace Memory_IO {
         __declare_io_type(COM_DELAY, uint32_t,
+            static constexpr auto CodyValue = IOValueSet::from_to(2, 4);
+
             void setup() {
                 this->raw_value = 0x1325;
             }