cpufreq: New module to initialize CPU p-states

Signed-off-by: Hector Martin <marcan@marcan.st>

Makefile

@@ -41,6 +41,7 @@ OBJECTS := \
 	aic.o \
 	bootlogo_128.o bootlogo_256.o \
 	chickens.o \
+	cpufreq.o \
 	dart.o \
 	exception.o exception_asm.o \
 	fb.o font.o font_retina.o \

src/cpufreq.c

@@ -0,0 +1,88 @@
+/* SPDX-License-Identifier: MIT */
+
+#include "cpufreq.h"
+#include "adt.h"
+#include "utils.h"
+
+#define CLUSTER_PSTATE 0x20
+#define CLUSTER_CONFIG 0x6b8
+
+#define CLUSTER_PSTATE_
+
+#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},
+    {},
+};
+
+int cpufreq_init(void)
+{
+    printf("cpufreq: Initializing clusters\n");
+
+    const struct cluster_t *cluster;
+
+    switch (chip_id) {
+        case T8103:
+            cluster = t8103_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;
+}

src/cpufreq.h

@@ -0,0 +1,8 @@
+/* SPDX-License-Identifier: MIT */
+
+#ifndef CPUFREQ_H
+#define CPUFREQ_H
+
+int cpufreq_init(void);
+
+#endif

src/main.c

@@ -4,6 +4,7 @@
 
 #include "adt.h"
 #include "aic.h"
+#include "cpufreq.h"
 #include "exception.h"
 #include "fb.h"
 #include "heapblock.h"
@@ -91,6 +92,7 @@ void m1n1_main(void)
     aic_init();
     wdt_disable();
     pmgr_init();
+    cpufreq_init();
 
     printf("Initialization complete.\n");