From af69318aa5d7d93c308d1004a9d088725502bef3 Mon Sep 17 00:00:00 2001
From: Alejandro Javier Asenjo Nitti <alejandro.asenjo88@gmail.com>
Date: Wed, 18 Oct 2023 10:49:56 -0300
Subject: [PATCH] osAiSetFrequency

---
 src/libultra/io/aisetfreq.c | 28 ++++++++++++++++++++++++++--
 1 file changed, 26 insertions(+), 2 deletions(-)

diff --git a/src/libultra/io/aisetfreq.c b/src/libultra/io/aisetfreq.c
index 8bff39d2..29a9cdf5 100644
--- a/src/libultra/io/aisetfreq.c
+++ b/src/libultra/io/aisetfreq.c
@@ -1,3 +1,27 @@
-#include "common.h"
+#include "global.h"
+#include "hardware.h"
 
-#pragma GLOBAL_ASM("asm/us/nonmatchings/libultra/io/aisetfreq/osAiSetFrequency.s")
+extern s32 osViClock;
+
+s32 osAiSetFrequency(u32 freq) {
+    register u32 a1;
+    register s32 a2;
+    register float ftmp;
+    ftmp = osViClock / (float) freq + .5f;
+
+    a1 = ftmp;
+
+    if (a1 < 0x84) {
+        return -1;
+    }
+
+    a2 = (a1 / 66) & 0xff;
+    if (a2 > 16) {
+        a2 = 16;
+    }
+
+    HW_REG(AI_DACRATE_REG, u32) = a1 - 1;
+    HW_REG(AI_BITRATE_REG, u32) = a2 - 1;
+    HW_REG(AI_CONTROL_REG, u32) = 1; // enable dma
+    return osViClock / (s32) a1;
+}