From 8d527a1f3fd9d03417bb4cc57556f420ab038764 Mon Sep 17 00:00:00 2001
From: Vegard Storheil Eriksen <zyp@jvnv.net>
Date: Sun, 13 Jun 2021 14:03:43 +0200
Subject: [PATCH] soc/software/bios: Allow registering init functions.

---
 litex/soc/software/bios/helpers.c |  8 ++++++++
 litex/soc/software/bios/helpers.h |  1 +
 litex/soc/software/bios/init.h    | 10 ++++++++++
 litex/soc/software/bios/linker.ld |  7 +++++++
 litex/soc/software/bios/main.c    |  2 ++
 5 files changed, 28 insertions(+)
 create mode 100644 litex/soc/software/bios/init.h

diff --git a/litex/soc/software/bios/helpers.c b/litex/soc/software/bios/helpers.c
index 746a792c3..5c1686759 100644
--- a/litex/soc/software/bios/helpers.c
+++ b/litex/soc/software/bios/helpers.c
@@ -10,6 +10,7 @@
 #include "readline.h"
 #include "helpers.h"
 #include "command.h"
+#include "init.h"
 
 extern unsigned int _ftext, _edata_rom;
 
@@ -126,3 +127,10 @@ struct command_struct *command_dispatcher(char *command, int nb_params, char **p
 
 	return NULL;
 }
+
+void init_dispatcher(void)
+{
+	for (const init_func* fp = __bios_init_start; fp != __bios_init_end; fp++) {
+		(*fp)();
+	}
+}
diff --git a/litex/soc/software/bios/helpers.h b/litex/soc/software/bios/helpers.h
index 6f1f2deeb..23853afce 100644
--- a/litex/soc/software/bios/helpers.h
+++ b/litex/soc/software/bios/helpers.h
@@ -5,5 +5,6 @@ void dump_bytes(unsigned int *ptr, int count, unsigned long addr);
 void crcbios(void);
 int get_param(char *buf, char **cmd, char **params);
 struct command_struct *command_dispatcher(char *command, int nb_params, char **params);
+void init_dispatcher(void);
 
 #endif
diff --git a/litex/soc/software/bios/init.h b/litex/soc/software/bios/init.h
new file mode 100644
index 000000000..17720f3a8
--- /dev/null
+++ b/litex/soc/software/bios/init.h
@@ -0,0 +1,10 @@
+#pragma once
+
+typedef void (*init_func)(void);
+
+extern init_func const __bios_init_start[];
+extern init_func const __bios_init_end[];
+
+#define define_init_func(f)                                           \
+    const init_func __bios_init_##f __attribute__((__used__)) \
+    __attribute__((__section__(".bios_init"))) = f
diff --git a/litex/soc/software/bios/linker.ld b/litex/soc/software/bios/linker.ld
index 6ebd50cdd..af6c43be2 100644
--- a/litex/soc/software/bios/linker.ld
+++ b/litex/soc/software/bios/linker.ld
@@ -41,6 +41,13 @@ SECTIONS
 		PROVIDE_HIDDEN (__bios_cmd_end = .);
 	} > rom
 
+	.init :
+	{
+		PROVIDE_HIDDEN (__bios_init_start = .);
+		KEEP(*(.bios_init))
+		PROVIDE_HIDDEN (__bios_init_end = .);
+	} > rom
+
 	.data :
 	{
 		. = ALIGN(8);
diff --git a/litex/soc/software/bios/main.c b/litex/soc/software/bios/main.c
index 5997118f9..4494387af 100644
--- a/litex/soc/software/bios/main.c
+++ b/litex/soc/software/bios/main.c
@@ -173,6 +173,8 @@ int main(int i, char **c)
 	video_framebuffer_dma_enable_write(1);
 #endif
 
+	init_dispatcher();
+
 	if(sdr_ok) {
 		printf("--============== \e[1mBoot\e[0m ==================--\n");
 		boot_sequence();