1 files changed, 120 insertions, 0 deletions
diff --git a/tools/src/cpufreq.c b/tools/src/cpufreq.c
new file mode 100644
index 0000000..e7c4f41
--- /dev/null
+++ b/tools/src/cpufreq.c
@@ -0,0 +1,120 @@
+/* SPDX-License-Identifier: MIT */
+
+#include "cpufreq.h"
+#include "adt.h"
+#include "soc.h"
+#include "utils.h"
+
+#define CLUSTER_PSTATE 0x20
+#define CLUSTER_CONFIG 0x6b8
+
+#define CLUSTER_PSTATE_BUSY     BIT(31)
+#define CLUSTER_PSTATE_SET      BIT(25)
+#define CLUSTER_PSTATE_DESIRED2 GENMASK(15, 12)
+#define CLUSTER_PSTATE_DESIRED1 GENMASK(3, 0)
+
+#define CLUSTER_CONFIG_ENABLE BIT(63)
+#define CLUSTER_CONFIG_DVMR1  BIT(32)
+#define CLUSTER_CONFIG_DVMR2  BIT(31)
+
+#define CLUSTER_SWITCH_TIMEOUT 100
+
+struct cluster_t {
+    const char *name;
+    u64 base;
+    bool dvmr;
+    uint32_t boot_pstate;
+};
+
+int cpufreq_init_cluster(const struct cluster_t *cluster)
+{
+    u64 enable = CLUSTER_CONFIG_ENABLE;
+    if (cluster->dvmr)
+        enable |= CLUSTER_CONFIG_DVMR1 | CLUSTER_CONFIG_DVMR2;
+
+    u64 val = read64(cluster->base + CLUSTER_CONFIG);
+    if ((val & enable) != enable) {
+        printf("cpufreq: Configuring cluster %s (dvmr: %d)\n", cluster->name, cluster->dvmr);
+        write64(cluster->base + CLUSTER_CONFIG, val | enable);
+    }
+
+    val = read64(cluster->base + CLUSTER_PSTATE);
+
+    if (FIELD_GET(CLUSTER_PSTATE_DESIRED1, val) != cluster->boot_pstate) {
+        val &= CLUSTER_PSTATE_DESIRED1 | CLUSTER_PSTATE_DESIRED2;
+        val |= CLUSTER_PSTATE_SET | FIELD_PREP(CLUSTER_PSTATE_DESIRED1, cluster->boot_pstate) |
+               FIELD_PREP(CLUSTER_PSTATE_DESIRED2, cluster->boot_pstate);
+        printf("cpufreq: Switching cluster %s to P-State %d\n", cluster->name,
+               cluster->boot_pstate);
+        write64(cluster->base + CLUSTER_PSTATE, val);
+        if (poll32(cluster->base + CLUSTER_PSTATE, CLUSTER_PSTATE_BUSY, 0, CLUSTER_SWITCH_TIMEOUT) <
+            0) {
+            printf("cpufreq: Timed out waiting for cluster %s P-State switch\n", cluster->name);
+            return -1;
+        }
+    }
+
+    return 0;
+}
+
+static const struct cluster_t t8103_clusters[] = {
+    {"ECPU", 0x210e20000, false, 5},
+    {"PCPU", 0x211e20000, true, 7},
+    {},
+};
+
+static const struct cluster_t t6000_clusters[] = {
+    {"ECPU0", 0x210e20000, false, 5},
+    {"PCPU0", 0x211e20000, false, 7},
+    {"PCPU1", 0x212e20000, false, 7},
+    {},
+};
+
+static const struct cluster_t t6002_clusters[] = {
+    {"ECPU0", 0x0210e20000, false, 5},
+    {"PCPU0", 0x0211e20000, false, 7},
+    {"PCPU1", 0x0212e20000, false, 7},
+    {"ECPU1", 0x2210e20000, false, 5},
+    {"PCPU2", 0x2211e20000, false, 7},
+    {"PCPU3", 0x2212e20000, false, 7},
+    {},
+};
+
+static const struct cluster_t t8112_clusters[] = {
+    {"ECPU", 0x210e20000, false, 7},
+    {"PCPU", 0x211e20000, true, 6},
+    {},
+};
+
+int cpufreq_init(void)
+{
+    printf("cpufreq: Initializing clusters\n");
+
+    const struct cluster_t *cluster;
+
+    switch (chip_id) {
+        case T8103:
+            cluster = t8103_clusters;
+            break;
+        case T6000:
+        case T6001:
+            cluster = t6000_clusters;
+            break;
+        case T6002:
+            cluster = t6002_clusters;
+            break;
+        case T8112:
+            cluster = t8112_clusters;
+            break;
+        default:
+            printf("cpufreq: Chip 0x%x is unsupported\n", chip_id);
+            return -1;
+    }
+
+    bool err = false;
+    while (cluster->base) {
+        err |= cpufreq_init_cluster(cluster++);
+    }
+
+    return err ? -1 : 0;
+}