Add some GTE code

include/PSX/GTE/gte.hpp

@@ -1,3 +1,27 @@
 #pragma once
+#include "gte_instruction.hpp"
 
-namespace JabyEngine {}
+namespace JabyEngine {
+    namespace GTE {
+        /*
+            gte_SetGeomOffset(ofx,ofy)
+
+            Load GTE-offset.
+        */
+        static void set_geom_offset(int32_t off_x, int32_t off_y) {
+            __asm__ volatile("sll  $12, %0, 16" :: "r"(off_x), "r"(off_y) : "$12", "$13");
+            __asm__ volatile("sll  $13, %1, 16" :: "r"(off_x), "r"(off_y) : "$12", "$13");
+            __asm__ volatile("ctc2 $12, $24"    :: "r"(off_x), "r"(off_y) : "$12", "$13");
+            __asm__ volatile("ctc2 $13, $25"    :: "r"(off_x), "r"(off_y) : "$12", "$13");
+        }
+
+        /*
+            gte_SetGeomScreen(h)
+            
+            Load distance from viewpoint to screen.
+        */
+        static void set_geom_screen(int32_t h) {
+            __asm__ volatile("ctc2 %0, $26" :: "r"(h));
+        }
+    }
+}

include/PSX/GTE/gte_instruction.hpp

@@ -0,0 +1,21 @@
+#pragma once
+#include "gte_types.hpp"
+
+namespace JabyEngine {
+    namespace GTE {
+        static __always_inline void ldv0(const SVECTOR& vector) {
+            __asm__ volatile("lwc2 $0, 0(%0)":: "r"(&vector));
+            __asm__ volatile("lwc2 $1, 4(%0)":: "r"(&vector));
+        }
+
+        static __always_inline void ldv1(const SVECTOR& vector) {
+            __asm__ volatile("lwc2 $2, 0(%0)":: "r"(&vector));
+            __asm__ volatile("lwc2 $3, 4(%0)":: "r"(&vector));
+        }
+
+        static __always_inline void ldv2(const SVECTOR& vector) {
+            __asm__ volatile("lwc2 $4, 0(%0)":: "r"(&vector));
+            __asm__ volatile("lwc2 $5, 4(%0)":: "r"(&vector));
+        }
+    }
+}

include/PSX/GTE/gte_types.hpp

@@ -0,0 +1,43 @@
+#pragma once
+#include "../jabyengine_defines.h"
+
+namespace JabyEngine {
+    namespace GTE {
+        namespace internal {
+            template<typename T>
+            struct VECTOR {
+                T x;
+                T y;
+                T z;
+                T pad;
+
+                static constexpr VECTOR create() {
+                    return VECTOR::create(0, 0, 0);
+                }
+
+                static constexpr VECTOR create(T x, T y, T z) {
+                    return VECTOR{.x = x, .y = y, .z = z, .pad = 0};
+                }
+            };
+        }
+
+        struct MATRIX {
+            int16_t rot[3][3]; // Rotation matrix
+            int32_t trans[3];  // Translation vector
+
+            static constexpr MATRIX identity() {
+                return MATRIX{
+                    .rot = {
+                        {1, 0, 0},
+                        {0, 1, 0},
+                        {0, 0, 1}
+                    },
+                    .trans = {0, 0, 0}
+                };
+            }
+        };
+
+        using VECTOR  = internal::VECTOR<int32_t>;
+        using SVECTOR = internal::VECTOR<int16_t>;
+    }
+}