/*
 * IBM Dynamic Probes
 * Copyright (c) International Business Machines Corp., 2000
 *
 * 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.
 */

#include <linux/dprobes.h>
#include <linux/vmalloc.h>
#include <linux/serial_reg.h>

#include <asm/uaccess.h>
#include <asm/io.h>
#include <asm/dprobes_exclude.h>
#include <asm/debugreg.h>
#ifdef CONFIG_DR_ALLOC
#include <asm/dr_alloc.h>
#endif
#include <linux/hook.h>

void dprobes_asm_code_start(void) { }

static inline void emu_nop(struct pt_regs * regs)
{
	return;
}

static inline void emu_cmc(struct pt_regs * regs)
{
	regs->eflags &= ~(regs->eflags & EF_CF);
	return;
}

static inline void emu_clc(struct pt_regs * regs)
{
	regs->eflags &= ~EF_CF;
	return;
}

static inline void emu_stc(struct pt_regs * regs)
{
	regs->eflags |= EF_CF;
	return;
}

static inline void emu_cld(struct pt_regs * regs)
{
	regs->eflags &= ~EF_DF;
	return;
}

static inline void emulate(struct dprobes_struct *dp, struct pt_regs * regs)
{
	switch(dp->opcode) {
		case 0x90: emu_nop(regs); break; /* nop */
		case 0xf5: emu_cmc(regs); break; /* cmc */
		case 0xf8: emu_clc(regs); break; /* clc */
		case 0xf9: emu_stc(regs); break; /* stc */
		case 0xfc: emu_cld(regs); break; /* cld */
		default: printk(KERN_ERR "BUG: Opcode not emulated\n"); break;
	}
}

/*
 * returns non-zero if the opcode is emulated.
 */
static inline int is_emulated(byte_t opcode)
{
	switch(opcode) {
		case 0x90: 	/* nop */
		case 0xf5:	/* cmc */
		case 0xf8:	/* clc */
		case 0xf9:	/* stc */
		case 0xfc:	/* cld */
			return 1;
	}
	return 0;
}

/*
 * returns non-zero if opcode modifies the interrupt flag.
 */
static inline int is_if_modifier(byte_t opcode)
{
	switch(opcode) {
		case 0xfa: 	/* cli */
		case 0xfb:	/* sti */
		case 0xcf:	/* iret/iretd */
		case 0x9d:	/* popf/popfd */
			return 1;
	}
	return 0;
}

/*
 * Locate the vma, and search by the vm_file's inode + offset rather
 * than by this page so that this works for COWed pages too.
 *
 * We'll need to hold the mm lock here when finding user mode probes.
 * No deadlock considerations here as it is used only for user space probes.
 */
static inline struct dp_record_struct * find_record_at(unsigned long addr, int ptype)
{
	if (addr < PAGE_OFFSET) {
		struct mm_struct *mm = current->mm; 
		struct vm_area_struct *vma;
		unsigned long offset;
		struct inode *inode;

		if (ptype == DP_PROBE_WATCHPOINT)
			return find_rec(NULL, addr);

		spin_lock(&mm->page_table_lock); 
		vma = find_vma(mm, addr);
		offset= addr - vma->vm_start + (vma->vm_pgoff << PAGE_SHIFT);
		if (!vma->vm_file) {
			spin_unlock(&mm->page_table_lock);
			return NULL;
		}
		inode = vma->vm_file->f_dentry->d_inode;
		spin_unlock(&mm->page_table_lock);
		return find_rec(inode, offset);
	}
	else {
	 	return find_rec(NULL, addr);
	}
}

/*
 * Here we know for sure that the addr is valid and is paged in.
 * Hence used with no error checks.
 */
static inline pte_t *get_one_pte_nocheck(struct mm_struct *mm, unsigned long addr)
{
	return pte_offset(pmd_offset(pgd_offset(mm, addr), addr), addr);
}

/*
 * Note that we use __flush_tlb_one() directly instead of flush_tlb_page().
 * We know what we are doing here does not really need us to determine vma.
 */
static inline void make_alias(unsigned long addr)
{
	register pte_t * pte;
	pte = get_one_pte_nocheck(current->mm, addr);
	set_pte(dprobes.alias_pte, __pte(pte_val(*pte) | _PAGE_RW | _PAGE_DIRTY));
	__flush_tlb_one(dprobes.alias_addr); 
	return;
}

/*
 * Restores the original instruction at the current probe location.
 */
static inline void restore_original(struct dprobes_struct *dp)
{
	if (dp->reset_addr)
		goto restore;
	if (dp->status & DP_KERNEL_PROBE) {
		dp->reset_addr = dp->probe_addr;
	}
	else {
		make_alias(dp->probe_addr);
		dp->reset_addr = (unsigned long) dp->alias_addr + (dp->probe_addr & ~PAGE_MASK);
	}
restore:
	*((byte_t *)dp->reset_addr) = dp->opcode;
	flush_page_to_ram(dp->reset_addr);
	return;
}

/*
 * Does necessary setup for single stepping.
 */
static inline void setup_ss(struct dprobes_struct *dp, struct pt_regs *regs)
{
	restore_original(dp);
	regs->eflags |= EF_TF; /* single-step */
	regs->eflags &= ~EF_IE; /* disable interrupt */
	regs->eip = dp->probe_addr;
	dp->status |= DP_STATUS_SS;
	return;
}

/*
 * Called after single-stepping, successfully or not, to reinstate the probe.
 */
static inline void reset_probe(struct dprobes_struct *dp)
{
	*((byte_t *)dp->reset_addr) = DP_INSTR_BREAKPOINT;
	flush_page_to_ram(dp->reset_addr);
	return;
}

/*
 * Called after a probe is completely handled to release locks etc.
 */
static inline void done_probe(struct dprobes_struct *dp)
{
	dp->status &= ~DP_STATUS_DONE;
	spin_unlock(&dp->rec->lock);
	dp->sem--;
	return;
}

/*
 * We changed trap3/1 to an intr gate. So, restore the status of IF,
 * if necessary, before executing the original int3/1 (trap) handler.
 */
static inline void restore_interrupts(struct pt_regs *regs)
{
	if (regs->eflags & EF_IE)
		__asm__ __volatile__ ("sti");
	return;
}

/*
 * Format strings for printing out the log.
 */
#define DP_LOG_HDR_FMT		"dprobes(%d,%d) "
#define DP_LOG_CPU_FMT		"cpu=%d "
#define DP_LOG_PROCNAME_FMT	"name=%s "
#define DP_LOG_PID_FMT		"pid=%d "
#define DP_LOG_UID_FMT		"uid=%d "
#define DP_LOG_CS_EIP_FMT	"cs=%x eip=%08lx "
#define DP_LOG_SS_ESP_FMT	"ss=%x esp=%08lx "
#define DP_LOG_TSC_FMT		"tsc=%08lx:%08lx "
#define DP_LOG_NEWLINE_FMT	"\n"
#define DP_LOG_DATA_FMT		"%x "

static void log_stack_trace(struct dprobes_struct *dp)
{
	unsigned long i;
	byte_t *ex_log;
	unsigned char *tmp = dp->log_hdr;
	unsigned long ex_log_len = dp->ex_log_len;

	if (ex_log_len > MIN_ST_SIZE) {
		tmp += sprintf(tmp, DP_LOG_HDR_FMT, 0, 0);

#ifdef CONFIG_SMP
		tmp += sprintf(tmp, DP_LOG_CPU_FMT, current->cpu);
#endif
		printk("%s ", dp->log_hdr);

		ex_log_len -= MIN_ST_SIZE;
		ex_log = dp->ex_log + MIN_ST_SIZE;
		for (i = 0; i < ex_log_len; i++)
			printk(DP_LOG_DATA_FMT, ex_log[i]);
		printk(DP_LOG_NEWLINE_FMT);
	}
}

static void fmt_log_hdr(struct dprobes_struct *dp)
{
	unsigned char *tmp = dp->log_hdr;
	unsigned long log_flags = dp->mod->pgm.flags & DP_LOG_MASK;

	tmp += sprintf(tmp, DP_LOG_HDR_FMT, dp->major, dp->minor);

#ifdef CONFIG_SMP
	tmp += sprintf(tmp, DP_LOG_CPU_FMT, current->cpu);
#endif
	if (log_flags & DP_LOG_PID)
		tmp += sprintf(tmp, DP_LOG_PID_FMT, current->pid);
	if (log_flags & DP_LOG_UID)
		tmp += sprintf(tmp, DP_LOG_UID_FMT, current->uid);
	if (log_flags & DP_LOG_CS_EIP)
		tmp += sprintf(tmp, DP_LOG_CS_EIP_FMT, dp->cs, dp->eip);
	if (log_flags & DP_LOG_SS_ESP)
		tmp += sprintf(tmp, DP_LOG_SS_ESP_FMT, dp->ss, dp->esp);
	if (log_flags & DP_LOG_TSC) {
		struct timeval ts;
		do_gettimeofday(&ts);
		tmp += sprintf(tmp, DP_LOG_TSC_FMT, ts.tv_sec, ts.tv_usec);
	}
	if (log_flags & DP_LOG_PROCNAME)
		tmp += sprintf(tmp, DP_LOG_PROCNAME_FMT, current->comm);
	tmp += sprintf(tmp, DP_LOG_NEWLINE_FMT);

	tmp += sprintf(tmp, DP_LOG_HDR_FMT, dp->major, dp->minor);
	*tmp = '\0';

	return;
}

/*
 * Saves (writes) the log to the standard kernel log.
 */
static int log_to_klog(struct dprobes_struct *dp)
{
	unsigned long i;
	byte_t *log = dp->log + dp->mod->hdr.len;
	unsigned long log_len = dp->log_len - dp->mod->hdr.len;

	fmt_log_hdr(dp);
	printk("%s ", dp->log_hdr);

	for (i = 0; i < log_len; i++)
		printk(DP_LOG_DATA_FMT, log[i]);
	printk(DP_LOG_NEWLINE_FMT);

	return 0;
}	

/*
 * The order in which the optional header elements is written out has to be the
 * same order in which the DP_HDR_* constants are defined in 
 * include/linux/dprobes.h. The formatter is expected to look for the optional
 * elements in the same order.
 */
static void fmt_log_hdr_raw(struct dprobes_struct *dp)
{
	unsigned char *tmp = dp->log;
	unsigned long mask = dp->mod->hdr.mask;

	dp->mod->hdr.major = dp->major;
	dp->mod->hdr.minor = dp->minor;
	*(struct dp_trace_hdr_struct *)tmp = dp->mod->hdr;	
	tmp += sizeof(dp->mod->hdr);

#ifdef CONFIG_SMP
	*(unsigned int *)tmp = current->cpu;
	tmp += sizeof(int);
#endif
	if (mask & DP_HDR_PID) {
		*(pid_t *)tmp = current->pid;
		tmp += sizeof(pid_t);
	}
	if (mask & DP_HDR_UID) {
		*(uid_t *)tmp = current->uid;
		tmp += sizeof(uid_t);
	}
	if (mask & DP_HDR_CS) {
		*(unsigned short *)tmp = dp->cs;
		tmp += sizeof(dp->cs);
	}
	if (mask & DP_HDR_EIP) {
		*(unsigned long *)tmp = dp->eip;
		tmp += sizeof(dp->eip);
	}
	if (mask & DP_HDR_SS) {
		*(unsigned short *)tmp = dp->ss;
		tmp += sizeof(dp->ss);
	}
	if (mask & DP_HDR_ESP) {
		*(unsigned long *)tmp = dp->esp;
		tmp += sizeof(dp->esp);
	}
	if (mask & DP_HDR_TSC) {
		struct timeval ts;
		do_gettimeofday(&ts);
		*(struct timeval *)tmp = ts;
		tmp += sizeof(ts);
	}
	if (mask & DP_HDR_PROCNAME) {
		memcpy(tmp, current->comm, 16);
		tmp += 16;
	}
	return;
}

/*
 * dp->mod->hdr is changed here. Hence should be called only after
 * fmt_log_hdr_raw().
 */
static void fmt_st_hdr_raw(struct dprobes_struct *dp)
{
	unsigned char *tmp = dp->ex_log;
	dp->mod->hdr.mask = DP_HDR_MAJOR | DP_HDR_MINOR;
	dp->mod->hdr.major = DP_ST_MAJOR;
	dp->mod->hdr.minor = DP_ST_MINOR;
	dp->mod->hdr.len = MIN_ST_SIZE;
	*(struct dp_trace_hdr_struct *)tmp = dp->mod->hdr;	
	tmp += sizeof(dp->mod->hdr);
#ifdef CONFIG_SMP
	dp->mod->hdr.mask |= DP_HDR_CPU;
	*(unsigned int *)tmp = current->cpu;
	tmp += sizeof(int);
	dp->mod->hdr.len += sizeof(int);
#endif
}

#if defined(CONFIG_TRACE) || defined(CONFIG_TRACE_MODULE)
#include <linux/trace.h>

static int log_to_ltt(struct dprobes_struct *dp)
{
	trace_raw_event(dp->mod->trace_id, dp->log_len, dp->log);	
	return 0;
}

static int log_st_to_ltt(struct dprobes_struct *dp)
{
	trace_raw_event(dp->mod->trace_id, dp->ex_log_len, dp->ex_log);
	return 0;
}
#else 
static int log_to_ltt(struct dprobes_struct *dp)
{
	printk(KERN_WARNING "dprobes: Linux Trace Toolkit not available.\n");
	return 0;
}

static int log_st_to_ltt(struct dprobes_struct *dp)
{
	printk(KERN_WARNING "dprobes: Linux Trace Toolkit not available.\n");
	return 0;
}
#endif

#ifdef CONFIG_EVLOG
#include <linux/evl_log.h>
static int log_to_evl(struct dprobes_struct *dp)
{
	/*
	 * We will need to modify this in future to create a DPROBES facility
	 * type and use dp->id instead of (major,minor) for event type. Even the
	 * priority ought to be something other than LOG_KERN.
	 */
	posix_log_write(LOG_KERN, 
			(dp->major << 16) | dp->minor, /* dp->id */
			LOG_INFO,
			(dp->log + dp->mod->hdr.len),
			(dp->log_len - dp->mod->hdr.len),
			POSIX_LOG_BINARY,
			0);
	return 0;
}
#else
static int log_to_evl(struct dprobes_struct *dp)
{
	printk(KERN_WARNING "dprobes: POSIX Event Logging not available.\n");
	return 0;
}
#endif

#define BOTH_EMPTY_TR 	(UART_LSR_TEMT | UART_LSR_THRE)
#define COM1_ADDR	0x3f8
#define COM2_ADDR	0x2f8

/*
 * Wait for the transmitter buffer to become empty.
 */  
static inline void wait_for_xmitr_empty(int port)
{
	int lsr;
	unsigned int tmout = 1000000;
	do {
               	lsr = inb(port + UART_LSR);
               	if (--tmout == 0) break;
       	} while ((lsr & BOTH_EMPTY_TR) != BOTH_EMPTY_TR);
}

/*
 * This routine sends the raw binary data to com port. The receiving
 * end should be capable of handling raw bytes.
 */  
static int log_to_com_port(struct dprobes_struct *dp, int port)
{
	int i, ier;
	byte_t *log = dp->log;
	byte_t *ex_log = dp->ex_log;
	unsigned long log_len = dp->log_len;
	unsigned long ex_log_len = dp->ex_log_len;
	
	/* store UART configuration and program it in polled mode */
	ier = inb(port + UART_IER);	
	outb(0x00, port + UART_IER);

	/* send the log data to com port */
	fmt_log_hdr_raw(dp);
	for (i = 0; i < log_len; i++) {
		wait_for_xmitr_empty(port);
		outb(log[i], port + UART_TX);
	}
	
	/* send CR and LF characters */
	wait_for_xmitr_empty(port);
	outb(0x0d, port + UART_TX);
	wait_for_xmitr_empty(port);
	outb(0x0a, port + UART_TX);

	/* send the stack trace data(if present) to com port */
	if (ex_log_len > MIN_ST_SIZE) {
		fmt_st_hdr_raw(dp);
		for (i = 0; i < ex_log_len; i++) {
			wait_for_xmitr_empty(port);
			outb(ex_log[i], port + UART_TX);
		}

		/* send CR and LF characters */
		wait_for_xmitr_empty(port);
		outb(0x0d, port + UART_TX);
		wait_for_xmitr_empty(port);
		outb(0x0a, port + UART_TX);
	}	

	/* restore the configuration of UART */
	wait_for_xmitr_empty(port);
	outb(ier, port + UART_IER);
	return 0;
}

static inline void save_log(struct dprobes_struct *dp)
{
	switch(dp->mod->pgm.flags & DP_LOG_TARGET_MASK) {
	case DP_LOG_TARGET_COM1:
		log_to_com_port(dp, COM1_ADDR);
		break;
	case DP_LOG_TARGET_COM2:
		log_to_com_port(dp, COM2_ADDR);
		break;
	case DP_LOG_TARGET_LTT:
		fmt_log_hdr_raw(dp);
		log_to_ltt(dp);
		if (dp->ex_log_len > MIN_ST_SIZE) {
			fmt_st_hdr_raw(dp);
			log_st_to_ltt(dp);
		}
		break;
	case DP_LOG_TARGET_EVL:
		log_to_evl(dp);
		break;
	default:
		log_to_klog(dp);
		log_stack_trace(dp);
		break;
	}
	return;
}

/*
 * Exits to the selected facility after the original instruction is
 * successfully executed.
 */
static inline void exit_probe(struct dprobes_struct *dp)
{
	dp->rec->count++;
	if (dp->rec->count >= dp->rec->point.maxhits) {
		dp->rec->status &= ~DP_REC_STATUS_ACTIVE;
	       	dp->rec->status |= DP_REC_STATUS_DISABLED;
		if (dp->rec->point.probe == DP_PROBE_BREAKPOINT)
			restore_original(dp);
	}
	if (dp->status & DP_STATUS_ABORT)
		return;
	save_log(dp);
	return;
}

/*
 * Handles the case when the single stepping has failed. For eg. does
 * necessary cleanup if the single-stepped instruction faults.
 */
static inline void ss_failed(struct dprobes_struct *dp, struct pt_regs *regs)
{
	reset_probe(dp);
	regs->eflags &= ~EF_TF;
	regs->eflags |= dp->old_eflags;
	done_probe(dp);
	return;
}

/*
 * Handles the case when the single stepping has failed and we still want
 * to save the log as in the case of ss'ing causing a bounds exception
 * which does not mean the instruction has failed to execute.
 */
static inline void ss_failed_save_log(struct dprobes_struct *dp, struct pt_regs *regs)
{
	reset_probe(dp);
	exit_probe(dp);
	regs->eflags &= ~EF_TF;
	regs->eflags |= dp->old_eflags;
	done_probe(dp);
	return;
}

/*
 * If the original opcode was pushf, it would not have executed correctly
 * when we sigle-stepped as we have modified eflags to keep interrupts
 * disabled while single-stepping. We need fix it specially.
 */
static inline void
fix_special_instructions(struct dprobes_struct *dp, struct pt_regs *regs)
{
	unsigned long *faddr;

	if (dp->opcode == 0x9c) {	/* PUSHF */
		if (dp->probe_addr < PAGE_OFFSET) {
			dp_seg_to_flat(regs->xss, regs->esp, (void **)&faddr);
			*faddr &= ~(EF_TF | EF_IE);
			*faddr |= dp->old_eflags;
		} else {
			/* regs->esp is actually the TOS in kernel space */
			regs->esp &= ~(EF_TF | EF_IE);
			regs->esp |= dp->old_eflags;
		}
	}
}

/*
 * Does post-processing after the original instruction is successfully
 * single stepped.
 */
static inline void ss_done(struct dprobes_struct *dp, struct pt_regs *regs)
{
	reset_probe(dp);
	exit_probe(dp);
	fix_special_instructions(dp, regs);
	regs->eflags &= ~EF_TF;
	regs->eflags |= dp->saved_eflags;
	done_probe(dp);
	return;
}

#ifdef CONFIG_DR_ALLOC
static inline void set_rf(struct dp_record_struct *rec, struct pt_regs *regs)
{
	unsigned short wtype = rec->point.probe & DP_WATCHTYPE_MASK;
	if (wtype == DP_WATCHTYPE_EXECUTE) {
		regs->eflags |= EF_RF;
	}
	return;
}

inline void delete_watchpoint(unsigned int cond, struct pt_regs *regs, struct dp_record_struct *rec)
{
	int dbno = rec->dbregno;
	unsigned long dr = read_dr(7);
	RESET_DR7(dr, dbno);
	dr_free(dbno);
	write_dr(7, dr);
}

#else
inline void delete_watchpoint(unsigned int cond, struct pt_regs *regs, struct dp_record_struct *rec) 
{
	return;
}

static inline void set_rf(struct dp_record_struct *rec, struct pt_regs *regs) 
{
	return; 
}

inline int dr_trap_type(unsigned int cond) 
{
	return 0;
}
#endif

/*
 * Silently, permanently remove a reentered probe. As this _must_ have 
 * been in kernel. No need for setting up an alias.
 * Ignore the reentered watchpoint. No permanent removal.
 */
static inline int remove_reentered(struct dp_record_struct *rec, struct pt_regs * regs)
{
	unsigned long addr;
	unsigned short probe = rec->point.probe & DP_PROBE_MASK;
	if (probe == DP_PROBE_BREAKPOINT) {
		addr = regs->eip - 1;
		*(byte_t *)addr = rec->point.opcode;
		flush_page_to_ram(addr);
		regs->eip = addr;
		rec->status &= ~DP_REC_STATUS_ACTIVE;
		rec->status |= DP_REC_STATUS_REMOVED;
	} else {
		set_rf(rec, regs);
	} 
	spin_unlock(&rec->lock); /* release the per-record spinlock */
	return 1;
}

/* 
 * For breakpoint probes,
 * If we recurse into trap3 on the same probe, then we put the original
 * instruction back and let it run effectively ignoring second probe 
 * hit. Note that unlike recursion due to a different probe, here we
 * don't permanently remove this probe: no change in record status
 * no locking it (we already have the rec->lock.
 *
 * For watchpoint probes,
 * If we recurse into trap1 on same probe, we just (setting RF for
 * instruction breakpoint) return, effectively ignoring the second probe hit.
 */
static inline int reentered_on_same_probe(int ptype, unsigned long addr, struct pt_regs *regs, unsigned int condition)
{
	struct dprobes_struct *dp = &dprobes;
	if (ptype == DP_PROBE_BREAKPOINT) {
		*(byte_t *)addr = dp->opcode;
		flush_page_to_ram(addr);
		regs->eip = addr;
	}
	else {
		if (dr_trap_type(condition) == DP_WATCHTYPE_EXECUTE)
			regs->eflags |= EF_RF;
	}
	return 1;
}

/*
 * Main probe handling routine, common for both trap3 and trap1 handling.
 * Called from do_int3 and do_debug in arch/i386/kernel/traps.c.
 */

int dp_trap(struct pt_regs * regs, int probe_type, unsigned int condition, unsigned long addr)
{
	struct dprobes_struct *dp = &dprobes;
	struct dp_record_struct *rec;

	if (dp->sem && dp->probe_addr == addr) {
		return reentered_on_same_probe(probe_type, addr, regs, condition);
	}

	rec = find_record_at(addr, probe_type);
	if (!rec)
		goto no_probe;
	/*
	 * We found a probe at the addr, but first check if it is reentrant
	 * before running it.
	 * Note that find_record_at returns with rec->lock held.
	 */
	if (dp->sem) {
		return remove_reentered(rec, regs);
	}
	dp->sem++;

	/*
	 * valid probe, handle it.
	 */
	dp->rec = rec;
	dp->opcode = rec->point.opcode;
	dp->probe_addr = addr;
	dp->reset_addr = 0; /* alias not yet created. */

	if (rec->status & (DP_REC_STATUS_DISABLED | DP_REC_STATUS_REMOVED)) {
#ifdef DEBUG_DPROBES
		printk("disabled probe fired !!!\n");
#endif
		goto remove_it;
       	}

	dp->regs = regs;
	dp->mod = rec->mod;
	dp->major = rec->point.major;
	dp->minor = rec->point.minor;
	dp->cs = regs->xcs;
	dp->eip = regs->eip - 1;
	dp->ss = regs->xss;
	dp->esp = regs->esp;
	dp->old_eflags = regs->eflags & (EF_TF | EF_IE);
	if (regs->xcs & 3) {
		dp->status |= DP_USER_PROBE;
		dp->uregs = regs;
	} else {
		dp->status |= DP_KERNEL_PROBE;
		dp->uregs = (struct pt_regs * )((unsigned long)(current) + 2 * PAGE_SIZE - sizeof(struct pt_regs));
	}
	dp->status |= DP_STATUS_FIRSTFPU;

	/* rec->count > 0 for passcount */
	if (rec->status & DP_REC_STATUS_ACTIVE && rec->count >= 0) {
		dp_interpreter();
		if (rec->status & (DP_REC_STATUS_DISABLED | DP_REC_STATUS_REMOVED)) {
			if (rec->status & DP_REC_STATUS_ACTIVE)
				goto remove_it;
			else
				goto removed;
		}
	} else {
		dp->status |= DP_STATUS_ABORT;
	}
	
	if (probe_type == DP_PROBE_BREAKPOINT) {
		if (rec->status & DP_REC_OPCODE_IF_MODIFIER)
			dp->saved_eflags = regs->eflags & EF_TF;
		else
			dp->saved_eflags = regs->eflags & (EF_TF | EF_IE);

		if (rec->status & DP_REC_OPCODE_EMULATED) {
			emulate(dp, regs);
	 		exit_probe(dp);
 			done_probe(dp);
 		}
		else {
			setup_ss(dp, regs);
		}
	} else {
		exit_probe(dp);
		done_probe(dp);
		set_rf(rec, regs);
	}
	return 1;

remove_it:
	rec->status &= ~DP_REC_STATUS_ACTIVE;
	rec->status |= DP_REC_STATUS_DISABLED;
	if (probe_type == DP_PROBE_BREAKPOINT)
		restore_original(dp);
	else
		delete_watchpoint(condition, regs, rec);

removed:
	if (probe_type == DP_PROBE_BREAKPOINT)
		regs->eip = addr;
	done_probe(dp);
	return 1;

no_probe:
	/*
 	 * If the trapping location does not have INT3 instruction, 
	 * we can probably consider that this was a probe location 
	 * that was recently removed. Tell do_int3 that we handled 
	 * it so that the process will not get a SIGTRAP.
	 */
	if (probe_type == DP_PROBE_BREAKPOINT) {
		if (*(byte_t *)addr != DP_INSTR_BREAKPOINT) {
			regs->eip = addr;
			return 1;
		}
	}
#ifdef CONFIG_DR_ALLOC
	/*
	 * If the local or global enable bit in dr7 is not set for
	 * the debug register which caused the trap, we assume that
	 * this trap is due to a dprobes watchpoint which was recently
	 * removed. Tell do_debug that we handled it so that the
	 * process will not get a SIGTRAP.
	 */
	else {
		int dbno;
		unsigned long dr7;
		dbno = dr_trap(condition);
		if (dbno != -1) {
			dr7 = read_dr(7);
			if (!(dr7 & (3UL<<dbno*2))) {
					if (dr_trap_type(condition) == DP_WATCHTYPE_EXECUTE)
						regs->eflags |= EF_RF;
				return 1;
			}
		}
	}	
#endif	
	restore_interrupts(regs);
	return 0;
}

/*
 * Return values 0 means that this trap1 is NOT handled by us. Any other
 * return value indicated otherwise.
 */
int dp_trap1(struct pt_regs * regs)
{
	register struct dprobes_struct *dp = &dprobes;

	if (!(dp->status & DP_STATUS_SS))
		goto no_probe;

	/* ss_success */
	ss_done(dp, regs);
	/*
	 * 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 & EF_TF)
		goto no_probe;

	return 1;

no_probe:
	restore_interrupts(regs);
	return 0;
}

/*
 *
 * Return values 0 means that this fault is NOT handled by us. Any other
 * return value indicated otherwise.
 */
int dp_gpf(struct pt_regs * regs)
{
	register struct dprobes_struct *dp = &dprobes;
	unsigned long fixup;

	if (dp->status & DP_STATUS_INTERPRETER) {
		dp->status |= DP_STATUS_GPF;
		/* Are we prepared to handle this kernel fault?  */
		if ((fixup = search_exception_table(regs->eip)) != 0) {
			regs->eip = fixup;
			return 1;
		}

		/*
		 * Major problem. We are in the interpreter, got a page fault
		 * and don't have a local exception handler. Let the default
		 * fault handler take effect, which will oops.
		 */
		return 0;
	}
	else if (dp->status & DP_STATUS_SS) {
		if (!dp->rec->point.logonfault) {
			ss_failed(dp, regs);
		}
		else 
			ss_failed_save_log(dp, regs);
	}
	return 0;
}

/*
 * Return values 0 means that this fault is NOT handled by us. Any other
 * return value indicated otherwise.
 */
int dp_pf(struct pt_regs * regs)
{
	register struct dprobes_struct *dp = &dprobes;
	unsigned long fixup;
	if (dp->status & DP_STATUS_INTERPRETER) {
		dp->status |= DP_STATUS_PF;
		/* Are we prepared to handle this kernel fault?  */
		if ((fixup = search_exception_table(regs->eip)) != 0) {
			regs->eip = fixup;
			return 1;
		}

		/*
		 * Major problem. We are in the interpreter, got a page fault
		 * and don't have a local exception handler. Let the default
		 * fault handler take effect, which will oops.
		 */
		return 0;
	}
	else if (dp->status & DP_STATUS_SS) {
		if (!dp->rec->point.logonfault) {
			ss_failed(dp, regs);
		}
		else 
			ss_failed_save_log(dp, regs);
	}
	return 0;
}

/*
 * If there are multiple debug traps simultaneously, we handle the
 * single step condition first (DR_STEP) as it is related to completing
 * a previous debug trap. We then handle the debug register hits.
 */
int dp_do_debug(struct pt_regs * regs, unsigned long condition)
{
	int retval = 0;

	if (condition & DR_STEP) {
		if (dp_trap1(regs)) {
			regs->eflags &= ~TF_MASK;
			retval = 1;
		}
	}
#ifdef CONFIG_DR_ALLOC
	/* assume that only one watchpoint possible on an address. */
	if (condition & (DR_TRAP0|DR_TRAP1|DR_TRAP2|DR_TRAP3)) {
		if (dp_trap(regs, DP_PROBE_WATCHPOINT, condition, dr_trap_addr(condition))) {
			retval = 1;
		}
	}
#endif
	return retval;
}

/*
 * If any other fault occurs while we are single-stepping, we need to
 * reinstate the probe. If any fault occurs, we still want to 
 * save the log.
 */
int dp_handle_fault(struct pt_regs * regs)
{
	register struct dprobes_struct *dp = &dprobes;

	if (dp->status & DP_STATUS_SS) {
		if (!dp->rec->point.logonfault) {
			ss_failed(dp, regs);
		}
		else 
			ss_failed_save_log(dp, regs);
	}
	return 0;
}

#include <asm/desc.h>
#include <asm/ldt.h>
#ifndef GDT_ENTRIES
#define GDT_ENTRIES     (__TSS(NR_CPUS))
#endif
int dp_seg_to_flat(unsigned long sel, unsigned long off, void **faddr)
{
	struct Xgt_desc_struct g;
	unsigned long ldt_desc, ldt_base, ldtr, seg_desc, seg_base;
	unsigned long gdt_index = (sel & ~7);
	__asm__ __volatile__ ("sgdt (%0)" : : "r"((unsigned long) &g));

	if (sel & 4) {
		if (!(current->mm->context.segments && ((sel >> 3) < LDT_ENTRIES)))
			return 1;

		__asm__ __volatile__ ("sldt %0" :"=r"(ldtr));
		ldt_desc = g.address + ((ldtr & ~7));
		ldt_base = _get_base ((char *)ldt_desc);
		seg_desc = ldt_base + (gdt_index);
		seg_base = _get_base ((char *)seg_desc);
	}
	else {
		if ((sel >> 3) > GDT_ENTRIES)
			return 1;
		seg_desc = g.address + gdt_index;
		seg_base = _get_base ((char *)seg_desc);
	}
	*faddr = (void *)(seg_base + off);
	return 0;
}

static inline int is_fpu_instn(byte_t *addr)
{
	if (((*addr & 0xf8) == 0xd8) || (*addr == 0x9b)) 
		return 1;
	return 0;
}

#ifdef CONFIG_DR_ALLOC
struct db_smp {
	int dr;
	unsigned long addr;
	int io_watch;
};

static inline void write_smp_dr(void *info)
{
	struct db_smp *db = (struct db_smp *)info;
	if (cpu_has_de &&  db->io_watch)
		set_in_cr4(X86_CR4_DE);
	write_dr(db->dr, db->addr);
}

/* Update the debug register on all CPUs */
static inline void sync_dr(int dr, unsigned long addr, int io_watch)
{
	struct db_smp db;
	db.dr = dr;
	db.addr = addr;
	db.io_watch = io_watch;
	smp_call_function(write_smp_dr, (void *)&db, 0, 0);
}
	 
/* Check the range for debug register watchpoint */
static inline int invalid_range(unsigned long len)
{
	if (len > 3) {
		return 1;
	}
	return 0;
}

/* last thing to do is to update dr7 to enable the new watchpoint */
static inline int insert_wp(byte_t *addr, struct dp_record_struct *rec,
	struct dp_module_struct *m, struct page * page)
{
	unsigned long dr;
	int db_index;
	int io_watch = 0;

	if (rec->point.probe & DP_WATCHTYPE_EXECUTE && 
		excluded((unsigned long)addr)) {
		rec->status |= DP_REC_STATUS_EXCLUDED;
		return -1;
	}
	if (invalid_range(rec->point.len)) {
		rec->status |= DP_REC_STATUS_WATCHPOINT_LEN_INVALID;
		return -1;
	}
      	db_index = dr_alloc(DR_ANY, DR_ALLOC_GLOBAL);
	if (db_index == -1) {
		rec->status |= DP_REC_STATUS_DEBUGREG_UNAVAIL;
		return -1;
	} 
	if (rec->point.probe & DP_WATCHTYPE_IO)
	       io_watch = 1;

	write_dr(db_index, (unsigned long)addr);
	sync_dr(db_index, (unsigned long)addr, io_watch);

	if (cpu_has_de && io_watch)
		set_in_cr4(X86_CR4_DE);

	rec->dbregno = db_index;
	dr = read_dr(7);
	SET_DR7(dr, db_index, (rec->point.probe & DP_WATCHTYPE_MASK), 
		rec->point.len);

	rec->status |= DP_REC_STATUS_ACTIVE;
	add_rec_to_hash_queue(rec, m->inode, rec->point.offset);

	write_dr(7, dr);
	sync_dr(7, dr, 0);
	return 0;
}

static inline int remove_wp(byte_t *addr, struct dp_record_struct *rec)
{
	unsigned long dr;
	if (rec->dbregno == -1) {
		return -1;
	}		
		
	dr = read_dr(7);
	RESET_DR7(dr, rec->dbregno);
	write_dr(7, dr);
	sync_dr(7, dr, 0);

	dr_free(rec->dbregno);
	return 0;
}	
#else
int dr_alloc(int regnum, int flag) 
{
	return -1;
}

int dr_free(int regnum)
{
	return -1;
}

static inline int remove_wp(byte_t *addr, struct dp_record_struct *rec)
{
	return 0;
}

static inline int insert_wp(byte_t *addr, struct dp_record_struct *rec,
	struct dp_module_struct *m, struct page * page)
{
	rec->status |= DP_REC_STATUS_DEBUGREG_PATCH_NEEDED;
	return 0;
}
#endif


/* 
 * Writing the breakpoint instruction should be the last thing done
 * in this function to ensure that all the structures that
 * are needed to locate the probe are in place before a probe can
 * be hit.
 */
static inline int insert_bp(byte_t *addr, struct dp_record_struct *rec,
	struct dp_module_struct *m, struct page * page)
{
	if (excluded((unsigned long)addr)) {
		rec->status |= DP_REC_STATUS_EXCLUDED;
		return -1;
	}
	if (!(m->pgm.flags & DP_MODTYPE_USER) &&
		!(((unsigned long)addr) >= m->base && 
		((unsigned long)addr) < (m->end + m->base))) {
		rec->status |= DP_REC_STATUS_INVALID_OFFSET;
		return -1;
	}
	if (*addr != rec->point.opcode)  {
		if (m->pgm.flags & DP_DONT_VERIFY_OPCODES) {
			rec->point.opcode = *addr;
		} else {
			if (!(m->pgm.flags & DP_MODTYPE_USER) || 
			    !IS_COW_PAGE(page, m->inode)) {
				rec->point.actual_opcode = *addr;
				rec->status |= DP_REC_STATUS_MISMATCH;
			}
			return -1;
		}
	}
	if (!HAVE_HWFP && is_fpu_instn(addr)) {
		rec->status |= DP_REC_STATUS_EXCLUDED_OPCODE;
		return -1;
	}

	if (is_if_modifier(rec->point.opcode))
		rec->status |= DP_REC_OPCODE_IF_MODIFIER;
	if (is_emulated(rec->point.opcode))
		rec->status |= DP_REC_OPCODE_EMULATED;
	rec->status |= DP_REC_STATUS_ACTIVE;
	add_rec_to_hash_queue(rec, m->inode, rec->point.offset);
	*addr = DP_INSTR_BREAKPOINT;
	flush_page_to_ram(addr);
	return 0;
}

int __insert_probe(byte_t *addr, struct dp_record_struct *rec, 
	struct dp_module_struct *m, struct page * page)
{
	if (rec->point.probe & DP_PROBE_BREAKPOINT)
		return insert_bp(addr, rec, m, page);
	else 
		return insert_wp(addr, rec, m, page);
}

static inline void remove_bp(byte_t *addr, struct dp_record_struct *rec)
{
	if (*addr == DP_INSTR_BREAKPOINT) {
		*addr = rec->point.opcode;
		flush_page_to_ram(addr);
	}
}

int __remove_probe(byte_t * addr, struct dp_record_struct *rec)
{
	if (rec->point.probe & DP_PROBE_BREAKPOINT)
		remove_bp(addr, rec);
	else 
		remove_wp(addr, rec);
	return 0;
}

void dprobes_asm_code_end(void) { }
