[Fault-injection-developer] [PATCH] kmmio patch (based on kprobes)

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

Hi all
	This is updated kmmio patch. Please comment ;->
  - Louis

diff -Nur a/arch/i386/Kconfig b/arch/i386/Kconfig
--- a/arch/i386/Kconfig	2002-12-21 22:51:54.000000000 +0800
+++ b/arch/i386/Kconfig	2002-12-21 22:46:34.000000000 +0800
@@ -1506,6 +1506,14 @@
 	  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 (EXPERIMENTAL)"
+	depends on EXPERIMENTAL
+	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 DEBUG_SLAB
 	bool "Debug memory allocations"
diff -Nur a/arch/i386/kernel/Makefile b/arch/i386/kernel/Makefile
--- a/arch/i386/kernel/Makefile	2002-12-21 22:53:56.000000000 +0800
+++ b/arch/i386/kernel/Makefile	2002-12-21 22:48:11.000000000 +0800
@@ -30,6 +30,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 -Nur a/arch/i386/kernel/kmmio.c b/arch/i386/kernel/kmmio.c
--- a/arch/i386/kernel/kmmio.c	1970-01-01 08:00:00.000000000 +0800
+++ b/arch/i386/kernel/kmmio.c	2002-12-21 22:21:25.000000000 +0800
@@ -0,0 +1,140 @@
+/* 
+ * KMMIO
+ * Benfit many code from kprobes
+ * (C) 2002 Louis Zhuang <lou...@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>
+#include <asm/highmem.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;
+	}
+
+	current_kmmio = p;
+	kmmio_saved_eflags = (regs->eflags & (TF_MASK|IF_MASK));
+	
+	if (is_trigger) p->pre_handler(p, regs, addr);
+	
+	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
);
+	__flush_tlb_one(get_kmmio_fault_page(current_kmmio->addr)->page);
+	
+	regs->eflags &= ~TF_MASK;
+	regs->eflags |= kmmio_saved_eflags;
+
+	current_kmmio = NULL;
+	
+	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((unsigned long)page, _PAGE_PRESENT);
+}
+
+void disarm_kmmio_fault_page(kmmio_addr_t page)
+{
+	(unsigned long)page &= PAGE_MASK;
+	set_pte_bits((unsigned long)page, _PAGE_PRESENT);
+}
diff -Nur a/arch/i386/kernel/traps.c b/arch/i386/kernel/traps.c
--- a/arch/i386/kernel/traps.c	2002-12-21 22:52:41.000000000 +0800
+++ b/arch/i386/kernel/traps.c	2002-12-21 22:20:35.000000000 +0800
@@ -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>
@@ -598,6 +599,9 @@
 	if (post_kprobe_handler(regs))
 		return 1;
 
+	if (post_kmmio_handler(condition, regs))
+		return 1;	
+
 	/* Interrupts not disabled for normal trap handling. */
 	restore_interrupts(regs);
 
diff -Nur a/arch/i386/mm/fault.c b/arch/i386/mm/fault.c
--- a/arch/i386/mm/fault.c	2002-12-21 22:54:30.000000000 +0800
+++ b/arch/i386/mm/fault.c	2002-12-21 22:20:11.000000000 +0800
@@ -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>
@@ -165,6 +166,9 @@
 	if (kprobe_running() && kprobe_fault_handler(regs, 14))
 		return;
 	
+	if (is_kmmio_active() && kmmio_handler(regs, address))
+		return;
+	
 	/* It's safe to allow irq's after cr2 has been saved */
 	if (regs->eflags & X86_EFLAGS_IF)
 		local_irq_enable();
diff -Nur a/include/asm-i386/kmmio.h b/include/asm-i386/kmmio.h
--- a/include/asm-i386/kmmio.h	1970-01-01 08:00:00.000000000 +0800
+++ b/include/asm-i386/kmmio.h	2002-12-21 22:22:19.000000000 +0800
@@ -0,0 +1,24 @@
+/*
+ * Benfit many code from kprobes
+ * (C) 2002 Louis Zhuang <lou...@in...>.
+ */
+
+#ifndef _ASM_KMMIO_H
+#define _ASM_KMMIO_H
+#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 -Nur a/include/linux/kmmio.h b/include/linux/kmmio.h
--- a/include/linux/kmmio.h	1970-01-01 08:00:00.000000000 +0800
+++ b/include/linux/kmmio.h	2002-12-21 22:20:52.000000000 +0800
@@ -0,0 +1,62 @@
+#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;
+};
+
+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 -Nur a/kernel/Makefile b/kernel/Makefile
--- a/kernel/Makefile	2002-12-21 22:54:51.000000000 +0800
+++ b/kernel/Makefile	2002-12-21 22:20:36.000000000 +0800
@@ -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
 
 ifneq ($(CONFIG_IA64),y)
 # According to Alan Modra <al...@li...>, the
-fno-omit-frame-pointer is
diff -Nur a/kernel/kmmio.c b/kernel/kmmio.c
--- a/kernel/kmmio.c	1970-01-01 08:00:00.000000000 +0800
+++ b/kernel/kmmio.c	2002-12-21 22:21:57.000000000 +0800
@@ -0,0 +1,144 @@
+/* Support for MMIO probes.
+ * Benfit 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>
+#include <asm/highmem.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_PAGE_HASH_BITS 4
+#define KMMIO_PAGE_TABLE_SIZE (1 << KMMIO_PAGE_HASH_BITS)
+static struct list_head kmmio_page_table[KMMIO_PAGE_TABLE_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;
+	
+	spin_lock_irq(&kmmio_lock);
+	kmmio_count++;
+	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);
+	flush_tlb_all();
+	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);
+	kmmio_count--;
+	spin_unlock_irq(&kmmio_lock);
+}
+
+struct kmmio_fault_page *get_kmmio_fault_page(kmmio_addr_t page)
+{
+	struct list_head *head, *tmp;
+
+	(unsigned long)page &= PAGE_MASK;
+	head = &kmmio_page_table[hash_ptr(page, KMMIO_PAGE_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_page_table[hash_ptr(f->page,
KMMIO_PAGE_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]);
+	for (i = 0; i < KMMIO_PAGE_TABLE_SIZE; i++) 
+		INIT_LIST_HEAD(&kmmio_page_table[i]);
+	return 0;
+}
+__initcall(init_kmmio);
+
+EXPORT_SYMBOL_GPL(register_kmmio_probe);
+EXPORT_SYMBOL_GPL(unregister_kmmio_probe);