need help for mcount for MIPS

[Scholz Maik (K7/EEM15) *]

Hi all,
i am using the gnu compiler (gnupro 98r2). 
Our target is mips lsi in mips16/mips32 mode.
With the "-pg" option the compiler adds the 
following code to all C functions.

example C function ...

int main(void) 
#endif
{
 988:	27bdffe8 	addiu	$sp,$sp,-24
 98c:	afbf0010 	sw	$ra,16($sp)
 990:	03e00821 	move	$at,$ra
 994:	0c000000 	jal	_mcount
 998:	27bdfff8 	addiu	$sp,$sp,-8
	...
}
 b8c:	8fbf0010 	lw	$ra,16($sp)
 b90:	00000000 	nop
 b94:	03e00008 	jr	$ra
 b98:	27bd0018 	addiu	$sp,$sp,24

The needed "_mcount" functions, implemented by
my self.

	.text
	.set noat
	.set noreorder
	.globl _mcount
	.ent _mcount
		
_mcount:
	/* trace some data (RA,TIME,...) */
	
_mcount_ret: /* return */	
	addu	sp,sp,8				# _mcount push 2 words from
stack 
	jr	ra
	move    ra,at				# restore return address ra
	
	.set reorder
	.end _mcount

Ok this is working fine. Now i want to trace the 
function end of the calling method. My idea was
to modify the return address (ra) on the stack pointer.
But where can i find the original return address?

_mcount_end:
	/* trace some data (RA,TIME,...) */
	
	move    ra,at
	jr	ra
	nop
	.set reorder
	.end _mcount

Ist there any similar working use of the "-pg" feature.
"-pg" is currently not implemented in the mips16 mode.

gcc/config/mips/mips.h
#define FUNCTION_PROFILER(FILE, LABELNO)				\
{									\
  if (TARGET_MIPS16)							\
    sorry ("mips16 function profiling");				\
  fprintf (FILE, "\t.set\tnoreorder\n");				\
  fprintf (FILE, "\t.set\tnoat\n");					\
  fprintf (FILE, "\tmove\t%s,%s\t\t# save current return address\n",	\
	   reg_names[GP_REG_FIRST + 1], reg_names[GP_REG_FIRST + 31]);	\
  fprintf (FILE, "\tjal\t_mcount\n");					\
  fprintf (FILE,							\
	   "\t%s\t%s,%s,%d\t\t# _mcount pops 2 words from  stack\n",	\
	   TARGET_64BIT ? "dsubu" : "subu",				\
	   reg_names[STACK_POINTER_REGNUM],				\
	   reg_names[STACK_POINTER_REGNUM],				\
	   Pmode == DImode ? 16 : 8);					\
  fprintf (FILE, "\t.set\treorder\n");					\
  fprintf (FILE, "\t.set\tat\n");					\
}

How can i implement this. Is this only change this macro or 
are there any other changes needed in the gcc source code.

Thanks for any help.

Regards

Maik Scholz