/*
gcc -o itrace.so -shared -fPIC -Wall -ggdb2 itrace.c -ldw
-finstrument-functions
LD_PRELOAD=./itrace.so
*/

#include <stdio.h>
#include <elfutils/libdwfl.h>
#include <assert.h>
#include <errno.h>
#include <unistd.h>
#include <stdlib.h>

#define PREFIX "itrace: "
#define INDENT_MAX 20

static Dwfl *
dwfl_get (void)
{
  static char *debuginfo_path;
  static Dwfl *dwfl;

  static const Dwfl_Callbacks proc_callbacks =
   {
     .find_debuginfo = dwfl_standard_find_debuginfo,
     .debuginfo_path = &debuginfo_path,

     .find_elf = dwfl_linux_proc_find_elf,
   };

  if (dwfl == NULL)
    {
      dwfl = dwfl_begin (&proc_callbacks);
      assert (dwfl != NULL);

      errno = 0;
      if (dwfl_linux_proc_report (dwfl, getpid ()) != 0
	 || dwfl_report_end (dwfl, NULL, NULL) != 0)
	{
	  fprintf (stderr, "dwfl reporting: %m\n");
	  dwfl_end (dwfl);
	  dwfl = NULL;
	  abort ();
	}
    }
  return dwfl;
}

struct getmodules_callback_arg
  {
    void *addr;
    const char *name;
  };

static int getmodules_callback (Dwfl_Module *module,
				void **module_userdata_pointer,
				const char *module_name,
				Dwarf_Addr module_low_addr, void *arg_voidp)
{
  struct getmodules_callback_arg *arg = arg_voidp;

  arg->name = dwfl_module_addrname (module, (GElf_Addr) arg->addr);
  return arg->name ? DWARF_CB_ABORT : DWARF_CB_OK;
}

static const char *
addr_lookup (void *addr)
{
  Dwfl *dwfl = dwfl_get ();
  struct getmodules_callback_arg arg;

  arg.name = NULL;
  arg.addr = addr;
  dwfl_getmodules (dwfl, getmodules_callback, &arg, 0);

  return arg.name;
}

static const char *
addr_print (void *addr)
{
  const char *name;

  name = addr_lookup (addr);
  if (!name)
    {
      static char buf[20];

      sprintf (buf, "%p", addr);
      name = buf;
    }
  return name;
}

struct stack
  {
    void *this_fn;
    void *call_site;
  };
static struct stack *stack;
static unsigned stack_at, stack_size;

void __cyg_profile_func_enter (void *this_fn, void *call_site)
{
  int indent;

  if (stack_at == stack_size)
    {
      unsigned size_new = stack_size ? stack_size * 2 : 0x100;
      struct stack *stack_new;

      stack_new = realloc (stack, sizeof (*stack) * size_new);
      if (!stack_new)
	{
	  fprintf (stderr, PREFIX "stack size %u allocation failure!\n",
	           size_new);
	  return;
	}
      stack = stack_new;
      stack_size = size_new;
    }
  stack[stack_at].this_fn = this_fn;
  stack[stack_at].call_site = call_site;
  stack_at++;

  indent = stack_at - 1 <= INDENT_MAX ? stack_at - 1 : INDENT_MAX;

  fprintf (stderr, PREFIX "%*s->%s", indent, "", addr_print (this_fn));
  fprintf (stderr, " (%s)\n", addr_print (call_site));
}

void __cyg_profile_func_exit  (void *this_fn, void *call_site)
{
  int indent;

  indent = stack_at - 1 <= INDENT_MAX ? stack_at - 1 : INDENT_MAX;

  fprintf (stderr, PREFIX "%*s<-%s", indent, "", addr_print (this_fn));
  fprintf (stderr, " (%s)\n", addr_print (call_site));

  if (!stack_at)
    {
      fprintf (stderr, PREFIX "stack empty!\n");
      return;
    }
  if (stack[stack_at - 1].this_fn != this_fn
      || stack[stack_at - 1].call_site != call_site)
    {
      fprintf (stderr, PREFIX "Unexpected function leave, resetting the stack\n");
      stack_at = 0;
      return;
    }
  stack_at--;
}