add printk_hook function

include/linux/printk.h

@@ -9,6 +9,7 @@
 #include <linux/cache.h>
 #include <linux/ratelimit_types.h>
 #include <linux/once_lite.h>
+#include <linux/printk_hook.h>
 
 extern const char linux_banner[];
 extern const char linux_proc_banner[];

include/linux/printk_hook.h

@@ -0,0 +1,24 @@
+
+#ifndef __PRINTK_HOOK_
+#define __PRINTK_HOOK_
+
+#ifdef CONFIG_PRINTK_HOOK
+
+#include <stdarg.h>
+
+#define PRINTK_HOOK_ID_EMERGENCY 0
+#define PRINTK_HOOK_ID_ALERT 1
+#define PRINTK_HOOK_ID_CRIT 2
+#define PRINTK_HOOK_ID_ERROR 3
+
+#define PRINTK_HOOK_ID_MAX 32
+
+typedef int (*PRINTK_HOOK_FUNC)(void* priv, char const* fmt, va_list va);
+
+int register_printk_hook(int id, PRINTK_HOOK_FUNC func, void* data);
+void call_printk_hook(int id, char const* fmt, va_list args);
+int unregister_printk_hook(int id, PRINTK_HOOK_FUNC func);
+
+#endif
+
+#endif

init/Kconfig

@@ -1542,6 +1542,13 @@ config PRINTK
 	  very difficult to diagnose system problems, saying N here is
 	  strongly discouraged.
 
+config PRINTK_HOOK
+	default n
+	bool "Enable support for printk hook"
+	depends on PRINTK
+	help
+	  Add your custom hooks in before printk runs.
+
 config BUG
 	bool "BUG() support" if EXPERT
 	default y

kernel/printk/Makefile

@@ -4,3 +4,4 @@ obj-$(CONFIG_PRINTK)	+= printk_safe.o
 obj-$(CONFIG_A11Y_BRAILLE_CONSOLE)	+= braille.o
 obj-$(CONFIG_PRINTK)	+= printk_ringbuffer.o
 obj-$(CONFIG_PRINTK_INDEX)	+= index.o
+obj-$(CONFIG_PRINTK_HOOK) += printk_hook.o

kernel/printk/printk.c

@@ -47,6 +47,7 @@
 #include <linux/sched/clock.h>
 #include <linux/sched/debug.h>
 #include <linux/sched/task_stack.h>
+#include <linux/printk_hook.h>
 
 #include <linux/uaccess.h>
 #include <asm/sections.h>
@@ -2251,6 +2252,14 @@ int vprintk_store(int facility, int level,
 	ret = text_len + trunc_msg_len;
 out:
 	printk_exit_irqrestore(recursion_ptr, irqflags);
+
+	#if defined(CONFIG_PRINTK_HOOK)
+	if (level == LOGLEVEL_EMERG || level == LOGLEVEL_ALERT) {
+		va_copy(args2, args);
+		call_printk_hook(level, fmt, args2);
+		va_end(args2);
+	}
+	#endif
 	return ret;
 }
 

kernel/printk/printk_hook.c

@@ -0,0 +1,132 @@
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/smp.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/mutex.h>
+#include "linux/printk_hook.h"
+#include <stdarg.h>
+
+struct printk_hook_registry {
+    struct list_head node;
+    int id;
+    PRINTK_HOOK_FUNC func;
+    void* priv;
+};
+
+struct printk_hook_manager {
+    int inited;
+    rwlock_t entries_lock[PRINTK_HOOK_ID_MAX];
+    struct list_head printk_hooks[PRINTK_HOOK_ID_MAX];
+};
+
+static struct printk_hook_manager printk_hook_manager;
+static DEFINE_MUTEX(norecall_lock);
+static volatile int called;
+
+static int init_printk_hook(void) {
+    int i;
+    for (i = 0; i < PRINTK_HOOK_ID_MAX; i++) {
+        INIT_LIST_HEAD(printk_hook_manager.printk_hooks + i);
+        rwlock_init(printk_hook_manager.entries_lock + i);
+    }
+    printk_hook_manager.inited = 1;
+    return 0;
+}
+
+int register_printk_hook(int id, PRINTK_HOOK_FUNC func, void* data) {
+    int ret = 0;
+    rwlock_t *lock;
+    struct printk_hook_registry *new;
+
+    if (id < 0 || id >= PRINTK_HOOK_ID_MAX) {
+        return -EINVAL;
+    }
+    mutex_lock(&norecall_lock);
+    if (!printk_hook_manager.inited) {
+        init_printk_hook();
+    }
+    mutex_unlock(&norecall_lock);
+    lock = printk_hook_manager.entries_lock + id;
+    write_lock(lock);
+    new = kmalloc(sizeof(*new), GFP_KERNEL);
+    if (!new) {
+        ret = -ENOMEM;
+        goto end;
+    }
+    new->id = id;
+    new->func = func;
+    new->priv = data;
+    list_add_tail(&(new->node), printk_hook_manager.printk_hooks + id);
+end:
+    write_unlock(lock);
+    return ret;
+}
+EXPORT_SYMBOL(register_printk_hook);
+
+int unregister_printk_hook(int id, PRINTK_HOOK_FUNC func) {
+    rwlock_t *lock;
+    struct list_head *head;
+    struct printk_hook_registry *pos = NULL;
+
+    if (id < 0 || id >= PRINTK_HOOK_ID_MAX) {
+        return -EINVAL;
+    }
+    mutex_lock(&norecall_lock);
+    if (!printk_hook_manager.inited) {
+        init_printk_hook();
+    }
+    mutex_unlock(&norecall_lock);
+    lock = printk_hook_manager.entries_lock + id;
+    write_lock(lock);
+    // loop all id
+    head = printk_hook_manager.printk_hooks + id;
+    list_for_each_entry(pos, head, node) {
+        if (pos->func == func) {
+            list_del(&(pos->node));
+            break;
+        }
+    }
+    if (pos) {
+        kfree(pos);
+    }
+    write_unlock(lock);
+    return 0;
+}
+EXPORT_SYMBOL(unregister_printk_hook);
+
+void call_printk_hook(int id, char const* fmt, va_list args) {
+    va_list args2;
+    rwlock_t *lock;
+    struct printk_hook_registry *pos;
+    struct list_head *head;
+
+    if (id < 0 || id >= PRINTK_HOOK_ID_MAX) {
+        return;
+    }
+    mutex_lock(&norecall_lock);
+    if (!printk_hook_manager.inited) {
+        init_printk_hook();
+    }
+    // prevent recall loop
+    if (called) {
+        mutex_unlock(&norecall_lock);
+        return;
+    }
+    called = 1;
+    mutex_unlock(&norecall_lock);
+    lock = printk_hook_manager.entries_lock + id;
+    read_lock(lock);
+    head = printk_hook_manager.printk_hooks + id;
+    list_for_each_entry(pos, head, node) {
+        va_copy(args2, args);
+        pos->func(pos->priv, fmt, args2);
+        va_end(args2);
+    }
+    read_unlock(lock);
+
+    mutex_lock(&norecall_lock);
+    called = 0;
+    mutex_unlock(&norecall_lock);
+}
+EXPORT_SYMBOL(call_printk_hook);