struct futex_q {
	struct list_head list;
	struct task_struct *task;
	/* Page struct and offset within it. */
	struct page *page;
	unsigned int offset;
};

long futex(uint32_t *uaddr, int futex_op, uint32_t val,
                 const struct timespec *timeout,   /* or: uint32_t val2 */
                 uint32_t *uaddr2, uint32_t val3);

/*
 * Wake up waiters matching bitset queued on this futex (uaddr).
 */
static int
futex_wake(u32 __user *uaddr, unsigned int flags, int nr_wake, u32 bitset)
{
	struct futex_hash_bucket *hb;
	struct futex_q *this, *next;
	struct plist_head *head;
	union futex_key key = FUTEX_KEY_INIT;
	int ret;

	if (!bitset)
		return -EINVAL;

	ret = get_futex_key(uaddr, flags & FLAGS_SHARED, &key);
	if (unlikely(ret != 0))
		goto out;

	hb = hash_futex(&key);
	spin_lock(&hb->lock);
	head = &hb->chain;

	plist_for_each_entry_safe(this, next, head, list) {
		if (match_futex (&this->key, &key)) {
			if (this->pi_state || this->rt_waiter) {
				ret = -EINVAL;
				break;
			}

			/* Check if one of the bits is set in both bitsets */
			if (!(this->bitset & bitset))
				continue;

			wake_futex(this);
			if (++ret >= nr_wake)
				break;
		}
	}

	spin_unlock(&hb->lock);
	put_futex_key(&key);
out:
	return ret;
}

static int futex_wait(u32 __user *uaddr, unsigned int flags, u32 val,
		      ktime_t *abs_time, u32 bitset)
{
	struct hrtimer_sleeper timeout, *to = NULL;
	struct restart_block *restart;
	struct futex_hash_bucket *hb;
	struct futex_q q = futex_q_init;
	int ret;

	if (!bitset)
		return -EINVAL;
	q.bitset = bitset;

	if (abs_time) {
		to = &timeout;

		hrtimer_init_on_stack(&to->timer, (flags & FLAGS_CLOCKRT) ?
				      CLOCK_REALTIME : CLOCK_MONOTONIC,
				      HRTIMER_MODE_ABS);
		hrtimer_init_sleeper(to, current);
		hrtimer_set_expires_range_ns(&to->timer, *abs_time,
					     current->timer_slack_ns);
	}

retry:
	/*
	 * Prepare to wait on uaddr. On success, holds hb lock and increments
	 * q.key refs.
	 */
	ret = futex_wait_setup(uaddr, val, flags, &q, &hb);
	if (ret)
		goto out;

	/* queue_me and wait for wakeup, timeout, or a signal. */
	futex_wait_queue_me(hb, &q, to);

	/* If we were woken (and unqueued), we succeeded, whatever. */
	ret = 0;
	/* unqueue_me() drops q.key ref */
	if (!unqueue_me(&q))
		goto out;
	ret = -ETIMEDOUT;
	if (to && !to->task)
		goto out;

	/*
	 * We expect signal_pending(current), but we might be the
	 * victim of a spurious wakeup as well.
	 */
	if (!signal_pending(current))
		goto retry;

	ret = -ERESTARTSYS;
	if (!abs_time)
		goto out;

	restart = &current_thread_info()->restart_block;
	restart->fn = futex_wait_restart;
	restart->futex.uaddr = uaddr;
	restart->futex.val = val;
	restart->futex.time = abs_time->tv64;
	restart->futex.bitset = bitset;
	restart->futex.flags = flags | FLAGS_HAS_TIMEOUT;

	ret = -ERESTART_RESTARTBLOCK;

out:
	if (to) {
		hrtimer_cancel(&to->timer);
		destroy_hrtimer_on_stack(&to->timer);
	}
	return ret;
}

	/*
	 * Prepare to wait on uaddr. On success, holds hb lock and increments
	 * q.key refs.
	 */
	ret = futex_wait_setup(uaddr, val, flags, &q, &hb);
	if (ret)
		goto out;
	/* queue_me and wait for wakeup, timeout, or a signal. */
	futex_wait_queue_me(hb, &q, to);
	/* If we were woken (and unqueued), we succeeded, whatever. */
	ret = 0;
	/* unqueue_me() drops q.key ref */
	if (!unqueue_me(&q))
		goto out;
    /* blah blah blah, other codes */
out:
	return ret;

	ret = get_futex_key(uaddr, flags & FLAGS_SHARED, &q->key);
	if (unlikely(ret != 0))
		return ret;

static inline struct futex_hash_bucket *queue_lock(struct futex_q *q)
	__acquires(&hb->lock)
{
	struct futex_hash_bucket *hb;

	hb = hash_futex(&q->key);
	q->lock_ptr = &hb->lock;

	spin_lock(&hb->lock);
	return hb;
}

ret = get_futex_value_locked(&uval, uaddr);
if (uval != val) {
		queue_unlock(q, *hb);
		ret = -EWOULDBLOCK;
}

/**
 * futex_wait_queue_me() - queue_me() and wait for wakeup, timeout, or signal
 * @hb:		the futex hash bucket, must be locked by the caller
 * @q:		the futex_q to queue up on
 * @timeout:	the prepared hrtimer_sleeper, or null for no timeout
 */
static void futex_wait_queue_me(struct futex_hash_bucket *hb, struct futex_q *q,
				struct hrtimer_sleeper *timeout)
{
	/*
	 * The task state is guaranteed to be set before another task can
	 * wake it. set_current_state() is implemented using set_mb() and
	 * queue_me() calls spin_unlock() upon completion, both serializing
	 * access to the hash list and forcing another memory barrier.
	 */
	set_current_state(TASK_INTERRUPTIBLE);
	queue_me(q, hb);

	/* Arm the timer */
	if (timeout) {
		hrtimer_start_expires(&timeout->timer, HRTIMER_MODE_ABS);
		if (!hrtimer_active(&timeout->timer))
			timeout->task = NULL;
	}

	/*
	 * If we have been removed from the hash list, then another task
	 * has tried to wake us, and we can skip the call to schedule().
	 */
	if (likely(!plist_node_empty(&q->list))) {
		/*
		 * If the timer has already expired, current will already be
		 * flagged for rescheduling. Only call schedule if there
		 * is no timeout, or if it has yet to expire.
		 */
		if (!timeout || timeout->task)
			schedule();
	}
	__set_current_state(TASK_RUNNING);
}

#define errExit(msg)    do { perror(msg); exit(EXIT_FAILURE); } while (0)

static uint32_t *futex1, *futex2, *iaddr;

static int
futex(uint32_t *uaddr, int futex_op, uint32_t val,
	 const struct timespec *timeout, uint32_t *uaddr2, uint32_t val3)
{
   return syscall(SYS_futex, uaddr, futex_op, val,
				  timeout, uaddr2, val3);
}

/* Acquire the futex pointed to by 'futexp': wait for its value to
  become 1, and then set the value to 0. */
static void
fwait(uint32_t *futexp)
{
   long s;

   /* atomic_compare_exchange_strong(ptr, oldval, newval)
	  atomically performs the equivalent of:

    if (*ptr == *oldval)
	    *ptr = newval;

	  It returns true if the test yielded true and *ptr was updated. */

   while (1) {
	   /* Is the futex available? */
	   const uint32_t one = 1;
	   if (atomic_compare_exchange_strong(futexp, &one, 0))
		   break;      /* Yes */

	   /* Futex is not available; wait. */
	   s = futex(futexp, FUTEX_WAIT, 0, NULL, NULL, 0);
	   if (s == -1 && errno != EAGAIN)
		   errExit("futex-FUTEX_WAIT");
   }
}

/* Release the futex pointed to by 'futexp': if the futex currently
  has the value 0, set its value to 1 and the wake any futex waiters,
  so that if the peer is blocked in fwait(), it can proceed. */
static void
fpost(uint32_t *futexp)
{
   long s;

   const uint32_t zero = 0;
   if (atomic_compare_exchange_strong(futexp, &zero, 1)) {
	   s = futex(futexp, FUTEX_WAKE, 1, NULL, NULL, 0);
	   if (s  == -1)
		   errExit("futex-FUTEX_WAKE");
   }
}

_Bool atomic_compare_exchange_strong(volatile A *object, C *expected, C desired);

desired = ...;
expected = current;
while (current.atomic_compare_exchange_weak(expected, desired))
  ;

if (memcmp(obj, expected, sizeof *obj) == 0) {
    memcpy(obj, &desired, sizeof *obj);
    return true;
} else {
    memcpy(expected, obj, sizeof *obj);
    return false;
}

/*
 *  Fast Userspace Mutexes (which I call "Futexes!").
 *  (C) Rusty Russell, IBM 2002
 *
 *  Thanks to Ben LaHaise for yelling "hashed waitqueues" loudly
 *  enough at me, Linus for the original (flawed) idea, Matthew
 *  Kirkwood for proof-of-concept implementation.
 *
 *  "The futexes are also cursed."
 *  "But they come in a choice of three flavours!"
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
#include <linux/kernel.h>
#include <linux/spinlock.h>
#include <linux/sched.h>
#include <linux/mm.h>
#include <linux/hash.h>
#include <linux/init.h>
#include <linux/fs.h>
#include <linux/futex.h>
#include <linux/highmem.h>
#include <asm/atomic.h>

/* These mutexes are a very simple counter: the winner is the one who
   decrements from 1 to 0.  The counter starts at 1 when the lock is
   free.  A value other than 0 or 1 means someone may be sleeping.
   This is simple enough to work on all architectures, but has the
   problem that if we never "up" the semaphore it could eventually
   wrap around. */

/* FIXME: This may be way too small. --RR */
#define FUTEX_HASHBITS 6

/* We use this instead of a normal wait_queue_t, so we can wake only
   the relevent ones (hashed queues may be shared) */
struct futex_q {
	struct list_head list;
	struct task_struct *task;
	/* Page struct and offset within it. */
	struct page *page;
	unsigned int offset;
};

/* The key for the hash is the address + index + offset within page */
static struct list_head futex_queues[1<<FUTEX_HASHBITS];
static spinlock_t futex_lock = SPIN_LOCK_UNLOCKED;

static inline struct list_head *hash_futex(struct page *page,
					   unsigned long offset)
{
	unsigned long h;

	/* struct page is shared, so we can hash on its address */
	h = (unsigned long)page + offset;
	return &futex_queues[hash_long(h, FUTEX_HASHBITS)];
}

static inline void wake_one_waiter(struct list_head *head,
				   struct page *page,
				   unsigned int offset)
{
	struct list_head *i;

	spin_lock(&futex_lock);
	list_for_each(i, head) {
		struct futex_q *this = list_entry(i, struct futex_q, list);

		if (this->page == page && this->offset == offset) {
			wake_up_process(this->task);
			break;
		}
	}
	spin_unlock(&futex_lock);
}

/* Add at end to avoid starvation */
static inline void queue_me(struct list_head *head,
			    struct futex_q *q,
			    struct page *page,
			    unsigned int offset)
{
	q->task = current;
	q->page = page;
	q->offset = offset;

	spin_lock(&futex_lock);
	list_add_tail(&q->list, head);
	spin_unlock(&futex_lock);
}

static inline void unqueue_me(struct futex_q *q)
{
	spin_lock(&futex_lock);
	list_del(&q->list);
	spin_unlock(&futex_lock);
}

/* Get kernel address of the user page and pin it. */
static struct page *pin_page(unsigned long page_start)
{
	struct mm_struct *mm = current->mm;
	struct page *page;
	int err;

	down_read(&mm->mmap_sem);
	err = get_user_pages(current, current->mm, page_start,
			     1 /* one page */,
			     1 /* writable */,
			     0 /* don't force */,
			     &page,
			     NULL /* don't return vmas */);
	up_read(&mm->mmap_sem);

	if (err < 0)
		return ERR_PTR(err);
	return page;
}

/* Try to decrement the user count to zero. */
static int decrement_to_zero(struct page *page, unsigned int offset)
{
	atomic_t *count;
	int ret = 0;

	count = kmap(page) + offset;
	/* If we take the semaphore from 1 to 0, it's ours.  If it's
           zero, decrement anyway, to indicate we are waiting.  If
           it's negative, don't decrement so we don't wrap... */
	if (atomic_read(count) >= 0 && atomic_dec_and_test(count))
		ret = 1;
	kunmap(page);
	return ret;
}

/* Simplified from arch/ppc/kernel/semaphore.c: Paul M. is a genius. */
static int futex_down(struct list_head *head, struct page *page, int offset)
{
	int retval = 0;
	struct futex_q q;

	current->state = TASK_INTERRUPTIBLE;
	queue_me(head, &q, page, offset);

	while (!decrement_to_zero(page, offset)) {
		if (signal_pending(current)) {
			retval = -EINTR;
			break;
		}
		schedule();
		current->state = TASK_INTERRUPTIBLE;
	}
	current->state = TASK_RUNNING;
	unqueue_me(&q);
	/* If we were signalled, we might have just been woken: we
	   must wake another one.  Otherwise we need to wake someone
	   else (if they are waiting) so they drop the count below 0,
	   and when we "up" in userspace, we know there is a
	   waiter. */
	wake_one_waiter(head, page, offset);
	return retval;
}

static int futex_up(struct list_head *head, struct page *page, int offset)
{
	atomic_t *count;

	count = kmap(page) + offset;
	atomic_set(count, 1);
	smp_wmb();
	kunmap(page);
	wake_one_waiter(head, page, offset);
	return 0;
}

asmlinkage int sys_futex(void *uaddr, int op)
{
	int ret;
	unsigned long pos_in_page;
	struct list_head *head;
	struct page *page;

	pos_in_page = ((unsigned long)uaddr) % PAGE_SIZE;

	/* Must be "naturally" aligned, and not on page boundary. */
	if ((pos_in_page % __alignof__(atomic_t)) != 0
	    || pos_in_page + sizeof(atomic_t) > PAGE_SIZE)
		return -EINVAL;

	/* Simpler if it doesn't vanish underneath us. */
	page = pin_page((unsigned long)uaddr - pos_in_page);
	if (IS_ERR(page))
		return PTR_ERR(page);

	head = hash_futex(page, pos_in_page);
	switch (op) {
	case FUTEX_UP:
		ret = futex_up(head, page, pos_in_page);
		break;
	case FUTEX_DOWN:
		ret = futex_down(head, page, pos_in_page);
		break;
	/* Add other lock types here... */
	default:
		ret = -EINVAL;
	}
	put_page(page);

	return ret;
}

static int __init init(void)
{
	unsigned int i;

	for (i = 0; i < ARRAY_SIZE(futex_queues); i++)
		INIT_LIST_HEAD(&futex_queues[i]);
	return 0;
}
__initcall(init);

val = val + 1;

#define _GNU_SOURCE
#include <stddef.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <pthread.h>                   /* for threads */
#include <semaphore.h>                 /* sem_post sem_wait */
#include <assert.h>                    /* assert() */
#include <errno.h>
#include <sys/time.h>
#include <sys/types.h>

#define NUM_THREAD_MAX 3200

sem_t start_sem[NUM_THREAD_MAX];

void * periodic_task(void* arg);

volatile int val;

void * periodic_task(void* arg)
{
    int tid = *((int *)arg);

    while (1) {
        assert(sem_wait(&start_sem[tid]) == 0);
        if (tid%4 != 0) {
            val = val + 1;
        } else {
            __sync_add_and_fetch(&val, 1);        
        }
    }
}

void timer_addus(struct timespec *tm, int us) 
{
    tm->tv_nsec += us*1000;
    if (tm->tv_nsec > 1000000000L) {
        tm->tv_nsec -= 1000000000L;
        tm->tv_sec += 1;
    }
}

int main(int argc, const char *argv[])
{
    struct timespec timer1;
    struct sched_param sp;
    pthread_t periodic_task_thread[NUM_THREAD_MAX];
    pthread_attr_t attr;
    int ii, jj;
    int threadID[NUM_THREAD_MAX];

    /*  input (1) num_threads  (2) step_size(us)  */
    if (argc != 3) {
        printf("%s  num_threads  step_size(us)\n", argv[0]);
        exit(0);
    }
    int num_thread = atoi(argv[1]);
    if (num_thread >= NUM_THREAD_MAX) {
        printf("num_thread is greater than NUM_THREAD_MAX(%d)\n", NUM_THREAD_MAX);
        exit(0);
    }
    int step_ms = atoi(argv[2]);

    val = 0;

    /* init semaphores */
    for(ii=0; ii<num_thread; ++ii) 
        assert(sem_init(&start_sem[ii],0,0) == 0);
    pthread_attr_init(&attr);
    
    /*  create threads */
    for (ii=0; ii<num_thread; ++ii) {
        threadID[ii] = ii;
        assert(pthread_create(&periodic_task_thread[ii], &attr, periodic_task, \
                    (void *)(&threadID[ii])) == 0);
    }
    
    clock_gettime(CLOCK_MONOTONIC, &timer1);
    timer_addus(&timer1, 100000000);    // delay 100ms to start

    printf("val=%d\n", val);
    int loops = 10000;
    for (ii=0; ii<loops; ++ii) {
        for (jj=0; jj<num_thread; ++jj) 
            assert(sem_post(&start_sem[jj]) == 0);
        timer_addus(&timer1, step_ms);
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &timer1, NULL);
    }

    printf("Initial 0, After %d loops\n", loops);
    printf("Expected %d\n", num_thread*loops);
    printf("Real value: %d\n", val);

    return 0;
}