/* 
 * This file is subject to the terms and conditions of the GNU General Public
 * License.  See the file "COPYING" in the main directory of this archive
 * for more details.
 *
 * Copyright (C) Hewlett-Packard (Paul Bame) paul_bame@hp.com
 */
#include <stdio.h>

#define GET_REG(reg)	 ({			\
	unsigned long r;			\
	__asm__ __volatile__(			\
		"copy " #reg ",%0" : "=r" (r)	\
	);					\
	r;					\
})

#define GET_SP() GET_REG(30)
#define GET_RP() GET_REG(2)

#define GET_PC()	 ({			\
	unsigned long r;			\
	__asm__ __volatile__(			\
		"ldil L%%(.+8), %0\n\t"		\
		"ldo  R%%(.+4)(%0), %0" : "=r" (r)	\
	);					\
	r;					\
})

/* These two would be MUCH easier with a quick asm() instruction! */
/* extract some bits, least-sig bit is numbered 0, unsigned */
#define GETBITSLSB0(i, start, length)	\
	(((i) >> (start)) & ((1 << (length)) - 1))

/* extract some bits, least-sig bit is numbered 0, signed */
#define GETBITSLSB0S(i, start, length)	\
	(((signed)((i) << (32 - (start + length)))) >> (32 - length))

/* bl   xxx,%r2 */
#define BL2_BITS	0xe8400000
#define BL2_MASK	0xffe0e000
#define IS_BL2(i)	(((i) & BL2_MASK) == BL2_BITS)

/********** Not finished ************/
#define BLR_BITS	0xe8004
#define BLR_MASK	0xfc00e

union bl
{
    unsigned instr;
    struct
    {
	unsigned opcode:6;
	unsigned t:5;
	unsigned w1:5;
	unsigned dummy:3;
	unsigned w2_0_9:10;
	unsigned w2_10:1;
	unsigned n:1;
	unsigned w:1;
    } bits;
};

static int
assemble_17(unsigned instr)
{
    union bl bl;
    int x;
    int sx;

    bl.instr = instr;

    x = bl.bits.w2_0_9;
    x |= bl.bits.w2_10 << 10;
    x |= bl.bits.w1 << 11;
    x |= bl.bits.w << (5 + 11);

    sx = x << (32 - 17);
    sx >>= (32 - 17 - 2);

#if 0
    printf("op %x t %x w1 %x w20 %x n %x w %x x %x 4x %x sx %d\n",
	bl.bits.opcode,
	bl.bits.t,
	bl.bits.w1,
	bl.bits.w2_0_9,
	bl.bits.n,
	bl.bits.w, x, x << 2, sx);
#endif
    return sx;
}

static unsigned *
find_rp(unsigned *sp, unsigned *dot, unsigned **btargetp)
{
    unsigned *lim = sp - 1024;
    extern unsigned _etext, __text_start;

    while(sp > lim)
    {
	unsigned *maybe_rp = (unsigned *)*sp;

	if (0) printf("%p: %p\n", sp, maybe_rp);

	if (((unsigned)maybe_rp & 0x3) == 0x3
		&& maybe_rp < &_etext
		&& maybe_rp >= &__text_start)
	{
	    unsigned instr;
	    unsigned *btarget;

	    maybe_rp = (unsigned *)((unsigned)maybe_rp & ~0x3);

	    /* check if instr - 2 is a bl  ..., %r2 */
	    instr = maybe_rp[-2];
	    if (IS_BL2(instr))
	    {
		btarget = (unsigned *)((unsigned)maybe_rp + assemble_17(instr));
		printf("%p: %p: bl %p, %%r2\n",
		    sp,
		    maybe_rp - 2,
		    btarget
		    );
		if (btarget > dot)
		{
		    printf("unlikely bl, .+0x%x\n",
			(unsigned)btarget - (unsigned)maybe_rp);
		}
		else
		{
		    *btargetp = btarget;
		    return sp;
		}
	    }
	}
	sp--;
    }

    return 0;
}

void unwind()
{
    unsigned *rp = (unsigned *)(GET_RP() & ~0x3);
    unsigned *sp = (unsigned *)GET_SP();
    /* lie about these data types... _end could be 1-byte aligned */
    extern unsigned _end;
    extern unsigned _etext;
    extern unsigned __text_start;
    unsigned *lastrpsp = sp;
    extern unsigned dotlabel;
    unsigned *dot;
    unsigned *our_entry;
    unsigned *lim;
    unsigned *btarget;

    asm("\ndotlabel:");

    dot = &dotlabel;

    printf("Initial rp: %p\tsp: %p\tetext: %p\t.: %p\n", rp, sp, &_etext, dot);

    /* find where my RP is stored, hence my caller's stack frame */
    for (   ; *sp != rp; sp--)
    {
    }
    printf("Found my RP on stack at %p\n", sp);

    /* stack frames are 16 words minimum, rp is stored in word -5 relative */
    /* to stack pointer on entry to a routine, so we can calculate the sp */
    /* on entry to the current routine */
    sp += 5;
    printf("My entry SP was %p\n", sp);

    /* by disassembling the target of the branch which brought us here, */
    /* we can learn our entry point address */
    our_entry = (unsigned *)(assemble_17(rp[-2]) + (unsigned)rp);
    printf("Entry point to this routine is %p\n", our_entry);

    /* can't be sure about args, but some may be on stack... */
    printf("%p: %p(%08x, %08x, %08x, %08x)\n",
					rp - 2,
					our_entry,
					sp[-9],
					sp[-10],
					sp[-11],
					sp[-12]);

    /* become the previous procedure */
    dot = rp;

    /* this is the minimum stack frame size -- skip over it */
    sp -= 16;

    sp = find_rp(sp, dot, &btarget);
    if (sp != 0)
    {
	rp = (unsigned *)((unsigned)*sp & ~0x3);
	printf("Found my RP (%p) on stack at %p\n", rp, sp);
	/* stack frames are 16 words minimum, rp is stored in word -5 relative */
	/* to stack pointer on entry to a routine, so we can calculate the sp */
	/* on entry to the current routine */
	sp += 5;
	printf("My entry SP was %p\n", sp);

	/* by disassembling the target of the branch which brought us here, */
	/* we can learn our entry point address */
	if (0) our_entry = (unsigned *)(assemble_17(rp[-2]) + (unsigned)rp);
	printf("Entry point to this routine is %p\n", btarget);

	/* can't be sure about args, but some may be on stack... */
	printf("%p: %p()\n", rp - 2, our_entry);

	/* become the previous procedure */
	dot = rp;
    }
}
#if 0
void ounwind()
{
    unsigned *sp = (unsigned *)GET_SP();
    unsigned *rp = (unsigned *)GET_RP();
    /* lie about these data types... _end could be 1-byte aligned */
    extern unsigned _end;
    extern unsigned _etext;
    extern unsigned __text_start;
    unsigned *lim = sp - 1024*4;
    unsigned *lastrpsp = sp;
    extern unsigned dotlabel;
    unsigned *dot;

    asm("\ndotlabel:");

    dot = &dotlabel;

    printf("Initial rp: %p\tsp: %p\tetext: %p\t.: %p\n", rp, sp, &_etext, dot);

    /* walk backwards in stack looking for likely RPs */
    while(sp > lim)
    {
	unsigned *maybe_rp = (unsigned *)*sp;

	if (0) printf("%p: %p\n", sp, maybe_rp);

	if (((unsigned)maybe_rp & 0x3) == 0x3
		&& maybe_rp < &_etext
		&& maybe_rp >= &__text_start)
	{
	    unsigned instr;
	    unsigned *btarget;

	    maybe_rp = (unsigned *)((unsigned)maybe_rp & ~0x3);

	    /* check if instr - 2 is a bl  ..., %r2 */
	    instr = maybe_rp[-2];
	    if (IS_BL2(instr))
	    {
		btarget = (unsigned)maybe_rp + assemble_17(instr);
		printf("%p: %p: bl %p, %%r2, stack delta 0x%x\n",
		    sp,
		    maybe_rp - 2,
		    btarget,
		    (unsigned)lastrpsp - (unsigned)sp
		    );
		if (btarget > dot)
		{
		    printf("unlikely bl, .+0x%x\n",
			(unsigned)btarget - (unsigned)maybe_rp);
		}
		lastrpsp = sp;
	    }
	}
	sp--;
    }
    exit(77);
}
#endif