/*
 * 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>

/* TODO - Implement watchpoint probes */ 
#if ia32_watchpoint_code_sample
#ifdef CONFIG_DR_ALLOC
#include <asm/dr_alloc.h>
#endif
#endif /* ia32_watchpoint_code_sample */

#include <asm/pgalloc.h>  /* for flush_tlb_all() definition */
#include <linux/hook.h>

/*  Macros for IA-64 instruction field extraction */
#define IA64_MAJ_OPCODE(instr) \
    ((instr & (0x0ful << 37)) >> 37) /* major opcode value */
#define IA64_M44_X3(instr) \
    ((instr & (0x07ul << 33)) >> 33) /* M44 x3 opcode extension value */
#define IA64_M44_X4(instr) \
    ((instr & (0x0ful << 27)) >> 27) /* M44 x4 opcode extension value */
#define IA64_M44_IMM14(instr) \
    ((instr & (0x01ul << 20)) >> 20) /* M44 imm bit 14, 1 if set, 0 otherwise */
#define IA64_M35_X3(instr) \
    ((instr & (0x07ul << 33)) >> 33) /* M35 x3 opcode extension value */
#define IA64_M35_X6(instr) \
    ((instr & (0x3ful << 27)) >> 27) /* M35 x6 opcode extension value */
#define IA64_M35_R2(instr) \
    ((instr & (0x7ful << 13)) >> 13) /* M35 r2 operand value */
#define IA64_M36_X3(instr) \
    ((instr & (0x07ul << 33)) >> 33) /* M36 x3 opcode extension value */
#define IA64_M36_X6(instr) \
    ((instr & (0x3ful << 27)) >> 27) /* M36 x6 opcode extension value */
#define IA64_M36_R1(instr) \
    ((instr & (0x7ful << 6)) >> 6)   /* M36 r1 operand value */

void dprobes_asm_code_start(void) { }

#if ia32_emulation_code_sample
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;
}
#endif /* ia32_emulation_code_sample */

static inline void emulate(struct dprobes_struct *dp, struct pt_regs * regs)
{
	/* TODO - Implement instruction emulation */ 
#if ia32_emulation_code_sample
	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;
	}
#endif /* ia32_emulation_code_sample */
	return;
}

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

/*
 * returns non-zero if instruction modifies the interrupt flag.
 */
static inline int is_if_modifier(unsigned long instr, struct pt_regs * regs,  
				    struct switch_stack *sw)
{
	unsigned long val;
	unsigned short regnum;

	/* 
	 * Reset System Mask (rsm imm24)
	 * changes PSR.i from 1 to 0 if:
	 *    - rsm immediate bit 14 is set to 1 and
	 *    - PSR.i is set to 1 prior to rsm execution
	 */
	if (IA64_MAJ_OPCODE(instr) == 0 &&
	    IA64_M44_X3(instr) == 0 &&
	    IA64_M44_X4(instr) == 7 &&
	    IA64_M44_IMM14(instr) &&
	    ia64_psr(regs)->i == 1) {
		return 1;
	}

	/* 
	 * Set System Mask (ssm imm24) 
	 * changes PSR.i from 0 to 1 if:
	 *   - rsm immediate bit 14 is set to 1 and
	 *   - PSR.i is set to 0 prior to rsm execution
	 */
	if (IA64_MAJ_OPCODE(instr) == 0 &&
	    IA64_M44_X3(instr) == 0 &&
	    IA64_M44_X4(instr) == 6 &&
	    IA64_M44_IMM14(instr) &&
	    ia64_psr(regs)->i == 0) {
		return 1;
	}

	/* 
	 * Move to PSR (mov psr.l=rX) 
	 * changes PSR.i if:
	 *  - the current PSR.i differs from bit 14 of the 
	 *    value stored in the source register
	 */
	if (IA64_MAJ_OPCODE(instr) == 1 &&
	    IA64_M35_X3(instr) == 0 &&
	    IA64_M35_X6(instr) == 0x2d) {
		regnum = IA64_M35_R2(instr);
		val = dp_get_gr(regnum, regs, sw);
		if (ia64_psr(regs)->i != (val & IA64_PSR_I)) {
			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_mkwrite(pte_mkdirty(*pte)));
	flush_tlb_all();
	return;
}

/* Save a copy of the current slot 0 instruction and then replace 
   it with a break instruction */
static void insert_bp_instr(byte_t *addr, struct dp_record_struct *rec)
{
	unsigned long *bundle_start_addr = 
		(unsigned long *)((unsigned long)addr & ~0x0f);

	rec->point.actual_instr = (*bundle_start_addr & DP_SLOT0_INSTR_MASK) >> 5;
	*bundle_start_addr = (*bundle_start_addr & ~DP_SLOT0_INSTR_MASK) | 
				(DP_INSTR_BREAKPOINT << 5);
	return;
}

/* Restore the original instruction */
static void remove_bp_instr(byte_t *addr, struct dp_record_struct *rec)
{
        unsigned long *bundle_start_addr = 
		(unsigned long *)((unsigned long)addr & ~0x0f);

	*bundle_start_addr = (*bundle_start_addr & ~DP_SLOT0_INSTR_MASK) | 
				(rec->point.actual_instr << 5);
	return;
}

/* Return true if break instruction exists at addr, otherwise return false */
static int is_bp_instr_at_addr(byte_t *addr)
{
	unsigned long *bundle_start_addr = (unsigned long *)((unsigned long)addr & ~0x0f);
	unsigned long instr;

	instr = (*bundle_start_addr & DP_SLOT0_INSTR_MASK) >> 5;
	return (instr == DP_INSTR_BREAKPOINT);
}

/*
 * 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:
	remove_bp_instr((byte_t *)dp->reset_addr, dp->rec);
	flush_icache_range(dp->reset_addr, dp->reset_addr+16);
	return;
}

/*
 * Does necessary setup for single stepping.
 */
static inline void setup_ss(struct dprobes_struct *dp, struct pt_regs *regs)
{
	restore_original(dp);
	ia64_psr(regs)->ss = 1; /* enable single-step */
	ia64_psr(regs)->i  = 0; /* disable interrupts */
	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)
{
	insert_bp_instr((byte_t *)dp->reset_addr, dp->rec);
	flush_icache_range(dp->reset_addr, dp->reset_addr+16);
	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 previously set processor to ignore interrupts. We now
 * restore the original status, if necessary, before executing 
 * the original break trap handler.
 */
static inline void restore_interrupts(struct pt_regs *regs)
{
	if (ia64_psr(regs)->i == 0)
		__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_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, smp_processor_id());
#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, smp_processor_id());
#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_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 = smp_processor_id();
	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_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 = smp_processor_id();
	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);
	ia64_psr(regs)->ss = 0;
	regs->cr_ipsr |= dp->old_psr;
	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);
        ia64_psr(regs)->ss = 0;
        regs->cr_ipsr |= dp->old_psr;
	done_probe(dp);
	return;
}

/*
 * If the original instruction is move from PSR, it would have not
 * have executed correctly when we single-stepped as we have modified
 * PSR.i to keep interrupts disabled while single_stepping.  We need
 * to fix it.
 */
static inline void
fix_special_instructions(struct dprobes_struct *dp, struct pt_regs *regs, 
				struct switch_stack *sw)
{
	unsigned long instr = dp->rec->point.actual_instr;
 
	if (IA64_MAJ_OPCODE(instr) == 1 &&
	    IA64_M36_X3(instr) == 0 &&
	    IA64_M36_X6(instr) == 0x25) {
		/* mov from psr.l */
		unsigned long regnum = IA64_M36_R1(instr);
		unsigned long val;

		/* Get incorrect PSR value that was just copied to GR */
		val = dp_get_gr(regnum, regs, sw);
		/* 
		 *  At this point the PSR value contained in 'val' has:
		 *   - PSR.i set to 0 because we set it to this value
		 *     prior to the single step
		 *   - PSR.ss set to 0 because the move from PSR 
		 *     instruction that was just executed always 
		 *     reads PSR.ss as 0
		 */

		/* 
		 *  Adjust the PSR value:
		 *   - setting PSR.i to it's original state 
		 *   - leaving PSR.ss set to 0 regardless of it's
		 *     original state since move from PSR always
		 *     reads PSR.ss as 0 
		 */
		val |= (dp->old_psr & ~IA64_PSR_SS); 

		/* Set GR to corrected PSR value */
		dp_set_gr(regnum, val, regs, sw);
	}
	return;
}

/*
 * Does post-processing after the original instruction is successfully
 * single stepped.
 */
static inline void ss_done(struct dprobes_struct *dp, struct pt_regs *regs, struct switch_stack *sw)
{
	reset_probe(dp);
	exit_probe(dp);
	fix_special_instructions(dp, regs, sw);
        ia64_psr(regs)->ss = 0;
        regs->cr_ipsr |= dp->saved_psr;
	done_probe(dp);
	return;
}

/* TODO - Implement watchpoint probes */ 
#if ia32_watchpoint_code_sample
#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
#endif //ia32_watchpoint_code_sample

/*
 * 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->cr_iip;
		remove_bp_instr((byte_t *)addr, rec);
		flush_icache_range(addr, addr+16);
		rec->status &= ~DP_REC_STATUS_ACTIVE;
		rec->status |= DP_REC_STATUS_REMOVED;
	} else {
		/* TODO - Implement watchpoint probes */ 
#if ia32_watchpoint_code_sample 
		set_rf(rec, regs);
#endif /* ia32_watchpoint_code_sample */
	} 
	spin_unlock(&rec->lock); /* release the per-record spinlock */
	return 1;
}

/*
 * For breakpoint probes,
 * If we recurse into dp_trap on the same probe, 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.
 *
 * TODO - Implement watchpoint probes for IA-64
 * #if ia32_watchpoint_code_sample
 * 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.
 * #endif ia32_watchpoint_code_sample
 */
static inline int reentered_on_same_probe(int ptype, unsigned long addr, struct pt_regs *regs)
{
	struct dprobes_struct *dp = &dprobes;
	if (ptype == DP_PROBE_BREAKPOINT) {
		remove_bp_instr((byte_t *)addr, dp->rec);
		flush_icache_range(addr, addr+16);
	}
	else {
		/* TODO - Implement watchpoint probes */
#if ia32_watchpoint_code_sample
		if (dr_trap_type(condition) == DP_WATCHTYPE_EXECUTE)
			regs->eflags |= EF_RF;
#endif /* ia32_watchpoint_code_sample */
	}
	return 1;
}

static void do_dp_trap(struct unw_frame_info *info, void *arg)
{
	register struct dprobes_struct *dp = &dprobes;
	struct dp_trap_data *data = arg;
	int probe_type = data->probe_type;
	struct pt_regs *regs = data->regs;
	struct dp_record_struct *rec = data->rec;
	unsigned long addr;

	addr = regs->cr_iip;

	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->sw = info->sw;
	dp->old_psr = regs->cr_ipsr & (IA64_PSR_SS | IA64_PSR_I);
	if (user_mode(regs)) {
		dp->status |= DP_USER_PROBE;
		dp->uregs = regs;
	} else {
		dp->status |= DP_KERNEL_PROBE;
		dp->uregs = ia64_task_regs(current);
	}
	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 (is_if_modifier(rec->point.actual_instr, regs, info->sw)) 
			dp->saved_psr = regs->cr_ipsr & IA64_PSR_SS;
		else
                        dp->saved_psr = regs->cr_ipsr & (IA64_PSR_SS | IA64_PSR_I);

		if (rec->status & DP_REC_OPCODE_EMULATED) {
			emulate(dp, regs);	
	 		exit_probe(dp);
 			done_probe(dp);
 		}
		else {
			setup_ss(dp, regs);
		}
	} else {
		/* TODO - Implement watchpoint probes */
#if ia32_watchpoint_code_sample 
		exit_probe(dp);
		done_probe(dp);
		set_rf(rec, regs);
#endif /* ia32_watchpoint_code_sample */
	}
	data->return_val = 1;
	return;

remove_it:
	rec->status &= ~DP_REC_STATUS_ACTIVE;
	rec->status |= DP_REC_STATUS_DISABLED;
	if (probe_type == DP_PROBE_BREAKPOINT)
		restore_original(dp);
	else {
		/* TODO - Implement watchpoint probes */ 
#if ia32_watchpoint_code_sample 
		delete_watchpoint(condition, regs, rec);
#endif /* ia32_watchpoint_code_sample */
	}

removed:
	done_probe(dp);
	data->return_val = 1;
	return;
}

/*
 * Main probe handling routine, common for both breakpoint and
 * watchpoint probes.  Called from ia64_bad_break (for breakpoint) 
 * and ia64_fault (for watchpoint) in arch/ia64/kernel/traps.c.
 */
int dp_trap(struct pt_regs * regs, int probe_type)
{
	register struct dprobes_struct *dp = &dprobes; 
	struct dp_record_struct *rec; 
	struct dp_trap_data data;
	unsigned long addr;
 
	addr = regs->cr_iip;

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

        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.
	 */
        data.probe_type = probe_type;
	data.regs = regs;
	data.rec = rec;
        unw_init_running(do_dp_trap, &data);
        return(data.return_val);

no_probe:
	/*
 	 * If the trapping location does not have a break instruction, 
	 * we can probably consider that this was a probe location 
	 * that was recently removed. Tell ia64_bad_break that we handled 
	 * it so that the process will not get a SIGTRAP.
	 */
	if (probe_type == DP_PROBE_BREAKPOINT) {
		if (!is_bp_instr_at_addr((byte_t *)addr)) {
			return 1;
		}
	}
	/* TODO - Implement watchpoint probes */ 
#if ia32_watchpoint_code_sample
#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	/* CONFIG_DR_ALLOC */
#endif /* ia32_watchpoint_code_sample */
	restore_interrupts(regs);
	return 0;
}

/*
 * A return value of 0 means that this single step trap is not
 * handled by us.  Any other return value indicates otherwise.
 */
static void do_dp_ss_fault(struct unw_frame_info *info, void *arg)
{
	register struct dprobes_struct *dp = &dprobes;
        struct dp_trap_data *data = arg;
        struct pt_regs *regs = data->regs;
	struct switch_stack *sw = info->sw;

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

	/* ss_success */
	ss_done(dp, regs, sw);

	/*
	 * if somebody else is singlestepping across a probe point, the psr
	 * single step enable bit will be set, in which case, continue the
	 * remaining processing of do_debug, as if this is not a probe hit.
	 */
	if (ia64_psr(regs)->ss)
		goto no_probe;

	data->return_val = 1;
	return;

no_probe:
	restore_interrupts(regs);
	data->return_val = 0;
	return;
}

int dp_ss_fault(struct pt_regs * regs)
{
        struct dp_trap_data data;
 
        data.regs = regs;
        unw_init_running(do_dp_ss_fault, &data);
        return(data.return_val);
}

/*
 *
 * 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;
	struct exception_fixup fix;

	if (dp->status & DP_STATUS_INTERPRETER) {
		dp->status |= DP_STATUS_GPF;
		/* Are we prepared to handle this kernel fault?  */
#ifdef GAS_HAS_LOCAL_TAGS
		fix = search_exception_table(regs->cr_iip + ia64_psr(regs)->ri);
#else
		fix = search_exception_table(regs->cr_iip);
#endif
		if (fix.cont) {
			handle_exception(regs, fix);
			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;
        struct exception_fixup fix;

	if (dp->status & DP_STATUS_INTERPRETER) {
		dp->status |= DP_STATUS_PF;
		/* Are we prepared to handle this kernel fault?  */
#ifdef GAS_HAS_LOCAL_TAGS
		fix = search_exception_table(regs->cr_iip + ia64_psr(regs)->ri);
#else
		fix = search_exception_table(regs->cr_iip);
#endif
		if (fix.cont) {
			handle_exception(regs, fix);
			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 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;
}

/* TODO - Implement watchpoint probes */
#if ia32_watchpoint_code_sample
#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)
{
	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
#endif /* ia32_watchpoint_code_sample */

/* 
 * 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;
		}
	}

	rec->status |= DP_REC_STATUS_ACTIVE;
	add_rec_to_hash_queue(rec, m->inode, rec->point.offset);
	insert_bp_instr(addr, rec);
	flush_icache_range((unsigned long)addr, (unsigned long)addr+16);
	if (is_emulated(rec->point.actual_instr))
		rec->status |= DP_REC_OPCODE_EMULATED;
	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
		/* TODO - Implement watchpoint probes */
		return 0;
#if ia32_watchpoint_code_sample
		return insert_wp(addr, rec, m, page);
#endif /* ia32_watchpoint_code_sample */
}

static inline void remove_bp(byte_t *addr, struct dp_record_struct *rec)
{
	if (is_bp_instr_at_addr(addr)) {
		remove_bp_instr(addr, rec);
		flush_icache_range((unsigned long)addr, (unsigned long)addr+16);
	}
}

int __remove_probe(byte_t * addr, struct dp_record_struct *rec)
{
	if (rec->point.probe & DP_PROBE_BREAKPOINT)
		remove_bp(addr, rec);
	else 
		; /* TODO - Implement watchpoint probes */
#if ia32_watchpoint_code_sample
		remove_wp(addr, rec);
#endif /* ia32_watchpoint_code_sample */
	return 0;
}

unsigned long dp_get_gr(unsigned short regnum, struct pt_regs *regs, struct switch_stack *sw)
{
	register struct dprobes_struct *dp = &dprobes;
	unsigned long val = 0, sof = 0, sol, soo, ndirty, bsp, ip, cfm, *addr;
	unsigned long *krbs, num_regs_on_krbs, *bspstore, *urbs_end;
        char nat = 0;
        struct unw_frame_info info;

	if (regnum < 32) {
		/* gr0-gr31, Static General Registers */
		switch(regnum) {
			case 0:  /* r0, read only, always 0 */
				val = 0;
				break;
			case 1:  /* r1, gp */
			case 2:  /* r2 */
			case 3:  /* r3 */
				addr = &regs->r1 + (regnum - 1);
				val = *addr;
				break;
			case 4:  /* r4 */
			case 5:  /* r5 */
			case 6:  /* r6 */
			case 7:  /* r7 */
				addr = &sw->r4 + (regnum - 4);
				val = *addr;
				break;
			case 8:  /* r8, ret0 */
			case 9:  /* r9, ret1 */
			case 10: /* r10, ret2 */
			case 11: /* r11, ret3 */
				addr = &regs->r8 + (regnum - 8);
				val = *addr;
				break;
			case 12: /* r12, sp */
			case 13: /* r13 */
			case 14: /* r14 */
			case 15: /* r15 */
				addr = &regs->r12 + (regnum - 12);
				val = *addr;
				break;
			case 16:  /* r16 */
			case 17:  /* r17 */
			case 18:  /* r18 */
			case 19:  /* r19 */
			case 20:  /* r20 */
			case 21:  /* r21 */
			case 22:  /* r22 */
			case 23:  /* r23 */
			case 24:  /* r24 */
			case 25:  /* r25 */
			case 26:  /* r26 */
			case 27:  /* r27 */
			case 28:  /* r28 */
			case 29:  /* r29 */
			case 30:  /* r30 */
			case 31:  /* r31 */
				addr = &regs->r16 + (regnum - 16);
				val = *addr;
				break;
			default:
				/* Shouldn't get here */
				break;
		}
	} else {
		/* r32-r127, Stacked General Registers */
		if (dp->regs == dp->uregs) {
			/*
			 * - Probe point in user code.
			 * - RPN instructions: 'push u' (user context) or
			 *                     'push r' (current context).
			 * - All stacked GR values are all located on the kernel
			 *   register backing store.
			 */
			krbs = (unsigned long *) current + IA64_RBS_OFFSET/8;
			sof = regs->cr_ifs & 0x7f;
			ndirty = ia64_rse_num_regs(krbs, krbs + (regs->loadrs >> 19));
			bsp = (unsigned long) ia64_rse_skip_regs(krbs, ndirty - sof);
			if ((regnum - 32) < sof)
				val = *ia64_rse_skip_regs((unsigned long *)bsp, regnum - 32);
		} else {
			if (regs == ia64_task_regs(current)) {
				/*
				 * - Probe point in kernel code.
				 * - RPN instruction: 'push u' (user context).
				 * - Some stacked GR values are on the kernel
				 *   register backing store and some are on the user
				 *   register backing store.
				 *
				 * - This code appears to successfully obtain the stacked
				 *   GR values for the frame corresponding to the user
				 *   function that made a system call.  The code may need
				 *   to be updated to correctly handle other 
				 *   (non system call) kernel entries.
				 */
				krbs = (unsigned long *) current + IA64_RBS_OFFSET/8;
				sof = regs->ar_pfs & 0x7f;
				sol = (regs->ar_pfs & 0x3f80) >> 7;
				soo = sof - sol;
 
				/* unwind to return function and grab it's bsp */
				unw_init_frame_info(&info, current, sw);
				do {
					/* nothing */
				} while (unw_unwind(&info) >= 0);
				unw_get_bsp(&info, &bsp);
 
				/* The bsp for the return function points to out0 for the
				 * currently executing user application function so we
				 * need to add the number of user application function output
				 * registers (soo) to obtain a correct count of the number of
				 * registers on the kernel register backing store.
				 */
				num_regs_on_krbs = ia64_rse_num_regs(krbs, (unsigned long *)bsp) + soo;
				addr = ia64_rse_skip_regs((unsigned long *) bsp, soo - sof + (regnum - 32));
				if (addr >= krbs) {
					/* register value is on the krbs at address addr */
					if ((regnum - 32) < sof)
					val = *addr;
				} else {
					/* register value is on the urbs */
					bspstore = (unsigned long *)regs->ar_bspstore;
					urbs_end = ia64_rse_skip_regs(bspstore, num_regs_on_krbs);
					bsp = (unsigned long) ia64_rse_skip_regs(urbs_end, -sof);
					addr = ia64_rse_skip_regs((unsigned long *) bsp, regnum - 32);
					ia64_peek(current, sw, (unsigned long) urbs_end,
							(unsigned long) addr, (unsigned long *) &val);
				}
			} else {
				/* - Probe point in kernel code.
				 * - RPN instruction: 'push r' (current context).
				 * - Stacked GR values are on kernel backing store so we can
				 *   unwind to the frame for the function where the break
				 *   instruction is located and get register value using
				 *   the standard kernel unwind API.
				 */
				unw_init_frame_info(&info, current, sw);
				do {
					unw_get_ip(&info, &ip);
					if (ip == regs->cr_iip) {
						unw_get_cfm(&info, &cfm);
						sof = cfm & 0x7f;
						break;
					}
				} while (unw_unwind(&info) >= 0);
				if ((regnum - 32) < sof)
					unw_get_gr(&info, regnum, &val, &nat);
			}
		}
	}
	return val;
}

extern void dp_set_gr(unsigned short regnum, unsigned long val, 
			struct pt_regs *regs,  struct switch_stack *sw)
{
	register struct dprobes_struct *dp = &dprobes;
	unsigned long sof = 0, sol, soo, ndirty, bsp, ip, cfm;
	unsigned long *krbs, num_regs_on_krbs, *bspstore, *urbs_end;
        char nat = 0;
        unsigned long *addr;
        struct unw_frame_info info;

	if (regnum < 32) {
		/* gr0-gr31, Static General Registers */
		switch(regnum) {
			case 0:  /* r0, read only, always 0 */
				/* Shouldn't get here */
				break;
			case 1:  /* r1, gp */
			case 2:  /* r2 */
			case 3:  /* r3 */
				addr = &regs->r1 + (regnum - 1);
				*addr = val;
				break;
			case 4:  /* r4 */
			case 5:  /* r5 */
			case 6:  /* r6 */
			case 7:  /* r7 */
				addr = &sw->r4 + (regnum - 4);
				*addr = val;
				break;
			case 8:  /* r8, ret0 */
			case 9:  /* r9, ret1 */
			case 10: /* r10, ret2 */
			case 11: /* r11, ret3 */
				addr = &regs->r8 + (regnum - 8);
				*addr = val;
				break;
			case 12: /* r12, sp */
			case 13: /* r13 */
			case 14: /* r14 */
			case 15: /* r15 */
				addr = &regs->r12 + (regnum - 12);
				*addr = val;
				break;
			case 16:  /* r16 */
			case 17:  /* r17 */
			case 18:  /* r18 */
			case 19:  /* r19 */
			case 20:  /* r20 */
			case 21:  /* r21 */
			case 22:  /* r22 */
			case 23:  /* r23 */
			case 24:  /* r24 */
			case 25:  /* r25 */
			case 26:  /* r26 */
			case 27:  /* r27 */
			case 28:  /* r28 */
			case 29:  /* r29 */
			case 30:  /* r30 */
			case 31:  /* r31 */
				addr = &regs->r16 + (regnum - 16);
				*addr = val;
				break;
			default:
				/* Shouldn't get here */
				break;
		}
	} else {
		/* r32-r127, Stacked General Registers */
		if (dp->regs == dp->uregs) {
			/*
			 * - Probe point in user code.
			 * - RPN instructions: 'push u' (user context) or
			 *                     'push r' (current context).
			 * - All stacked GR values are located on the kernel
			 *   register backing store.
			 */
			krbs = (unsigned long *) current + IA64_RBS_OFFSET/8;
			sof = regs->cr_ifs & 0x7f;
			ndirty = ia64_rse_num_regs(krbs, krbs + (regs->loadrs >> 19));
			bsp = (unsigned long) ia64_rse_skip_regs(krbs, ndirty - sof);
			if ((regnum - 32) < sof) {
				addr = ia64_rse_skip_regs((unsigned long *)bsp, regnum - 32);
				*addr = val;
			}
		} else {
			if (regs == ia64_task_regs(current)) {
				/*
				 * - Probe point in kernel code.
				 * - RPN instruction: 'push u' (user context).
				 * - Some stacked GR values are on the kernel
				 *   register backing store and some are on the user
				 *   register backing store.
				 *
				 * - This code appears to successfully obtain the stacked
				 *   GR values for the frame corresponding to the user
				 *   function that made a system call.  The code may need
				 *   to be updated to correctly handle other 
				 *   (non system call) kernel entries.
				 */
				krbs = (unsigned long *) current + IA64_RBS_OFFSET/8;
				sof = regs->ar_pfs & 0x7f;
				sol = (regs->ar_pfs & 0x3f80) >> 7;
				soo = sof - sol;
 
				/* unwind to return function and grab it's bsp */
				unw_init_frame_info(&info, current, sw);
				do {
					/* nothing */
				} while (unw_unwind(&info) >= 0);
				unw_get_bsp(&info, &bsp);
 
				/* The bsp for the return function points to out0 for the
				 * currently executing user application function so we
				 * need to add the number of user application function output
				 * registers (soo) to obtain a correct count of the number of
				 * registers on the kernel register backing store.
				 */
				num_regs_on_krbs = ia64_rse_num_regs(krbs, (unsigned long *)bsp) + soo;
				addr = ia64_rse_skip_regs((unsigned long *) bsp, soo - sof + (regnum - 32));
				if (addr >= krbs) {
					/* register value is on the krbs at address addr */
					if ((regnum - 32) < sof)
						*addr = val;
				} else {
					/* register value is on the urbs */
					bspstore = (unsigned long *)regs->ar_bspstore;
					urbs_end = ia64_rse_skip_regs(bspstore, num_regs_on_krbs);
					bsp = (unsigned long) ia64_rse_skip_regs(urbs_end, -sof);
					addr = ia64_rse_skip_regs((unsigned long *) bsp, regnum - 32);
					ia64_poke(current, sw, (unsigned long) urbs_end, (unsigned long) addr, (long) val);
				}
			} else {
				/* 
				 * - Probe point in kernel code.
				 * - RPN instruction: 'push r' (current context).
				 * - All stacked GR values are on kernel backing store.
				 *   We unwind to the frame for the function where the break
				 *   instruction is located and set register value using
				 *   the standard kernel unwind API.
				 */
				unw_init_frame_info(&info, current, sw);
				do {
					unw_get_ip(&info, &ip);
					if (ip == regs->cr_iip) {
						unw_get_cfm(&info, &cfm);
						sof = cfm & 0x7f;
						break;
					}
				} while (unw_unwind(&info) >= 0);
				if ((regnum - 32) < sof)
					unw_set_gr (&info, regnum, val, nat);
			}
		}
	}
}
void dprobes_asm_code_end(void) { }
