#include <sys/syscall.h>
#include <unistd.h>
#include <stdio.h>
#include <pthread.h>

#define ITERS	10000000

int mem;

static int
sched_setaffinity(int mask) 
{
	return (syscall(475, mask));
}

static uint64_t
rdtsc()
{
	uint32_t low, high;

	__asm __volatile("rdtsc" : "=a" (low), "=d" (high));

	return (((uint64_t)high << 32) | low);
}

static int
apicid()
{
	int eax, ebx, ecx, edx;

	__asm __volatile("cpuid" : "=a" (eax), "=b" (ebx), "=c" (ecx),
	    "=d" (edx) : "a" (1));
	return (ebx >> 24);
}

void *
pong(void *arg)
{
	int cpu, i;

	cpu = (long)arg;
	sched_setaffinity(1 << cpu);

	while (1) {
		if (mem == 2)
			break;
		mem = 0;	
		__asm __volatile("" ::: "memory");
		while (mem == 0)
			__asm __volatile("" ::: "memory");
	}

	return (NULL);
}

void *
ping(void *arg)
{
	uint64_t end, start, tot;
	int cpu, i;

	cpu = (long)arg;
	sched_setaffinity(1 << cpu);

	tot = 0;
	for (i = 0; i < ITERS; i++) {
		start = rdtsc();
		mem = 1;
		__asm __volatile("" ::: "memory");

		while (mem != 0)
			__asm __volatile("" ::: "memory");
		end = rdtsc();
		tot += end - start;
	}

	mem = 2;

	printf("%lld", tot / ITERS);

	return (NULL);
}

int
main(void)
{
	pthread_t t1, t2;
	long i, j;
	int ncpus;

	ncpus = sysconf(_SC_NPROCESSORS_CONF);

	for (i = 0; i < ncpus; i++)
		printf("\tCPU%d", i);
	printf("\n");

	for (i = 0; i < ncpus; i++) {
		printf("CPU%d\t", i);
		for (j = 0; j < ncpus; j++) {
			if (i == j) {
				printf("\t");
				continue;
			}
			mem = 0;
			pthread_create(&t1, NULL, ping, (void *)i);
			pthread_create(&t2, NULL, pong, (void *)j);
			pthread_join(t1, NULL);
			printf("\t");
		}
		printf("\n");
	}

	return (0);
}