[Fault-injection-developer] [RFC] kmmio mechanism

[Zhuang, L. <lou...@in...>]

> We'd like split instruction analysis code and general mmio 
> probe code in pf
> module. That is, we'd like support a general mmio probe 
> mechanism interface
> as the infrastructure of pf module. the interface is like
> 
> struct kmmio_probe {
> 	kmmio_addr_t *addr;
> 	kmmio_pre_handler_t pre_handler;
> 	kmmio_post_handler_t post_handler;
> 	void *data; //opaque data structure
> }
> 
> kmmio_probe_register(struct kmmio_probe *);
> kmmio_probe_unregister(struct kmmio_probe *);
> 
> Then, pf module place probe on the address and analysis instruction in
> pre/post_handler and feedback to fi_core.
> 
I've developed a *very* primarily patch against our tip BK tree. This is
only a compileable code, *NOT TEST AT ALL*. All comments?
  - Louis

----------------------------------------------------------------------------
----------------
# This is a BitKeeper generated patch for the following project:
# Project Name: Linux kernel tree
# This patch format is intended for GNU patch command version 2.5 or higher.
# This patch includes the following deltas:
#	           ChangeSet	1.887   -> 1.888  
#	arch/i386/mm/fault.c	1.23    -> 1.24   
#	   arch/i386/Kconfig	1.18    -> 1.19   
#	arch/i386/kernel/traps.c	1.38    -> 1.39   
#	arch/i386/kernel/Makefile	1.33    -> 1.34   
#	     kernel/Makefile	1.26    -> 1.27   
#	               (new)	        -> 1.2     include/linux/kmmio.h
#	               (new)	        -> 1.1     arch/i386/kernel/kmmio.c
#	               (new)	        -> 1.2     kernel/kmmio.c 
#	               (new)	        -> 1.1     include/asm-i386/kmmio.h
#
# The following is the BitKeeper ChangeSet Log
# --------------------------------------------
# 02/12/18	lo...@ha...	1.888
# add kmmio mechanism.
# *NO TEST AT ALL*
# --------------------------------------------
#
diff -Nru a/arch/i386/Kconfig b/arch/i386/Kconfig
--- a/arch/i386/Kconfig	Wed Dec 18 20:02:43 2002
+++ b/arch/i386/Kconfig	Wed Dec 18 20:02:43 2002
@@ -1506,6 +1506,13 @@
 	  register_kprobe(), and providing a callback function.  This is
useful
 	  for kernel debugging, non-intrusive instrumentation and testing.
If
 	  in doubt, say "N".
+config KMMIO
+	bool "KMMIO"
+	depends on KPROBES
+	help
+	  KMMIO is a Kprobes add-ons for placing a probe on MMIO access,
using
+	  register_kmmio(), and providing a callback function. This is
useful
+	  for monitoring driver access specific MMIO address.
 
 config FI
 	bool "Fault Injection (EXPERIMENTAL)"
diff -Nru a/arch/i386/kernel/Makefile b/arch/i386/kernel/Makefile
--- a/arch/i386/kernel/Makefile	Wed Dec 18 20:02:43 2002
+++ b/arch/i386/kernel/Makefile	Wed Dec 18 20:02:43 2002
@@ -31,6 +31,7 @@
 obj-$(CONFIG_EDD)             	+= edd.o
 obj-$(CONFIG_MODULES)		+= module.o
 obj-$(CONFIG_KPROBES)		+= kprobes.o
+obj-$(CONFIG_KMMIO)		+= kmmio.o
 obj-y				+= sysenter.o
 
 EXTRA_AFLAGS   := -traditional
diff -Nru a/arch/i386/kernel/kmmio.c b/arch/i386/kernel/kmmio.c
--- /dev/null	Wed Dec 31 16:00:00 1969
+++ b/arch/i386/kernel/kmmio.c	Wed Dec 18 20:02:43 2002
@@ -0,0 +1,134 @@
+/* 
+ * Support for kernel probes.
+ * (C) 2002 Vamsi Krishna S <vam...@in...>.
+ */
+
+#include <linux/config.h>
+#include <linux/kmmio.h>
+#include <linux/ptrace.h>
+#include <linux/spinlock.h>
+#include <linux/preempt.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <asm/io.h>
+
+static struct kmmio_probe *current_kmmio = NULL;
+static int is_trigger;
+static unsigned long kmmio_saved_eflags;
+/*
+ * Interrupts are disabled on entry as trap3 is an interrupt gate and they
+ * remain disabled thorough out this function.
+ */
+int kmmio_handler(struct pt_regs *regs, unsigned long addr)
+{
+	struct kmmio_probe *p;
+	struct kmmio_fault_page *f;
+
+	/* We're in an interrupt, but this is clear and BUG()-safe. */
+	preempt_disable();
+
+	lock_kmmio();
+
+	is_trigger = 1;
+	f = get_kmmio_fault_page((void *)addr);
+	if (!f) {
+		/* this page fault is not caused by kmmio */
+		/* XXX some pending fault on other cpu may cause problem! */

+		unlock_kmmio();
+		goto no_kmmio;
+	}
+	
+	p = get_kmmio_probe((void *)addr); 
+	if (!p) {
+		
+		/* The fault is caused by kmmio, but no trigger care the
adress */
+		is_trigger = 0;
+		goto no_trigger;
+	}
+
+	current_kmmio = p;
+	kmmio_saved_eflags = (regs->eflags & (TF_MASK|IF_MASK));
+	
+	p->pre_handler(p, regs, addr);
+	
+no_trigger:	
+	regs->eflags |= TF_MASK;
+	regs->eflags &= ~IF_MASK;
+
+	/* We hold lock, now we set present bit in PTE and single step. */
+	disarm_kmmio_fault_page(f->page);
+	
+
+	return 1;
+
+no_kmmio:
+	preempt_enable_no_resched();
+	return 0;
+}
+
+/*
+ * Interrupts are disabled on entry as trap1 is an interrupt gate and they
+ * remain disabled thorough out this function.  And we hold kmmio lock.
+ */
+int post_kmmio_handler(unsigned long condition, struct pt_regs *regs)
+{
+	if (!is_kmmio_active())
+		return 0;
+	if (!current_kmmio)
+		return 0;
+
+	if (is_trigger && current_kmmio->post_handler)
+		current_kmmio->post_handler(current_kmmio, condition, regs);
+
+	arm_kmmio_fault_page(get_kmmio_fault_page(current_kmmio->addr)->page
);
+	regs->eflags &= ~TF_MASK;
+	regs->eflags |= kmmio_saved_eflags;
+	
+	unlock_kmmio();
+	preempt_enable_no_resched();
+
+        /*
+	 * if somebody else is singlestepping across a probe point, eflags
+	 * will have TF set, in which case, continue the remaining
processing
+	 * of do_debug, as if this is not a probe hit.
+	 */
+	if (regs->eflags & TF_MASK)
+		return 0;
+
+	return 1;
+}
+
+static pte_t *get_pte(unsigned long address) {
+        pgd_t *pgd = pgd_offset_k(address);
+	pmd_t *pmd = pmd_offset(pgd, address);
+	if (pmd_large(*pmd))
+		return (pte_t *)pmd;
+	return pte_offset_kernel(pmd, address);
+};
+
+/**
+ * Set/Clear pte bits
+ */
+static void clr_pte_bits(unsigned long addr, unsigned long bitmask) {
+	pte_t *pte;
+	pte = get_pte(addr);
+	set_pte( pte, __pte( pte_val(*pte) & ~bitmask) );	
+};
+
+static void set_pte_bits(unsigned long addr, unsigned long bitmask) {
+	pte_t *pte;
+	pte = get_pte(addr);
+	set_pte( pte, __pte( pte_val(*pte) | bitmask) );	
+};	
+
+void arm_kmmio_fault_page(kmmio_addr_t page)
+{	
+	(unsigned long)page &= PAGE_MASK;
+	clr_pte_bits(page, _PAGE_PRESENT);
+}
+
+void disarm_kmmio_fault_page(kmmio_addr_t page)
+{
+	(unsigned long)page &= PAGE_MASK;
+	set_pte_bits(page, _PAGE_PRESENT);
+}
diff -Nru a/arch/i386/kernel/traps.c b/arch/i386/kernel/traps.c
--- a/arch/i386/kernel/traps.c	Wed Dec 18 20:02:43 2002
+++ b/arch/i386/kernel/traps.c	Wed Dec 18 20:02:43 2002
@@ -25,6 +25,7 @@
 #include <linux/highmem.h>
 #include <linux/kallsyms.h>
 #include <linux/kprobes.h>
+#include <linux/kmmio.h>
 
 #ifdef CONFIG_EISA
 #include <linux/ioport.h>
@@ -47,7 +48,6 @@
 #include <asm/smp.h>
 #include <asm/pgalloc.h>
 #include <asm/arch_hooks.h>
-#include <asm/fi.h>
 
 #include <linux/irq.h>
 #include <linux/module.h>
@@ -599,7 +599,7 @@
 	if (post_kprobe_handler(regs))
 		return 1;
 
-	if (fi_post_page_fault(condition, regs))
+	if (post_kmmio_handler(condition, regs))
 		return 1;	
 
 	/* Interrupts not disabled for normal trap handling. */
diff -Nru a/arch/i386/mm/fault.c b/arch/i386/mm/fault.c
--- a/arch/i386/mm/fault.c	Wed Dec 18 20:02:43 2002
+++ b/arch/i386/mm/fault.c	Wed Dec 18 20:02:43 2002
@@ -20,6 +20,7 @@
 #include <linux/tty.h>
 #include <linux/vt_kern.h>		/* For unblank_screen() */
 #include <linux/kprobes.h>
+#include <linux/kmmio.h>
 
 #include <asm/system.h>
 #include <asm/uaccess.h>
@@ -166,7 +167,7 @@
 	if (kprobe_running() && kprobe_fault_handler(regs, 14))
 		return;
 	
-	if (fi_page_fault(regs, address))
+	if (is_kmmio_active() && kmmio_handler(regs, address))
 		return;
 	
 	/* It's safe to allow irq's after cr2 has been saved */
diff -Nru a/include/asm-i386/kmmio.h b/include/asm-i386/kmmio.h
--- /dev/null	Wed Dec 31 16:00:00 1969
+++ b/include/asm-i386/kmmio.h	Wed Dec 18 20:02:43 2002
@@ -0,0 +1,24 @@
+#ifndef _ASM_KMMIO_H
+#define _ASM_KMMIO_H
+/*
+ *  Dynamic Probes (kprobes) support
+ *  	Vamsi Krishna S <vam...@in...>, July, 2002
+ *	Mailing list: dp...@ww...
+ */
+#include <linux/types.h>
+#include <linux/ptrace.h>
+
+struct pt_regs;
+
+typedef void* kmmio_addr_t;
+
+#ifdef CONFIG_KMMIO
+extern void arm_kmmio_fault_page(kmmio_addr_t page);
+extern void disarm_kmmio_fault_page(kmmio_addr_t page);
+extern int post_kmmio_handler(unsigned long condition, struct pt_regs
*regs);
+extern int kmmio_handler(struct pt_regs *regs, unsigned long addr);
+#else /* !CONFIG_KMMIO */
+static inline int post_kmmio_handler(unsigned long condition, struct
pt_regs *regs) { return 0; }
+static inline int kmmio_handler(struct pt_regs *regs, unsigned long addr) {
return 0; }
+#endif
+#endif /* _ASM_KMMIO_H */
diff -Nru a/include/linux/kmmio.h b/include/linux/kmmio.h
--- /dev/null	Wed Dec 31 16:00:00 1969
+++ b/include/linux/kmmio.h	Wed Dec 18 20:02:43 2002
@@ -0,0 +1,65 @@
+#ifndef _LINUX_KMMIO_H
+#define _LINUX_KMMIO_H
+#include <linux/config.h>
+#include <linux/list.h>
+#include <linux/notifier.h>
+#include <linux/smp.h>
+#include <asm/kmmio.h>
+
+struct kmmio_probe;
+struct kmmio_fault_page;
+struct pt_regs;
+
+typedef void (*kmmio_pre_handler_t)(struct kmmio_probe *, struct pt_regs *,
unsigned long addr);
+typedef void (*kmmio_post_handler_t)(struct kmmio_probe *, unsigned long
condition, struct pt_regs *);
+struct kmmio_probe {
+	struct list_head list;
+
+	/* location of the probe point */
+	kmmio_addr_t addr;
+
+	 /* Called before addr is executed. */
+	kmmio_pre_handler_t pre_handler;
+
+	/* Called after addr is executed, unless... */
+	kmmio_post_handler_t post_handler;
+	
+	/* opaque data structure used by register */
+	void *data;	
+};
+
+struct kmmio_fault_page {
+	struct list_head list;
+
+	/* location of the fault page */
+	kmmio_addr_t page;
+
+	int count;
+};
+
+#ifdef CONFIG_KMMIO
+/* Locks kmmio: irq must be disabled */
+void lock_kmmio(void);
+void unlock_kmmio(void);
+
+/* kmmio is active by some kmmio_probes? */
+static inline int is_kmmio_active(void)
+{
+	extern unsigned int kmmio_count;
+	return kmmio_count;
+}
+
+/* Get the kmmio at this addr (if any).  Must have called lock_kmmio */
+struct kmmio_probe *get_kmmio_probe(kmmio_addr_t addr);
+struct kmmio_fault_page *get_kmmio_fault_page(kmmio_addr_t page);
+int add_kmmio_fault_page(kmmio_addr_t page);
+void release_kmmio_fault_page(kmmio_addr_t page);
+
+int register_kmmio_probe(struct kmmio_probe *p);
+void unregister_kmmio_probe(struct kmmio_probe *p);
+#else
+static inline int is_kmmio_active(void) { return 0; }
+static inline int register_kmmio_probe(struct kmmio_probe *p) { return
-ENOSYS; }
+static inline void unregister_kmmio_probe(struct kmmio_probe *p) { }
+#endif
+#endif /* _LINUX_KMMIO_H */
diff -Nru a/kernel/Makefile b/kernel/Makefile
--- a/kernel/Makefile	Wed Dec 18 20:02:43 2002
+++ b/kernel/Makefile	Wed Dec 18 20:02:43 2002
@@ -23,6 +23,7 @@
 obj-$(CONFIG_SOFTWARE_SUSPEND) += suspend.o
 obj-$(CONFIG_COMPAT) += compat.o
 obj-$(CONFIG_KPROBES) += kprobes.o
+obj-$(CONFIG_KMMIO)   += kmmio.o
 obj-$(CONFIG_FI) += fi_core.o
 
 ifneq ($(CONFIG_IA64),y)
diff -Nru a/kernel/kmmio.c b/kernel/kmmio.c
--- /dev/null	Wed Dec 31 16:00:00 1969
+++ b/kernel/kmmio.c	Wed Dec 18 20:02:43 2002
@@ -0,0 +1,141 @@
+/* Support for MMIO probes.
+ * Stolen many code from kprobes
+ * (C) 2002 Louis Zhuang <lou...@in...>.
+*/
+
+#include <linux/kmmio.h>
+#include <linux/spinlock.h>
+#include <linux/hash.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <asm/cacheflush.h>
+#include <asm/errno.h>
+
+#define KMMIO_HASH_BITS 6
+#define KMMIO_TABLE_SIZE (1 << KMMIO_HASH_BITS)
+
+static struct list_head kmmio_table[KMMIO_TABLE_SIZE];
+
+#define KMMIO_FAULT_HASH_BITS 4
+#define KMMIO_FAULT_HASH_SIZE (1 << KMMIO_FAULT_HASH_BITS)
+static struct list_head kmmio_fault_table[KMMIO_FAULT_HASH_SIZE];
+
+unsigned int kmmio_count = 0;
+static spinlock_t kmmio_lock = SPIN_LOCK_UNLOCKED;
+
+/* Locks kmmio: irqs must be disabled */
+void lock_kmmio(void)
+{
+	spin_lock(&kmmio_lock);
+}
+
+void unlock_kmmio(void)
+{
+	spin_unlock(&kmmio_lock);
+}
+
+/* You have to be holding the kmmio_lock */
+struct kmmio_probe *get_kmmio_probe(void *addr)
+{
+	struct list_head *head, *tmp;
+
+	head = &kmmio_table[hash_ptr(addr, KMMIO_HASH_BITS)];
+	list_for_each(tmp, head) {
+		struct kmmio_probe *p = list_entry(tmp, struct kmmio_probe,
list);
+		if (p->addr == addr)
+			return p;
+	}
+	return NULL;
+}
+
+int register_kmmio_probe(struct kmmio_probe *p)
+{
+	int ret = 0;
+	
+	kmmio_count++;
+	spin_lock_irq(&kmmio_lock);
+	if (get_kmmio_probe(p->addr)) {
+		ret = -EEXIST;
+		goto out;
+	}
+	list_add(&p->list, &kmmio_table[hash_ptr(p->addr,
KMMIO_HASH_BITS)]);
+
+	add_kmmio_fault_page(p->addr);
+	
+ out:
+	spin_unlock_irq(&kmmio_lock);
+	return ret;
+}
+
+void unregister_kmmio_probe(struct kmmio_probe *p)
+{
+	spin_lock_irq(&kmmio_lock);
+	release_kmmio_fault_page(p->addr);
+	list_del(&p->list);
+	spin_unlock_irq(&kmmio_lock);
+	kmmio_count--;
+}
+
+struct kmmio_fault_page *get_kmmio_fault_page(kmmio_addr_t page)
+{
+	struct list_head *head, *tmp;
+
+	(unsigned long)page &= PAGE_MASK;
+	head = &kmmio_fault_table[hash_ptr(page, KMMIO_HASH_BITS)];
+	list_for_each(tmp, head) {
+		struct kmmio_fault_page *p = list_entry(tmp, struct
kmmio_fault_page, list);
+		if (p->page == page)
+			return p;
+	}
+	return NULL;
+}
+
+int add_kmmio_fault_page(kmmio_addr_t page)
+{
+	struct kmmio_fault_page *f;
+	
+	(unsigned long)page &= PAGE_MASK;
+	f = get_kmmio_fault_page(page);
+	if (f) {
+		f->count++;
+		return 0;
+	}
+	f = (struct kmmio_fault_page *)kmalloc(sizeof(*f), GFP_KERNEL);
+	f->count = 1;
+	f->page = page;
+	list_add(&f->list, &kmmio_fault_table[hash_ptr(f->page,
KMMIO_HASH_BITS)]);
+
+	arm_kmmio_fault_page(f->page);
+	return 0;
+}
+
+void release_kmmio_fault_page(kmmio_addr_t page)
+{
+	struct kmmio_fault_page *f;
+
+	(unsigned long)page &= PAGE_MASK;
+	f = get_kmmio_fault_page(page);
+	if (!f)	return;
+	f->count--;
+	if(!f->count) {
+		disarm_kmmio_fault_page(f->page);
+		list_del(&f->list);
+	}
+}
+
+static int __init init_kmmio(void)
+{
+	int i;
+
+	/* FIXME allocate the probe table, currently defined statically */
+	/* initialize all list heads */
+	for (i = 0; i < KMMIO_TABLE_SIZE; i++)
+		INIT_LIST_HEAD(&kmmio_table[i]);
+
+	return 0;
+}
+__initcall(init_kmmio);
+
+EXPORT_SYMBOL_GPL(register_kmmio_probe);
+EXPORT_SYMBOL_GPL(unregister_kmmio_probe);