#define _GNU_SOURCE 1
#include <errno.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/syscall.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

#define FUTEX_WAIT		0
#define FUTEX_WAKE		1
#define FUTEX_FD		2
#define FUTEX_REQUEUE		3
#define FUTEX_CMP_REQUEUE	4
#define FUTEX_WAKE_OP		5

#define FUTEX_OP_SET		0	/* *(int *)UADDR2 = OPARG; */
#define FUTEX_OP_ADD		1	/* *(int *)UADDR2 += OPARG; */
#define FUTEX_OP_OR		2	/* *(int *)UADDR2 |= OPARG; */
#define FUTEX_OP_ANDN		3	/* *(int *)UADDR2 &= ~OPARG; */
#define FUTEX_OP_XOR		4	/* *(int *)UADDR2 ^= OPARG; */

#define FUTEX_OP_OPARG_SHIFT	8	/* Use (1 << OPARG) instead of OPARG.  */

#define FUTEX_OP_CMP_EQ		0	/* if (oldval == CMPARG) wake */
#define FUTEX_OP_CMP_NE		1	/* if (oldval != CMPARG) wake */
#define FUTEX_OP_CMP_LT		2	/* if (oldval < CMPARG) wake */
#define FUTEX_OP_CMP_LE		3	/* if (oldval <= CMPARG) wake */
#define FUTEX_OP_CMP_GT		4	/* if (oldval > CMPARG) wake */
#define FUTEX_OP_CMP_GE		5	/* if (oldval >= CMPARG) wake */

/* FUTEX_WAKE_OP will perform atomically
   int oldval = *(int *)UADDR2;
   *(int *)UADDR2 = oldval OP OPARG;
   if (oldval CMP CMPARG)
     wake UADDR2;  */

#define FUTEX_OP(op, oparg, cmp, cmparg) \
  (((op & 0xf) << 28) | ((cmp & 0xf) << 24)		\
   | ((oparg & 0xfff) << 12) | (cmparg & 0xfff))

int futex;
int lock;
volatile int wake2;
pthread_barrier_t b;

#define N (5 * 2 * 6 * 20 + 1)

static void *
tf (void *arg)
{
  int i;
  for (i = 0; i < N; i++)
    {
      int xpect;
      pthread_barrier_wait (&b);
      xpect = arg ? lock : i;
      int ret = syscall (SYS_futex, arg ? &lock : &futex, FUTEX_WAIT,
			 xpect, 0);
      if (ret < 0 && errno == ENOSYS)
	error (EXIT_FAILURE, 0, "SYS_futex syscall FUTEX_WAIT returned ENOSYS");
      if (arg && !wake2)
	error (EXIT_FAILURE, 0, "%d: arg && !wake2", i);
      pthread_barrier_wait (&b);
      if (futex != i + 1)
	error (EXIT_FAILURE, 0, "tf%d: futex %d != i %d", arg ? 1 : 0, futex, i + 1);
    }
  return NULL;
}

int
main (int argc, char **argv)
{
  int verbose = (argc > 1 && strcmp (argv[1], "-v"));

  pthread_barrier_init (&b, NULL, 3);
  pthread_t th[2];
  if (pthread_create (&th[0], NULL, tf, (void *) 0L) != 0)
    error (EXIT_FAILURE, 0, "create failed");

  if (pthread_create (&th[1], NULL, tf, (void *) 1L) != 0)
    error (EXIT_FAILURE, 0, "create failed");

  int i;
  for (i = 0; i < N; i++)
    {
      int op, cmp, shift, cmd, cmparg, oparg, xpect;

      if (i != 0)
	{
	  cmd = i - 1;
	  op = cmd % 5;
	  cmd /= 5;
	  cmp = cmd % 6;
	  cmd /= 6;
	  shift = cmd % 2;
	  lock = (random () << 31) ^ random ();
	  oparg = (random () << 31) ^ random ();
	  cmparg = ((int) random () << 20) >> 20;
	  if (shift)
	    oparg &= 31;
	  else
	    {
	      lock = (lock << 20) >> 20;
	      oparg = (oparg << 20) >> 20;
	    }
	  cmd = FUTEX_OP (op | (shift ? FUTEX_OP_OPARG_SHIFT : 0),
			  oparg, cmp, cmparg);
	  if (shift)
	    oparg = (1 << oparg);
	  switch (cmp)
	    {
	    case FUTEX_OP_CMP_EQ: wake2 = (lock == cmparg); break;
	    case FUTEX_OP_CMP_NE: wake2 = (lock != cmparg); break;
	    case FUTEX_OP_CMP_LT: wake2 = (lock < cmparg); break;
	    case FUTEX_OP_CMP_LE: wake2 = (lock <= cmparg); break;
	    case FUTEX_OP_CMP_GT: wake2 = (lock > cmparg); break;
	    case FUTEX_OP_CMP_GE: wake2 = (lock >= cmparg); break;
	    default:
	      abort ();
	    }
	  xpect = lock;
	  switch (op)
	    {
	    case FUTEX_OP_SET: xpect = oparg; break;
	    case FUTEX_OP_ADD: xpect += oparg; break;
	    case FUTEX_OP_OR: xpect |= oparg; break;
	    case FUTEX_OP_ANDN: xpect &= ~oparg; break;
	    case FUTEX_OP_XOR: xpect ^= oparg; break;
	    default:
	      abort ();
	    }
	  if (verbose)
	    printf ("cmd %08x op %d shift %d cmp %d oparg %d cmparg %d lock %d\n",
		    (unsigned int) cmd, op, shift, cmp, oparg, cmparg, lock);
	}
      else
	{
	  xpect = lock;
	  wake2 = 0;
	}
      if (verbose)
	printf ("%d: wake2 %d\n", i, wake2);
      pthread_barrier_wait (&b);
      struct timespec ts = { 0, 10000000 };
      nanosleep (&ts, NULL);
      futex++;
      int ret;
      if (i == 0)
	ret = syscall (SYS_futex, &futex, FUTEX_WAKE, 1);
      else
	ret = syscall (SYS_futex, &futex, FUTEX_WAKE_OP, 1, 1, &lock, cmd);
      if (ret < 0 && errno == ENOSYS)
	error (EXIT_FAILURE, 0,
	       "SYS_futex syscall FUTEX_WAKE%s returned ENOSYS",
	       i ? "_OP" : "");
      if (lock != xpect)
	error (EXIT_FAILURE, 0, "%d: lock %d != xpect %d", i, lock, xpect);
      if (!wake2)
	{
	  nanosleep (&ts, NULL);
	  wake2 = 1;
	  syscall (SYS_futex, &lock, FUTEX_WAKE, 1);
	}
      pthread_barrier_wait (&b);
    }

  if (pthread_join (th[0], NULL) != 0)
    error (EXIT_FAILURE, 0, "join failed");
  if (pthread_join (th[1], NULL) != 0)
    error (EXIT_FAILURE, 0, "join failed");

  return 0;
}