Index: madwifi-ng-r2568-20070710/Makefile =================================================================== --- madwifi-ng-r2568-20070710.orig/Makefile 2007-09-25 01:33:00.437395704 +0200 +++ madwifi-ng-r2568-20070710/Makefile 2007-09-25 01:33:26.154861261 +0200 @@ -52,7 +52,7 @@ DIRS_MODULES = $(ATH) $(ATH_HAL) $(ATH_RATE) $(WLAN) -obj-y := ath/ ath_hal/ ath_rate/ net80211/ +obj-y := ath/ ath_hal/ ath_rate/ net80211/ $(if $(MMIOTRACE),mmiotrace/) all: modules tools Index: madwifi-ng-r2568-20070710/mmiotrace/kmmio.c =================================================================== --- /dev/null 1970-01-01 00:00:00.000000000 +0000 +++ madwifi-ng-r2568-20070710/mmiotrace/kmmio.c 2007-09-25 01:33:26.190863309 +0200 @@ -0,0 +1,367 @@ +/* Support for MMIO probes. + * Benfit many code from kprobes + * (C) 2002 Louis Zhuang . +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "kmmio.h" + +#define KMMIO_HASH_BITS 6 +#define KMMIO_TABLE_SIZE (1 << KMMIO_HASH_BITS) + +static LIST_HEAD(kmmio_probes); + +#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; +unsigned int handler_registered = 0; +static spinlock_t kmmio_lock = SPIN_LOCK_UNLOCKED; +static int cpu = -1; +static struct kmmio_fault_page *cur_page = NULL; +static struct kmmio_probe *cur_probe = NULL; +static unsigned long kmmio_saved_eflags; +static int large_page = 0; + +int kmmio_page_fault_notifier(struct notifier_block *nb, unsigned long val, void *args); +int kmmio_die_notifier(struct notifier_block *nb, unsigned long val, void *args); + +struct notifier_block nb_page_fault = { + .notifier_call = kmmio_page_fault_notifier +}; + +struct notifier_block nb_die = { + .notifier_call = kmmio_die_notifier +}; + +/* Locks kmmio: irqs must be disabled */ +void lock_kmmio(void) +{ + spin_lock(&kmmio_lock); +} + +void unlock_kmmio(void) +{ + spin_unlock(&kmmio_lock); +} + +int init_kmmio(void) +{ + int i; + + /* FIXME allocate the probe table, currently defined statically */ + /* initialize all list heads */ + for (i = 0; i < KMMIO_PAGE_TABLE_SIZE; i++) + INIT_LIST_HEAD(&kmmio_page_table[i]); + + register_die_notifier(&nb_die); + + return 0; +} + +void cleanup_kmmio(void) +{ + if (handler_registered) + unregister_page_fault_notifier(&nb_page_fault); + + unregister_die_notifier(&nb_die); +} + +/* You have to be holding the kmmio_lock */ +/* this is basically a dynamic stabbing problem: + * Could use the existing prio tree code or + * Possible better implementations: + * The Interval Skip List: A Data Structure for Finding All Intervals That Overlap a Point (might be simple) + * Space Efficient Dynamic Stabbing with Fast Queries - Mikkel Thorup */ +struct kmmio_probe *get_kmmio_probe(unsigned long addr) +{ + struct kmmio_probe *p; + list_for_each_entry(p, &kmmio_probes, list) { + if (addr >= p->addr && addr <= (p->addr + p->len)) + return p; + } + return NULL; +} + +int register_kmmio_probe(struct kmmio_probe *p) +{ + int ret = 0; + unsigned long size = 0; + + spin_lock_irq(&kmmio_lock); + kmmio_count++; + if (get_kmmio_probe(p->addr)) { + ret = -EEXIST; + goto out; + } + list_add(&p->list, &kmmio_probes); + /*printk("adding fault pages...\n");*/ + while (size < p->len) { + if (add_kmmio_fault_page(p->addr + size)) { + printk(KERN_ERR "mmio: Unable to set page fault.\n"); + } + size += PAGE_SIZE; + } + + if (!handler_registered) { + register_page_fault_notifier(&nb_page_fault); + handler_registered++; + } + +out: + spin_unlock_irq(&kmmio_lock); + global_flush_tlb(); + return ret; +} + +void unregister_kmmio_probe(struct kmmio_probe *p) +{ + unsigned long size = 0; + + spin_lock_irq(&kmmio_lock); + while (size < p->len) { + release_kmmio_fault_page(p->addr + size); + size += PAGE_SIZE; + } + 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; + + page &= PAGE_MASK; + head = &kmmio_page_table[hash_long(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; + + page &= PAGE_MASK; + f = get_kmmio_fault_page(page); + if (f) { + f->count++; + return 0; + } + + f = (struct kmmio_fault_page *)kmalloc(sizeof(*f), GFP_ATOMIC); + if (!f) + return -1; + + f->count = 1; + f->page = page; + list_add(&f->list, + &kmmio_page_table[hash_long(f->page, KMMIO_PAGE_HASH_BITS)]); + + arm_kmmio_fault_page(f->page, NULL); + + return 0; +} + +void release_kmmio_fault_page(kmmio_addr_t page) +{ + struct kmmio_fault_page *f; + + page &= PAGE_MASK; + f = get_kmmio_fault_page(page); + if (!f) + return; + + f->count--; + if(!f->count) { + disarm_kmmio_fault_page(f->page, NULL); + list_del(&f->list); + } +} + +int kmmio_page_fault_notifier(struct notifier_block *nb, unsigned long val, void *args) +{ + unsigned long address; + struct die_args *arg = args; + + if (val == DIE_PAGE_FAULT) { + /* get address */ +#if defined(__i386__) + address = read_cr2(); +#else + __asm__("movq %%cr2,%0":"=r" (address)); +#endif + if (is_kmmio_active()) + if(kmmio_handler(arg->regs, address) == 1) + return NOTIFY_STOP; + + } + return NOTIFY_DONE; +} + +int kmmio_die_notifier(struct notifier_block *nb, unsigned long val, void *args) +{ + struct die_args *arg = args; + + if (val == DIE_DEBUG) + if(post_kmmio_handler(arg->err, arg->regs) == 1) + return NOTIFY_STOP; + + return NOTIFY_DONE; +} + +/* + * 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_fault_page *tmp_page = NULL; + + /* We're in an interrupt, but this is clear and BUG()-safe. */ + preempt_disable(); + + lock_kmmio(); + + tmp_page = get_kmmio_fault_page(addr); + if (!tmp_page) { + /* this page fault is not caused by kmmio */ + /* XXX some pending fault on other cpu may cause problem! */ + unlock_kmmio(); + goto no_kmmio; + } + cur_page = tmp_page; + cpu = smp_processor_id(); + + cur_probe = get_kmmio_probe(addr); + kmmio_saved_eflags = (regs->eflags & (TF_MASK|IF_MASK)); + + if (cur_probe && cur_probe->pre_handler) { + cur_probe->pre_handler(cur_probe, 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(cur_page->page, &large_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 (smp_processor_id() != cpu) + return 0; + + if (cur_probe && cur_probe->post_handler) { + cur_probe->post_handler(cur_probe, condition, regs); + } + + arm_kmmio_fault_page(cur_page->page, &large_page); + + regs->eflags &= ~TF_MASK; + regs->eflags |= kmmio_saved_eflags; + + cpu = -1; + + 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; +} + +void arm_kmmio_fault_page(kmmio_addr_t page, int *large) +{ + unsigned long address = page & PAGE_MASK; + pgd_t *pgd = pgd_offset_k(address); + pud_t *pud = pud_offset(pgd, address); + pmd_t *pmd = pmd_offset(pud, address); + pte_t *pte = pte_offset_kernel(pmd, address); + + if (pmd_large(*pmd)) { + set_pmd(pmd, __pmd(pmd_val(*pmd) & ~_PAGE_PRESENT)); + if (large) + *large = 1; + } else + set_pte(pte, __pte(pte_val(*pte) & ~_PAGE_PRESENT)); + + __flush_tlb_one(page); +} + +void disarm_kmmio_fault_page(kmmio_addr_t page, int *large) +{ + unsigned long address = page & PAGE_MASK; + pgd_t *pgd = pgd_offset_k(address); + pud_t *pud = pud_offset(pgd, address); + pmd_t *pmd = pmd_offset(pud, address); + pte_t *pte = pte_offset_kernel(pmd, address); + + if (large && *large) { + set_pmd(pmd, __pmd(pmd_val(*pmd) | _PAGE_PRESENT)); + *large = 0; + } else + set_pte(pte, __pte(pte_val(*pte) | _PAGE_PRESENT)); + + __flush_tlb_one(page); +} + +/* the function is only used to make virt map to bus */ +/* Unused +void *kmmio_invert_map(void *virt_addr, unsigned long bus_addr) +{ + long offset; + pte_t *pte; + + if((unsigned long)virt_addr & ~PAGE_MASK) + BUG(); + + offset = bus_addr & ~PAGE_MASK; + bus_addr &= PAGE_MASK; + pte = get_pte((unsigned long)virt_addr); + + set_pte(pte, __pte((pte_val(*pte) & ~PAGE_MASK) | bus_addr)); + return virt_addr+offset; +} +EXPORT_SYMBOL_GPL(kmmio_invert_map); +*/ Index: madwifi-ng-r2568-20070710/mmiotrace/kmmio.h =================================================================== --- /dev/null 1970-01-01 00:00:00.000000000 +0000 +++ madwifi-ng-r2568-20070710/mmiotrace/kmmio.h 2007-09-25 01:33:26.226865360 +0200 @@ -0,0 +1,79 @@ +#ifndef _LINUX_KMMIO_H +#define _LINUX_KMMIO_H +#include +#include +#include +#include +#include +#include +#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,22) +#include +#else +#include +#endif + +struct kmmio_probe; +struct kmmio_fault_page; +struct pt_regs; + +typedef unsigned long kmmio_addr_t; +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; + + /* start location of the probe point */ + kmmio_addr_t addr; + + /* length of the probe region */ + unsigned long len; + + /* 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; +}; + +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; +} + +/* Init kmmio */ +int init_kmmio(void); +void cleanup_kmmio(void); +/* 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); + +void arm_kmmio_fault_page(kmmio_addr_t page, int *large); +void disarm_kmmio_fault_page(kmmio_addr_t page, int *large); +int post_kmmio_handler(unsigned long condition, + struct pt_regs *regs); +int kmmio_handler(struct pt_regs *regs, unsigned long addr); + +#endif /* _LINUX_KMMIO_H */ Index: madwifi-ng-r2568-20070710/mmiotrace/Makefile =================================================================== --- /dev/null 1970-01-01 00:00:00.000000000 +0000 +++ madwifi-ng-r2568-20070710/mmiotrace/Makefile 2007-09-25 01:33:26.250866729 +0200 @@ -0,0 +1,4 @@ +obj-m := mmio.o +mmio-objs := mmio-mod.o pf_in.o kmmio.o + + Index: madwifi-ng-r2568-20070710/mmiotrace/mmio-kernel-api.h =================================================================== --- /dev/null 1970-01-01 00:00:00.000000000 +0000 +++ madwifi-ng-r2568-20070710/mmiotrace/mmio-kernel-api.h 2007-09-25 01:33:26.266867640 +0200 @@ -0,0 +1,65 @@ +#ifndef MMIO_KERNEL_API_H +#define MMIO_KERNEL_API_H + +#include + +#define MMIO_VERSION 0x04 + +/* mm_io_header.type */ +#define MMIO_OPCODE_MASK 0xff +#define MMIO_OPCODE_SHIFT 0 +#define MMIO_WIDTH_MASK 0xff00 +#define MMIO_WIDTH_SHIFT 8 +#define MMIO_MAGIC (0x6f000000 | (MMIO_VERSION<<16)) +#define MMIO_MAGIC_MASK 0xffff0000 + +enum mm_io_opcode { /* payload type: */ + MMIO_READ = 0x1, /* struct mm_io_rw */ + MMIO_WRITE = 0x2, /* struct mm_io_rw */ + MMIO_PROBE = 0x3, /* struct mm_io_map */ + MMIO_UNPROBE = 0x4, /* struct mm_io_map */ + MMIO_MARKER = 0x5, /* raw char data */ + MMIO_UNKNOWN_OP = 0x6, /* struct mm_io_rw */ +}; + +struct mm_io_header { + __u32 type; + /* timestamp */ + __u32 sec; + __u32 nsec; + /* PID of the process, or 0 for kernel core */ + __u32 pid; + /* length of the following payload */ + __u16 data_len; +}; + +struct mm_io_rw { + __u64 address; /**< virtual address of register */ + __u64 value; + __u64 pc; /**< optional program counter */ +}; + +struct mm_io_map { + __u64 phys; /**< base address in PCI space */ + __u64 addr; /**< virtual base address */ + __u64 len; /**< mapping size */ + __u64 pc; /**< optional program counter */ +}; + + +/* + * These structures are used to allow a single relay_write() + * call to write a full packet. + */ + +struct mm_io_header_rw { + struct mm_io_header header; + struct mm_io_rw rw; +} __attribute__((packed)); + +struct mm_io_header_map { + struct mm_io_header header; + struct mm_io_map map; +} __attribute__((packed)); + +#endif /* MMIO_KERNEL_API_H */ Index: madwifi-ng-r2568-20070710/mmiotrace/mmio-mod.c =================================================================== --- /dev/null 1970-01-01 00:00:00.000000000 +0000 +++ madwifi-ng-r2568-20070710/mmiotrace/mmio-mod.c 2007-09-25 02:19:19.507765898 +0200 @@ -0,0 +1,314 @@ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * Copyright (C) IBM Corporation, 2005 + * Jeff Muizelaar, 2006 + * + * Derived from the read-mod example from relay-examples by Tom Zanussi. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "kmmio.h" +#include "mmio-kernel-api.h" +#include "pf_in.h" + +#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19) +enum { + false = 0, + true = 1 +}; +typedef _Bool bool; +#endif + +void (*kmmio_logmsg)(void *ah, u8 write, u_int reg, u_int32_t val) = NULL; +EXPORT_SYMBOL(kmmio_logmsg); + +struct { + unsigned long addr; + unsigned long ip; + enum reason_type type; + unsigned long val; + int index; + int active_traces; +} pf_reason[NR_CPUS] = { [0 ... NR_CPUS-1] = { 0, 0, NOT_ME, 0, 0, 0} }; + +static struct mm_io_header_rw cpu_trace[NR_CPUS]; + +/* This app's relay channel/control files will appear in /debug/mmio-trace */ +#define APP_DIR "mmio-trace" +/* the marker injection file in /proc */ +#define MARKER_FILE "mmio-marker" + +#define MODULE_NAME "mmio" + +/*#define MAX_EVENT_SIZE 256*/ + +/* app data */ +static int suspended = 0; +static struct proc_dir_entry *proc_marker_file; + +/* module parameters */ +static unsigned long filter_offset = 0; + +module_param(filter_offset, ulong, 0); + +static inline void print_pte(unsigned long address) +{ + pgd_t *pgd = pgd_offset_k(address); + pud_t *pud = pud_offset(pgd, address); + pmd_t *pmd = pmd_offset(pud, address); + if (pmd_large(*pmd)) { + printk(KERN_EMERG MODULE_NAME + ": 4MB pages are not currently supported: %lx\n", + address); + BUG(); + } + printk(KERN_DEBUG MODULE_NAME ": pte for 0x%lx: 0x%lx 0x%lx\n", + address, pte_val(*pte_offset_kernel(pmd, address)), + pte_val(*pte_offset_kernel(pmd, address)) & _PAGE_PRESENT); +}; + +void pre(struct kmmio_probe *p, struct pt_regs *regs, unsigned long addr) +{ + const unsigned long cpu = smp_processor_id(); + const unsigned long instptr = instruction_pointer(regs); + const enum reason_type type = get_ins_type(instptr); + + /* it doesn't make sense to have more than one active trace per cpu */ + if (pf_reason[cpu].active_traces) { + /* for some reason the pre/post pairs have been called in an + unmatched order */ + printk(KERN_EMERG MODULE_NAME + ": unexpected fault for address: %lx\n", addr); + printk(KERN_EMERG "last fault for address: %lx\n", + pf_reason[cpu].addr); + print_pte(addr); +#ifdef __i386__ + print_symbol(KERN_EMERG "faulting EIP is at %s\n", regs->eip); + print_symbol(KERN_EMERG "last faulting EIP was at %s\n", + pf_reason[cpu].ip); + printk(KERN_EMERG "eax: %08lx ebx: %08lx ecx: %08lx " + "edx: %08lx\n", regs->eax, regs->ebx, regs->ecx, + regs->edx); + printk(KERN_EMERG "esi: %08lx edi: %08lx ebp: %08lx " + "esp: %08lx\n", regs->esi, regs->edi, regs->ebp, + regs->esp); +#else + print_symbol(KERN_EMERG "faulting RIP is at %s\n", regs->rip); + print_symbol(KERN_EMERG "last faulting RIP was at %s\n", + pf_reason[cpu].ip); + printk(KERN_EMERG "rax: %016lx rcx: %016lx rdx: %016lx\n", + regs->rax, regs->rcx, regs->rdx); + printk(KERN_EMERG "rsi: %016lx rdi: %016lx rbp: %016lx " + " rsp: %016lx\n", regs->rsi, regs->rdi, regs->rbp, + regs->rsp); +#endif + BUG(); + } else { + pf_reason[cpu].active_traces++; + } + + pf_reason[cpu].type = type; + pf_reason[cpu].addr = addr; + pf_reason[cpu].ip = instptr; + + cpu_trace[cpu].header.type = MMIO_MAGIC; + cpu_trace[cpu].header.pid = 0; + cpu_trace[cpu].header.data_len = sizeof(struct mm_io_rw); + cpu_trace[cpu].rw.address = addr; + + /* only record the program counter when requested + * this may be needed for clean-room reverse engineering */ + cpu_trace[cpu].rw.pc = instptr; + + switch (type) + { + case REG_READ: + cpu_trace[cpu].header.type |= + (MMIO_READ << MMIO_OPCODE_SHIFT) | + (get_ins_mem_width(instptr) + << MMIO_WIDTH_SHIFT); + break; + case REG_WRITE: + cpu_trace[cpu].header.type |= + (MMIO_WRITE << MMIO_OPCODE_SHIFT) | + (get_ins_mem_width(instptr) + << MMIO_WIDTH_SHIFT); + cpu_trace[cpu].rw.value = + get_ins_reg_val(instptr, regs); + break; + case IMM_WRITE: + cpu_trace[cpu].header.type |= + (MMIO_WRITE << MMIO_OPCODE_SHIFT) | + (get_ins_mem_width(instptr) + << MMIO_WIDTH_SHIFT); + cpu_trace[cpu].rw.value = get_ins_imm_val(instptr); + break; + default: + { + unsigned char *ip = (unsigned char *)instptr; + cpu_trace[cpu].header.type |= + (MMIO_UNKNOWN_OP << MMIO_OPCODE_SHIFT); + cpu_trace[cpu].rw.value = (*ip)<<16 | + *(ip+1)<<8 | *(ip+2); + } + } + +} + +void post(struct kmmio_probe *p, unsigned long condition, struct pt_regs *regs) +{ + const unsigned long cpu = smp_processor_id(); + + /* this should always return the active_trace count to 0 */ + pf_reason[cpu].active_traces--; + if (pf_reason[cpu].active_traces) { + printk(KERN_EMERG MODULE_NAME ": unexpected post handler"); + BUG(); + } + + switch (pf_reason[cpu].type) { + case REG_READ: + cpu_trace[cpu].rw.value = + get_ins_reg_val(pf_reason[cpu].ip, regs); + break; + default: + break; + } + /*if ((cpu_trace[cpu].header.type & MMIO_OPCODE_MASK) == 0) + printk("post(): opcode missing!\n");*/ +// relay_write(chan, &cpu_trace[cpu], sizeof(struct mm_io_header_rw)); + if (kmmio_logmsg) + kmmio_logmsg(NULL, (pf_reason[cpu].type != REG_READ), cpu_trace[cpu].rw.address - p->addr, cpu_trace[cpu].rw.value); +} + +/** + * module init - creates channel management control files + * + * Returns 0 on success, negative otherwise. + */ + +struct remap_trace { + struct list_head list; + struct kmmio_probe probe; +}; +static LIST_HEAD(trace_list); + +static void do_ioremap_trace_core(unsigned long offset, + unsigned long size, void __iomem *addr) +{ + struct remap_trace *trace = kmalloc(sizeof(*trace), GFP_KERNEL); + *trace = (struct remap_trace) { .probe = { + .addr = (unsigned long)addr, + .len = size, + .pre_handler = pre, + .post_handler = post, + } + }; + + list_add_tail(&trace->list, &trace_list); + printk(KERN_DEBUG MODULE_NAME ": registering probe for %lx-%lx\n", + offset, offset+size); + register_kmmio_probe(&trace->probe); + printk(KERN_DEBUG MODULE_NAME ": register done\n"); +} + +#define ISA_START_ADDRESS 0xa0000 +#define ISA_END_ADDRESS 0x100000 + +static void __iomem *ioremap_trace_core(unsigned long offset, + unsigned long size, void __iomem *addr) +{ + if ((filter_offset) && (offset != filter_offset)) + return addr; + + /* Don't trace the low PCI/ISA area, it's always mapped.. */ + if ((offset >= ISA_START_ADDRESS) + && (offset + size - 1 < ISA_END_ADDRESS)) { + printk(KERN_NOTICE MODULE_NAME + ": Ignoring map of low PCI/ISA area (0x%lx-0x%lx)\n", + offset, offset + size); + return addr; + } + do_ioremap_trace_core(offset, size, addr); + return addr; +} + +void __iomem * __ioremap_trace( + unsigned long offset, unsigned long size, unsigned long flags) +{ + void __iomem * p; + printk(KERN_DEBUG MODULE_NAME ": __ioremap(0x%lx, 0x%lx, 0x%lx)\n", + offset, size, flags); + p = ioremap_trace_core(offset, size, __ioremap(offset, size, flags)); + /*printk(" = %p\n", p);*/ + return p; +} + +void __iomem * ioremap_nocache_trace(unsigned long offset, unsigned long size) +{ + void __iomem * p; + printk(KERN_DEBUG MODULE_NAME ": ioremap_nocache(0x%lx, 0x%lx)\n", + offset, size); + p = ioremap_trace_core(offset, size, ioremap_nocache(offset, size)); + /*printk(" = %p\n", p);*/ + return p; +} + +void iounmap_trace(volatile void __iomem *addr) +{ + struct remap_trace *trace; + struct remap_trace *tmp; + + list_for_each_entry_safe(trace, tmp, &trace_list, list) { + if ((unsigned long)addr == trace->probe.addr) { + unregister_kmmio_probe(&trace->probe); + list_del(&trace->list); + kfree(trace); + break; + } + } + iounmap(addr); + printk(KERN_DEBUG MODULE_NAME ": Unmap %p done\n", addr); +} + +EXPORT_SYMBOL(__ioremap_trace); +EXPORT_SYMBOL(ioremap_nocache_trace); +EXPORT_SYMBOL(iounmap_trace); + +static int __init init(void) +{ + init_kmmio(); + + return 0; +} + +static void __exit cleanup(void) +{ + cleanup_kmmio(); +} + +module_init(init); +module_exit(cleanup); +MODULE_LICENSE("GPL"); Index: madwifi-ng-r2568-20070710/mmiotrace/pf_in.c =================================================================== --- /dev/null 1970-01-01 00:00:00.000000000 +0000 +++ madwifi-ng-r2568-20070710/mmiotrace/pf_in.c 2007-09-25 01:33:26.298869466 +0200 @@ -0,0 +1,749 @@ +/****************************************************************************** + * Fault Injection Test harness (FI) + * Copyright (C) Intel Crop. + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + ****************************************************************************** + */ + +/* $Id: pf_in.c,v 1.1.1.1 2002/11/12 05:56:32 brlock Exp $ + * Copyright by Intel Crop., 2002 + * Louis Zhuang (louis.zhuang@intel.com) + */ + +/** + * @file pf_hooks.c + */ + +#include +/* +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +*/ +#include +#define PREFIX_NAME "FI_PF" +/*#include +#include +#include +*/ +#include "pf_in.h" + +#define NR_ELEMENTS(array) (sizeof(array)/sizeof(array[0])) + +#ifdef __i386__ +/* IA32 Manual 3, 2-1 */ +static unsigned char prefix_codes[] = { + 0xF0, 0xF2, 0xF3, 0x2E, 0x36, 0x3E, 0x26, 0x64, + 0x65, 0x2E, 0x3E, 0x66, 0x67 +}; +#else +static unsigned char prefix_codes[] = { + 0x66, 0x67, 0x2E, 0x3E, 0x26, 0x64, 0x65, 0x36, + 0xF0, 0xF3, 0xF2, + /* REX Prefixes */ + 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, + 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f +}; +#endif + +int skip_prefix(unsigned char *addr, int *shorted, int *enlarged, int *rexr) +{ + int i; + unsigned char *p = addr; + *shorted = 0; + *enlarged = 0; + *rexr = 0; + +restart: + for (i = 0; i < NR_ELEMENTS(prefix_codes); i++) { + if (*p == prefix_codes[i]) { + if (*p == 0x66) *shorted = 1; +#ifdef __amd64__ + if ((*p & 0xf8) == 0x48) *enlarged = 1; + if ((*p & 0xf4) == 0x44) *rexr = 1; +#endif + p++; + goto restart; + } + } + + return p-addr; +} + +int get_opcode(unsigned char *addr, unsigned int *opcode) +{ + int len; + + if (*addr == 0x0F) { /*0x0F is extension instruction*/ + *opcode = *(unsigned short*)addr; + len = 2; + } else { + *opcode = *addr; + len = 1; + } + + return len; +} + +#ifdef __i386__ +/* IA32 Manual 3, 3-432*/ +static unsigned int reg_rop[] = {0x8A, 0x8B, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F}; +static unsigned int reg_wop[] = {0x88, 0x89}; +static unsigned int imm_wop[] = {0xC6, 0xC7}; +#else +/* AMD64 Manual 3, Appendix A*/ +static unsigned int reg_rop[] = {0x8A, 0x8B, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F}; +static unsigned int reg_wop[] = {0x88, 0x89}; +static unsigned int imm_wop[] = {0xC6, 0xC7}; +#endif + +#define CHECK_OP_TYPE(opcode, array, type) \ + for(i = 0; i < NR_ELEMENTS(array); i++) { \ + if (array[i] == opcode) { \ + rv = type; \ + goto exit; \ + } \ + } + +enum reason_type get_ins_type(unsigned long ins_addr) +{ + unsigned int opcode; + unsigned char *p; + int shorted, enlarged, rexr; + int i; + enum reason_type rv = OTHERS; + + p = (unsigned char *)ins_addr; + p += skip_prefix(p, &shorted, &enlarged, &rexr); + p += get_opcode(p, &opcode); + + CHECK_OP_TYPE(opcode, reg_rop, REG_READ); + CHECK_OP_TYPE(opcode, reg_wop, REG_WRITE); + CHECK_OP_TYPE(opcode, imm_wop, IMM_WRITE); + +exit: + return rv; +} +#undef CHECK_OP_TYPE + +#ifdef __i386__ +/* IA32 Manual 3, 3-432*/ +static unsigned int rw8[] = {0x88, 0x8A, 0xC6}; +static unsigned int rw32[]= {0x89, 0x8B, 0xC7, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F}; +#else +/* AMD64 */ +static unsigned int rw8[] = {0xC6, 0x88, 0x8A}; +static unsigned int rw32[] = {0xC7,0x89, 0x8B, 0xB60F, 0xB70F, 0xBE0F, 0xBF0F}; +#endif + +unsigned int get_ins_reg_width(unsigned long ins_addr) +{ + unsigned int opcode; + unsigned char *p; + int i, shorted, enlarged, rexr; + + p = (unsigned char *)ins_addr; + p += skip_prefix(p, &shorted, &enlarged, &rexr); + p += get_opcode(p, &opcode); + + for (i = 0; i < NR_ELEMENTS(rw8); i++) + if (rw8[i] == opcode) + return 1; + + for (i = 0; i < NR_ELEMENTS(rw32); i++) + if (rw32[i] == opcode) + return shorted ? 2 : (enlarged ? 8 : 4); + + PERROR("Unknow opcode=0x%02x\n", opcode); + return 0; +} + +#ifdef __i386__ +static unsigned int mw8[] = {0x88, 0x8A, 0xC6, 0xB60F, 0xBE0F}; +static unsigned int mw16[] = {0xB70F, 0xBF0F}; +static unsigned int mw32[] = {0x89, 0x8B, 0xC7}; +static unsigned int mw64[] = {}; +#else +/* 8 bit only */ +static unsigned int mw8[] = {0xC6, 0x88, 0x8A, 0xB60F, 0xBE0F}; +/* 16 bit only */ +static unsigned int mw16[] = {0xB70F, 0xBF0F}; +/* 16 or 32 bit */ +static unsigned int mw32[] = {0xC7}; +/* 16, 32 or 64 bit */ +static unsigned int mw64[] = {0x89, 0x8B}; +#endif + +unsigned int get_ins_mem_width(unsigned long ins_addr) { + unsigned int opcode; + unsigned char *p; + int i, shorted, enlarged, rexr; + + p = (unsigned char *)ins_addr; + p += skip_prefix(p, &shorted, &enlarged, &rexr); + p += get_opcode(p, &opcode); + + for (i = 0; i < NR_ELEMENTS(mw8); i++) + if (mw8[i] == opcode) + return 1; + + for (i = 0; i < NR_ELEMENTS(mw16); i++) + if (mw16[i] == opcode) + return 2; + + for (i = 0; i < NR_ELEMENTS(mw32); i++) + if (mw32[i] == opcode) + return shorted ? 2 : 4; + + for (i = 0; i < NR_ELEMENTS(mw64); i++) + if (mw64[i] == opcode) + return shorted ? 2 : (enlarged ? 8 : 4); + + PERROR("Unknow opcode=0x%02x\n", opcode); + return 0; + + /* ?!? */ + return get_ins_reg_width(ins_addr); +} + +/* define register ident in mod/rm byte after undefine it in ptrace*/ +#ifdef __i386__ +#undef EAX +#define EAX 0 +#define ECX 1 +#define EDX 2 +#undef EBX +#define EBX 3 +#define ESP 4 +#define EBP 5 +#undef ESI +#define ESI 6 +#undef EDI +#define EDI 7 + +#define AL 0 +#define CL 1 +#define DL 2 +#define BL 3 +#define AH 4 +#define CH 5 +#define DH 6 +#define BH 7 +#else +#undef RAX +#undef RBX +#undef RCX +#undef RDX +#undef RSI +#undef RDI +#undef RSP +#undef RBP + +#define RAX 0 +#define RCX 1 +#define RDX 2 +#define RBX 3 +#define RSP 4 +#define RBP 5 +#define RSI 6 +#define RDI 7 +#define R8 8 +#define R9 9 +#define R10 10 +#define R11 11 +#define R12 12 +#define R13 13 +#define R14 14 +#define R15 15 + +#define AL 0 +#define CL 1 +#define DL 2 +#define BL 3 +#define AH 4 +#define CH 5 +#define DH 6 +#define BH 7 +#define R8B 8 +#define R9B 9 +#define R10B 10 +#define R11B 11 +#define R12B 12 +#define R13B 13 +#define R14B 14 +#define R15B 15 +#endif + +#ifdef __i386__ +unsigned char *get_reg_w8(int no, regs_t *regs) +{ + unsigned char *rv = NULL; + + switch (no) { + case AL: + rv = (unsigned char *)®s->eax; + break; + case BL: + rv = (unsigned char *)®s->ebx; + break; + case CL: + rv = (unsigned char *)®s->ecx; + break; + case DL: + rv = (unsigned char *)®s->edx; + break; + case AH: + rv = 1+(unsigned char *)®s->eax; + break; + case BH: + rv = 1+(unsigned char *)®s->ebx; + break; + case CH: + rv = 1+(unsigned char *)®s->ecx; + break; + case DH: + rv = 1+(unsigned char *)®s->edx; + break; + default: + PERROR("Error reg no# %d\n", no); + break; + } + + return rv; +} + +unsigned long *get_reg_w32(int no, regs_t *regs) +{ + unsigned long *rv = NULL; + + switch (no) { + case EAX: + rv = ®s->eax; + break; + case EBX: + rv = ®s->ebx; + break; + case ECX: + rv = ®s->ecx; + break; + case EDX: + rv = ®s->edx; + break; + case ESP: + rv = ®s->esp; + break; + case EBP: + rv = ®s->ebp; + break; + case ESI: + rv = ®s->esi; + break; + case EDI: + rv = ®s->edi; + break; + default: + PERROR("Error reg no# %d\n", no); + } + + return rv; +} +#else // AMD64 +unsigned char *get_reg_w8(int no, regs_t *regs) +{ + unsigned char *rv = NULL; + + switch (no) { + case AL: + rv = (unsigned char *)®s->rax; + break; + case BL: + rv = (unsigned char *)®s->rbx; + break; + case CL: + rv = (unsigned char *)®s->rcx; + break; + case DL: + rv = (unsigned char *)®s->rdx; + break; + case AH: + rv = 1+(unsigned char *)®s->rax; + break; + case BH: + rv = 1+(unsigned char *)®s->rbx; + break; + case CH: + rv = 1+(unsigned char *)®s->rcx; + break; + case DH: + rv = 1+(unsigned char *)®s->rdx; + break; + case R8B: + rv = (unsigned char *)®s->r8; + break; + case R9B: + rv = (unsigned char *)®s->r9; + break; + case R10B: + rv = (unsigned char *)®s->r10; + break; + case R11B: + rv = (unsigned char *)®s->r11; + break; + case R12B: + rv = (unsigned char *)®s->r12; + break; + case R13B: + rv = (unsigned char *)®s->r13; + break; + case R14B: + rv = (unsigned char *)®s->r14; + break; + case R15B: + rv = (unsigned char *)®s->r15; + break; + default: + PERROR("Error reg no# %d\n", no); + break; + } + return rv; +} + +unsigned long *get_reg_w32(int no, regs_t *regs) +{ + unsigned long *rv = NULL; + + switch (no) { + case RAX: + rv = ®s->rax; + break; + case RBX: + rv = ®s->rbx; + break; + case RCX: + rv = ®s->rcx; + break; + case RDX: + rv = ®s->rdx; + break; + case RSP: + rv = ®s->rsp; + break; + case RBP: + rv = ®s->rbp; + break; + case RSI: + rv = ®s->rsi; + break; + case RDI: + rv = ®s->rdi; + break; + case R8: + rv = ®s->r8; + break; + case R9: + rv = ®s->r9; + break; + case R10: + rv = ®s->r10; + break; + case R11: + rv = ®s->r11; + break; + case R12: + rv = ®s->r12; + break; + case R13: + rv = ®s->r13; + break; + case R14: + rv = ®s->r14; + break; + case R15: + rv = ®s->r15; + break; + default: + PERROR("Error reg no# %d\n", no); + } + + return rv; +} +#endif + +unsigned long get_ins_reg_val(unsigned long ins_addr, regs_t *regs) +{ + unsigned int opcode; + unsigned char mod_rm; + int reg; + unsigned char *p; + int i, shorted, enlarged, rexr; + unsigned long rv; + + p = (unsigned char *)ins_addr; + p += skip_prefix(p, &shorted, &enlarged, &rexr); + p += get_opcode(p, &opcode); + for (i = 0; i < NR_ELEMENTS(reg_rop); i++) + if (reg_rop[i] == opcode) { + rv = REG_READ; + goto do_work; + } + + for (i = 0; i < NR_ELEMENTS(reg_wop); i++) + if (reg_wop[i] == opcode) { + rv = REG_WRITE; + goto do_work; + } + + PERROR("Not a register instruction, opcode=0x%02x\n", opcode); + goto err; + +do_work: + mod_rm = *p; + reg = ((mod_rm >> 3) & 0x7) | (rexr << 3); + switch (get_ins_reg_width(ins_addr)) { + case 1: + return *get_reg_w8(reg, regs); + + case 2: + return *(unsigned short*)get_reg_w32(reg, regs); + + case 4: + return *(unsigned int*)get_reg_w32(reg, regs); + +#ifdef __amd64__ + case 8: + return *(unsigned long*)get_reg_w32(reg, regs); +#endif + + default: + PERROR("Error width# %d\n", reg); + } + +err: + return 0; +} + + +void set_ins_reg_val(unsigned long ins_addr, regs_t *regs, unsigned long val) +{ + unsigned int opcode; + unsigned char mod_rm; + int reg; + unsigned char *p; + int i, shorted, enlarged, rexr; + unsigned long rv; + + p = (unsigned char *)ins_addr; + p += skip_prefix(p, &shorted, &enlarged, &rexr); + p += get_opcode(p, &opcode); + for (i = 0; i < NR_ELEMENTS(reg_rop); i++) + if (reg_rop[i] == opcode) { + rv = REG_READ; + goto do_work; + } + + for (i = 0; i < NR_ELEMENTS(reg_wop); i++) + if (reg_wop[i] == opcode) { + rv = REG_WRITE; + goto do_work; + } + + PERROR("Not a register instruction, opcode=0x%02x\n", opcode); + goto err; + +do_work: + mod_rm = *p; + reg = ((mod_rm >> 3) & 0x7) | (rexr << 3); + + switch (get_ins_reg_width(ins_addr)) { + case 1: + *get_reg_w8(reg, regs) = val; + break; + + case 2: + *(unsigned short*)get_reg_w32(reg, regs) = val; + break; + + case 4: + *(unsigned int*)get_reg_w32(reg, regs) = val; + break; + +#ifdef __amd64__ + case 8: + *(unsigned long*)get_reg_w32(reg, regs) = val; + break; +#endif + + default: + PERROR("Error width, reg=%d\n", reg); + } +err: + return; +} + +unsigned long get_ins_imm_val(unsigned long ins_addr) { + unsigned int opcode; + unsigned char mod_rm; + unsigned char mod; + unsigned char *p; + int i, shorted, enlarged, rexr; + unsigned long rv; + + p = (unsigned char *)ins_addr; + p += skip_prefix(p, &shorted, &enlarged, &rexr); + p += get_opcode(p, &opcode); + for (i = 0; i < NR_ELEMENTS(imm_wop); i++) + if (imm_wop[i] == opcode) { + rv = IMM_WRITE; + goto do_work; + } + + PERROR("Not a imm instruction, opcode=0x%02x\n", opcode); + goto err; + +do_work: + mod_rm = *p; + mod = mod_rm >> 6; + p++; + switch (mod) { + case 0: + /* if r/m is 5 we have a 32 disp (IA32 Manual 3, Table 2-2) */ + /* AMD64: XXX Check for address size prefix? */ + if ((mod_rm & 0x7) == 0x5) + p += 4; + break; + + case 1: + p += 1; + break; + + case 2: + p += 4; + break; + + case 3: + default: + PERROR("it is not a memory access instruction at 0x%lx, rm_mod=0x%02x\n", ins_addr, mod_rm); + } + + switch (get_ins_reg_width(ins_addr)) { + case 1: + return *(unsigned char *)p; + + case 2: + return *(unsigned short*)p; + + case 4: + return *(unsigned int*)p; + +#ifdef __amd64__ + case 8: + return *(unsigned long*)p; +#endif + + default: + PERROR("Error width%s\n", "."); + } + +err: + return 0; +} + +void set_ins_imm_val(unsigned long ins_addr, unsigned long val) +{ + unsigned int opcode; + unsigned char mod_rm; + unsigned char mod; + unsigned char *p; + int i, shorted, enlarged, rexr; + unsigned long rv; + + p = (unsigned char *)ins_addr; + p += skip_prefix(p, &shorted, &enlarged, &rexr); + p += get_opcode(p, &opcode); + for (i = 0; i < NR_ELEMENTS(imm_wop); i++) + if (imm_wop[i] == opcode) { + rv = IMM_WRITE; + goto do_work; + } + + PERROR("Not a imm instruction, opcode=0x%02x\n", opcode); + goto err; + +do_work: + mod_rm = *p; + mod = mod_rm >> 6; + p++; + switch (mod) { + case 0: + /* if r/m is 5 we have a 32 disp (IA32 Manual 3, Table 2-2) */ + /* AMD64: XXX Check for address size prefix? */ + if ((mod_rm & 0x7) == 0x5) + p += 4; + break; + + case 1: + p += 1; + break; + + case 2: + p += 4; + break; + + case 3: + default: + PERROR("it is not a memory access instruction, rm_mod=0x%02x\n", mod_rm); + } + + switch (get_ins_reg_width(ins_addr)) { + case 1: + *(unsigned char *)p = val; + break; + + case 2: + *(unsigned short*)p = val; + break; + + case 4: + *(unsigned int*)p = val; + break; + +#ifdef __adm64__ + case 8: + *(unsigned long*)p = val; + break; +#endif + + default: + PERROR("Error width%s\n", "."); + } + +err: + return; +} Index: madwifi-ng-r2568-20070710/mmiotrace/pf_in.h =================================================================== --- /dev/null 1970-01-01 00:00:00.000000000 +0000 +++ madwifi-ng-r2568-20070710/mmiotrace/pf_in.h 2007-09-25 01:33:26.338871745 +0200 @@ -0,0 +1,91 @@ +/****************************************************************************** + * Fault Injection Test harness (FI) + * Copyright (C) Intel Crop. + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + ****************************************************************************** + */ + +#ifndef __PF_H_ +#define __PF_H_ + + +#define PF_MAJOR 1 +#define PF_MINOR 1 +#define PF_PATCH 0 +//#include +//#include +#define PERROR printk + +/* + * Delcare typedefs + */ + +enum reason_type{ + NOT_ME, /* page fault is not in regions */ + NOTHING, /* access others point in regions */ + REG_READ, /* read from addr to reg */ + REG_WRITE, /* write from reg to addr */ + IMM_WRITE, /* write from imm to addr */ + OTHERS /* Other instructions can not intercept */ +}; + +/* + * Declare functions in pf_interface.c + */ +void register_pf_interface(void); +void unregister_pf_interface(void); + + +/* + * Declare functions in pf_region.c + */ +void pf_lock_region_irqsave(unsigned long *flags); +void pf_lock_region(void); +void pf_unlock_region_irqrestore(unsigned long flags); +void pf_unlock_region(void); +int pf_find_region(unsigned long addr); +int pf_add_region(unsigned long addr); +void pf_release_region(int ri); +int pf_is_removed_regions(unsigned long addr); +void pf_clr_pte_bits(unsigned long addr, unsigned long bitmask); +void pf_set_pte_bits(unsigned long addr, unsigned long bitmask); +/* + * Declare functions in pf_in.c + */ +typedef struct pt_regs regs_t; +//typedef struct sigcontext regs_t; +enum reason_type get_ins_type(unsigned long ins_addr); +unsigned int get_ins_reg_width(unsigned long ins_addr); +unsigned int get_ins_mem_width(unsigned long ins_addr); +unsigned long get_ins_reg_val(unsigned long ins_addr, regs_t *regs); +void set_ins_reg_val(unsigned long addr, regs_t *regs, unsigned long val); +unsigned long get_ins_imm_val(unsigned long ins_addr); +void set_ins_imm_val(unsigned long ins_addr, unsigned long val); + +/* + * Declare functions in pf_utils.c + */ +unsigned long fi_phy_to_virt(unsigned long phy_addr); +#if 0 +/* + * Declare functions in pf.c +*/ +int pf_register(struct watchpoint wp, int id, struct intercept_hook *hook); +int pf_unregister(int id); +int pf_corrupt(int id, __u32 dirty, void *data); +#endif +#endif//__PF_H_