/*
 * linux_vdso_myclockgettime1.c - use Linux VDSO
 * shortcut for |clock_gettime()|, even if userland (g)libc
 * does not have support for VDSO
 *
 * Compile with:
 * $ clang -std=c99 -g -m32 -Wall -Wextra linux_vdso_myclockgettime1.c \
 * 	-lpthread -o linux_vdso_myclockgettime1
 *
 * Written by Roland Mainz <roland.mainz@nrubsig.org>
 *
 */

#define _XOPEN_SOURCE 600

#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <elf.h>
#include <time.h>
#include <pthread.h>
#include <sys/auxv.h>

#if (_XOPEN_SOURCE < 600)
#error _XOPEN_SOURCE wrong version
#endif

#if (__PTRDIFF_WIDTH__ == 32)
typedef Elf32_Ehdr	ElfXX_Ehdr;
typedef Elf32_Shdr	ElfXX_Shdr;
typedef Elf32_Sym	ElfXX_Sym;
#elif (__PTRDIFF_WIDTH__ == 64)
typedef Elf64_Ehdr	ElfXX_Ehdr;
typedef Elf64_Shdr	ElfXX_Shdr;
typedef Elf64_Sym	ElfXX_Sym;
#else
#error Unsupported __PTRDIFF_WIDTH__
#endif

/* debug */
#if 1
#define D(x) x
#else
#define D(x)
#endif

static
void *get_linux_vdso_sym(const char *funcname)
{
	const char	*dynstr = NULL;
	void		*ret = NULL;
	unsigned char	*vdso_addr;
	ssize_t		i;
	size_t		si;
	const char	*name;
	ElfXX_Ehdr	*elf_header;
	ElfXX_Shdr	*section_header;
	ElfXX_Shdr	*s;
	ElfXX_Shdr	*ss_;
	ElfXX_Sym	*sym;

	/*
	 * valgrind can disable |AT_SYSINFO_EHDR| - see
	 * https://www.mail-archive.com/kde-bugs-dist@kde.org/msg550927.html
	 */
	vdso_addr = (unsigned char *)getauxval(AT_SYSINFO_EHDR);
	if (!vdso_addr) {
		D((void)fprintf(stderr, "get_linux_vdso_sym: getauxval(AT_SYSINFO_EHDR) failed.\n"));
		return NULL;
	}
    
	elf_header = (ElfXX_Ehdr *)vdso_addr;
	section_header = (ElfXX_Shdr *)(vdso_addr + elf_header->e_shoff);

	for (i=0L; i < elf_header->e_shnum; i++) {
		s = &section_header[i];
		ss_ = &section_header[elf_header->e_shstrndx];
		name = (const char*)(vdso_addr + ss_->sh_offset + s->sh_name);

		if (strcmp(name, ".dynstr") == 0) {
			dynstr = (const char *)(vdso_addr + s->sh_offset);
			break;
		}
	}

	for (i=0L; i < elf_header->e_shnum; i++) {
		s = &section_header[i];
		ss_ = &section_header[elf_header->e_shstrndx];
		name = (const char*)(vdso_addr + ss_->sh_offset + s->sh_name);

		/* Not .dynsym ? Then look at next section... */
		if (strcmp(name, ".dynsym"))
			continue;

		for (si=0L; si < (s->sh_size/s->sh_entsize); si++) {
			sym = &(((ElfXX_Sym*)(vdso_addr + s->sh_offset))[si]);
			name = dynstr + sym->st_name;

			D((void)fprintf(stderr, "get_linux_vdso_sym: symbol=|%s| vs. |%s|\n",
				funcname, name));
			if (!strcmp(name, funcname)) {
				ret = (void *)(vdso_addr + sym->st_value);
				break;
			}
		}
		if (ret)
			break;
	}

	return ret;
}

typedef int (clock_gettime_func_t)(clockid_t clk_id, struct timespec *tp);

static pthread_once_t my_clock_gettime_func_is_initialized = PTHREAD_ONCE_INIT;
clock_gettime_func_t *my_clock_gettime_func = NULL;

void my_clock_gettime_func_init(void)
{
	my_clock_gettime_func = (clock_gettime_func_t *)get_linux_vdso_sym("clock_gettime");
	if (!my_clock_gettime_func)
		my_clock_gettime_func = (clock_gettime_func_t *)get_linux_vdso_sym("__vdso_clock_gettime");

	D((void)fprintf(stderr, "my_clock_gettime_func_init: "
		"my_clock_gettime_func = 0x%lx (%s)\n",
		(long)my_clock_gettime_func,
		(my_clock_gettime_func?
			"Using fast codepath":
			"Using slow codepath")));
	if (!my_clock_gettime_func) {
		(void)fprintf(stderr, "my_clock_gettime_func_init: "
			"WARNING: aux symbol for clock_gettime not found"
			", using slow libc codepath.\n");
	}
}


/*
 * my_clock_gettime() - our "public" interface
 *
 * Notes:
 * - The first call might be slower because the call to
 * |my_clock_gettime_func_init()|
 */
int my_clock_gettime(clockid_t clk_id, struct timespec *tp)
{
	(void)pthread_once(&my_clock_gettime_func_is_initialized,
		my_clock_gettime_func_init);

	if (my_clock_gettime_func) {
		/* our own "fast" codepath */
		return (*my_clock_gettime_func)(clk_id, tp);
	}
	else
	{
		/* libc codepath, whatever they use - might be VDSO, might be syscall */
		return clock_gettime(clk_id, tp);
	}
}

static
void test_my_clock_gettime1(void)
{
	/*
	 * dummy call to |my_clock_gettime()| to force initalisation
	 * via |my_clock_gettime_func_init()|
	 */
	struct timespec dummy;
	(void)my_clock_gettime(CLOCK_REALTIME, &dummy);
	
#define PRINT_CLOCK(clock) \
	{ \
		struct timespec tp1, tp2; \
		/* \
		 * first libc implementation, then our implementation, \
		 * so that the difference is the call time of our \
		 * codepath. Requires |my_clock_gettime_func_init()| \
		 * to run before this sequence!! \
		 */ \
		(void)clock_gettime((clock), &tp1); \
		(void)my_clock_gettime((clock), &tp2); \
		(void)printf( \
			"%-20s libc_impl = \t(%-10ld\t%-10ld), " \
			"our_impl = \t(%-10ld\t%-10ld)\n", \
			#clock ":", \
			(long)tp1.tv_sec, (long)tp1.tv_nsec, \
			(long)tp2.tv_sec, (long)tp2.tv_nsec); \
	}

	PRINT_CLOCK(CLOCK_REALTIME);
	PRINT_CLOCK(CLOCK_MONOTONIC);
}

int main(int ac, char *av[])
{
	(void)ac;
	(void)av;
	
	test_my_clock_gettime1();

	return EXIT_SUCCESS;
}