/*
 * Copyright (C) 2008 The Android Open Source Project
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */
#include <sys/types.h>
#include <unistd.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/mman.h>
#include <time.h>
#include <limits.h>
#include <memory.h>
#include <assert.h>
#include <malloc.h>
#include <cutils/atomics.h>
#include <cutils/mot_pthread.h>

#define  __likely(cond)    __builtin_expect(!!(cond), 1)
#define  __unlikely(cond)  __builtin_expect(!!(cond), 0)

#define FUTEX_WAIT 0
#define FUTEX_WAKE 1

//see atomics_x86.S, atomics_arm.S
int __futex_wait(volatile void *ftx, int val, const struct timespec *timeout);

int __futex_wake(volatile void *ftx, int count);

int __futex_syscall3(volatile void *ftx, int op, int val);
int __futex_syscall4(volatile void *ftx, int op, int val, const struct timespec *timeout);

#ifndef FUTEX_PRIVATE_FLAG
#define FUTEX_PRIVATE_FLAG  128
#endif

#ifndef FUTEX_WAIT_PRIVATE
#define FUTEX_WAIT_PRIVATE  (FUTEX_WAIT|FUTEX_PRIVATE_FLAG)
#endif

#ifndef FUTEX_WAKE_PRIVATE
#define FUTEX_WAKE_PRIVATE  (FUTEX_WAKE|FUTEX_PRIVATE_FLAG)
#endif

// mutex lock states
//
// 0: unlocked
// 1: locked, no waiters
// 2: locked, maybe waiters

/* a mutex is implemented as a 32-bit integer holding the following fields
 *
 * bits:     name     description
 * 31-16     tid      owner thread's kernel id (recursive and errorcheck only)
 * 15-14     type     mutex type
 * 13-2      counter  counter of recursive mutexes
 * 1-0       state    lock state (0, 1 or 2)
 */

#define  MUTEX_OWNER(m)  (((m)->value >> 16) & 0xffff)
#define  MUTEX_COUNTER(m) (((m)->value >> 2) & 0xfff)


#define  MUTEX_TYPE_MASK       0xc000
#define  MUTEX_TYPE_NORMAL     0x0000
#define  MUTEX_TYPE_RECURSIVE  0x4000
#define  MUTEX_TYPE_ERRORCHECK 0x8000

#define  MUTEX_COUNTER_SHIFT  2
#define  MUTEX_COUNTER_MASK   0x1ffc
#define  MUTEX_SHARED_MASK    0x2000

#define __inline__ __inline__ __attribute((always_inline))

/*
 * Lock a non-recursive mutex.
 *
 * As noted above, there are three states:
 *   0 (unlocked, no contention)
 *   1 (locked, no contention)
 *   2 (locked, contention)
 *
 * Non-recursive mutexes don't use the thread-id or counter fields, and the
 * "type" value is zero, so the only bits that will be set are the ones in
 * the lock state field.
 */
static __inline__ void
_normal_lock(mot_pthread_mutex_t*  mutex)
{
    /* We need to preserve the shared flag during operations */
    int  shared = mutex->value & MUTEX_SHARED_MASK;
    /*
     * The common case is an unlocked mutex, so we begin by trying to
     * change the lock's state from 0 to 1.  __atomic_cmpxchg() returns 0
     * if it made the swap successfully.  If the result is nonzero, this
     * lock is already held by another thread.
     */
    if (__atomic_cmpxchg(shared|0, shared|1, &mutex->value ) != 0) {
        /*
         * We want to go to sleep until the mutex is available, which
         * requires promoting it to state 2.  We need to swap in the new
         * state value and then wait until somebody wakes us up.
         *
         * __atomic_swap() returns the previous value.  We swap 2 in and
         * see if we got zero back; if so, we have acquired the lock.  If
         * not, another thread still holds the lock and we wait again.
         *
         * The second argument to the __futex_wait() call is compared
         * against the current value.  If it doesn't match, __futex_wait()
         * returns immediately (otherwise, it sleeps for a time specified
         * by the third argument; 0 means sleep forever).  This ensures
         * that the mutex is in state 2 when we go to sleep on it, which
         * guarantees a wake-up call.
         */
        int  wait_op = shared ? FUTEX_WAIT : FUTEX_WAIT_PRIVATE;

        while (__atomic_swap(shared|2, &mutex->value ) != (shared|0))
            __futex_syscall4(&mutex->value, wait_op, shared|2, 0);
    }
}

/*
 * Release a non-recursive mutex.  The caller is responsible for determining
 * that we are in fact the owner of this lock.
 */
static __inline__ void
_normal_unlock(mot_pthread_mutex_t*  mutex)
{
    /* We need to preserve the shared flag during operations */
    int  shared = mutex->value & MUTEX_SHARED_MASK;

    /*
     * The mutex state will be 1 or (rarely) 2.  We use an atomic decrement
     * to release the lock.  __atomic_dec() returns the previous value;
     * if it wasn't 1 we have to do some additional work.
     */
    if (__atomic_dec(&mutex->value) != (shared|1)) {
        int  wake_op = shared ? FUTEX_WAKE : FUTEX_WAKE_PRIVATE;
        /*
         * Start by releasing the lock.  The decrement changed it from
         * "contended lock" to "uncontended lock", which means we still
         * hold it, and anybody who tries to sneak in will push it back
         * to state 2.
         *
         * Once we set it to zero the lock is up for grabs.  We follow
         * this with a __futex_wake() to ensure that one of the waiting
         * threads has a chance to grab it.
         *
         * This doesn't cause a race with the swap/wait pair in
         * _normal_lock(), because the __futex_wait() call there will
         * return immediately if the mutex value isn't 2.
         */
        mutex->value = shared;

        /*
         * Wake up one waiting thread.  We don't know which thread will be
         * woken or when it'll start executing -- futexes make no guarantees
         * here.  There may not even be a thread waiting.
         *
         * The newly-woken thread will replace the 0 we just set above
         * with 2, which means that when it eventually releases the mutex
         * it will also call FUTEX_WAKE.  This results in one extra wake
         * call whenever a lock is contended, but lets us avoid forgetting
         * anyone without requiring us to track the number of sleepers.
         *
         * It's possible for another thread to sneak in and grab the lock
         * between the zero assignment above and the wake call below.  If
         * the new thread is "slow" and holds the lock for a while, we'll
         * wake up a sleeper, which will swap in a 2 and then go back to
         * sleep since the lock is still held.  If the new thread is "fast",
         * running to completion before we call wake, the thread we
         * eventually wake will find an unlocked mutex and will execute.
         * Either way we have correct behavior and nobody is orphaned on
         * the wait queue.
         */
        __futex_syscall3(&mutex->value, wake_op, 1);
    }
}

static mot_pthread_mutex_t  __recursive_lock = MOT_PTHREAD_MUTEX_INITIALIZER;

static void
_recursive_lock(void)
{
    _normal_lock( &__recursive_lock);
}

static void
_recursive_unlock(void)
{
    _normal_unlock( &__recursive_lock );
}

int mot_pthread_mutex_lock(mot_pthread_mutex_t *mutex)
{
    int mtype, tid, new_lock_type, shared, wait_op;

    if (__unlikely(mutex == NULL))
        return EINVAL;

    mtype = (mutex->value & MUTEX_TYPE_MASK);
    shared = (mutex->value & MUTEX_SHARED_MASK);

    /* Handle normal case first */
    if ( __likely(mtype == MUTEX_TYPE_NORMAL) ) {
        _normal_lock(mutex);
        return 0;
    }

    /* Do we already own this recursive or error-check mutex ? */
    //    int  tid = __get_thread()->kernel_id;
	     tid = getpid();
	
    if ( tid == MUTEX_OWNER(mutex) )
    {
        int  oldv, counter;

        if (mtype == MUTEX_TYPE_ERRORCHECK) {
            /* trying to re-lock a mutex we already acquired */
            return EDEADLK;
        }
        /*
         * We own the mutex, but other threads are able to change
         * the contents (e.g. promoting it to "contended"), so we
         * need to hold the global lock.
         */
        _recursive_lock();
        oldv         = mutex->value;
        counter      = (oldv + (1 << MUTEX_COUNTER_SHIFT)) & MUTEX_COUNTER_MASK;
        mutex->value = (oldv & ~MUTEX_COUNTER_MASK) | counter;
        _recursive_unlock();
        return 0;
    }

    /* We don't own the mutex, so try to get it.
     *
     * First, we try to change its state from 0 to 1, if this
     * doesn't work, try to change it to state 2.
     */
    new_lock_type = 1;

    /* compute futex wait opcode and restore shared flag in mtype */
    wait_op = shared ? FUTEX_WAIT : FUTEX_WAIT_PRIVATE;
    mtype  |= shared;

    for (;;) {
        int  oldv;

        _recursive_lock();
        oldv = mutex->value;
        if (oldv == mtype) { /* uncontended released lock => 1 or 2 */
            mutex->value = ((tid << 16) | mtype | new_lock_type);
        } else if ((oldv & 3) == 1) { /* locked state 1 => state 2 */
            oldv ^= 3;
            mutex->value = oldv;
        }
        _recursive_unlock();

        if (oldv == mtype)
            break;

        /*
         * The lock was held, possibly contended by others.  From
         * now on, if we manage to acquire the lock, we have to
         * assume that others are still contending for it so that
         * we'll wake them when we unlock it.
         */
        new_lock_type = 2;

        __futex_syscall4(&mutex->value, wait_op, oldv, NULL);
    }
    return 0;
}


int mot_pthread_mutex_unlock(mot_pthread_mutex_t *mutex)
{
    int mtype, tid, oldv, shared;

    if (__unlikely(mutex == NULL))
        return EINVAL;

    mtype  = (mutex->value & MUTEX_TYPE_MASK);
    shared = (mutex->value & MUTEX_SHARED_MASK);

    /* Handle common case first */
    if (__likely(mtype == MUTEX_TYPE_NORMAL)) {
        _normal_unlock(mutex);
        return 0;
    }

    /* Do we already own this recursive or error-check mutex ? */
    //    int  tid = __get_thread()->kernel_id;
	     tid = getpid();

    if ( tid != MUTEX_OWNER(mutex) )
        return EPERM;

    /* We do, decrement counter or release the mutex if it is 0 */
    _recursive_lock();
    oldv = mutex->value;
    if (oldv & MUTEX_COUNTER_MASK) {
        mutex->value = oldv - (1 << MUTEX_COUNTER_SHIFT);
        oldv = 0;
    } else {
        mutex->value = shared | mtype;
    }
    _recursive_unlock();

    /* Wake one waiting thread, if any */
    if ((oldv & 3) == 2) {
        int wake_op = shared ? FUTEX_WAKE : FUTEX_WAKE_PRIVATE;
        __futex_syscall3(&mutex->value, wake_op, 1);
    }
    return 0;
}


int mot_pthread_mutex_trylock(mot_pthread_mutex_t *mutex)
{
    int mtype, tid, oldv, shared;

    if (__unlikely(mutex == NULL))
        return EINVAL;

    mtype  = (mutex->value & MUTEX_TYPE_MASK);
    shared = (mutex->value & MUTEX_SHARED_MASK);

    /* Handle common case first */
    if ( __likely(mtype == MUTEX_TYPE_NORMAL) )
    {
        if (__atomic_cmpxchg(shared|0, shared|1, &mutex->value) == 0)
            return 0;

        return EBUSY;
    }

    /* Do we already own this recursive or error-check mutex ? */
    //    int  tid = __get_thread()->kernel_id;
	     tid = getpid();
    if ( tid == MUTEX_OWNER(mutex) )
    {
        int counter;

        if (mtype == MUTEX_TYPE_ERRORCHECK) {
            /* already locked by ourselves */
            return EDEADLK;
        }

        _recursive_lock();
        oldv = mutex->value;
        counter = (oldv + (1 << MUTEX_COUNTER_SHIFT)) & MUTEX_COUNTER_MASK;
        mutex->value = (oldv & ~MUTEX_COUNTER_MASK) | counter;
        _recursive_unlock();
        return 0;
    }

    /* Restore sharing bit in mtype */
    mtype |= shared;

    /* Try to lock it, just once. */
    _recursive_lock();
    oldv = mutex->value;
    if (oldv == mtype)  /* uncontended released lock => state 1 */
        mutex->value = ((tid << 16) | mtype | 1);
    _recursive_unlock();

    if (oldv != mtype)
        return EBUSY;

    return 0;
}


/* initialize 'ts' with the difference between 'abstime' and the current time
 * according to 'clock'. Returns -1 if abstime already expired, or 0 otherwise.
 */
static int
__timespec_to_absolute(struct timespec*  ts, const struct timespec*  abstime, clockid_t  clock)
{
    clock_gettime(clock, ts);
    ts->tv_sec  = abstime->tv_sec - ts->tv_sec;
    ts->tv_nsec = abstime->tv_nsec - ts->tv_nsec;
    if (ts->tv_nsec < 0) {
        ts->tv_sec--;
        ts->tv_nsec += 1000000000;
    }
    if ((ts->tv_nsec < 0) || (ts->tv_sec < 0))
        return -1;

    return 0;
}


/* We use one bit in condition variable values as the 'shared' flag
 * The rest is a counter.
 */
#define COND_SHARED_MASK        0x0001
#define COND_COUNTER_INCREMENT  0x0002
#define COND_COUNTER_MASK       (~COND_SHARED_MASK)

#define COND_IS_SHARED(c)  (((c)->value & COND_SHARED_MASK) != 0)

/* XXX *technically* there is a race condition that could allow
 * XXX a signal to be missed.  If thread A is preempted in _wait()
 * XXX after unlocking the mutex and before waiting, and if other
 * XXX threads call signal or broadcast UINT_MAX/2 times (exactly),
 * XXX before thread A is scheduled again and calls futex_wait(),
 * XXX then the signal will be lost.
 */


/* This function is used by pthread_cond_broadcast and
 * pthread_cond_signal to atomically decrement the counter
 * then wake-up 'counter' threads.
 */
static int
mot_pthread_cond_pulse(mot_pthread_cond_t *cond, int  counter)
{
    long flags;
    int  wake_op;

    if (__unlikely(cond == NULL))
        return EINVAL;

    flags = (cond->value & ~COND_COUNTER_MASK);
    for (;;) {
        long oldval = cond->value;
        long newval = ((oldval - COND_COUNTER_INCREMENT) & COND_COUNTER_MASK)
                      | flags;
        if (__atomic_cmpxchg(oldval, newval, &cond->value) == 0)
            break;
    }

    wake_op = COND_IS_SHARED(cond) ? FUTEX_WAKE : FUTEX_WAKE_PRIVATE;
    __futex_syscall3(&cond->value, wake_op, counter);
    return 0;
}

int mot_pthread_cond_broadcast(mot_pthread_cond_t *cond)
{
    return mot_pthread_cond_pulse(cond, INT_MAX);
}

int mot_pthread_cond_signal(mot_pthread_cond_t *cond)
{
    return mot_pthread_cond_pulse(cond, 1);
}

int mot_pthread_cond_wait(mot_pthread_cond_t *cond, mot_pthread_mutex_t *mutex)
{
    return mot_pthread_cond_timedwait(cond, mutex, NULL);
}

int __mot_pthread_cond_timedwait_relative(mot_pthread_cond_t *cond,
                                      mot_pthread_mutex_t * mutex,
                                      const struct timespec *reltime)
{
    int  status;
    int  oldvalue = cond->value;
    int  wait_op  = COND_IS_SHARED(cond) ? FUTEX_WAIT : FUTEX_WAIT_PRIVATE;

    mot_pthread_mutex_unlock(mutex);
    status = __futex_syscall4(&cond->value, wait_op, oldvalue, reltime);
    mot_pthread_mutex_lock(mutex);

    if (status == (-ETIMEDOUT)) return ETIMEDOUT;
    return 0;
}

int __mot_pthread_cond_timedwait(mot_pthread_cond_t *cond,
                             mot_pthread_mutex_t * mutex,
                             const struct timespec *abstime,
                             clockid_t clock)
{
    struct timespec ts;
    struct timespec * tsp;

    if (abstime != NULL) {
        if (__timespec_to_absolute(&ts, abstime, clock) < 0)
            return ETIMEDOUT;
        tsp = &ts;
    } else {
        tsp = NULL;
    }

    return __mot_pthread_cond_timedwait_relative(cond, mutex, tsp);
}

int mot_pthread_cond_timedwait(mot_pthread_cond_t *cond,
                           mot_pthread_mutex_t * mutex,
                           const struct timespec *abstime)
{
    return __mot_pthread_cond_timedwait(cond, mutex, abstime, CLOCK_REALTIME);
}

int mot_pthread_cond_timedwait_relative_np(mot_pthread_cond_t *cond,
                                      mot_pthread_mutex_t * mutex,
                                      const struct timespec *reltime)
{
    return __mot_pthread_cond_timedwait_relative(cond, mutex, reltime);
}

