diff --git a/Makefile b/Makefile index aa79c0e2..cf41cc7c 100644 --- a/Makefile +++ b/Makefile @@ -62,6 +62,7 @@ OBJECTS := \ asc.o \ bootlogo_128.o bootlogo_256.o \ chickens.o \ + clk.o \ cpufreq.o \ dart.o \ dcp.o \ diff --git a/src/clk.c b/src/clk.c new file mode 100644 index 00000000..ec0c77d2 --- /dev/null +++ b/src/clk.c @@ -0,0 +1,36 @@ +/* SPDX-License-Identifier: MIT */ + +#include "clk.h" +#include "adt.h" +#include "types.h" +#include "utils.h" + +#define CLK_MUX GENMASK(27, 24) + +#define NCO_BASE 5 +#define NUM_NCOS 5 + +void clk_init(void) +{ + int path[8]; + int node = adt_path_offset_trace(adt, "/arm-io/mca-switch", path); + + if (node < 0) { + printf("mca-switch node not found!\n"); + return; + } + + u64 mca_clk_base, mca_clk_size; + if (adt_get_reg(adt, path, "reg", 2, &mca_clk_base, &mca_clk_size)) { + printf("Failed to get mca-switch reg property!\n"); + return; + } + + printf("CLK: MCA clock registers @ 0x%lx (0x%lx)\n", mca_clk_base, mca_clk_size); + + unsigned int i; + for (i = 0; i < (mca_clk_size / 4); i++) + mask32(mca_clk_base + 4 * i, CLK_MUX, FIELD_PREP(CLK_MUX, NCO_BASE + min(NUM_NCOS - 1, i))); + + printf("CLK: Initialized %d MCA clock muxes\n", i); +} diff --git a/src/clk.h b/src/clk.h new file mode 100644 index 00000000..bb79fa44 --- /dev/null +++ b/src/clk.h @@ -0,0 +1,8 @@ +/* SPDX-License-Identifier: MIT */ + +#ifndef __CLK_H__ +#define __CLK_H__ + +void clk_init(void); + +#endif diff --git a/src/main.c b/src/main.c index 2a627d54..e8fece08 100644 --- a/src/main.c +++ b/src/main.c @@ -4,6 +4,7 @@ #include "adt.h" #include "aic.h" +#include "clk.h" #include "cpufreq.h" #include "display.h" #include "exception.h" @@ -144,6 +145,7 @@ void m1n1_main(void) aic_init(); wdt_disable(); pmgr_init(); + clk_init(); cpufreq_init(); printf("Initialization complete.\n");