/* 
 * Copyright (c) 1995
 *      The Regents of the University of California.  All rights reserved.
 * Copyright (C) 1997
 *      John S. Dyson.  All rights reserved.
 * Copyright (C) 2013-2017
 *      Matthew Dillon, All rights reserved.
 *
 * This code contains ideas from software contributed to Berkeley by
 * Avadis Tevanian, Jr., Michael Wayne Young, and the Mach Operating
 * System project at Carnegie-Mellon University.
 *
 * This code is derived from software contributed to The DragonFly Project
 * by Matthew Dillon <dillon@backplane.com>.  Extensively rewritten.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name of the University nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS 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 REGENTS 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 "opt_lint.h"

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/proc.h>
#include <sys/lock.h>
#include <sys/sysctl.h>
#include <sys/spinlock.h>
#include <sys/spinlock2.h>
#include <sys/indefinite2.h>

static void undo_shreq(struct lock *lkp);
static int undo_upreq(struct lock *lkp);
static int undo_exreq(struct lock *lkp);

#ifdef DEBUG_CANCEL_LOCKS

static int sysctl_cancel_lock(SYSCTL_HANDLER_ARGS);
static int sysctl_cancel_test(SYSCTL_HANDLER_ARGS);

static struct lock cancel_lk;
LOCK_SYSINIT(cancellk, &cancel_lk, "cancel", 0);
SYSCTL_PROC(_kern, OID_AUTO, cancel_lock, CTLTYPE_INT|CTLFLAG_RW, 0, 0,
            sysctl_cancel_lock, "I", "test cancelable locks");
SYSCTL_PROC(_kern, OID_AUTO, cancel_test, CTLTYPE_INT|CTLFLAG_RW, 0, 0,
            sysctl_cancel_test, "I", "test cancelable locks");

#endif

__read_frequently int lock_test_mode;
SYSCTL_INT(_debug, OID_AUTO, lock_test_mode, CTLFLAG_RW,
           &lock_test_mode, 0, "");

/*
 * Locking primitives implementation.
 * Locks provide shared/exclusive sychronization.
 */

#ifdef DEBUG_LOCKS
#define COUNT(td, x) (td)->td_locks += (x)
#else
#define COUNT(td, x) do { } while (0)
#endif

/*
 * Helper, assert basic conditions
 */
static __inline void
_lockmgr_assert(struct lock *lkp, u_int flags)
{
        if (mycpu->gd_intr_nesting_level &&
            (flags & LK_NOWAIT) == 0 &&
            (flags & LK_TYPE_MASK) != LK_RELEASE &&
            panic_cpu_gd != mycpu
        ) {
                panic("lockmgr %s from %p: called from interrupt, ipi, "
                      "or hard code section",
                      lkp->lk_wmesg, ((int **)&lkp)[-1]);
        }
}

/*
 * Acquire a shared lock
 */
int
lockmgr_shared(struct lock *lkp, u_int flags)
{
        uint32_t extflags;
        thread_t td;
        uint64_t count;
        int error;
        int pflags;
        int timo;
        int didloop;

        _lockmgr_assert(lkp, flags);
        extflags = (flags | lkp->lk_flags) & LK_EXTFLG_MASK;
        td = curthread;

        count = lkp->lk_count;
        cpu_ccfence();

        /*
         * If the caller already holds the lock exclusively then
         * we silently obtain another count on the exclusive lock.
         * Avoid accessing lk_lockholder until testing exclusivity.
         *
         * WARNING!  The old FreeBSD behavior was to downgrade,
         *           but this creates a problem when recursions
         *           return to the caller and the caller expects
         *           its original exclusive lock to remain exclusively
         *           locked.
         */
        if ((count & LKC_XMASK) && lkp->lk_lockholder == td) {
                KKASSERT(lkp->lk_count & LKC_XMASK);
                if ((extflags & LK_CANRECURSE) == 0) {
                        if (extflags & LK_NOWAIT)
                                return EBUSY;
                        panic("lockmgr: locking against myself");
                }
                atomic_add_64(&lkp->lk_count, 1);
                COUNT(td, 1);
                return 0;
        }

        /*
         * Unless TDF_DEADLKTREAT is set, we cannot add LKC_SCOUNT while
         * SHARED is set and either EXREQ or UPREQ are set.
         *
         * NOTE: In the race-to-0 case (see undo_shreq()), we could
         *       theoretically work the SMASK == 0 case here.
         */
        if ((td->td_flags & TDF_DEADLKTREAT) == 0) {
                while ((count & LKC_SHARED) &&
                       (count & (LKC_EXREQ | LKC_UPREQ))) {
                        /*
                         * Immediate failure conditions
                         */
                        if (extflags & LK_CANCELABLE) {
                                if (count & LKC_CANCEL)
                                        return ENOLCK;
                        }
                        if (extflags & LK_NOWAIT)
                                return EBUSY;

                        /*
                         * Interlocked tsleep
                         */
                        pflags = (extflags & LK_PCATCH) ? PCATCH : 0;
                        timo = (extflags & LK_TIMELOCK) ? lkp->lk_timo : 0;

                        tsleep_interlock(lkp, pflags);
                        count = atomic_fetchadd_long(&lkp->lk_count, 0);

                        if ((count & LKC_SHARED) &&
                            (count & (LKC_EXREQ | LKC_UPREQ))) {
                                error = tsleep(lkp, pflags | PINTERLOCKED,
                                               lkp->lk_wmesg, timo);
                                if (error)
                                        return error;
                                count = lkp->lk_count;
                                cpu_ccfence();
                                continue;
                        }
                        break;
                }
        }

        /*
         * Bump the SCOUNT field.  The shared lock is granted only once
         * the SHARED flag gets set.  If it is already set, we are done.
         *
         * (Racing an EXREQ or UPREQ operation is ok here, we already did
         * our duty above).
         */
        count = atomic_fetchadd_64(&lkp->lk_count, LKC_SCOUNT) + LKC_SCOUNT;
        error = 0;
        didloop = 0;

        for (;;) {
                /*
                 * We may be able to grant ourselves the bit trivially.
                 * We're done once the SHARED bit is granted.
                 */
                if ((count & (LKC_XMASK | LKC_EXREQ |
                              LKC_UPREQ | LKC_SHARED)) == 0) {
                        if (atomic_fcmpset_64(&lkp->lk_count,
                                              &count, count | LKC_SHARED)) {
                                /* count |= LKC_SHARED; NOT USED */
                                break;
                        }
                        continue;
                }
                if ((td->td_flags & TDF_DEADLKTREAT) &&
                    (count & (LKC_XMASK | LKC_SHARED)) == 0) {
                        if (atomic_fcmpset_64(&lkp->lk_count,
                                              &count, count | LKC_SHARED)) {
                                /* count |= LKC_SHARED; NOT USED */
                                break;
                        }
                        continue;
                }
                if (count & LKC_SHARED)
                        break;

                /*
                 * Slow path
                 */
                pflags = (extflags & LK_PCATCH) ? PCATCH : 0;
                timo = (extflags & LK_TIMELOCK) ? lkp->lk_timo : 0;

                if (extflags & LK_CANCELABLE) {
                        if (count & LKC_CANCEL) {
                                undo_shreq(lkp);
                                error = ENOLCK;
                                break;
                        }
                }
                if (extflags & LK_NOWAIT) {
                        undo_shreq(lkp);
                        error = EBUSY;
                        break;
                }

                /*
                 * Interlocked after the first loop.
                 */
                if (didloop) {
                        error = tsleep(lkp, pflags | PINTERLOCKED,
                                       lkp->lk_wmesg, timo);
                        if (extflags & LK_SLEEPFAIL) {
                                undo_shreq(lkp);
                                error = ENOLCK;
                                break;
                        }
                        if (error) {
                                undo_shreq(lkp);
                                break;
                        }
                }
                didloop = 1;

                /*
                 * Reload, shortcut grant case, then loop interlock
                 * and loop.
                 */
                count = lkp->lk_count;
                if (count & LKC_SHARED)
                        break;
                tsleep_interlock(lkp, pflags);
                count = atomic_fetchadd_64(&lkp->lk_count, 0);
        }
        if (error == 0)
                COUNT(td, 1);

        return error;
}

/*
 * Acquire an exclusive lock
 */
int
lockmgr_exclusive(struct lock *lkp, u_int flags)
{
        uint64_t count;
        uint64_t ncount;
        uint32_t extflags;
        thread_t td;
        int error;
        int pflags;
        int timo;

        _lockmgr_assert(lkp, flags);
        extflags = (flags | lkp->lk_flags) & LK_EXTFLG_MASK;
        td = curthread;

        error = 0;
        count = lkp->lk_count;
        cpu_ccfence();

        /*
         * Recursive lock if we already hold it exclusively.  Avoid testing
         * lk_lockholder until after testing lk_count.
         */
        if ((count & LKC_XMASK) && lkp->lk_lockholder == td) {
                if ((extflags & LK_CANRECURSE) == 0) {
                        if (extflags & LK_NOWAIT)
                                return EBUSY;
                        panic("lockmgr: locking against myself");
                }
                count = atomic_fetchadd_64(&lkp->lk_count, 1) + 1;
                KKASSERT((count & LKC_XMASK) > 1);
                COUNT(td, 1);
                return 0;
        }

        /*
         * Trivially acquire the lock, or block until we can set EXREQ.
         * Set EXREQ2 if EXREQ is already set or the lock is already
         * held exclusively.  EXREQ2 is an aggregation bit to request
         * a wakeup.
         *
         * WARNING! We cannot set EXREQ if the lock is already held
         *          exclusively because it may race another EXREQ
         *          being cleared and granted.  We use the exclusivity
         *          to prevent both EXREQ and UPREQ from being set.
         *
         *          This means that both shared and exclusive requests
         *          have equal priority against a current exclusive holder's
         *          release.  Exclusive requests still have priority over
         *          new shared requests when the lock is already held shared.
         */
        for (;;) {
                /*
                 * Normal trivial case
                 */
                if ((count & (LKC_UPREQ | LKC_EXREQ |
                              LKC_XMASK)) == 0 &&
                    ((count & LKC_SHARED) == 0 ||
                     (count & LKC_SMASK) == 0)) {
                        ncount = (count + 1) & ~LKC_SHARED;
                        if (atomic_fcmpset_64(&lkp->lk_count,
                                              &count, ncount)) {
                                lkp->lk_lockholder = td;
                                COUNT(td, 1);
                                return 0;
                        }
                        continue;
                }

                if (extflags & LK_CANCELABLE) {
                        if (count & LKC_CANCEL)
                                return ENOLCK;
                }
                if (extflags & LK_NOWAIT)
                        return EBUSY;

                /*
                 * Interlock to set EXREQ or EXREQ2
                 */
                pflags = (extflags & LK_PCATCH) ? PCATCH : 0;
                timo = (extflags & LK_TIMELOCK) ? lkp->lk_timo : 0;

                if (count & (LKC_EXREQ | LKC_XMASK))
                        ncount = count | LKC_EXREQ2;
                else
                        ncount = count | LKC_EXREQ;
                tsleep_interlock(lkp, pflags);
                if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                        /*
                         * If we successfully transitioned to EXREQ we
                         * can break out, otherwise we had set EXREQ2 and
                         * we block.
                         */
                        if ((count & (LKC_EXREQ | LKC_XMASK)) == 0) {
                                count = ncount;
                                break;
                        }

                        error = tsleep(lkp, pflags | PINTERLOCKED,
                                       lkp->lk_wmesg, timo);
                        count = lkp->lk_count;  /* relod */
                        cpu_ccfence();
                }
#ifdef INVARIANTS
                if (lock_test_mode > 0) {
                        --lock_test_mode;
                        print_backtrace(8);
                }
#endif
                if (error)
                        return error;
                if (extflags & LK_SLEEPFAIL)
                        return ENOLCK;
        }

        /*
         * Once EXREQ has been set, wait for it to be granted
         * We enter the loop with tsleep_interlock() already called.
         */
        for (;;) {
                /*
                 * Waiting for EXREQ to be granted to us.
                 *
                 * The granting thread will handle the count for us, but we
                 * still have to set lk_lockholder.
                 *
                 * NOTE! If we try to trivially get the exclusive lock
                 *       (basically by racing undo_shreq()) and succeed,
                 *       we must still wakeup(lkp) for another exclusive
                 *       lock trying to acquire EXREQ.  Easier to simply
                 *       wait for our own wakeup.
                 */
                if ((count & LKC_EXREQ) == 0) {
                        KKASSERT(count & LKC_XMASK);
                        lkp->lk_lockholder = td;
                        COUNT(td, 1);
                        break;
                }

                /*
                 * Block waiting for our exreq to be granted.
                 * Check cancelation.  NOWAIT was already dealt with.
                 */
                if (extflags & LK_CANCELABLE) {
                        if (count & LKC_CANCEL) {
                                if (undo_exreq(lkp) == 0) {
                                        lkp->lk_lockholder = LK_KERNTHREAD;
                                        lockmgr_release(lkp, 0);
                                }
                                error = ENOLCK;
                                break;
                        }
                }

                pflags = (extflags & LK_PCATCH) ? PCATCH : 0;
                timo = (extflags & LK_TIMELOCK) ? lkp->lk_timo : 0;

                error = tsleep(lkp, pflags | PINTERLOCKED, lkp->lk_wmesg, timo);
#ifdef INVARIANTS
                if (lock_test_mode > 0) {
                        --lock_test_mode;
                        print_backtrace(8);
                }
#endif
                /*
                 * A tsleep error is uncommon.  If it occurs we have to
                 * undo our EXREQ.  If we are granted the exclusive lock
                 * as we try to undo we have to deal with it.
                 */
                if (extflags & LK_SLEEPFAIL) {
                        if (undo_exreq(lkp) == 0) {
                                lkp->lk_lockholder = LK_KERNTHREAD;
                                lockmgr_release(lkp, 0);
                        }
                        if (error == 0)
                                error = ENOLCK;
                        break;
                }
                if (error) {
                        if (undo_exreq(lkp))
                                break;
                        lkp->lk_lockholder = td;
                        COUNT(td, 1);
                        error = 0;
                        break;
                }

                /*
                 * Reload after sleep, shortcut grant case.
                 * Then set the interlock and loop.
                 *
                 * The granting thread will handle the count for us, but we
                 * still have to set lk_lockholder.
                 */
                count = lkp->lk_count;
                cpu_ccfence();
                if ((count & LKC_EXREQ) == 0) {
                        KKASSERT(count & LKC_XMASK);
                        lkp->lk_lockholder = td;
                        COUNT(td, 1);
                        break;
                }
                tsleep_interlock(lkp, pflags);
                count = atomic_fetchadd_64(&lkp->lk_count, 0);
        }
        return error;
}

/*
 * Downgrade an exclusive lock to shared.
 *
 * This function always succeeds as long as the caller owns a legal
 * exclusive lock with one reference.  UPREQ and EXREQ is ignored.
 */
int
lockmgr_downgrade(struct lock *lkp, u_int flags)
{
        uint64_t count;
        uint64_t ncount;
        uint32_t extflags;
        thread_t otd;
        thread_t td;

        extflags = (flags | lkp->lk_flags) & LK_EXTFLG_MASK;
        td = curthread;
        count = lkp->lk_count;

        for (;;) {
                cpu_ccfence();

                /*
                 * Downgrade an exclusive lock into a shared lock.  All
                 * counts on a recursive exclusive lock become shared.
                 *
                 * NOTE: Currently to reduce confusion we only allow
                 *       there to be one exclusive lock count, and panic
                 *       if there are more.
                 */
                if (lkp->lk_lockholder != td || (count & LKC_XMASK) != 1) {
                        panic("lockmgr: not holding exclusive lock: "
                              "%p/%p %016jx", lkp->lk_lockholder, td, count);
                }

                /*
                 * NOTE! Must NULL-out lockholder before releasing the
                 *       exclusive lock.
                 *
                 * NOTE! There might be pending shared requests, check
                 *       and wake them up.
                 */
                otd = lkp->lk_lockholder;
                lkp->lk_lockholder = NULL;
                ncount = (count & ~(LKC_XMASK | LKC_EXREQ2)) +
                         ((count & LKC_XMASK) << LKC_SSHIFT);
                ncount |= LKC_SHARED;

                if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                        /*
                         * Wakeup any shared waiters (prior SMASK), or
                         * any exclusive requests that couldn't set EXREQ
                         * because the lock had been held exclusively.
                         */
                        if (count & (LKC_SMASK | LKC_EXREQ2))
                                wakeup(lkp);
                        /* count = ncount; NOT USED */
                        break;
                }
                lkp->lk_lockholder = otd;
                /* retry */
        }
        return 0;
}

/*
 * Upgrade a shared lock to exclusive.  If LK_EXCLUPGRADE then guarantee
 * that no other exclusive requester can get in front of us and fail
 * immediately if another upgrade is pending.  If we fail, the shared
 * lock is released.
 *
 * If LK_EXCLUPGRADE is not set and we cannot upgrade because someone
 * else is in front of us, we release the shared lock and acquire the
 * exclusive lock normally.  If a failure occurs, the shared lock is
 * released.
 *
 * The way this works is that if we cannot instantly upgrade the
 * shared lock due to various conditions, but we can acquire UPREQ,
 * we then set UPREQ and wait for the thread blocking us to grant
 * our upgrade.  The other thread grants our upgrade by incrementing
 * the excl count (to 1) and clearing UPREQ, but it doesn't know 'who'
 * requested the upgrade so it can't set lk_lockholder.  Our thread notices
 * that LK_UPREQ is now clear and finishes up by setting lk_lockholder.
 */
int
lockmgr_upgrade(struct lock *lkp, u_int flags)
{
        uint64_t count;
        uint64_t ncount;
        uint32_t extflags;
        thread_t td;
        int error;
        int pflags;
        int timo;

        _lockmgr_assert(lkp, flags);
        extflags = (flags | lkp->lk_flags) & LK_EXTFLG_MASK;
        td = curthread;
        error = 0;
        count = lkp->lk_count;
        cpu_ccfence();

        /*
         * If we already hold the lock exclusively this operation
         * succeeds and is a NOP.
         */
        if (count & LKC_XMASK) {
                if (lkp->lk_lockholder == td)
                        return 0;
                panic("lockmgr: upgrade unowned lock");
        }
        if ((count & LKC_SMASK) == 0)
                panic("lockmgr: upgrade unowned lock");

        /*
         * Loop to acquire LKC_UPREQ
         */
        for (;;) {
                /*
                 * If UPREQ is already pending, release the shared lock
                 * and acquire an exclusive lock normally.
                 *
                 * If NOWAIT or EXCLUPGRADE the operation must be atomic,
                 * and this isn't, so we fail.
                 */
                if (count & LKC_UPREQ) {
                        lockmgr_release(lkp, 0);
                        if ((flags & LK_TYPE_MASK) == LK_EXCLUPGRADE)
                                error = EBUSY;
                        else if (extflags & LK_NOWAIT)
                                error = EBUSY;
                        else
                                error = lockmgr_exclusive(lkp, flags);
                        return error;
                }

                /*
                 * Try to immediately grant the upgrade, handle NOWAIT,
                 * or release the shared lock and simultaneously set UPREQ.
                 */
                if ((count & LKC_SMASK) == LKC_SCOUNT) {
                        /*
                         * Immediate grant
                         */
                        ncount = (count - LKC_SCOUNT + 1) & ~LKC_SHARED;
                        if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                                lkp->lk_lockholder = td;
                                return 0;
                        }
                } else if (extflags & LK_NOWAIT) {
                        /*
                         * Early EBUSY if an immediate grant is impossible
                         */
                        lockmgr_release(lkp, 0);
                        return EBUSY;
                } else {
                        /*
                         * Multiple shared locks present, request the
                         * upgrade and break to the next loop.
                         */
                        pflags = (extflags & LK_PCATCH) ? PCATCH : 0;
                        tsleep_interlock(lkp, pflags);
                        ncount = (count - LKC_SCOUNT) | LKC_UPREQ;
                        if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                                count = ncount;
                                break;
                        }
                }
                /* retry */
        }

        /*
         * We have acquired LKC_UPREQ, wait until the upgrade is granted
         * or the tsleep fails.
         *
         * NOWAIT and EXCLUPGRADE have already been handled.  The first
         * tsleep_interlock() has already been associated.
         */
        for (;;) {
                cpu_ccfence();

                /*
                 * We were granted our upgrade.  No other UPREQ can be
                 * made pending because we are now exclusive.
                 *
                 * The granting thread will handle the count for us, but we
                 * still have to set lk_lockholder.
                 */
                if ((count & LKC_UPREQ) == 0) {
                        KKASSERT((count & LKC_XMASK) == 1);
                        lkp->lk_lockholder = td;
                        break;
                }

                if (extflags & LK_CANCELABLE) {
                        if (count & LKC_CANCEL) {
                                if (undo_upreq(lkp) == 0) {
                                        lkp->lk_lockholder = LK_KERNTHREAD;
                                        lockmgr_release(lkp, 0);
                                }
                                error = ENOLCK;
                                break;
                        }
                }

                pflags = (extflags & LK_PCATCH) ? PCATCH : 0;
                timo = (extflags & LK_TIMELOCK) ? lkp->lk_timo : 0;

                error = tsleep(lkp, pflags | PINTERLOCKED, lkp->lk_wmesg, timo);
                if (extflags & LK_SLEEPFAIL) {
                        if (undo_upreq(lkp) == 0) {
                                lkp->lk_lockholder = LK_KERNTHREAD;
                                lockmgr_release(lkp, 0);
                        }
                        if (error == 0)
                                error = ENOLCK;
                        break;
                }
                if (error) {
                        if (undo_upreq(lkp))
                                break;
                        error = 0;
                }

                /*
                 * Reload the lock, short-cut the UPGRANT code before
                 * taking the time to interlock and loop.
                 *
                 * The granting thread will handle the count for us, but we
                 * still have to set lk_lockholder.
                 */
                count = lkp->lk_count;
                if ((count & LKC_UPREQ) == 0) {
                        KKASSERT((count & LKC_XMASK) == 1);
                        lkp->lk_lockholder = td;
                        break;
                }
                tsleep_interlock(lkp, pflags);
                count = atomic_fetchadd_64(&lkp->lk_count, 0);
                /* retry */
        }
        return error;
}

/*
 * Release a held lock
 *
 * NOTE: When releasing to an unlocked state, we set the SHARED bit
 *       to optimize shared lock requests.
 */
int
lockmgr_release(struct lock *lkp, u_int flags)
{
        uint64_t count;
        uint64_t ncount;
        uint32_t extflags;
        thread_t otd;
        thread_t td;

        extflags = (flags | lkp->lk_flags) & LK_EXTFLG_MASK;
        td = curthread;

        count = lkp->lk_count;
        cpu_ccfence();

        for (;;) {
                /*
                 * Release the currently held lock, grant all requests
                 * possible.
                 *
                 * WARNING! lksleep() assumes that LK_RELEASE does not
                 *          block.
                 *
                 * Always succeeds.
                 * Never blocks.
                 */
                if ((count & (LKC_SMASK | LKC_XMASK)) == 0)
                        panic("lockmgr: LK_RELEASE: no lock held");

                if (count & LKC_XMASK) {
                        /*
                         * Release exclusively held lock
                         */
                        if (lkp->lk_lockholder != LK_KERNTHREAD &&
                            lkp->lk_lockholder != td) {
                                panic("lockmgr: pid %d, not exclusive "
                                      "lock holder thr %p/%p unlocking",
                                    (td->td_proc ? td->td_proc->p_pid : -1),
                                    td, lkp->lk_lockholder);
                        }
                        if ((count & (LKC_UPREQ | LKC_EXREQ |
                                      LKC_XMASK)) == 1) {
                                /*
                                 * Last exclusive count is being released
                                 * with no UPREQ or EXREQ.  The SHARED
                                 * bit can be set or not without messing
                                 * anything up, so precondition it to
                                 * SHARED (which is the most cpu-optimal).
                                 *
                                 * Wakeup any EXREQ2.  EXREQ cannot be
                                 * set while an exclusive count is present
                                 * so we have to wakeup any EXREQ2 we find.
                                 *
                                 * We could hint the EXREQ2 by leaving
                                 * SHARED unset, but atm I don't see any
                                 * usefulness.
                                 */
                                otd = lkp->lk_lockholder;
                                lkp->lk_lockholder = NULL;
                                ncount = (count - 1);
                                ncount &= ~(LKC_CANCEL | LKC_EXREQ2);
                                ncount |= LKC_SHARED;
                                if (atomic_fcmpset_64(&lkp->lk_count,
                                                      &count, ncount)) {
                                        if (count & (LKC_SMASK | LKC_EXREQ2))
                                                wakeup(lkp);
                                        if (otd != LK_KERNTHREAD)
                                                COUNT(td, -1);
                                        /* count = ncount; NOT USED */
                                        break;
                                }
                                lkp->lk_lockholder = otd;
                                /* retry */
                        } else if ((count & (LKC_UPREQ | LKC_XMASK)) ==
                                   (LKC_UPREQ | 1)) {
                                /*
                                 * Last exclusive count is being released but
                                 * an upgrade request is present, automatically
                                 * grant an exclusive state to the owner of
                                 * the upgrade request.  Transfer count to
                                 * grant.
                                 *
                                 * The owner of LK_UPREQ is still responsible
                                 * for setting lk_lockholder.
                                 *
                                 * EXREQ cannot be set while an exclusive
                                 * holder exists, so do not clear EXREQ2.
                                 */
                                otd = lkp->lk_lockholder;
                                lkp->lk_lockholder = NULL;
                                ncount = count & ~LKC_UPREQ;
                                if (atomic_fcmpset_64(&lkp->lk_count,
                                                      &count, ncount)) {
                                        wakeup(lkp);
                                        if (otd != LK_KERNTHREAD)
                                                COUNT(td, -1);
                                        /* count = ncount; NOT USED */
                                        break;
                                }
                                lkp->lk_lockholder = otd;
                                /* retry */
                        } else if ((count & (LKC_EXREQ | LKC_XMASK)) ==
                                   (LKC_EXREQ | 1)) {
                                /*
                                 * Last exclusive count is being released but
                                 * an exclusive request is present.  We
                                 * automatically grant an exclusive state to
                                 * the owner of the exclusive request,
                                 * transfering our count.
                                 *
                                 * This case virtually never occurs because
                                 * EXREQ is not set while exclusive holders
                                 * exist.  However, it might be set if a
                                 * an exclusive request is pending and a
                                 * shared holder upgrades.
                                 *
                                 * Don't bother clearing EXREQ2.  A thread
                                 * waiting to set EXREQ can't do it while
                                 * an exclusive lock is present.
                                 */
                                otd = lkp->lk_lockholder;
                                lkp->lk_lockholder = NULL;
                                ncount = count & ~LKC_EXREQ;
                                if (atomic_fcmpset_64(&lkp->lk_count,
                                                      &count, ncount)) {
                                        wakeup(lkp);
                                        if (otd != LK_KERNTHREAD)
                                                COUNT(td, -1);
                                        /* count = ncount; NOT USED */
                                        break;
                                }
                                lkp->lk_lockholder = otd;
                                /* retry */
                        } else {
                                /*
                                 * Multiple exclusive counts, drop by 1.
                                 * Since we are the holder and there is more
                                 * than one count, we can just decrement it.
                                 */
                                count =
                                    atomic_fetchadd_long(&lkp->lk_count, -1);
                                /* count = count - 1  NOT NEEDED */
                                if (lkp->lk_lockholder != LK_KERNTHREAD)
                                        COUNT(td, -1);
                                break;
                        }
                        /* retry */
                } else {
                        /*
                         * Release shared lock
                         */
                        KKASSERT((count & LKC_SHARED) && (count & LKC_SMASK));
                        if ((count & (LKC_EXREQ | LKC_UPREQ | LKC_SMASK)) ==
                            LKC_SCOUNT) {
                                /*
                                 * Last shared count is being released,
                                 * no exclusive or upgrade request present.
                                 * Generally leave the shared bit set.
                                 * Clear the CANCEL bit.
                                 */
                                ncount = (count - LKC_SCOUNT) & ~LKC_CANCEL;
                                if (atomic_fcmpset_64(&lkp->lk_count,
                                                      &count, ncount)) {
                                        COUNT(td, -1);
                                        /* count = ncount; NOT USED */
                                        break;
                                }
                                /* retry */
                        } else if ((count & (LKC_UPREQ | LKC_SMASK)) ==
                                   (LKC_UPREQ | LKC_SCOUNT)) {
                                /*
                                 * Last shared count is being released but
                                 * an upgrade request is present, automatically
                                 * grant an exclusive state to the owner of
                                 * the upgrade request and transfer the count.
                                 *
                                 * The owner of the upgrade request is still
                                 * responsible for setting lk_lockholder.
                                 */
                                ncount = (count - LKC_SCOUNT + 1) &
                                         ~(LKC_UPREQ | LKC_CANCEL | LKC_SHARED);
                                if (atomic_fcmpset_64(&lkp->lk_count,
                                                      &count, ncount)) {
                                        wakeup(lkp);
                                        COUNT(td, -1);
                                        /* count = ncount; NOT USED */
                                        break;
                                }
                                /* retry */
                        } else if ((count & (LKC_EXREQ | LKC_SMASK)) ==
                                   (LKC_EXREQ | LKC_SCOUNT)) {
                                /*
                                 * Last shared count is being released but
                                 * an exclusive request is present, we
                                 * automatically grant an exclusive state to
                                 * the owner of the request and transfer
                                 * the count.
                                 */
                                ncount = (count - LKC_SCOUNT + 1) &
                                         ~(LKC_EXREQ | LKC_EXREQ2 |
                                           LKC_CANCEL | LKC_SHARED);
                                if (atomic_fcmpset_64(&lkp->lk_count,
                                                      &count, ncount)) {
                                        wakeup(lkp);
                                        COUNT(td, -1);
                                        /* count = ncount; NOT USED */
                                        break;
                                }
                                /* retry */
                        } else {
                                /*
                                 * Shared count is greater than 1.  We can
                                 * just use undo_shreq() to clean things up.
                                 * undo_shreq() will also handle races to 0
                                 * after the fact.
                                 */
                                undo_shreq(lkp);
                                COUNT(td, -1);
                                break;
                        }
                        /* retry */
                }
                /* retry */
        }
        return 0;
}

/*
 * Start canceling blocked or future requesters.  Only blocked/future
 * requesters who pass the CANCELABLE flag can be canceled.
 *
 * This is intended to then allow other requesters (usually the
 * caller) to obtain a non-cancelable lock.
 *
 * Don't waste time issuing a wakeup if nobody is pending.
 */
int
lockmgr_cancel_beg(struct lock *lkp, u_int flags)
{
        uint64_t count;

        count = lkp->lk_count;
        for (;;) {
                cpu_ccfence();

                KKASSERT((count & LKC_CANCEL) == 0);    /* disallowed case */

                /* issue w/lock held */
                KKASSERT((count & (LKC_XMASK | LKC_SMASK)) != 0);

                if (!atomic_fcmpset_64(&lkp->lk_count,
                                       &count, count | LKC_CANCEL)) {
                        continue;
                }
                /* count |= LKC_CANCEL; NOT USED */

                /*
                 * Wakeup any waiters.
                 *
                 * NOTE: EXREQ2 must be checked in addition to standard
                 *       wait sources, it is possible for EXREQ2 to be
                 *       set when EXREQ is clear.
                 */
                if (count & (LKC_EXREQ | LKC_EXREQ2 | LKC_SMASK | LKC_UPREQ)) {
                        wakeup(lkp);
                }
                break;
        }
        return 0;
}

/*
 * End our cancel request (typically after we have acquired
 * the lock ourselves).
 */
int
lockmgr_cancel_end(struct lock *lkp, u_int flags)
{
        atomic_clear_long(&lkp->lk_count, LKC_CANCEL);

        return 0;
}

/*
 * Backout SCOUNT from a failed shared lock attempt and handle any race
 * to 0.  This function is also used by the release code for the less
 * optimal race to 0 case.
 *
 * WARNING! Since we are unconditionally decrementing LKC_SCOUNT, it is
 *          possible for the lock to get into a LKC_SHARED + ZERO SCOUNT
 *          situation.  A shared request can block with a ZERO SCOUNT if
 *          EXREQ or UPREQ is pending in this situation.  Be sure to always
 *          issue a wakeup() in this situation if we are unable to
 *          transition to an exclusive lock, to handle the race.
 *
 * Always succeeds
 * Must not block
 */
static void
undo_shreq(struct lock *lkp)
{
        uint64_t count;
        uint64_t ncount;

        count = atomic_fetchadd_64(&lkp->lk_count, -LKC_SCOUNT) - LKC_SCOUNT;
        while ((count & (LKC_EXREQ | LKC_UPREQ | LKC_CANCEL)) &&
               (count & (LKC_SMASK | LKC_XMASK)) == 0) {
                /*
                 * Grant any UPREQ here.  This is handled in two parts.
                 * We grant the UPREQ by incrementing the excl count and
                 * clearing UPREQ and SHARED (and also CANCEL).
                 *
                 * The owner of UPREQ is still responsible for setting
                 * lockholder.
                 *
                 * Note that UPREQ must have priority over EXREQ, and EXREQ
                 * over CANCEL, so if the atomic op fails we have to loop up.
                 */
                if (count & LKC_UPREQ) {
                        ncount = (count + 1) & ~(LKC_UPREQ | LKC_CANCEL |
                                                 LKC_SHARED);
                        if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                                wakeup(lkp);
                                /* count = ncount; NOT USED */
                                break;
                        }
                        wakeup(lkp);    /* XXX probably not needed */
                        continue;
                }
                if (count & LKC_EXREQ) {
                        ncount = (count + 1) & ~(LKC_EXREQ | LKC_EXREQ2 |
                                                 LKC_CANCEL | LKC_SHARED);
                        if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                                wakeup(lkp);
                                /* count = ncount; NOT USED */
                                break;
                        }
                        wakeup(lkp);    /* XXX probably not needed */
                        continue;
                }
                if (count & LKC_CANCEL) {
                        ncount = count & ~LKC_CANCEL;
                        if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                                wakeup(lkp);
                                /* count = ncount; NOT USED */
                                break;
                        }
                }
                /* retry */
        }
}

/*
 * Undo an exclusive request.  Returns EBUSY if we were able to undo the
 * request, and 0 if the request was granted before we could undo it.
 * When 0 is returned, the lock state has not been modified.  The caller
 * is responsible for setting the lockholder to curthread.
 */
static
int
undo_exreq(struct lock *lkp)
{
        uint64_t count;
        uint64_t ncount;
        int error;

        count = lkp->lk_count;
        error = 0;

        for (;;) {
                cpu_ccfence();

                if ((count & LKC_EXREQ) == 0) {
                        /*
                         * EXREQ was granted.  We own the exclusive lock.
                         */
                        break;
                }
                if (count & LKC_XMASK) {
                        /*
                         * Clear the EXREQ we still own.  Only wakeup on
                         * EXREQ2 if no UPREQ.  There are still exclusive
                         * holders so do not wake up any shared locks or
                         * any UPREQ.
                         *
                         * If there is an UPREQ it will issue a wakeup()
                         * for any EXREQ wait looops, so we can clear EXREQ2
                         * now.
                         */
                        ncount = count & ~(LKC_EXREQ | LKC_EXREQ2);
                        if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                                if ((count & (LKC_EXREQ2 | LKC_UPREQ)) ==
                                    LKC_EXREQ2) {
                                        wakeup(lkp);
                                }
                                error = EBUSY;
                                /* count = ncount; NOT USED */
                                break;
                        }
                        /* retry */
                } else if (count & LKC_UPREQ) {
                        /*
                         * Clear the EXREQ we still own.  We cannot wakeup any
                         * shared or exclusive waiters because there is an
                         * uprequest pending (that we do not handle here).
                         *
                         * If there is an UPREQ it will issue a wakeup()
                         * for any EXREQ wait looops, so we can clear EXREQ2
                         * now.
                         */
                        ncount = count & ~(LKC_EXREQ | LKC_EXREQ2);
                        if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                                error = EBUSY;
                                break;
                        }
                        /* retry */
                } else if ((count & LKC_SHARED) && (count & LKC_SMASK)) {
                        /*
                         * No UPREQ, lock not held exclusively, but the lock
                         * is held shared.  Clear EXREQ, wakeup anyone trying
                         * to get the EXREQ bit (they have to set it
                         * themselves, EXREQ2 is an aggregation).
                         *
                         * We must also wakeup any shared locks blocked
                         * by the EXREQ, so just issue the wakeup
                         * unconditionally.  See lockmgr_shared() + 76 lines
                         * or so.
                         */
                        ncount = count & ~(LKC_EXREQ | LKC_EXREQ2);
                        if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                                wakeup(lkp);
                                error = EBUSY;
                                /* count = ncount; NOT USED */
                                break;
                        }
                        /* retry */
                } else {
                        /*
                         * No UPREQ, lock not held exclusively or shared.
                         * Grant the EXREQ and wakeup anyone waiting on
                         * EXREQ2.
                         *
                         * We must also issue a wakeup if SHARED is set,
                         * even without an SCOUNT, due to pre-shared blocking
                         * that can occur on EXREQ in lockmgr_shared().
                         */
                        ncount = (count + 1) & ~(LKC_EXREQ | LKC_EXREQ2);
                        if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                                if (count & (LKC_EXREQ2 | LKC_SHARED))
                                        wakeup(lkp);
                                /* count = ncount; NOT USED */
                                /* we are granting, error == 0 */
                                break;
                        }
                        /* retry */
                }
                /* retry */
        }
        return error;
}

/*
 * Undo an upgrade request.  Returns EBUSY if we were able to undo the
 * request, and 0 if the request was granted before we could undo it.
 * When 0 is returned, the lock state has not been modified.  The caller
 * is responsible for setting the lockholder to curthread.
 */
static
int
undo_upreq(struct lock *lkp)
{
        uint64_t count;
        uint64_t ncount;
        int error;

        count = lkp->lk_count;
        error = 0;

        for (;;) {
                cpu_ccfence();

                if ((count & LKC_UPREQ) == 0) {
                        /*
                         * UPREQ was granted
                         */
                        break;
                }
                if (count & LKC_XMASK) {
                        /*
                         * Clear the UPREQ we still own.  Nobody to wakeup
                         * here because there is an existing exclusive
                         * holder.
                         */
                        if (atomic_fcmpset_64(&lkp->lk_count, &count,
                                              count & ~LKC_UPREQ)) {
                                error = EBUSY;
                                /* count &= ~LKC_UPREQ; NOT USED */
                                break;
                        }
                } else if (count & LKC_EXREQ) {
                        /*
                         * Clear the UPREQ we still own.  Grant the exclusive
                         * request and wake it up.
                         */
                        ncount = (count + 1);
                        ncount &= ~(LKC_EXREQ | LKC_EXREQ2 | LKC_UPREQ);

                        if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                                wakeup(lkp);
                                error = EBUSY;
                                /* count = ncount; NOT USED */
                                break;
                        }
                } else {
                        /*
                         * Clear the UPREQ we still own.  Wakeup any shared
                         * waiters.
                         *
                         * We must also issue a wakeup if SHARED was set
                         * even if no shared waiters due to pre-shared blocking
                         * that can occur on UPREQ.
                         */
                        ncount = count & ~LKC_UPREQ;
                        if (count & LKC_SMASK)
                                ncount |= LKC_SHARED;

                        if (atomic_fcmpset_64(&lkp->lk_count, &count, ncount)) {
                                if ((count & LKC_SHARED) ||
                                    (ncount & LKC_SHARED)) {
                                        wakeup(lkp);
                                }
                                error = EBUSY;
                                /* count = ncount; NOT USED */
                                break;
                        }
                }
                /* retry */
        }
        return error;
}

void
lockmgr_kernproc(struct lock *lp)
{
        struct thread *td __debugvar = curthread;

        if (lp->lk_lockholder != LK_KERNTHREAD) {
                KASSERT(lp->lk_lockholder == td,
                    ("lockmgr_kernproc: lock not owned by curthread %p: %p",
                    td, lp->lk_lockholder));
                lp->lk_lockholder = LK_KERNTHREAD;
                COUNT(td, -1);
        }
}

/*
 * Initialize a lock; required before use.
 */
void
lockinit(struct lock *lkp, const char *wmesg, int timo, int flags)
{
        lkp->lk_flags = (flags & LK_EXTFLG_MASK);
        lkp->lk_count = 0;
        lkp->lk_wmesg = wmesg;
        lkp->lk_timo = timo;
        lkp->lk_lockholder = NULL;
}

/*
 * Reinitialize a lock that is being reused for a different purpose, but
 * which may have pending (blocked) threads sitting on it.  The caller
 * must already hold the interlock.
 */
void
lockreinit(struct lock *lkp, const char *wmesg, int timo, int flags)
{
        lkp->lk_wmesg = wmesg;
        lkp->lk_timo = timo;
}

/*
 * De-initialize a lock.  The structure must no longer be used by anyone.
 */
void
lockuninit(struct lock *lkp)
{
        uint64_t count __unused;

        count = lkp->lk_count;
        cpu_ccfence();
        KKASSERT((count & (LKC_EXREQ | LKC_UPREQ)) == 0 &&
                 ((count & LKC_SHARED) || (count & LKC_SMASK) == 0));
}

/*
 * Determine the status of a lock.
 */
int
lockstatus(struct lock *lkp, struct thread *td)
{
        int lock_type = 0;
        uint64_t count;

        count = lkp->lk_count;
        cpu_ccfence();

        if (count & (LKC_XMASK | LKC_SMASK | LKC_EXREQ | LKC_UPREQ)) {
                if (count & LKC_XMASK) {
                        if (td == NULL || lkp->lk_lockholder == td)
                                lock_type = LK_EXCLUSIVE;
                        else
                                lock_type = LK_EXCLOTHER;
                } else if ((count & LKC_SMASK) && (count & LKC_SHARED)) {
                        lock_type = LK_SHARED;
                }
        }
        return (lock_type);
}

/*
 * Return non-zero if the caller owns the lock shared or exclusive.
 * We can only guess re: shared locks.
 */
int
lockowned(struct lock *lkp)
{
        thread_t td = curthread;
        uint64_t count;

        count = lkp->lk_count;
        cpu_ccfence();

        if (count & LKC_XMASK)
                return(lkp->lk_lockholder == td);
        else
                return((count & LKC_SMASK) != 0);
}

#if 0
/*
 * Determine the number of holders of a lock.
 *
 * REMOVED - Cannot be used due to our use of atomic_fetchadd_64()
 *           for shared locks.  Caller can only test if the lock has
 *           a count or not using lockinuse(lk) (sys/lock.h)
 */
int
lockcount(struct lock *lkp)
{
        panic("lockcount cannot be used");
}

int
lockcountnb(struct lock *lkp)
{
        panic("lockcount cannot be used");
}
#endif

/*
 * Print out information about state of a lock. Used by VOP_PRINT
 * routines to display status about contained locks.
 */
void
lockmgr_printinfo(struct lock *lkp)
{
        struct thread *td = lkp->lk_lockholder;
        struct proc *p;
        uint64_t count;

        count = lkp->lk_count;
        cpu_ccfence();

        if (td && td != LK_KERNTHREAD)
                p = td->td_proc;
        else
                p = NULL;

        if (count & LKC_XMASK) {
                kprintf(" lock type %s: EXCLUS (count %016jx) by td %p pid %d",
                    lkp->lk_wmesg, (intmax_t)count, td,
                    p ? p->p_pid : -99);
        } else if ((count & LKC_SMASK) && (count & LKC_SHARED)) {
                kprintf(" lock type %s: SHARED (count %016jx)",
                    lkp->lk_wmesg, (intmax_t)count);
        } else {
                kprintf(" lock type %s: NOTHELD", lkp->lk_wmesg);
        }
        if ((count & (LKC_EXREQ | LKC_UPREQ)) ||
            ((count & LKC_XMASK) && (count & LKC_SMASK)))
                kprintf(" with waiters\n");
        else
                kprintf("\n");
}

void
lock_sysinit(struct lock_args *arg)
{
        lockinit(arg->la_lock, arg->la_desc, 0, arg->la_flags);
}

#ifdef DEBUG_CANCEL_LOCKS

static
int
sysctl_cancel_lock(SYSCTL_HANDLER_ARGS)
{
        int error;

        if (req->newptr) {
                SYSCTL_XUNLOCK();
                lockmgr(&cancel_lk, LK_EXCLUSIVE);
                error = tsleep(&error, PCATCH, "canmas", hz * 5);
                lockmgr(&cancel_lk, LK_CANCEL_BEG);
                error = tsleep(&error, PCATCH, "canmas", hz * 5);
                lockmgr(&cancel_lk, LK_RELEASE);
                SYSCTL_XLOCK();
                SYSCTL_OUT(req, &error, sizeof(error));
        }
        error = 0;

        return error;
}

static
int
sysctl_cancel_test(SYSCTL_HANDLER_ARGS)
{
        int error;

        if (req->newptr) {
                error = lockmgr(&cancel_lk, LK_EXCLUSIVE|LK_CANCELABLE);
                if (error == 0)
                        lockmgr(&cancel_lk, LK_RELEASE);
                SYSCTL_OUT(req, &error, sizeof(error));
                kprintf("test %d\n", error);
        }

        return 0;
}

#endif