The Design and Implementation of the FreeBSD Operating System, Second Edition
Now available: The Design and Implementation of the FreeBSD Operating System (Second Edition)


[ source navigation ] [ diff markup ] [ identifier search ] [ freetext search ] [ file search ] [ list types ] [ track identifier ]

FreeBSD/Linux Kernel Cross Reference
sys/kern/kern_umtx.c

Version: -  FREEBSD  -  FREEBSD-13-STABLE  -  FREEBSD-13-0  -  FREEBSD-12-STABLE  -  FREEBSD-12-0  -  FREEBSD-11-STABLE  -  FREEBSD-11-0  -  FREEBSD-10-STABLE  -  FREEBSD-10-0  -  FREEBSD-9-STABLE  -  FREEBSD-9-0  -  FREEBSD-8-STABLE  -  FREEBSD-8-0  -  FREEBSD-7-STABLE  -  FREEBSD-7-0  -  FREEBSD-6-STABLE  -  FREEBSD-6-0  -  FREEBSD-5-STABLE  -  FREEBSD-5-0  -  FREEBSD-4-STABLE  -  FREEBSD-3-STABLE  -  FREEBSD22  -  l41  -  OPENBSD  -  linux-2.6  -  MK84  -  PLAN9  -  xnu-8792 
SearchContext: -  none  -  3  -  10 

    1 /*-
    2  * Copyright (c) 2004, David Xu <davidxu@freebsd.org>
    3  * Copyright (c) 2002, Jeffrey Roberson <jeff@freebsd.org>
    4  * All rights reserved.
    5  *
    6  * Redistribution and use in source and binary forms, with or without
    7  * modification, are permitted provided that the following conditions
    8  * are met:
    9  * 1. Redistributions of source code must retain the above copyright
   10  *    notice unmodified, this list of conditions, and the following
   11  *    disclaimer.
   12  * 2. Redistributions in binary form must reproduce the above copyright
   13  *    notice, this list of conditions and the following disclaimer in the
   14  *    documentation and/or other materials provided with the distribution.
   15  *
   16  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
   17  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
   18  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
   19  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
   20  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
   21  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
   22  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
   23  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
   24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
   25  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   26  */
   27 
   28 #include <sys/cdefs.h>
   29 __FBSDID("$FreeBSD: releng/10.4/sys/kern/kern_umtx.c 305185 2016-09-01 07:21:42Z kib $");
   30 
   31 #include "opt_compat.h"
   32 #include "opt_umtx_profiling.h"
   33 
   34 #include <sys/param.h>
   35 #include <sys/kernel.h>
   36 #include <sys/limits.h>
   37 #include <sys/lock.h>
   38 #include <sys/malloc.h>
   39 #include <sys/mutex.h>
   40 #include <sys/priv.h>
   41 #include <sys/proc.h>
   42 #include <sys/sbuf.h>
   43 #include <sys/sched.h>
   44 #include <sys/smp.h>
   45 #include <sys/sysctl.h>
   46 #include <sys/sysent.h>
   47 #include <sys/systm.h>
   48 #include <sys/sysproto.h>
   49 #include <sys/syscallsubr.h>
   50 #include <sys/eventhandler.h>
   51 #include <sys/umtx.h>
   52 
   53 #include <vm/vm.h>
   54 #include <vm/vm_param.h>
   55 #include <vm/pmap.h>
   56 #include <vm/vm_map.h>
   57 #include <vm/vm_object.h>
   58 
   59 #include <machine/cpu.h>
   60 
   61 #ifdef COMPAT_FREEBSD32
   62 #include <compat/freebsd32/freebsd32_proto.h>
   63 #endif
   64 
   65 #define _UMUTEX_TRY             1
   66 #define _UMUTEX_WAIT            2
   67 
   68 #ifdef UMTX_PROFILING
   69 #define UPROF_PERC_BIGGER(w, f, sw, sf)                                 \
   70         (((w) > (sw)) || ((w) == (sw) && (f) > (sf)))
   71 #endif
   72 
   73 /* Priority inheritance mutex info. */
   74 struct umtx_pi {
   75         /* Owner thread */
   76         struct thread           *pi_owner;
   77 
   78         /* Reference count */
   79         int                     pi_refcount;
   80 
   81         /* List entry to link umtx holding by thread */
   82         TAILQ_ENTRY(umtx_pi)    pi_link;
   83 
   84         /* List entry in hash */
   85         TAILQ_ENTRY(umtx_pi)    pi_hashlink;
   86 
   87         /* List for waiters */
   88         TAILQ_HEAD(,umtx_q)     pi_blocked;
   89 
   90         /* Identify a userland lock object */
   91         struct umtx_key         pi_key;
   92 };
   93 
   94 /* A userland synchronous object user. */
   95 struct umtx_q {
   96         /* Linked list for the hash. */
   97         TAILQ_ENTRY(umtx_q)     uq_link;
   98 
   99         /* Umtx key. */
  100         struct umtx_key         uq_key;
  101 
  102         /* Umtx flags. */
  103         int                     uq_flags;
  104 #define UQF_UMTXQ       0x0001
  105 
  106         /* The thread waits on. */
  107         struct thread           *uq_thread;
  108 
  109         /*
  110          * Blocked on PI mutex. read can use chain lock
  111          * or umtx_lock, write must have both chain lock and
  112          * umtx_lock being hold.
  113          */
  114         struct umtx_pi          *uq_pi_blocked;
  115 
  116         /* On blocked list */
  117         TAILQ_ENTRY(umtx_q)     uq_lockq;
  118 
  119         /* Thread contending with us */
  120         TAILQ_HEAD(,umtx_pi)    uq_pi_contested;
  121 
  122         /* Inherited priority from PP mutex */
  123         u_char                  uq_inherited_pri;
  124         
  125         /* Spare queue ready to be reused */
  126         struct umtxq_queue      *uq_spare_queue;
  127 
  128         /* The queue we on */
  129         struct umtxq_queue      *uq_cur_queue;
  130 };
  131 
  132 TAILQ_HEAD(umtxq_head, umtx_q);
  133 
  134 /* Per-key wait-queue */
  135 struct umtxq_queue {
  136         struct umtxq_head       head;
  137         struct umtx_key         key;
  138         LIST_ENTRY(umtxq_queue) link;
  139         int                     length;
  140 };
  141 
  142 LIST_HEAD(umtxq_list, umtxq_queue);
  143 
  144 /* Userland lock object's wait-queue chain */
  145 struct umtxq_chain {
  146         /* Lock for this chain. */
  147         struct mtx              uc_lock;
  148 
  149         /* List of sleep queues. */
  150         struct umtxq_list       uc_queue[2];
  151 #define UMTX_SHARED_QUEUE       0
  152 #define UMTX_EXCLUSIVE_QUEUE    1
  153 
  154         LIST_HEAD(, umtxq_queue) uc_spare_queue;
  155 
  156         /* Busy flag */
  157         char                    uc_busy;
  158 
  159         /* Chain lock waiters */
  160         int                     uc_waiters;
  161 
  162         /* All PI in the list */
  163         TAILQ_HEAD(,umtx_pi)    uc_pi_list;
  164 
  165 #ifdef UMTX_PROFILING
  166         u_int                   length;
  167         u_int                   max_length;
  168 #endif
  169 };
  170 
  171 #define UMTXQ_LOCKED_ASSERT(uc)         mtx_assert(&(uc)->uc_lock, MA_OWNED)
  172 
  173 /*
  174  * Don't propagate time-sharing priority, there is a security reason,
  175  * a user can simply introduce PI-mutex, let thread A lock the mutex,
  176  * and let another thread B block on the mutex, because B is
  177  * sleeping, its priority will be boosted, this causes A's priority to
  178  * be boosted via priority propagating too and will never be lowered even
  179  * if it is using 100%CPU, this is unfair to other processes.
  180  */
  181 
  182 #define UPRI(td)        (((td)->td_user_pri >= PRI_MIN_TIMESHARE &&\
  183                           (td)->td_user_pri <= PRI_MAX_TIMESHARE) ?\
  184                          PRI_MAX_TIMESHARE : (td)->td_user_pri)
  185 
  186 #define GOLDEN_RATIO_PRIME      2654404609U
  187 #define UMTX_CHAINS             512
  188 #define UMTX_SHIFTS             (__WORD_BIT - 9)
  189 
  190 #define GET_SHARE(flags)        \
  191     (((flags) & USYNC_PROCESS_SHARED) == 0 ? THREAD_SHARE : PROCESS_SHARE)
  192 
  193 #define BUSY_SPINS              200
  194 
  195 struct abs_timeout {
  196         int clockid;
  197         struct timespec cur;
  198         struct timespec end;
  199 };
  200 
  201 static uma_zone_t               umtx_pi_zone;
  202 static struct umtxq_chain       umtxq_chains[2][UMTX_CHAINS];
  203 static MALLOC_DEFINE(M_UMTX, "umtx", "UMTX queue memory");
  204 static int                      umtx_pi_allocated;
  205 
  206 static SYSCTL_NODE(_debug, OID_AUTO, umtx, CTLFLAG_RW, 0, "umtx debug");
  207 SYSCTL_INT(_debug_umtx, OID_AUTO, umtx_pi_allocated, CTLFLAG_RD,
  208     &umtx_pi_allocated, 0, "Allocated umtx_pi");
  209 
  210 #ifdef UMTX_PROFILING
  211 static long max_length;
  212 SYSCTL_LONG(_debug_umtx, OID_AUTO, max_length, CTLFLAG_RD, &max_length, 0, "max_length");
  213 static SYSCTL_NODE(_debug_umtx, OID_AUTO, chains, CTLFLAG_RD, 0, "umtx chain stats");
  214 #endif
  215 
  216 static void umtxq_sysinit(void *);
  217 static void umtxq_hash(struct umtx_key *key);
  218 static struct umtxq_chain *umtxq_getchain(struct umtx_key *key);
  219 static void umtxq_lock(struct umtx_key *key);
  220 static void umtxq_unlock(struct umtx_key *key);
  221 static void umtxq_busy(struct umtx_key *key);
  222 static void umtxq_unbusy(struct umtx_key *key);
  223 static void umtxq_insert_queue(struct umtx_q *uq, int q);
  224 static void umtxq_remove_queue(struct umtx_q *uq, int q);
  225 static int umtxq_sleep(struct umtx_q *uq, const char *wmesg, struct abs_timeout *);
  226 static int umtxq_count(struct umtx_key *key);
  227 static struct umtx_pi *umtx_pi_alloc(int);
  228 static void umtx_pi_free(struct umtx_pi *pi);
  229 static int do_unlock_pp(struct thread *td, struct umutex *m, uint32_t flags);
  230 static void umtx_thread_cleanup(struct thread *td);
  231 static void umtx_exec_hook(void *arg __unused, struct proc *p __unused,
  232         struct image_params *imgp __unused);
  233 SYSINIT(umtx, SI_SUB_EVENTHANDLER+1, SI_ORDER_MIDDLE, umtxq_sysinit, NULL);
  234 
  235 #define umtxq_signal(key, nwake)        umtxq_signal_queue((key), (nwake), UMTX_SHARED_QUEUE)
  236 #define umtxq_insert(uq)        umtxq_insert_queue((uq), UMTX_SHARED_QUEUE)
  237 #define umtxq_remove(uq)        umtxq_remove_queue((uq), UMTX_SHARED_QUEUE)
  238 
  239 static struct mtx umtx_lock;
  240 
  241 #ifdef UMTX_PROFILING
  242 static void
  243 umtx_init_profiling(void) 
  244 {
  245         struct sysctl_oid *chain_oid;
  246         char chain_name[10];
  247         int i;
  248 
  249         for (i = 0; i < UMTX_CHAINS; ++i) {
  250                 snprintf(chain_name, sizeof(chain_name), "%d", i);
  251                 chain_oid = SYSCTL_ADD_NODE(NULL, 
  252                     SYSCTL_STATIC_CHILDREN(_debug_umtx_chains), OID_AUTO, 
  253                     chain_name, CTLFLAG_RD, NULL, "umtx hash stats");
  254                 SYSCTL_ADD_INT(NULL, SYSCTL_CHILDREN(chain_oid), OID_AUTO,
  255                     "max_length0", CTLFLAG_RD, &umtxq_chains[0][i].max_length, 0, NULL);
  256                 SYSCTL_ADD_INT(NULL, SYSCTL_CHILDREN(chain_oid), OID_AUTO,
  257                     "max_length1", CTLFLAG_RD, &umtxq_chains[1][i].max_length, 0, NULL);
  258         }
  259 }
  260 
  261 static int
  262 sysctl_debug_umtx_chains_peaks(SYSCTL_HANDLER_ARGS)
  263 {
  264         char buf[512];
  265         struct sbuf sb;
  266         struct umtxq_chain *uc;
  267         u_int fract, i, j, tot, whole;
  268         u_int sf0, sf1, sf2, sf3, sf4;
  269         u_int si0, si1, si2, si3, si4;
  270         u_int sw0, sw1, sw2, sw3, sw4;
  271 
  272         sbuf_new(&sb, buf, sizeof(buf), SBUF_FIXEDLEN);
  273         for (i = 0; i < 2; i++) {
  274                 tot = 0;
  275                 for (j = 0; j < UMTX_CHAINS; ++j) {
  276                         uc = &umtxq_chains[i][j];
  277                         mtx_lock(&uc->uc_lock);
  278                         tot += uc->max_length;
  279                         mtx_unlock(&uc->uc_lock);
  280                 }
  281                 if (tot == 0)
  282                         sbuf_printf(&sb, "%u) Empty ", i);
  283                 else {
  284                         sf0 = sf1 = sf2 = sf3 = sf4 = 0;
  285                         si0 = si1 = si2 = si3 = si4 = 0;
  286                         sw0 = sw1 = sw2 = sw3 = sw4 = 0;
  287                         for (j = 0; j < UMTX_CHAINS; j++) {
  288                                 uc = &umtxq_chains[i][j];
  289                                 mtx_lock(&uc->uc_lock);
  290                                 whole = uc->max_length * 100;
  291                                 mtx_unlock(&uc->uc_lock);
  292                                 fract = (whole % tot) * 100;
  293                                 if (UPROF_PERC_BIGGER(whole, fract, sw0, sf0)) {
  294                                         sf0 = fract;
  295                                         si0 = j;
  296                                         sw0 = whole;
  297                                 } else if (UPROF_PERC_BIGGER(whole, fract, sw1,
  298                                     sf1)) {
  299                                         sf1 = fract;
  300                                         si1 = j;
  301                                         sw1 = whole;
  302                                 } else if (UPROF_PERC_BIGGER(whole, fract, sw2,
  303                                     sf2)) {
  304                                         sf2 = fract;
  305                                         si2 = j;
  306                                         sw2 = whole;
  307                                 } else if (UPROF_PERC_BIGGER(whole, fract, sw3,
  308                                     sf3)) {
  309                                         sf3 = fract;
  310                                         si3 = j;
  311                                         sw3 = whole;
  312                                 } else if (UPROF_PERC_BIGGER(whole, fract, sw4,
  313                                     sf4)) {
  314                                         sf4 = fract;
  315                                         si4 = j;
  316                                         sw4 = whole;
  317                                 }
  318                         }
  319                         sbuf_printf(&sb, "queue %u:\n", i);
  320                         sbuf_printf(&sb, "1st: %u.%u%% idx: %u\n", sw0 / tot,
  321                             sf0 / tot, si0);
  322                         sbuf_printf(&sb, "2nd: %u.%u%% idx: %u\n", sw1 / tot,
  323                             sf1 / tot, si1);
  324                         sbuf_printf(&sb, "3rd: %u.%u%% idx: %u\n", sw2 / tot,
  325                             sf2 / tot, si2);
  326                         sbuf_printf(&sb, "4th: %u.%u%% idx: %u\n", sw3 / tot,
  327                             sf3 / tot, si3);
  328                         sbuf_printf(&sb, "5th: %u.%u%% idx: %u\n", sw4 / tot,
  329                             sf4 / tot, si4);
  330                 }
  331         }
  332         sbuf_trim(&sb);
  333         sbuf_finish(&sb);
  334         sysctl_handle_string(oidp, sbuf_data(&sb), sbuf_len(&sb), req);
  335         sbuf_delete(&sb);
  336         return (0);
  337 }
  338 
  339 static int
  340 sysctl_debug_umtx_chains_clear(SYSCTL_HANDLER_ARGS)
  341 {
  342         struct umtxq_chain *uc;
  343         u_int i, j;
  344         int clear, error;
  345 
  346         clear = 0;
  347         error = sysctl_handle_int(oidp, &clear, 0, req);
  348         if (error != 0 || req->newptr == NULL)
  349                 return (error);
  350 
  351         if (clear != 0) {
  352                 for (i = 0; i < 2; ++i) {
  353                         for (j = 0; j < UMTX_CHAINS; ++j) {
  354                                 uc = &umtxq_chains[i][j];
  355                                 mtx_lock(&uc->uc_lock);
  356                                 uc->length = 0;
  357                                 uc->max_length = 0;     
  358                                 mtx_unlock(&uc->uc_lock);
  359                         }
  360                 }
  361         }
  362         return (0);
  363 }
  364 
  365 SYSCTL_PROC(_debug_umtx_chains, OID_AUTO, clear,
  366     CTLTYPE_INT | CTLFLAG_RW | CTLFLAG_MPSAFE, 0, 0,
  367     sysctl_debug_umtx_chains_clear, "I", "Clear umtx chains statistics");
  368 SYSCTL_PROC(_debug_umtx_chains, OID_AUTO, peaks,
  369     CTLTYPE_STRING | CTLFLAG_RD | CTLFLAG_MPSAFE, 0, 0,
  370     sysctl_debug_umtx_chains_peaks, "A", "Highest peaks in chains max length");
  371 #endif
  372 
  373 static void
  374 umtxq_sysinit(void *arg __unused)
  375 {
  376         int i, j;
  377 
  378         umtx_pi_zone = uma_zcreate("umtx pi", sizeof(struct umtx_pi),
  379                 NULL, NULL, NULL, NULL, UMA_ALIGN_PTR, 0);
  380         for (i = 0; i < 2; ++i) {
  381                 for (j = 0; j < UMTX_CHAINS; ++j) {
  382                         mtx_init(&umtxq_chains[i][j].uc_lock, "umtxql", NULL,
  383                                  MTX_DEF | MTX_DUPOK);
  384                         LIST_INIT(&umtxq_chains[i][j].uc_queue[0]);
  385                         LIST_INIT(&umtxq_chains[i][j].uc_queue[1]);
  386                         LIST_INIT(&umtxq_chains[i][j].uc_spare_queue);
  387                         TAILQ_INIT(&umtxq_chains[i][j].uc_pi_list);
  388                         umtxq_chains[i][j].uc_busy = 0;
  389                         umtxq_chains[i][j].uc_waiters = 0;
  390 #ifdef UMTX_PROFILING
  391                         umtxq_chains[i][j].length = 0;
  392                         umtxq_chains[i][j].max_length = 0;      
  393 #endif
  394                 }
  395         }
  396 #ifdef UMTX_PROFILING
  397         umtx_init_profiling();
  398 #endif
  399         mtx_init(&umtx_lock, "umtx lock", NULL, MTX_DEF);
  400         EVENTHANDLER_REGISTER(process_exec, umtx_exec_hook, NULL,
  401             EVENTHANDLER_PRI_ANY);
  402 }
  403 
  404 struct umtx_q *
  405 umtxq_alloc(void)
  406 {
  407         struct umtx_q *uq;
  408 
  409         uq = malloc(sizeof(struct umtx_q), M_UMTX, M_WAITOK | M_ZERO);
  410         uq->uq_spare_queue = malloc(sizeof(struct umtxq_queue), M_UMTX, M_WAITOK | M_ZERO);
  411         TAILQ_INIT(&uq->uq_spare_queue->head);
  412         TAILQ_INIT(&uq->uq_pi_contested);
  413         uq->uq_inherited_pri = PRI_MAX;
  414         return (uq);
  415 }
  416 
  417 void
  418 umtxq_free(struct umtx_q *uq)
  419 {
  420         MPASS(uq->uq_spare_queue != NULL);
  421         free(uq->uq_spare_queue, M_UMTX);
  422         free(uq, M_UMTX);
  423 }
  424 
  425 static inline void
  426 umtxq_hash(struct umtx_key *key)
  427 {
  428         unsigned n = (uintptr_t)key->info.both.a + key->info.both.b;
  429         key->hash = ((n * GOLDEN_RATIO_PRIME) >> UMTX_SHIFTS) % UMTX_CHAINS;
  430 }
  431 
  432 static inline struct umtxq_chain *
  433 umtxq_getchain(struct umtx_key *key)
  434 {
  435         if (key->type <= TYPE_SEM)
  436                 return (&umtxq_chains[1][key->hash]);
  437         return (&umtxq_chains[0][key->hash]);
  438 }
  439 
  440 /*
  441  * Lock a chain.
  442  */
  443 static inline void
  444 umtxq_lock(struct umtx_key *key)
  445 {
  446         struct umtxq_chain *uc;
  447 
  448         uc = umtxq_getchain(key);
  449         mtx_lock(&uc->uc_lock);
  450 }
  451 
  452 /*
  453  * Unlock a chain.
  454  */
  455 static inline void
  456 umtxq_unlock(struct umtx_key *key)
  457 {
  458         struct umtxq_chain *uc;
  459 
  460         uc = umtxq_getchain(key);
  461         mtx_unlock(&uc->uc_lock);
  462 }
  463 
  464 /*
  465  * Set chain to busy state when following operation
  466  * may be blocked (kernel mutex can not be used).
  467  */
  468 static inline void
  469 umtxq_busy(struct umtx_key *key)
  470 {
  471         struct umtxq_chain *uc;
  472 
  473         uc = umtxq_getchain(key);
  474         mtx_assert(&uc->uc_lock, MA_OWNED);
  475         if (uc->uc_busy) {
  476 #ifdef SMP
  477                 if (smp_cpus > 1) {
  478                         int count = BUSY_SPINS;
  479                         if (count > 0) {
  480                                 umtxq_unlock(key);
  481                                 while (uc->uc_busy && --count > 0)
  482                                         cpu_spinwait();
  483                                 umtxq_lock(key);
  484                         }
  485                 }
  486 #endif
  487                 while (uc->uc_busy) {
  488                         uc->uc_waiters++;
  489                         msleep(uc, &uc->uc_lock, 0, "umtxqb", 0);
  490                         uc->uc_waiters--;
  491                 }
  492         }
  493         uc->uc_busy = 1;
  494 }
  495 
  496 /*
  497  * Unbusy a chain.
  498  */
  499 static inline void
  500 umtxq_unbusy(struct umtx_key *key)
  501 {
  502         struct umtxq_chain *uc;
  503 
  504         uc = umtxq_getchain(key);
  505         mtx_assert(&uc->uc_lock, MA_OWNED);
  506         KASSERT(uc->uc_busy != 0, ("not busy"));
  507         uc->uc_busy = 0;
  508         if (uc->uc_waiters)
  509                 wakeup_one(uc);
  510 }
  511 
  512 static inline void
  513 umtxq_unbusy_unlocked(struct umtx_key *key)
  514 {
  515 
  516         umtxq_lock(key);
  517         umtxq_unbusy(key);
  518         umtxq_unlock(key);
  519 }
  520 
  521 static struct umtxq_queue *
  522 umtxq_queue_lookup(struct umtx_key *key, int q)
  523 {
  524         struct umtxq_queue *uh;
  525         struct umtxq_chain *uc;
  526 
  527         uc = umtxq_getchain(key);
  528         UMTXQ_LOCKED_ASSERT(uc);
  529         LIST_FOREACH(uh, &uc->uc_queue[q], link) {
  530                 if (umtx_key_match(&uh->key, key))
  531                         return (uh);
  532         }
  533 
  534         return (NULL);
  535 }
  536 
  537 static inline void
  538 umtxq_insert_queue(struct umtx_q *uq, int q)
  539 {
  540         struct umtxq_queue *uh;
  541         struct umtxq_chain *uc;
  542 
  543         uc = umtxq_getchain(&uq->uq_key);
  544         UMTXQ_LOCKED_ASSERT(uc);
  545         KASSERT((uq->uq_flags & UQF_UMTXQ) == 0, ("umtx_q is already on queue"));
  546         uh = umtxq_queue_lookup(&uq->uq_key, q);
  547         if (uh != NULL) {
  548                 LIST_INSERT_HEAD(&uc->uc_spare_queue, uq->uq_spare_queue, link);
  549         } else {
  550                 uh = uq->uq_spare_queue;
  551                 uh->key = uq->uq_key;
  552                 LIST_INSERT_HEAD(&uc->uc_queue[q], uh, link);
  553 #ifdef UMTX_PROFILING
  554                 uc->length++;
  555                 if (uc->length > uc->max_length) {
  556                         uc->max_length = uc->length;
  557                         if (uc->max_length > max_length)
  558                                 max_length = uc->max_length;    
  559                 }
  560 #endif
  561         }
  562         uq->uq_spare_queue = NULL;
  563 
  564         TAILQ_INSERT_TAIL(&uh->head, uq, uq_link);
  565         uh->length++;
  566         uq->uq_flags |= UQF_UMTXQ;
  567         uq->uq_cur_queue = uh;
  568         return;
  569 }
  570 
  571 static inline void
  572 umtxq_remove_queue(struct umtx_q *uq, int q)
  573 {
  574         struct umtxq_chain *uc;
  575         struct umtxq_queue *uh;
  576 
  577         uc = umtxq_getchain(&uq->uq_key);
  578         UMTXQ_LOCKED_ASSERT(uc);
  579         if (uq->uq_flags & UQF_UMTXQ) {
  580                 uh = uq->uq_cur_queue;
  581                 TAILQ_REMOVE(&uh->head, uq, uq_link);
  582                 uh->length--;
  583                 uq->uq_flags &= ~UQF_UMTXQ;
  584                 if (TAILQ_EMPTY(&uh->head)) {
  585                         KASSERT(uh->length == 0,
  586                             ("inconsistent umtxq_queue length"));
  587 #ifdef UMTX_PROFILING
  588                         uc->length--;
  589 #endif
  590                         LIST_REMOVE(uh, link);
  591                 } else {
  592                         uh = LIST_FIRST(&uc->uc_spare_queue);
  593                         KASSERT(uh != NULL, ("uc_spare_queue is empty"));
  594                         LIST_REMOVE(uh, link);
  595                 }
  596                 uq->uq_spare_queue = uh;
  597                 uq->uq_cur_queue = NULL;
  598         }
  599 }
  600 
  601 /*
  602  * Check if there are multiple waiters
  603  */
  604 static int
  605 umtxq_count(struct umtx_key *key)
  606 {
  607         struct umtxq_chain *uc;
  608         struct umtxq_queue *uh;
  609 
  610         uc = umtxq_getchain(key);
  611         UMTXQ_LOCKED_ASSERT(uc);
  612         uh = umtxq_queue_lookup(key, UMTX_SHARED_QUEUE);
  613         if (uh != NULL)
  614                 return (uh->length);
  615         return (0);
  616 }
  617 
  618 /*
  619  * Check if there are multiple PI waiters and returns first
  620  * waiter.
  621  */
  622 static int
  623 umtxq_count_pi(struct umtx_key *key, struct umtx_q **first)
  624 {
  625         struct umtxq_chain *uc;
  626         struct umtxq_queue *uh;
  627 
  628         *first = NULL;
  629         uc = umtxq_getchain(key);
  630         UMTXQ_LOCKED_ASSERT(uc);
  631         uh = umtxq_queue_lookup(key, UMTX_SHARED_QUEUE);
  632         if (uh != NULL) {
  633                 *first = TAILQ_FIRST(&uh->head);
  634                 return (uh->length);
  635         }
  636         return (0);
  637 }
  638 
  639 static int
  640 umtxq_check_susp(struct thread *td)
  641 {
  642         struct proc *p;
  643         int error;
  644 
  645         /*
  646          * The check for TDF_NEEDSUSPCHK is racy, but it is enough to
  647          * eventually break the lockstep loop.
  648          */
  649         if ((td->td_flags & TDF_NEEDSUSPCHK) == 0)
  650                 return (0);
  651         error = 0;
  652         p = td->td_proc;
  653         PROC_LOCK(p);
  654         if (P_SHOULDSTOP(p) ||
  655             ((p->p_flag & P_TRACED) && (td->td_dbgflags & TDB_SUSPEND))) {
  656                 if (p->p_flag & P_SINGLE_EXIT)
  657                         error = EINTR;
  658                 else
  659                         error = ERESTART;
  660         }
  661         PROC_UNLOCK(p);
  662         return (error);
  663 }
  664 
  665 /*
  666  * Wake up threads waiting on an userland object.
  667  */
  668 
  669 static int
  670 umtxq_signal_queue(struct umtx_key *key, int n_wake, int q)
  671 {
  672         struct umtxq_chain *uc;
  673         struct umtxq_queue *uh;
  674         struct umtx_q *uq;
  675         int ret;
  676 
  677         ret = 0;
  678         uc = umtxq_getchain(key);
  679         UMTXQ_LOCKED_ASSERT(uc);
  680         uh = umtxq_queue_lookup(key, q);
  681         if (uh != NULL) {
  682                 while ((uq = TAILQ_FIRST(&uh->head)) != NULL) {
  683                         umtxq_remove_queue(uq, q);
  684                         wakeup(uq);
  685                         if (++ret >= n_wake)
  686                                 return (ret);
  687                 }
  688         }
  689         return (ret);
  690 }
  691 
  692 
  693 /*
  694  * Wake up specified thread.
  695  */
  696 static inline void
  697 umtxq_signal_thread(struct umtx_q *uq)
  698 {
  699         struct umtxq_chain *uc;
  700 
  701         uc = umtxq_getchain(&uq->uq_key);
  702         UMTXQ_LOCKED_ASSERT(uc);
  703         umtxq_remove(uq);
  704         wakeup(uq);
  705 }
  706 
  707 static inline int 
  708 tstohz(const struct timespec *tsp)
  709 {
  710         struct timeval tv;
  711 
  712         TIMESPEC_TO_TIMEVAL(&tv, tsp);
  713         return tvtohz(&tv);
  714 }
  715 
  716 static void
  717 abs_timeout_init(struct abs_timeout *timo, int clockid, int absolute,
  718         const struct timespec *timeout)
  719 {
  720 
  721         timo->clockid = clockid;
  722         if (!absolute) {
  723                 kern_clock_gettime(curthread, clockid, &timo->end);
  724                 timo->cur = timo->end;
  725                 timespecadd(&timo->end, timeout);
  726         } else {
  727                 timo->end = *timeout;
  728                 kern_clock_gettime(curthread, clockid, &timo->cur);
  729         }
  730 }
  731 
  732 static void
  733 abs_timeout_init2(struct abs_timeout *timo, const struct _umtx_time *umtxtime)
  734 {
  735 
  736         abs_timeout_init(timo, umtxtime->_clockid,
  737                 (umtxtime->_flags & UMTX_ABSTIME) != 0,
  738                 &umtxtime->_timeout);
  739 }
  740 
  741 static inline void
  742 abs_timeout_update(struct abs_timeout *timo)
  743 {
  744         kern_clock_gettime(curthread, timo->clockid, &timo->cur);
  745 }
  746 
  747 static int
  748 abs_timeout_gethz(struct abs_timeout *timo)
  749 {
  750         struct timespec tts;
  751 
  752         if (timespeccmp(&timo->end, &timo->cur, <=))
  753                 return (-1); 
  754         tts = timo->end;
  755         timespecsub(&tts, &timo->cur);
  756         return (tstohz(&tts));
  757 }
  758 
  759 /*
  760  * Put thread into sleep state, before sleeping, check if
  761  * thread was removed from umtx queue.
  762  */
  763 static inline int
  764 umtxq_sleep(struct umtx_q *uq, const char *wmesg, struct abs_timeout *abstime)
  765 {
  766         struct umtxq_chain *uc;
  767         int error, timo;
  768 
  769         uc = umtxq_getchain(&uq->uq_key);
  770         UMTXQ_LOCKED_ASSERT(uc);
  771         for (;;) {
  772                 if (!(uq->uq_flags & UQF_UMTXQ))
  773                         return (0);
  774                 if (abstime != NULL) {
  775                         timo = abs_timeout_gethz(abstime);
  776                         if (timo < 0)
  777                                 return (ETIMEDOUT);
  778                 } else
  779                         timo = 0;
  780                 error = msleep(uq, &uc->uc_lock, PCATCH | PDROP, wmesg, timo);
  781                 if (error != EWOULDBLOCK) {
  782                         umtxq_lock(&uq->uq_key);
  783                         break;
  784                 }
  785                 if (abstime != NULL)
  786                         abs_timeout_update(abstime);
  787                 umtxq_lock(&uq->uq_key);
  788         }
  789         return (error);
  790 }
  791 
  792 /*
  793  * Convert userspace address into unique logical address.
  794  */
  795 int
  796 umtx_key_get(void *addr, int type, int share, struct umtx_key *key)
  797 {
  798         struct thread *td = curthread;
  799         vm_map_t map;
  800         vm_map_entry_t entry;
  801         vm_pindex_t pindex;
  802         vm_prot_t prot;
  803         boolean_t wired;
  804 
  805         key->type = type;
  806         if (share == THREAD_SHARE) {
  807                 key->shared = 0;
  808                 key->info.private.vs = td->td_proc->p_vmspace;
  809                 key->info.private.addr = (uintptr_t)addr;
  810         } else {
  811                 MPASS(share == PROCESS_SHARE || share == AUTO_SHARE);
  812                 map = &td->td_proc->p_vmspace->vm_map;
  813                 if (vm_map_lookup(&map, (vm_offset_t)addr, VM_PROT_WRITE,
  814                     &entry, &key->info.shared.object, &pindex, &prot,
  815                     &wired) != KERN_SUCCESS) {
  816                         return EFAULT;
  817                 }
  818 
  819                 if ((share == PROCESS_SHARE) ||
  820                     (share == AUTO_SHARE &&
  821                      VM_INHERIT_SHARE == entry->inheritance)) {
  822                         key->shared = 1;
  823                         key->info.shared.offset = entry->offset + entry->start -
  824                                 (vm_offset_t)addr;
  825                         vm_object_reference(key->info.shared.object);
  826                 } else {
  827                         key->shared = 0;
  828                         key->info.private.vs = td->td_proc->p_vmspace;
  829                         key->info.private.addr = (uintptr_t)addr;
  830                 }
  831                 vm_map_lookup_done(map, entry);
  832         }
  833 
  834         umtxq_hash(key);
  835         return (0);
  836 }
  837 
  838 /*
  839  * Release key.
  840  */
  841 void
  842 umtx_key_release(struct umtx_key *key)
  843 {
  844         if (key->shared)
  845                 vm_object_deallocate(key->info.shared.object);
  846 }
  847 
  848 /*
  849  * Lock a umtx object.
  850  */
  851 static int
  852 do_lock_umtx(struct thread *td, struct umtx *umtx, u_long id,
  853         const struct timespec *timeout)
  854 {
  855         struct abs_timeout timo;
  856         struct umtx_q *uq;
  857         u_long owner;
  858         u_long old;
  859         int error = 0;
  860 
  861         uq = td->td_umtxq;
  862         if (timeout != NULL)
  863                 abs_timeout_init(&timo, CLOCK_REALTIME, 0, timeout);
  864 
  865         /*
  866          * Care must be exercised when dealing with umtx structure. It
  867          * can fault on any access.
  868          */
  869         for (;;) {
  870                 /*
  871                  * Try the uncontested case.  This should be done in userland.
  872                  */
  873                 owner = casuword(&umtx->u_owner, UMTX_UNOWNED, id);
  874 
  875                 /* The acquire succeeded. */
  876                 if (owner == UMTX_UNOWNED)
  877                         return (0);
  878 
  879                 /* The address was invalid. */
  880                 if (owner == -1)
  881                         return (EFAULT);
  882 
  883                 /* If no one owns it but it is contested try to acquire it. */
  884                 if (owner == UMTX_CONTESTED) {
  885                         owner = casuword(&umtx->u_owner,
  886                             UMTX_CONTESTED, id | UMTX_CONTESTED);
  887 
  888                         if (owner == UMTX_CONTESTED)
  889                                 return (0);
  890 
  891                         /* The address was invalid. */
  892                         if (owner == -1)
  893                                 return (EFAULT);
  894 
  895                         error = umtxq_check_susp(td);
  896                         if (error != 0)
  897                                 break;
  898 
  899                         /* If this failed the lock has changed, restart. */
  900                         continue;
  901                 }
  902 
  903                 /*
  904                  * If we caught a signal, we have retried and now
  905                  * exit immediately.
  906                  */
  907                 if (error != 0)
  908                         break;
  909 
  910                 if ((error = umtx_key_get(umtx, TYPE_SIMPLE_LOCK,
  911                         AUTO_SHARE, &uq->uq_key)) != 0)
  912                         return (error);
  913 
  914                 umtxq_lock(&uq->uq_key);
  915                 umtxq_busy(&uq->uq_key);
  916                 umtxq_insert(uq);
  917                 umtxq_unbusy(&uq->uq_key);
  918                 umtxq_unlock(&uq->uq_key);
  919 
  920                 /*
  921                  * Set the contested bit so that a release in user space
  922                  * knows to use the system call for unlock.  If this fails
  923                  * either some one else has acquired the lock or it has been
  924                  * released.
  925                  */
  926                 old = casuword(&umtx->u_owner, owner, owner | UMTX_CONTESTED);
  927 
  928                 /* The address was invalid. */
  929                 if (old == -1) {
  930                         umtxq_lock(&uq->uq_key);
  931                         umtxq_remove(uq);
  932                         umtxq_unlock(&uq->uq_key);
  933                         umtx_key_release(&uq->uq_key);
  934                         return (EFAULT);
  935                 }
  936 
  937                 /*
  938                  * We set the contested bit, sleep. Otherwise the lock changed
  939                  * and we need to retry or we lost a race to the thread
  940                  * unlocking the umtx.
  941                  */
  942                 umtxq_lock(&uq->uq_key);
  943                 if (old == owner)
  944                         error = umtxq_sleep(uq, "umtx", timeout == NULL ? NULL :
  945                             &timo);
  946                 umtxq_remove(uq);
  947                 umtxq_unlock(&uq->uq_key);
  948                 umtx_key_release(&uq->uq_key);
  949 
  950                 if (error == 0)
  951                         error = umtxq_check_susp(td);
  952         }
  953 
  954         if (timeout == NULL) {
  955                 /* Mutex locking is restarted if it is interrupted. */
  956                 if (error == EINTR)
  957                         error = ERESTART;
  958         } else {
  959                 /* Timed-locking is not restarted. */
  960                 if (error == ERESTART)
  961                         error = EINTR;
  962         }
  963         return (error);
  964 }
  965 
  966 /*
  967  * Unlock a umtx object.
  968  */
  969 static int
  970 do_unlock_umtx(struct thread *td, struct umtx *umtx, u_long id)
  971 {
  972         struct umtx_key key;
  973         u_long owner;
  974         u_long old;
  975         int error;
  976         int count;
  977 
  978         /*
  979          * Make sure we own this mtx.
  980          */
  981         owner = fuword(__DEVOLATILE(u_long *, &umtx->u_owner));
  982         if (owner == -1)
  983                 return (EFAULT);
  984 
  985         if ((owner & ~UMTX_CONTESTED) != id)
  986                 return (EPERM);
  987 
  988         /* This should be done in userland */
  989         if ((owner & UMTX_CONTESTED) == 0) {
  990                 old = casuword(&umtx->u_owner, owner, UMTX_UNOWNED);
  991                 if (old == -1)
  992                         return (EFAULT);
  993                 if (old == owner)
  994                         return (0);
  995                 owner = old;
  996         }
  997 
  998         /* We should only ever be in here for contested locks */
  999         if ((error = umtx_key_get(umtx, TYPE_SIMPLE_LOCK, AUTO_SHARE,
 1000                 &key)) != 0)
 1001                 return (error);
 1002 
 1003         umtxq_lock(&key);
 1004         umtxq_busy(&key);
 1005         count = umtxq_count(&key);
 1006         umtxq_unlock(&key);
 1007 
 1008         /*
 1009          * When unlocking the umtx, it must be marked as unowned if
 1010          * there is zero or one thread only waiting for it.
 1011          * Otherwise, it must be marked as contested.
 1012          */
 1013         old = casuword(&umtx->u_owner, owner,
 1014                 count <= 1 ? UMTX_UNOWNED : UMTX_CONTESTED);
 1015         umtxq_lock(&key);
 1016         umtxq_signal(&key,1);
 1017         umtxq_unbusy(&key);
 1018         umtxq_unlock(&key);
 1019         umtx_key_release(&key);
 1020         if (old == -1)
 1021                 return (EFAULT);
 1022         if (old != owner)
 1023                 return (EINVAL);
 1024         return (0);
 1025 }
 1026 
 1027 #ifdef COMPAT_FREEBSD32
 1028 
 1029 /*
 1030  * Lock a umtx object.
 1031  */
 1032 static int
 1033 do_lock_umtx32(struct thread *td, uint32_t *m, uint32_t id,
 1034         const struct timespec *timeout)
 1035 {
 1036         struct abs_timeout timo;
 1037         struct umtx_q *uq;
 1038         uint32_t owner;
 1039         uint32_t old;
 1040         int error = 0;
 1041 
 1042         uq = td->td_umtxq;
 1043 
 1044         if (timeout != NULL)
 1045                 abs_timeout_init(&timo, CLOCK_REALTIME, 0, timeout);
 1046 
 1047         /*
 1048          * Care must be exercised when dealing with umtx structure. It
 1049          * can fault on any access.
 1050          */
 1051         for (;;) {
 1052                 /*
 1053                  * Try the uncontested case.  This should be done in userland.
 1054                  */
 1055                 owner = casuword32(m, UMUTEX_UNOWNED, id);
 1056 
 1057                 /* The acquire succeeded. */
 1058                 if (owner == UMUTEX_UNOWNED)
 1059                         return (0);
 1060 
 1061                 /* The address was invalid. */
 1062                 if (owner == -1)
 1063                         return (EFAULT);
 1064 
 1065                 /* If no one owns it but it is contested try to acquire it. */
 1066                 if (owner == UMUTEX_CONTESTED) {
 1067                         owner = casuword32(m,
 1068                             UMUTEX_CONTESTED, id | UMUTEX_CONTESTED);
 1069                         if (owner == UMUTEX_CONTESTED)
 1070                                 return (0);
 1071 
 1072                         /* The address was invalid. */
 1073                         if (owner == -1)
 1074                                 return (EFAULT);
 1075 
 1076                         error = umtxq_check_susp(td);
 1077                         if (error != 0)
 1078                                 break;
 1079 
 1080                         /* If this failed the lock has changed, restart. */
 1081                         continue;
 1082                 }
 1083 
 1084                 /*
 1085                  * If we caught a signal, we have retried and now
 1086                  * exit immediately.
 1087                  */
 1088                 if (error != 0)
 1089                         return (error);
 1090 
 1091                 if ((error = umtx_key_get(m, TYPE_SIMPLE_LOCK,
 1092                         AUTO_SHARE, &uq->uq_key)) != 0)
 1093                         return (error);
 1094 
 1095                 umtxq_lock(&uq->uq_key);
 1096                 umtxq_busy(&uq->uq_key);
 1097                 umtxq_insert(uq);
 1098                 umtxq_unbusy(&uq->uq_key);
 1099                 umtxq_unlock(&uq->uq_key);
 1100 
 1101                 /*
 1102                  * Set the contested bit so that a release in user space
 1103                  * knows to use the system call for unlock.  If this fails
 1104                  * either some one else has acquired the lock or it has been
 1105                  * released.
 1106                  */
 1107                 old = casuword32(m, owner, owner | UMUTEX_CONTESTED);
 1108 
 1109                 /* The address was invalid. */
 1110                 if (old == -1) {
 1111                         umtxq_lock(&uq->uq_key);
 1112                         umtxq_remove(uq);
 1113                         umtxq_unlock(&uq->uq_key);
 1114                         umtx_key_release(&uq->uq_key);
 1115                         return (EFAULT);
 1116                 }
 1117 
 1118                 /*
 1119                  * We set the contested bit, sleep. Otherwise the lock changed
 1120                  * and we need to retry or we lost a race to the thread
 1121                  * unlocking the umtx.
 1122                  */
 1123                 umtxq_lock(&uq->uq_key);
 1124                 if (old == owner)
 1125                         error = umtxq_sleep(uq, "umtx", timeout == NULL ?
 1126                             NULL : &timo);
 1127                 umtxq_remove(uq);
 1128                 umtxq_unlock(&uq->uq_key);
 1129                 umtx_key_release(&uq->uq_key);
 1130 
 1131                 if (error == 0)
 1132                         error = umtxq_check_susp(td);
 1133         }
 1134 
 1135         if (timeout == NULL) {
 1136                 /* Mutex locking is restarted if it is interrupted. */
 1137                 if (error == EINTR)
 1138                         error = ERESTART;
 1139         } else {
 1140                 /* Timed-locking is not restarted. */
 1141                 if (error == ERESTART)
 1142                         error = EINTR;
 1143         }
 1144         return (error);
 1145 }
 1146 
 1147 /*
 1148  * Unlock a umtx object.
 1149  */
 1150 static int
 1151 do_unlock_umtx32(struct thread *td, uint32_t *m, uint32_t id)
 1152 {
 1153         struct umtx_key key;
 1154         uint32_t owner;
 1155         uint32_t old;
 1156         int error;
 1157         int count;
 1158 
 1159         /*
 1160          * Make sure we own this mtx.
 1161          */
 1162         owner = fuword32(m);
 1163         if (owner == -1)
 1164                 return (EFAULT);
 1165 
 1166         if ((owner & ~UMUTEX_CONTESTED) != id)
 1167                 return (EPERM);
 1168 
 1169         /* This should be done in userland */
 1170         if ((owner & UMUTEX_CONTESTED) == 0) {
 1171                 old = casuword32(m, owner, UMUTEX_UNOWNED);
 1172                 if (old == -1)
 1173                         return (EFAULT);
 1174                 if (old == owner)
 1175                         return (0);
 1176                 owner = old;
 1177         }
 1178 
 1179         /* We should only ever be in here for contested locks */
 1180         if ((error = umtx_key_get(m, TYPE_SIMPLE_LOCK, AUTO_SHARE,
 1181                 &key)) != 0)
 1182                 return (error);
 1183 
 1184         umtxq_lock(&key);
 1185         umtxq_busy(&key);
 1186         count = umtxq_count(&key);
 1187         umtxq_unlock(&key);
 1188 
 1189         /*
 1190          * When unlocking the umtx, it must be marked as unowned if
 1191          * there is zero or one thread only waiting for it.
 1192          * Otherwise, it must be marked as contested.
 1193          */
 1194         old = casuword32(m, owner,
 1195                 count <= 1 ? UMUTEX_UNOWNED : UMUTEX_CONTESTED);
 1196         umtxq_lock(&key);
 1197         umtxq_signal(&key,1);
 1198         umtxq_unbusy(&key);
 1199         umtxq_unlock(&key);
 1200         umtx_key_release(&key);
 1201         if (old == -1)
 1202                 return (EFAULT);
 1203         if (old != owner)
 1204                 return (EINVAL);
 1205         return (0);
 1206 }
 1207 #endif
 1208 
 1209 /*
 1210  * Fetch and compare value, sleep on the address if value is not changed.
 1211  */
 1212 static int
 1213 do_wait(struct thread *td, void *addr, u_long id,
 1214         struct _umtx_time *timeout, int compat32, int is_private)
 1215 {
 1216         struct abs_timeout timo;
 1217         struct umtx_q *uq;
 1218         u_long tmp;
 1219         uint32_t tmp32;
 1220         int error = 0;
 1221 
 1222         uq = td->td_umtxq;
 1223         if ((error = umtx_key_get(addr, TYPE_SIMPLE_WAIT,
 1224                 is_private ? THREAD_SHARE : AUTO_SHARE, &uq->uq_key)) != 0)
 1225                 return (error);
 1226 
 1227         if (timeout != NULL)
 1228                 abs_timeout_init2(&timo, timeout);
 1229 
 1230         umtxq_lock(&uq->uq_key);
 1231         umtxq_insert(uq);
 1232         umtxq_unlock(&uq->uq_key);
 1233         if (compat32 == 0) {
 1234                 error = fueword(addr, &tmp);
 1235                 if (error != 0)
 1236                         error = EFAULT;
 1237         } else {
 1238                 error = fueword32(addr, &tmp32);
 1239                 if (error == 0)
 1240                         tmp = tmp32;
 1241                 else
 1242                         error = EFAULT;
 1243         }
 1244         umtxq_lock(&uq->uq_key);
 1245         if (error == 0) {
 1246                 if (tmp == id)
 1247                         error = umtxq_sleep(uq, "uwait", timeout == NULL ?
 1248                             NULL : &timo);
 1249                 if ((uq->uq_flags & UQF_UMTXQ) == 0)
 1250                         error = 0;
 1251                 else
 1252                         umtxq_remove(uq);
 1253         } else if ((uq->uq_flags & UQF_UMTXQ) != 0) {
 1254                 umtxq_remove(uq);
 1255         }
 1256         umtxq_unlock(&uq->uq_key);
 1257         umtx_key_release(&uq->uq_key);
 1258         if (error == ERESTART)
 1259                 error = EINTR;
 1260         return (error);
 1261 }
 1262 
 1263 /*
 1264  * Wake up threads sleeping on the specified address.
 1265  */
 1266 int
 1267 kern_umtx_wake(struct thread *td, void *uaddr, int n_wake, int is_private)
 1268 {
 1269         struct umtx_key key;
 1270         int ret;
 1271         
 1272         if ((ret = umtx_key_get(uaddr, TYPE_SIMPLE_WAIT,
 1273                 is_private ? THREAD_SHARE : AUTO_SHARE, &key)) != 0)
 1274                 return (ret);
 1275         umtxq_lock(&key);
 1276         umtxq_signal(&key, n_wake);
 1277         umtxq_unlock(&key);
 1278         umtx_key_release(&key);
 1279         return (0);
 1280 }
 1281 
 1282 /*
 1283  * Lock PTHREAD_PRIO_NONE protocol POSIX mutex.
 1284  */
 1285 static int
 1286 do_lock_normal(struct thread *td, struct umutex *m, uint32_t flags,
 1287         struct _umtx_time *timeout, int mode)
 1288 {
 1289         struct abs_timeout timo;
 1290         struct umtx_q *uq;
 1291         uint32_t owner, old, id;
 1292         int error, rv;
 1293 
 1294         id = td->td_tid;
 1295         uq = td->td_umtxq;
 1296         error = 0;
 1297         if (timeout != NULL)
 1298                 abs_timeout_init2(&timo, timeout);
 1299 
 1300         /*
 1301          * Care must be exercised when dealing with umtx structure. It
 1302          * can fault on any access.
 1303          */
 1304         for (;;) {
 1305                 rv = fueword32(&m->m_owner, &owner);
 1306                 if (rv == -1)
 1307                         return (EFAULT);
 1308                 if (mode == _UMUTEX_WAIT) {
 1309                         if (owner == UMUTEX_UNOWNED || owner == UMUTEX_CONTESTED)
 1310                                 return (0);
 1311                 } else {
 1312                         /*
 1313                          * Try the uncontested case.  This should be done in userland.
 1314                          */
 1315                         rv = casueword32(&m->m_owner, UMUTEX_UNOWNED,
 1316                             &owner, id);
 1317                         /* The address was invalid. */
 1318                         if (rv == -1)
 1319                                 return (EFAULT);
 1320 
 1321                         /* The acquire succeeded. */
 1322                         if (owner == UMUTEX_UNOWNED)
 1323                                 return (0);
 1324 
 1325                         /* If no one owns it but it is contested try to acquire it. */
 1326                         if (owner == UMUTEX_CONTESTED) {
 1327                                 rv = casueword32(&m->m_owner,
 1328                                     UMUTEX_CONTESTED, &owner,
 1329                                     id | UMUTEX_CONTESTED);
 1330                                 /* The address was invalid. */
 1331                                 if (rv == -1)
 1332                                         return (EFAULT);
 1333 
 1334                                 if (owner == UMUTEX_CONTESTED)
 1335                                         return (0);
 1336 
 1337                                 rv = umtxq_check_susp(td);
 1338                                 if (rv != 0)
 1339                                         return (rv);
 1340 
 1341                                 /* If this failed the lock has changed, restart. */
 1342                                 continue;
 1343                         }
 1344                 }
 1345 
 1346                 if ((flags & UMUTEX_ERROR_CHECK) != 0 &&
 1347                     (owner & ~UMUTEX_CONTESTED) == id)
 1348                         return (EDEADLK);
 1349 
 1350                 if (mode == _UMUTEX_TRY)
 1351                         return (EBUSY);
 1352 
 1353                 /*
 1354                  * If we caught a signal, we have retried and now
 1355                  * exit immediately.
 1356                  */
 1357                 if (error != 0)
 1358                         return (error);
 1359 
 1360                 if ((error = umtx_key_get(m, TYPE_NORMAL_UMUTEX,
 1361                     GET_SHARE(flags), &uq->uq_key)) != 0)
 1362                         return (error);
 1363 
 1364                 umtxq_lock(&uq->uq_key);
 1365                 umtxq_busy(&uq->uq_key);
 1366                 umtxq_insert(uq);
 1367                 umtxq_unlock(&uq->uq_key);
 1368 
 1369                 /*
 1370                  * Set the contested bit so that a release in user space
 1371                  * knows to use the system call for unlock.  If this fails
 1372                  * either some one else has acquired the lock or it has been
 1373                  * released.
 1374                  */
 1375                 rv = casueword32(&m->m_owner, owner, &old,
 1376                     owner | UMUTEX_CONTESTED);
 1377 
 1378                 /* The address was invalid. */
 1379                 if (rv == -1) {
 1380                         umtxq_lock(&uq->uq_key);
 1381                         umtxq_remove(uq);
 1382                         umtxq_unbusy(&uq->uq_key);
 1383                         umtxq_unlock(&uq->uq_key);
 1384                         umtx_key_release(&uq->uq_key);
 1385                         return (EFAULT);
 1386                 }
 1387 
 1388                 /*
 1389                  * We set the contested bit, sleep. Otherwise the lock changed
 1390                  * and we need to retry or we lost a race to the thread
 1391                  * unlocking the umtx.
 1392                  */
 1393                 umtxq_lock(&uq->uq_key);
 1394                 umtxq_unbusy(&uq->uq_key);
 1395                 if (old == owner)
 1396                         error = umtxq_sleep(uq, "umtxn", timeout == NULL ?
 1397                             NULL : &timo);
 1398                 umtxq_remove(uq);
 1399                 umtxq_unlock(&uq->uq_key);
 1400                 umtx_key_release(&uq->uq_key);
 1401 
 1402                 if (error == 0)
 1403                         error = umtxq_check_susp(td);
 1404         }
 1405 
 1406         return (0);
 1407 }
 1408 
 1409 /*
 1410  * Unlock PTHREAD_PRIO_NONE protocol POSIX mutex.
 1411  */
 1412 static int
 1413 do_unlock_normal(struct thread *td, struct umutex *m, uint32_t flags)
 1414 {
 1415         struct umtx_key key;
 1416         uint32_t owner, old, id;
 1417         int error;
 1418         int count;
 1419 
 1420         id = td->td_tid;
 1421         /*
 1422          * Make sure we own this mtx.
 1423          */
 1424         error = fueword32(&m->m_owner, &owner);
 1425         if (error == -1)
 1426                 return (EFAULT);
 1427 
 1428         if ((owner & ~UMUTEX_CONTESTED) != id)
 1429                 return (EPERM);
 1430 
 1431         if ((owner & UMUTEX_CONTESTED) == 0) {
 1432                 error = casueword32(&m->m_owner, owner, &old, UMUTEX_UNOWNED);
 1433                 if (error == -1)
 1434                         return (EFAULT);
 1435                 if (old == owner)
 1436                         return (0);
 1437                 owner = old;
 1438         }
 1439 
 1440         /* We should only ever be in here for contested locks */
 1441         if ((error = umtx_key_get(m, TYPE_NORMAL_UMUTEX, GET_SHARE(flags),
 1442             &key)) != 0)
 1443                 return (error);
 1444 
 1445         umtxq_lock(&key);
 1446         umtxq_busy(&key);
 1447         count = umtxq_count(&key);
 1448         umtxq_unlock(&key);
 1449 
 1450         /*
 1451          * When unlocking the umtx, it must be marked as unowned if
 1452          * there is zero or one thread only waiting for it.
 1453          * Otherwise, it must be marked as contested.
 1454          */
 1455         error = casueword32(&m->m_owner, owner, &old,
 1456             count <= 1 ? UMUTEX_UNOWNED : UMUTEX_CONTESTED);
 1457         umtxq_lock(&key);
 1458         umtxq_signal(&key,1);
 1459         umtxq_unbusy(&key);
 1460         umtxq_unlock(&key);
 1461         umtx_key_release(&key);
 1462         if (error == -1)
 1463                 return (EFAULT);
 1464         if (old != owner)
 1465                 return (EINVAL);
 1466         return (0);
 1467 }
 1468 
 1469 /*
 1470  * Check if the mutex is available and wake up a waiter,
 1471  * only for simple mutex.
 1472  */
 1473 static int
 1474 do_wake_umutex(struct thread *td, struct umutex *m)
 1475 {
 1476         struct umtx_key key;
 1477         uint32_t owner;
 1478         uint32_t flags;
 1479         int error;
 1480         int count;
 1481 
 1482         error = fueword32(&m->m_owner, &owner);
 1483         if (error == -1)
 1484                 return (EFAULT);
 1485 
 1486         if ((owner & ~UMUTEX_CONTESTED) != 0)
 1487                 return (0);
 1488 
 1489         error = fueword32(&m->m_flags, &flags);
 1490         if (error == -1)
 1491                 return (EFAULT);
 1492 
 1493         /* We should only ever be in here for contested locks */
 1494         if ((error = umtx_key_get(m, TYPE_NORMAL_UMUTEX, GET_SHARE(flags),
 1495             &key)) != 0)
 1496                 return (error);
 1497 
 1498         umtxq_lock(&key);
 1499         umtxq_busy(&key);
 1500         count = umtxq_count(&key);
 1501         umtxq_unlock(&key);
 1502 
 1503         if (count <= 1) {
 1504                 error = casueword32(&m->m_owner, UMUTEX_CONTESTED, &owner,
 1505                     UMUTEX_UNOWNED);
 1506                 if (error == -1)
 1507                         error = EFAULT;
 1508         }
 1509 
 1510         umtxq_lock(&key);
 1511         if (error == 0 && count != 0 && (owner & ~UMUTEX_CONTESTED) == 0)
 1512                 umtxq_signal(&key, 1);
 1513         umtxq_unbusy(&key);
 1514         umtxq_unlock(&key);
 1515         umtx_key_release(&key);
 1516         return (error);
 1517 }
 1518 
 1519 /*
 1520  * Check if the mutex has waiters and tries to fix contention bit.
 1521  */
 1522 static int
 1523 do_wake2_umutex(struct thread *td, struct umutex *m, uint32_t flags)
 1524 {
 1525         struct umtx_key key;
 1526         uint32_t owner, old;
 1527         int type;
 1528         int error;
 1529         int count;
 1530 
 1531         switch(flags & (UMUTEX_PRIO_INHERIT | UMUTEX_PRIO_PROTECT)) {
 1532         case 0:
 1533                 type = TYPE_NORMAL_UMUTEX;
 1534                 break;
 1535         case UMUTEX_PRIO_INHERIT:
 1536                 type = TYPE_PI_UMUTEX;
 1537                 break;
 1538         case UMUTEX_PRIO_PROTECT:
 1539                 type = TYPE_PP_UMUTEX;
 1540                 break;
 1541         default:
 1542                 return (EINVAL);
 1543         }
 1544         if ((error = umtx_key_get(m, type, GET_SHARE(flags),
 1545             &key)) != 0)
 1546                 return (error);
 1547 
 1548         owner = 0;
 1549         umtxq_lock(&key);
 1550         umtxq_busy(&key);
 1551         count = umtxq_count(&key);
 1552         umtxq_unlock(&key);
 1553         /*
 1554          * Only repair contention bit if there is a waiter, this means the mutex
 1555          * is still being referenced by userland code, otherwise don't update
 1556          * any memory.
 1557          */
 1558         if (count > 1) {
 1559                 error = fueword32(&m->m_owner, &owner);
 1560                 if (error == -1)
 1561                         error = EFAULT;
 1562                 while (error == 0 && (owner & UMUTEX_CONTESTED) == 0) {
 1563                         error = casueword32(&m->m_owner, owner, &old,
 1564                             owner | UMUTEX_CONTESTED);
 1565                         if (error == -1) {
 1566                                 error = EFAULT;
 1567                                 break;
 1568                         }
 1569                         if (old == owner)
 1570                                 break;
 1571                         owner = old;
 1572                         error = umtxq_check_susp(td);
 1573                         if (error != 0)
 1574                                 break;
 1575                 }
 1576         } else if (count == 1) {
 1577                 error = fueword32(&m->m_owner, &owner);
 1578                 if (error == -1)
 1579                         error = EFAULT;
 1580                 while (error == 0 && (owner & ~UMUTEX_CONTESTED) != 0 &&
 1581                        (owner & UMUTEX_CONTESTED) == 0) {
 1582                         error = casueword32(&m->m_owner, owner, &old,
 1583                             owner | UMUTEX_CONTESTED);
 1584                         if (error == -1) {
 1585                                 error = EFAULT;
 1586                                 break;
 1587                         }
 1588                         if (old == owner)
 1589                                 break;
 1590                         owner = old;
 1591                         error = umtxq_check_susp(td);
 1592                         if (error != 0)
 1593                                 break;
 1594                 }
 1595         }
 1596         umtxq_lock(&key);
 1597         if (error == EFAULT) {
 1598                 umtxq_signal(&key, INT_MAX);
 1599         } else if (count != 0 && (owner & ~UMUTEX_CONTESTED) == 0)
 1600                 umtxq_signal(&key, 1);
 1601         umtxq_unbusy(&key);
 1602         umtxq_unlock(&key);
 1603         umtx_key_release(&key);
 1604         return (error);
 1605 }
 1606 
 1607 static inline struct umtx_pi *
 1608 umtx_pi_alloc(int flags)
 1609 {
 1610         struct umtx_pi *pi;
 1611 
 1612         pi = uma_zalloc(umtx_pi_zone, M_ZERO | flags);
 1613         TAILQ_INIT(&pi->pi_blocked);
 1614         atomic_add_int(&umtx_pi_allocated, 1);
 1615         return (pi);
 1616 }
 1617 
 1618 static inline void
 1619 umtx_pi_free(struct umtx_pi *pi)
 1620 {
 1621         uma_zfree(umtx_pi_zone, pi);
 1622         atomic_add_int(&umtx_pi_allocated, -1);
 1623 }
 1624 
 1625 /*
 1626  * Adjust the thread's position on a pi_state after its priority has been
 1627  * changed.
 1628  */
 1629 static int
 1630 umtx_pi_adjust_thread(struct umtx_pi *pi, struct thread *td)
 1631 {
 1632         struct umtx_q *uq, *uq1, *uq2;
 1633         struct thread *td1;
 1634 
 1635         mtx_assert(&umtx_lock, MA_OWNED);
 1636         if (pi == NULL)
 1637                 return (0);
 1638 
 1639         uq = td->td_umtxq;
 1640 
 1641         /*
 1642          * Check if the thread needs to be moved on the blocked chain.
 1643          * It needs to be moved if either its priority is lower than
 1644          * the previous thread or higher than the next thread.
 1645          */
 1646         uq1 = TAILQ_PREV(uq, umtxq_head, uq_lockq);
 1647         uq2 = TAILQ_NEXT(uq, uq_lockq);
 1648         if ((uq1 != NULL && UPRI(td) < UPRI(uq1->uq_thread)) ||
 1649             (uq2 != NULL && UPRI(td) > UPRI(uq2->uq_thread))) {
 1650                 /*
 1651                  * Remove thread from blocked chain and determine where
 1652                  * it should be moved to.
 1653                  */
 1654                 TAILQ_REMOVE(&pi->pi_blocked, uq, uq_lockq);
 1655                 TAILQ_FOREACH(uq1, &pi->pi_blocked, uq_lockq) {
 1656                         td1 = uq1->uq_thread;
 1657                         MPASS(td1->td_proc->p_magic == P_MAGIC);
 1658                         if (UPRI(td1) > UPRI(td))
 1659                                 break;
 1660                 }
 1661 
 1662                 if (uq1 == NULL)
 1663                         TAILQ_INSERT_TAIL(&pi->pi_blocked, uq, uq_lockq);
 1664                 else
 1665                         TAILQ_INSERT_BEFORE(uq1, uq, uq_lockq);
 1666         }
 1667         return (1);
 1668 }
 1669 
 1670 static struct umtx_pi *
 1671 umtx_pi_next(struct umtx_pi *pi)
 1672 {
 1673         struct umtx_q *uq_owner;
 1674 
 1675         if (pi->pi_owner == NULL)
 1676                 return (NULL);
 1677         uq_owner = pi->pi_owner->td_umtxq;
 1678         if (uq_owner == NULL)
 1679                 return (NULL);
 1680         return (uq_owner->uq_pi_blocked);
 1681 }
 1682 
 1683 /*
 1684  * Floyd's Cycle-Finding Algorithm.
 1685  */
 1686 static bool
 1687 umtx_pi_check_loop(struct umtx_pi *pi)
 1688 {
 1689         struct umtx_pi *pi1;    /* fast iterator */
 1690 
 1691         mtx_assert(&umtx_lock, MA_OWNED);
 1692         if (pi == NULL)
 1693                 return (false);
 1694         pi1 = pi;
 1695         for (;;) {
 1696                 pi = umtx_pi_next(pi);
 1697                 if (pi == NULL)
 1698                         break;
 1699                 pi1 = umtx_pi_next(pi1);
 1700                 if (pi1 == NULL)
 1701                         break;
 1702                 pi1 = umtx_pi_next(pi1);
 1703                 if (pi1 == NULL)
 1704                         break;
 1705                 if (pi == pi1)
 1706                         return (true);
 1707         }
 1708         return (false);
 1709 }
 1710 
 1711 /*
 1712  * Propagate priority when a thread is blocked on POSIX
 1713  * PI mutex.
 1714  */ 
 1715 static void
 1716 umtx_propagate_priority(struct thread *td)
 1717 {
 1718         struct umtx_q *uq;
 1719         struct umtx_pi *pi;
 1720         int pri;
 1721 
 1722         mtx_assert(&umtx_lock, MA_OWNED);
 1723         pri = UPRI(td);
 1724         uq = td->td_umtxq;
 1725         pi = uq->uq_pi_blocked;
 1726         if (pi == NULL)
 1727                 return;
 1728         if (umtx_pi_check_loop(pi))
 1729                 return;
 1730 
 1731         for (;;) {
 1732                 td = pi->pi_owner;
 1733                 if (td == NULL || td == curthread)
 1734                         return;
 1735 
 1736                 MPASS(td->td_proc != NULL);
 1737                 MPASS(td->td_proc->p_magic == P_MAGIC);
 1738 
 1739                 thread_lock(td);
 1740                 if (td->td_lend_user_pri > pri)
 1741                         sched_lend_user_prio(td, pri);
 1742                 else {
 1743                         thread_unlock(td);
 1744                         break;
 1745                 }
 1746                 thread_unlock(td);
 1747 
 1748                 /*
 1749                  * Pick up the lock that td is blocked on.
 1750                  */
 1751                 uq = td->td_umtxq;
 1752                 pi = uq->uq_pi_blocked;
 1753                 if (pi == NULL)
 1754                         break;
 1755                 /* Resort td on the list if needed. */
 1756                 umtx_pi_adjust_thread(pi, td);
 1757         }
 1758 }
 1759 
 1760 /*
 1761  * Unpropagate priority for a PI mutex when a thread blocked on
 1762  * it is interrupted by signal or resumed by others.
 1763  */
 1764 static void
 1765 umtx_repropagate_priority(struct umtx_pi *pi)
 1766 {
 1767         struct umtx_q *uq, *uq_owner;
 1768         struct umtx_pi *pi2;
 1769         int pri;
 1770 
 1771         mtx_assert(&umtx_lock, MA_OWNED);
 1772 
 1773         if (umtx_pi_check_loop(pi))
 1774                 return;
 1775         while (pi != NULL && pi->pi_owner != NULL) {
 1776                 pri = PRI_MAX;
 1777                 uq_owner = pi->pi_owner->td_umtxq;
 1778 
 1779                 TAILQ_FOREACH(pi2, &uq_owner->uq_pi_contested, pi_link) {
 1780                         uq = TAILQ_FIRST(&pi2->pi_blocked);
 1781                         if (uq != NULL) {
 1782                                 if (pri > UPRI(uq->uq_thread))
 1783                                         pri = UPRI(uq->uq_thread);
 1784                         }
 1785                 }
 1786 
 1787                 if (pri > uq_owner->uq_inherited_pri)
 1788                         pri = uq_owner->uq_inherited_pri;
 1789                 thread_lock(pi->pi_owner);
 1790                 sched_lend_user_prio(pi->pi_owner, pri);
 1791                 thread_unlock(pi->pi_owner);
 1792                 if ((pi = uq_owner->uq_pi_blocked) != NULL)
 1793                         umtx_pi_adjust_thread(pi, uq_owner->uq_thread);
 1794         }
 1795 }
 1796 
 1797 /*
 1798  * Insert a PI mutex into owned list.
 1799  */
 1800 static void
 1801 umtx_pi_setowner(struct umtx_pi *pi, struct thread *owner)
 1802 {
 1803         struct umtx_q *uq_owner;
 1804 
 1805         uq_owner = owner->td_umtxq;
 1806         mtx_assert(&umtx_lock, MA_OWNED);
 1807         if (pi->pi_owner != NULL)
 1808                 panic("pi_owner != NULL");
 1809         pi->pi_owner = owner;
 1810         TAILQ_INSERT_TAIL(&uq_owner->uq_pi_contested, pi, pi_link);
 1811 }
 1812 
 1813 
 1814 /*
 1815  * Disown a PI mutex, and remove it from the owned list.
 1816  */
 1817 static void
 1818 umtx_pi_disown(struct umtx_pi *pi)
 1819 {
 1820 
 1821         mtx_assert(&umtx_lock, MA_OWNED);
 1822         TAILQ_REMOVE(&pi->pi_owner->td_umtxq->uq_pi_contested, pi, pi_link);
 1823         pi->pi_owner = NULL;
 1824 }
 1825 
 1826 /*
 1827  * Claim ownership of a PI mutex.
 1828  */
 1829 static int
 1830 umtx_pi_claim(struct umtx_pi *pi, struct thread *owner)
 1831 {
 1832         struct umtx_q *uq;
 1833 
 1834         mtx_lock(&umtx_lock);
 1835         if (pi->pi_owner == owner) {
 1836                 mtx_unlock(&umtx_lock);
 1837                 return (0);
 1838         }
 1839 
 1840         if (pi->pi_owner != NULL) {
 1841                 /*
 1842                  * userland may have already messed the mutex, sigh.
 1843                  */
 1844                 mtx_unlock(&umtx_lock);
 1845                 return (EPERM);
 1846         }
 1847         umtx_pi_setowner(pi, owner);
 1848         uq = TAILQ_FIRST(&pi->pi_blocked);
 1849         if (uq != NULL) {
 1850                 int pri;
 1851 
 1852                 pri = UPRI(uq->uq_thread);
 1853                 thread_lock(owner);
 1854                 if (pri < UPRI(owner))
 1855                         sched_lend_user_prio(owner, pri);
 1856                 thread_unlock(owner);
 1857         }
 1858         mtx_unlock(&umtx_lock);
 1859         return (0);
 1860 }
 1861 
 1862 /*
 1863  * Adjust a thread's order position in its blocked PI mutex,
 1864  * this may result new priority propagating process.
 1865  */
 1866 void
 1867 umtx_pi_adjust(struct thread *td, u_char oldpri)
 1868 {
 1869         struct umtx_q *uq;
 1870         struct umtx_pi *pi;
 1871 
 1872         uq = td->td_umtxq;
 1873         mtx_lock(&umtx_lock);
 1874         /*
 1875          * Pick up the lock that td is blocked on.
 1876          */
 1877         pi = uq->uq_pi_blocked;
 1878         if (pi != NULL) {
 1879                 umtx_pi_adjust_thread(pi, td);
 1880                 umtx_repropagate_priority(pi);
 1881         }
 1882         mtx_unlock(&umtx_lock);
 1883 }
 1884 
 1885 /*
 1886  * Sleep on a PI mutex.
 1887  */
 1888 static int
 1889 umtxq_sleep_pi(struct umtx_q *uq, struct umtx_pi *pi,
 1890         uint32_t owner, const char *wmesg, struct abs_timeout *timo)
 1891 {
 1892         struct umtxq_chain *uc;
 1893         struct thread *td, *td1;
 1894         struct umtx_q *uq1;
 1895         int pri;
 1896         int error = 0;
 1897 
 1898         td = uq->uq_thread;
 1899         KASSERT(td == curthread, ("inconsistent uq_thread"));
 1900         uc = umtxq_getchain(&uq->uq_key);
 1901         UMTXQ_LOCKED_ASSERT(uc);
 1902         KASSERT(uc->uc_busy != 0, ("umtx chain is not busy"));
 1903         umtxq_insert(uq);
 1904         mtx_lock(&umtx_lock);
 1905         if (pi->pi_owner == NULL) {
 1906                 mtx_unlock(&umtx_lock);
 1907                 /* XXX Only look up thread in current process. */
 1908                 td1 = tdfind(owner, curproc->p_pid);
 1909                 mtx_lock(&umtx_lock);
 1910                 if (td1 != NULL) {
 1911                         if (pi->pi_owner == NULL)
 1912                                 umtx_pi_setowner(pi, td1);
 1913                         PROC_UNLOCK(td1->td_proc);
 1914                 }
 1915         }
 1916 
 1917         TAILQ_FOREACH(uq1, &pi->pi_blocked, uq_lockq) {
 1918                 pri = UPRI(uq1->uq_thread);
 1919                 if (pri > UPRI(td))
 1920                         break;
 1921         }
 1922 
 1923         if (uq1 != NULL)
 1924                 TAILQ_INSERT_BEFORE(uq1, uq, uq_lockq);
 1925         else
 1926                 TAILQ_INSERT_TAIL(&pi->pi_blocked, uq, uq_lockq);
 1927 
 1928         uq->uq_pi_blocked = pi;
 1929         thread_lock(td);
 1930         td->td_flags |= TDF_UPIBLOCKED;
 1931         thread_unlock(td);
 1932         umtx_propagate_priority(td);
 1933         mtx_unlock(&umtx_lock);
 1934         umtxq_unbusy(&uq->uq_key);
 1935 
 1936         error = umtxq_sleep(uq, wmesg, timo);
 1937         umtxq_remove(uq);
 1938 
 1939         mtx_lock(&umtx_lock);
 1940         uq->uq_pi_blocked = NULL;
 1941         thread_lock(td);
 1942         td->td_flags &= ~TDF_UPIBLOCKED;
 1943         thread_unlock(td);
 1944         TAILQ_REMOVE(&pi->pi_blocked, uq, uq_lockq);
 1945         umtx_repropagate_priority(pi);
 1946         mtx_unlock(&umtx_lock);
 1947         umtxq_unlock(&uq->uq_key);
 1948 
 1949         return (error);
 1950 }
 1951 
 1952 /*
 1953  * Add reference count for a PI mutex.
 1954  */
 1955 static void
 1956 umtx_pi_ref(struct umtx_pi *pi)
 1957 {
 1958         struct umtxq_chain *uc;
 1959 
 1960         uc = umtxq_getchain(&pi->pi_key);
 1961         UMTXQ_LOCKED_ASSERT(uc);
 1962         pi->pi_refcount++;
 1963 }
 1964 
 1965 /*
 1966  * Decrease reference count for a PI mutex, if the counter
 1967  * is decreased to zero, its memory space is freed.
 1968  */ 
 1969 static void
 1970 umtx_pi_unref(struct umtx_pi *pi)
 1971 {
 1972         struct umtxq_chain *uc;
 1973 
 1974         uc = umtxq_getchain(&pi->pi_key);
 1975         UMTXQ_LOCKED_ASSERT(uc);
 1976         KASSERT(pi->pi_refcount > 0, ("invalid reference count"));
 1977         if (--pi->pi_refcount == 0) {
 1978                 mtx_lock(&umtx_lock);
 1979                 if (pi->pi_owner != NULL)
 1980                         umtx_pi_disown(pi);
 1981                 KASSERT(TAILQ_EMPTY(&pi->pi_blocked),
 1982                         ("blocked queue not empty"));
 1983                 mtx_unlock(&umtx_lock);
 1984                 TAILQ_REMOVE(&uc->uc_pi_list, pi, pi_hashlink);
 1985                 umtx_pi_free(pi);
 1986         }
 1987 }
 1988 
 1989 /*
 1990  * Find a PI mutex in hash table.
 1991  */
 1992 static struct umtx_pi *
 1993 umtx_pi_lookup(struct umtx_key *key)
 1994 {
 1995         struct umtxq_chain *uc;
 1996         struct umtx_pi *pi;
 1997 
 1998         uc = umtxq_getchain(key);
 1999         UMTXQ_LOCKED_ASSERT(uc);
 2000 
 2001         TAILQ_FOREACH(pi, &uc->uc_pi_list, pi_hashlink) {
 2002                 if (umtx_key_match(&pi->pi_key, key)) {
 2003                         return (pi);
 2004                 }
 2005         }
 2006         return (NULL);
 2007 }
 2008 
 2009 /*
 2010  * Insert a PI mutex into hash table.
 2011  */
 2012 static inline void
 2013 umtx_pi_insert(struct umtx_pi *pi)
 2014 {
 2015         struct umtxq_chain *uc;
 2016 
 2017         uc = umtxq_getchain(&pi->pi_key);
 2018         UMTXQ_LOCKED_ASSERT(uc);
 2019         TAILQ_INSERT_TAIL(&uc->uc_pi_list, pi, pi_hashlink);
 2020 }
 2021 
 2022 /*
 2023  * Lock a PI mutex.
 2024  */
 2025 static int
 2026 do_lock_pi(struct thread *td, struct umutex *m, uint32_t flags,
 2027     struct _umtx_time *timeout, int try)
 2028 {
 2029         struct abs_timeout timo;
 2030         struct umtx_q *uq;
 2031         struct umtx_pi *pi, *new_pi;
 2032         uint32_t id, owner, old;
 2033         int error, rv;
 2034 
 2035         id = td->td_tid;
 2036         uq = td->td_umtxq;
 2037 
 2038         if ((error = umtx_key_get(m, TYPE_PI_UMUTEX, GET_SHARE(flags),
 2039             &uq->uq_key)) != 0)
 2040                 return (error);
 2041 
 2042         if (timeout != NULL)
 2043                 abs_timeout_init2(&timo, timeout);
 2044 
 2045         umtxq_lock(&uq->uq_key);
 2046         pi = umtx_pi_lookup(&uq->uq_key);
 2047         if (pi == NULL) {
 2048                 new_pi = umtx_pi_alloc(M_NOWAIT);
 2049                 if (new_pi == NULL) {
 2050                         umtxq_unlock(&uq->uq_key);
 2051                         new_pi = umtx_pi_alloc(M_WAITOK);
 2052                         umtxq_lock(&uq->uq_key);
 2053                         pi = umtx_pi_lookup(&uq->uq_key);
 2054                         if (pi != NULL) {
 2055                                 umtx_pi_free(new_pi);
 2056                                 new_pi = NULL;
 2057                         }
 2058                 }
 2059                 if (new_pi != NULL) {
 2060                         new_pi->pi_key = uq->uq_key;
 2061                         umtx_pi_insert(new_pi);
 2062                         pi = new_pi;
 2063                 }
 2064         }
 2065         umtx_pi_ref(pi);
 2066         umtxq_unlock(&uq->uq_key);
 2067 
 2068         /*
 2069          * Care must be exercised when dealing with umtx structure.  It
 2070          * can fault on any access.
 2071          */
 2072         for (;;) {
 2073                 /*
 2074                  * Try the uncontested case.  This should be done in userland.
 2075                  */
 2076                 rv = casueword32(&m->m_owner, UMUTEX_UNOWNED, &owner, id);
 2077                 /* The address was invalid. */
 2078                 if (rv == -1) {
 2079                         error = EFAULT;
 2080                         break;
 2081                 }
 2082 
 2083                 /* The acquire succeeded. */
 2084                 if (owner == UMUTEX_UNOWNED) {
 2085                         error = 0;
 2086                         break;
 2087                 }
 2088 
 2089                 /* If no one owns it but it is contested try to acquire it. */
 2090                 if (owner == UMUTEX_CONTESTED) {
 2091                         rv = casueword32(&m->m_owner,
 2092                             UMUTEX_CONTESTED, &owner, id | UMUTEX_CONTESTED);
 2093                         /* The address was invalid. */
 2094                         if (rv == -1) {
 2095                                 error = EFAULT;
 2096                                 break;
 2097                         }
 2098 
 2099                         if (owner == UMUTEX_CONTESTED) {
 2100                                 umtxq_lock(&uq->uq_key);
 2101                                 umtxq_busy(&uq->uq_key);
 2102                                 error = umtx_pi_claim(pi, td);
 2103                                 umtxq_unbusy(&uq->uq_key);
 2104                                 umtxq_unlock(&uq->uq_key);
 2105                                 if (error != 0) {
 2106                                         /*
 2107                                          * Since we're going to return an
 2108                                          * error, restore the m_owner to its
 2109                                          * previous, unowned state to avoid
 2110                                          * compounding the problem.
 2111                                          */
 2112                                         (void)casuword32(&m->m_owner,
 2113                                             id | UMUTEX_CONTESTED,
 2114                                             UMUTEX_CONTESTED);
 2115                                 }
 2116                                 break;
 2117                         }
 2118 
 2119                         error = umtxq_check_susp(td);
 2120                         if (error != 0)
 2121                                 break;
 2122 
 2123                         /* If this failed the lock has changed, restart. */
 2124                         continue;
 2125                 }
 2126 
 2127                 if ((owner & ~UMUTEX_CONTESTED) == id) {
 2128                         error = EDEADLK;
 2129                         break;
 2130                 }
 2131 
 2132                 if (try != 0) {
 2133                         error = EBUSY;
 2134                         break;
 2135                 }
 2136 
 2137                 /*
 2138                  * If we caught a signal, we have retried and now
 2139                  * exit immediately.
 2140                  */
 2141                 if (error != 0)
 2142                         break;
 2143                         
 2144                 umtxq_lock(&uq->uq_key);
 2145                 umtxq_busy(&uq->uq_key);
 2146                 umtxq_unlock(&uq->uq_key);
 2147 
 2148                 /*
 2149                  * Set the contested bit so that a release in user space
 2150                  * knows to use the system call for unlock.  If this fails
 2151                  * either some one else has acquired the lock or it has been
 2152                  * released.
 2153                  */
 2154                 rv = casueword32(&m->m_owner, owner, &old,
 2155                     owner | UMUTEX_CONTESTED);
 2156 
 2157                 /* The address was invalid. */
 2158                 if (rv == -1) {
 2159                         umtxq_unbusy_unlocked(&uq->uq_key);
 2160                         error = EFAULT;
 2161                         break;
 2162                 }
 2163 
 2164                 umtxq_lock(&uq->uq_key);
 2165                 /*
 2166                  * We set the contested bit, sleep. Otherwise the lock changed
 2167                  * and we need to retry or we lost a race to the thread
 2168                  * unlocking the umtx.
 2169                  */
 2170                 if (old == owner) {
 2171                         error = umtxq_sleep_pi(uq, pi, owner & ~UMUTEX_CONTESTED,
 2172                             "umtxpi", timeout == NULL ? NULL : &timo);
 2173                         if (error != 0)
 2174                                 continue;
 2175                 } else {
 2176                         umtxq_unbusy(&uq->uq_key);
 2177                         umtxq_unlock(&uq->uq_key);
 2178                 }
 2179 
 2180                 error = umtxq_check_susp(td);
 2181                 if (error != 0)
 2182                         break;
 2183         }
 2184 
 2185         umtxq_lock(&uq->uq_key);
 2186         umtx_pi_unref(pi);
 2187         umtxq_unlock(&uq->uq_key);
 2188 
 2189         umtx_key_release(&uq->uq_key);
 2190         return (error);
 2191 }
 2192 
 2193 /*
 2194  * Unlock a PI mutex.
 2195  */
 2196 static int
 2197 do_unlock_pi(struct thread *td, struct umutex *m, uint32_t flags)
 2198 {
 2199         struct umtx_key key;
 2200         struct umtx_q *uq_first, *uq_first2, *uq_me;
 2201         struct umtx_pi *pi, *pi2;
 2202         uint32_t owner, old, id;
 2203         int error;
 2204         int count;
 2205         int pri;
 2206 
 2207         id = td->td_tid;
 2208         /*
 2209          * Make sure we own this mtx.
 2210          */
 2211         error = fueword32(&m->m_owner, &owner);
 2212         if (error == -1)
 2213                 return (EFAULT);
 2214 
 2215         if ((owner & ~UMUTEX_CONTESTED) != id)
 2216                 return (EPERM);
 2217 
 2218         /* This should be done in userland */
 2219         if ((owner & UMUTEX_CONTESTED) == 0) {
 2220                 error = casueword32(&m->m_owner, owner, &old, UMUTEX_UNOWNED);
 2221                 if (error == -1)
 2222                         return (EFAULT);
 2223                 if (old == owner)
 2224                         return (0);
 2225                 owner = old;
 2226         }
 2227 
 2228         /* We should only ever be in here for contested locks */
 2229         if ((error = umtx_key_get(m, TYPE_PI_UMUTEX, GET_SHARE(flags),
 2230             &key)) != 0)
 2231                 return (error);
 2232 
 2233         umtxq_lock(&key);
 2234         umtxq_busy(&key);
 2235         count = umtxq_count_pi(&key, &uq_first);
 2236         if (uq_first != NULL) {
 2237                 mtx_lock(&umtx_lock);
 2238                 pi = uq_first->uq_pi_blocked;
 2239                 KASSERT(pi != NULL, ("pi == NULL?"));
 2240                 if (pi->pi_owner != td) {
 2241                         mtx_unlock(&umtx_lock);
 2242                         umtxq_unbusy(&key);
 2243                         umtxq_unlock(&key);
 2244                         umtx_key_release(&key);
 2245                         /* userland messed the mutex */
 2246                         return (EPERM);
 2247                 }
 2248                 uq_me = td->td_umtxq;
 2249                 umtx_pi_disown(pi);
 2250                 /* get highest priority thread which is still sleeping. */
 2251                 uq_first = TAILQ_FIRST(&pi->pi_blocked);
 2252                 while (uq_first != NULL && 
 2253                        (uq_first->uq_flags & UQF_UMTXQ) == 0) {
 2254                         uq_first = TAILQ_NEXT(uq_first, uq_lockq);
 2255                 }
 2256                 pri = PRI_MAX;
 2257                 TAILQ_FOREACH(pi2, &uq_me->uq_pi_contested, pi_link) {
 2258                         uq_first2 = TAILQ_FIRST(&pi2->pi_blocked);
 2259                         if (uq_first2 != NULL) {
 2260                                 if (pri > UPRI(uq_first2->uq_thread))
 2261                                         pri = UPRI(uq_first2->uq_thread);
 2262                         }
 2263                 }
 2264                 thread_lock(td);
 2265                 sched_lend_user_prio(td, pri);
 2266                 thread_unlock(td);
 2267                 mtx_unlock(&umtx_lock);
 2268                 if (uq_first)
 2269                         umtxq_signal_thread(uq_first);
 2270         } else {
 2271                 pi = umtx_pi_lookup(&key);
 2272                 /*
 2273                  * A umtx_pi can exist if a signal or timeout removed the
 2274                  * last waiter from the umtxq, but there is still
 2275                  * a thread in do_lock_pi() holding the umtx_pi.
 2276                  */
 2277                 if (pi != NULL) {
 2278                         /*
 2279                          * The umtx_pi can be unowned, such as when a thread
 2280                          * has just entered do_lock_pi(), allocated the
 2281                          * umtx_pi, and unlocked the umtxq.
 2282                          * If the current thread owns it, it must disown it.
 2283                          */
 2284                         mtx_lock(&umtx_lock);
 2285                         if (pi->pi_owner == td)
 2286                                 umtx_pi_disown(pi);
 2287                         mtx_unlock(&umtx_lock);
 2288                 }
 2289         }
 2290         umtxq_unlock(&key);
 2291 
 2292         /*
 2293          * When unlocking the umtx, it must be marked as unowned if
 2294          * there is zero or one thread only waiting for it.
 2295          * Otherwise, it must be marked as contested.
 2296          */
 2297         error = casueword32(&m->m_owner, owner, &old,
 2298             count <= 1 ? UMUTEX_UNOWNED : UMUTEX_CONTESTED);
 2299 
 2300         umtxq_unbusy_unlocked(&key);
 2301         umtx_key_release(&key);
 2302         if (error == -1)
 2303                 return (EFAULT);
 2304         if (old != owner)
 2305                 return (EINVAL);
 2306         return (0);
 2307 }
 2308 
 2309 /*
 2310  * Lock a PP mutex.
 2311  */
 2312 static int
 2313 do_lock_pp(struct thread *td, struct umutex *m, uint32_t flags,
 2314     struct _umtx_time *timeout, int try)
 2315 {
 2316         struct abs_timeout timo;
 2317         struct umtx_q *uq, *uq2;
 2318         struct umtx_pi *pi;
 2319         uint32_t ceiling;
 2320         uint32_t owner, id;
 2321         int error, pri, old_inherited_pri, su, rv;
 2322 
 2323         id = td->td_tid;
 2324         uq = td->td_umtxq;
 2325         if ((error = umtx_key_get(m, TYPE_PP_UMUTEX, GET_SHARE(flags),
 2326             &uq->uq_key)) != 0)
 2327                 return (error);
 2328 
 2329         if (timeout != NULL)
 2330                 abs_timeout_init2(&timo, timeout);
 2331 
 2332         su = (priv_check(td, PRIV_SCHED_RTPRIO) == 0);
 2333         for (;;) {
 2334                 old_inherited_pri = uq->uq_inherited_pri;
 2335                 umtxq_lock(&uq->uq_key);
 2336                 umtxq_busy(&uq->uq_key);
 2337                 umtxq_unlock(&uq->uq_key);
 2338 
 2339                 rv = fueword32(&m->m_ceilings[0], &ceiling);
 2340                 if (rv == -1) {
 2341                         error = EFAULT;
 2342                         goto out;
 2343                 }
 2344                 ceiling = RTP_PRIO_MAX - ceiling;
 2345                 if (ceiling > RTP_PRIO_MAX) {
 2346                         error = EINVAL;
 2347                         goto out;
 2348                 }
 2349 
 2350                 mtx_lock(&umtx_lock);
 2351                 if (UPRI(td) < PRI_MIN_REALTIME + ceiling) {
 2352                         mtx_unlock(&umtx_lock);
 2353                         error = EINVAL;
 2354                         goto out;
 2355                 }
 2356                 if (su && PRI_MIN_REALTIME + ceiling < uq->uq_inherited_pri) {
 2357                         uq->uq_inherited_pri = PRI_MIN_REALTIME + ceiling;
 2358                         thread_lock(td);
 2359                         if (uq->uq_inherited_pri < UPRI(td))
 2360                                 sched_lend_user_prio(td, uq->uq_inherited_pri);
 2361                         thread_unlock(td);
 2362                 }
 2363                 mtx_unlock(&umtx_lock);
 2364 
 2365                 rv = casueword32(&m->m_owner,
 2366                     UMUTEX_CONTESTED, &owner, id | UMUTEX_CONTESTED);
 2367                 /* The address was invalid. */
 2368                 if (rv == -1) {
 2369                         error = EFAULT;
 2370                         break;
 2371                 }
 2372 
 2373                 if (owner == UMUTEX_CONTESTED) {
 2374                         error = 0;
 2375                         break;
 2376                 }
 2377 
 2378                 if ((flags & UMUTEX_ERROR_CHECK) != 0 &&
 2379                     (owner & ~UMUTEX_CONTESTED) == id) {
 2380                         error = EDEADLK;
 2381                         break;
 2382                 }
 2383 
 2384                 if (try != 0) {
 2385                         error = EBUSY;
 2386                         break;
 2387                 }
 2388 
 2389                 /*
 2390                  * If we caught a signal, we have retried and now
 2391                  * exit immediately.
 2392                  */
 2393                 if (error != 0)
 2394                         break;
 2395 
 2396                 umtxq_lock(&uq->uq_key);
 2397                 umtxq_insert(uq);
 2398                 umtxq_unbusy(&uq->uq_key);
 2399                 error = umtxq_sleep(uq, "umtxpp", timeout == NULL ?
 2400                     NULL : &timo);
 2401                 umtxq_remove(uq);
 2402                 umtxq_unlock(&uq->uq_key);
 2403 
 2404                 mtx_lock(&umtx_lock);
 2405                 uq->uq_inherited_pri = old_inherited_pri;
 2406                 pri = PRI_MAX;
 2407                 TAILQ_FOREACH(pi, &uq->uq_pi_contested, pi_link) {
 2408                         uq2 = TAILQ_FIRST(&pi->pi_blocked);
 2409                         if (uq2 != NULL) {
 2410                                 if (pri > UPRI(uq2->uq_thread))
 2411                                         pri = UPRI(uq2->uq_thread);
 2412                         }
 2413                 }
 2414                 if (pri > uq->uq_inherited_pri)
 2415                         pri = uq->uq_inherited_pri;
 2416                 thread_lock(td);
 2417                 sched_lend_user_prio(td, pri);
 2418                 thread_unlock(td);
 2419                 mtx_unlock(&umtx_lock);
 2420         }
 2421 
 2422         if (error != 0) {
 2423                 mtx_lock(&umtx_lock);
 2424                 uq->uq_inherited_pri = old_inherited_pri;
 2425                 pri = PRI_MAX;
 2426                 TAILQ_FOREACH(pi, &uq->uq_pi_contested, pi_link) {
 2427                         uq2 = TAILQ_FIRST(&pi->pi_blocked);
 2428                         if (uq2 != NULL) {
 2429                                 if (pri > UPRI(uq2->uq_thread))
 2430                                         pri = UPRI(uq2->uq_thread);
 2431                         }
 2432                 }
 2433                 if (pri > uq->uq_inherited_pri)
 2434                         pri = uq->uq_inherited_pri;
 2435                 thread_lock(td);
 2436                 sched_lend_user_prio(td, pri);
 2437                 thread_unlock(td);
 2438                 mtx_unlock(&umtx_lock);
 2439         }
 2440 
 2441 out:
 2442         umtxq_unbusy_unlocked(&uq->uq_key);
 2443         umtx_key_release(&uq->uq_key);
 2444         return (error);
 2445 }
 2446 
 2447 /*
 2448  * Unlock a PP mutex.
 2449  */
 2450 static int
 2451 do_unlock_pp(struct thread *td, struct umutex *m, uint32_t flags)
 2452 {
 2453         struct umtx_key key;
 2454         struct umtx_q *uq, *uq2;
 2455         struct umtx_pi *pi;
 2456         uint32_t owner, id;
 2457         uint32_t rceiling;
 2458         int error, pri, new_inherited_pri, su;
 2459 
 2460         id = td->td_tid;
 2461         uq = td->td_umtxq;
 2462         su = (priv_check(td, PRIV_SCHED_RTPRIO) == 0);
 2463 
 2464         /*
 2465          * Make sure we own this mtx.
 2466          */
 2467         error = fueword32(&m->m_owner, &owner);
 2468         if (error == -1)
 2469                 return (EFAULT);
 2470 
 2471         if ((owner & ~UMUTEX_CONTESTED) != id)
 2472                 return (EPERM);
 2473 
 2474         error = copyin(&m->m_ceilings[1], &rceiling, sizeof(uint32_t));
 2475         if (error != 0)
 2476                 return (error);
 2477 
 2478         if (rceiling == -1)
 2479                 new_inherited_pri = PRI_MAX;
 2480         else {
 2481                 rceiling = RTP_PRIO_MAX - rceiling;
 2482                 if (rceiling > RTP_PRIO_MAX)
 2483                         return (EINVAL);
 2484                 new_inherited_pri = PRI_MIN_REALTIME + rceiling;
 2485         }
 2486 
 2487         if ((error = umtx_key_get(m, TYPE_PP_UMUTEX, GET_SHARE(flags),
 2488             &key)) != 0)
 2489                 return (error);
 2490         umtxq_lock(&key);
 2491         umtxq_busy(&key);
 2492         umtxq_unlock(&key);
 2493         /*
 2494          * For priority protected mutex, always set unlocked state
 2495          * to UMUTEX_CONTESTED, so that userland always enters kernel
 2496          * to lock the mutex, it is necessary because thread priority
 2497          * has to be adjusted for such mutex.
 2498          */
 2499         error = suword32(&m->m_owner, UMUTEX_CONTESTED);
 2500 
 2501         umtxq_lock(&key);
 2502         if (error == 0)
 2503                 umtxq_signal(&key, 1);
 2504         umtxq_unbusy(&key);
 2505         umtxq_unlock(&key);
 2506 
 2507         if (error == -1)
 2508                 error = EFAULT;
 2509         else {
 2510                 mtx_lock(&umtx_lock);
 2511                 if (su != 0)
 2512                         uq->uq_inherited_pri = new_inherited_pri;
 2513                 pri = PRI_MAX;
 2514                 TAILQ_FOREACH(pi, &uq->uq_pi_contested, pi_link) {
 2515                         uq2 = TAILQ_FIRST(&pi->pi_blocked);
 2516                         if (uq2 != NULL) {
 2517                                 if (pri > UPRI(uq2->uq_thread))
 2518                                         pri = UPRI(uq2->uq_thread);
 2519                         }
 2520                 }
 2521                 if (pri > uq->uq_inherited_pri)
 2522                         pri = uq->uq_inherited_pri;
 2523                 thread_lock(td);
 2524                 sched_lend_user_prio(td, pri);
 2525                 thread_unlock(td);
 2526                 mtx_unlock(&umtx_lock);
 2527         }
 2528         umtx_key_release(&key);
 2529         return (error);
 2530 }
 2531 
 2532 static int
 2533 do_set_ceiling(struct thread *td, struct umutex *m, uint32_t ceiling,
 2534         uint32_t *old_ceiling)
 2535 {
 2536         struct umtx_q *uq;
 2537         uint32_t save_ceiling;
 2538         uint32_t owner, id;
 2539         uint32_t flags;
 2540         int error, rv;
 2541 
 2542         error = fueword32(&m->m_flags, &flags);
 2543         if (error == -1)
 2544                 return (EFAULT);
 2545         if ((flags & UMUTEX_PRIO_PROTECT) == 0)
 2546                 return (EINVAL);
 2547         if (ceiling > RTP_PRIO_MAX)
 2548                 return (EINVAL);
 2549         id = td->td_tid;
 2550         uq = td->td_umtxq;
 2551         if ((error = umtx_key_get(m, TYPE_PP_UMUTEX, GET_SHARE(flags),
 2552            &uq->uq_key)) != 0)
 2553                 return (error);
 2554         for (;;) {
 2555                 umtxq_lock(&uq->uq_key);
 2556                 umtxq_busy(&uq->uq_key);
 2557                 umtxq_unlock(&uq->uq_key);
 2558 
 2559                 rv = fueword32(&m->m_ceilings[0], &save_ceiling);
 2560                 if (rv == -1) {
 2561                         error = EFAULT;
 2562                         break;
 2563                 }
 2564 
 2565                 rv = casueword32(&m->m_owner,
 2566                     UMUTEX_CONTESTED, &owner, id | UMUTEX_CONTESTED);
 2567                 if (rv == -1) {
 2568                         error = EFAULT;
 2569                         break;
 2570                 }
 2571 
 2572                 if (owner == UMUTEX_CONTESTED) {
 2573                         suword32(&m->m_ceilings[0], ceiling);
 2574                         suword32(&m->m_owner, UMUTEX_CONTESTED);
 2575                         error = 0;
 2576                         break;
 2577                 }
 2578 
 2579                 if ((owner & ~UMUTEX_CONTESTED) == id) {
 2580                         suword32(&m->m_ceilings[0], ceiling);
 2581                         error = 0;
 2582                         break;
 2583                 }
 2584 
 2585                 /*
 2586                  * If we caught a signal, we have retried and now
 2587                  * exit immediately.
 2588                  */
 2589                 if (error != 0)
 2590                         break;
 2591 
 2592                 /*
 2593                  * We set the contested bit, sleep. Otherwise the lock changed
 2594                  * and we need to retry or we lost a race to the thread
 2595                  * unlocking the umtx.
 2596                  */
 2597                 umtxq_lock(&uq->uq_key);
 2598                 umtxq_insert(uq);
 2599                 umtxq_unbusy(&uq->uq_key);
 2600                 error = umtxq_sleep(uq, "umtxpp", NULL);
 2601                 umtxq_remove(uq);
 2602                 umtxq_unlock(&uq->uq_key);
 2603         }
 2604         umtxq_lock(&uq->uq_key);
 2605         if (error == 0)
 2606                 umtxq_signal(&uq->uq_key, INT_MAX);
 2607         umtxq_unbusy(&uq->uq_key);
 2608         umtxq_unlock(&uq->uq_key);
 2609         umtx_key_release(&uq->uq_key);
 2610         if (error == 0 && old_ceiling != NULL)
 2611                 suword32(old_ceiling, save_ceiling);
 2612         return (error);
 2613 }
 2614 
 2615 /*
 2616  * Lock a userland POSIX mutex.
 2617  */
 2618 static int
 2619 do_lock_umutex(struct thread *td, struct umutex *m,
 2620     struct _umtx_time *timeout, int mode)
 2621 {
 2622         uint32_t flags;
 2623         int error;
 2624 
 2625         error = fueword32(&m->m_flags, &flags);
 2626         if (error == -1)
 2627                 return (EFAULT);
 2628 
 2629         switch(flags & (UMUTEX_PRIO_INHERIT | UMUTEX_PRIO_PROTECT)) {
 2630         case 0:
 2631                 error = do_lock_normal(td, m, flags, timeout, mode);
 2632                 break;
 2633         case UMUTEX_PRIO_INHERIT:
 2634                 error = do_lock_pi(td, m, flags, timeout, mode);
 2635                 break;
 2636         case UMUTEX_PRIO_PROTECT:
 2637                 error = do_lock_pp(td, m, flags, timeout, mode);
 2638                 break;
 2639         default:
 2640                 return (EINVAL);
 2641         }
 2642         if (timeout == NULL) {
 2643                 if (error == EINTR && mode != _UMUTEX_WAIT)
 2644                         error = ERESTART;
 2645         } else {
 2646                 /* Timed-locking is not restarted. */
 2647                 if (error == ERESTART)
 2648                         error = EINTR;
 2649         }
 2650         return (error);
 2651 }
 2652 
 2653 /*
 2654  * Unlock a userland POSIX mutex.
 2655  */
 2656 static int
 2657 do_unlock_umutex(struct thread *td, struct umutex *m)
 2658 {
 2659         uint32_t flags;
 2660         int error;
 2661 
 2662         error = fueword32(&m->m_flags, &flags);
 2663         if (error == -1)
 2664                 return (EFAULT);
 2665 
 2666         switch(flags & (UMUTEX_PRIO_INHERIT | UMUTEX_PRIO_PROTECT)) {
 2667         case 0:
 2668                 return (do_unlock_normal(td, m, flags));
 2669         case UMUTEX_PRIO_INHERIT:
 2670                 return (do_unlock_pi(td, m, flags));
 2671         case UMUTEX_PRIO_PROTECT:
 2672                 return (do_unlock_pp(td, m, flags));
 2673         }
 2674 
 2675         return (EINVAL);
 2676 }
 2677 
 2678 static int
 2679 do_cv_wait(struct thread *td, struct ucond *cv, struct umutex *m,
 2680         struct timespec *timeout, u_long wflags)
 2681 {
 2682         struct abs_timeout timo;
 2683         struct umtx_q *uq;
 2684         uint32_t flags, clockid, hasw;
 2685         int error;
 2686 
 2687         uq = td->td_umtxq;
 2688         error = fueword32(&cv->c_flags, &flags);
 2689         if (error == -1)
 2690                 return (EFAULT);
 2691         error = umtx_key_get(cv, TYPE_CV, GET_SHARE(flags), &uq->uq_key);
 2692         if (error != 0)
 2693                 return (error);
 2694 
 2695         if ((wflags & CVWAIT_CLOCKID) != 0) {
 2696                 error = fueword32(&cv->c_clockid, &clockid);
 2697                 if (error == -1) {
 2698                         umtx_key_release(&uq->uq_key);
 2699                         return (EFAULT);
 2700                 }
 2701                 if (clockid < CLOCK_REALTIME ||
 2702                     clockid >= CLOCK_THREAD_CPUTIME_ID) {
 2703                         /* hmm, only HW clock id will work. */
 2704                         umtx_key_release(&uq->uq_key);
 2705                         return (EINVAL);
 2706                 }
 2707         } else {
 2708                 clockid = CLOCK_REALTIME;
 2709         }
 2710 
 2711         umtxq_lock(&uq->uq_key);
 2712         umtxq_busy(&uq->uq_key);
 2713         umtxq_insert(uq);
 2714         umtxq_unlock(&uq->uq_key);
 2715 
 2716         /*
 2717          * Set c_has_waiters to 1 before releasing user mutex, also
 2718          * don't modify cache line when unnecessary.
 2719          */
 2720         error = fueword32(&cv->c_has_waiters, &hasw);
 2721         if (error == 0 && hasw == 0)
 2722                 suword32(&cv->c_has_waiters, 1);
 2723 
 2724         umtxq_unbusy_unlocked(&uq->uq_key);
 2725 
 2726         error = do_unlock_umutex(td, m);
 2727 
 2728         if (timeout != NULL)
 2729                 abs_timeout_init(&timo, clockid, ((wflags & CVWAIT_ABSTIME) != 0),
 2730                         timeout);
 2731         
 2732         umtxq_lock(&uq->uq_key);
 2733         if (error == 0) {
 2734                 error = umtxq_sleep(uq, "ucond", timeout == NULL ?
 2735                     NULL : &timo);
 2736         }
 2737 
 2738         if ((uq->uq_flags & UQF_UMTXQ) == 0)
 2739                 error = 0;
 2740         else {
 2741                 /*
 2742                  * This must be timeout,interrupted by signal or
 2743                  * surprious wakeup, clear c_has_waiter flag when
 2744                  * necessary.
 2745                  */
 2746                 umtxq_busy(&uq->uq_key);
 2747                 if ((uq->uq_flags & UQF_UMTXQ) != 0) {
 2748                         int oldlen = uq->uq_cur_queue->length;
 2749                         umtxq_remove(uq);
 2750                         if (oldlen == 1) {
 2751                                 umtxq_unlock(&uq->uq_key);
 2752                                 suword32(&cv->c_has_waiters, 0);
 2753                                 umtxq_lock(&uq->uq_key);
 2754                         }
 2755                 }
 2756                 umtxq_unbusy(&uq->uq_key);
 2757                 if (error == ERESTART)
 2758                         error = EINTR;
 2759         }
 2760 
 2761         umtxq_unlock(&uq->uq_key);
 2762         umtx_key_release(&uq->uq_key);
 2763         return (error);
 2764 }
 2765 
 2766 /*
 2767  * Signal a userland condition variable.
 2768  */
 2769 static int
 2770 do_cv_signal(struct thread *td, struct ucond *cv)
 2771 {
 2772         struct umtx_key key;
 2773         int error, cnt, nwake;
 2774         uint32_t flags;
 2775 
 2776         error = fueword32(&cv->c_flags, &flags);
 2777         if (error == -1)
 2778                 return (EFAULT);
 2779         if ((error = umtx_key_get(cv, TYPE_CV, GET_SHARE(flags), &key)) != 0)
 2780                 return (error); 
 2781         umtxq_lock(&key);
 2782         umtxq_busy(&key);
 2783         cnt = umtxq_count(&key);
 2784         nwake = umtxq_signal(&key, 1);
 2785         if (cnt <= nwake) {
 2786                 umtxq_unlock(&key);
 2787                 error = suword32(&cv->c_has_waiters, 0);
 2788                 if (error == -1)
 2789                         error = EFAULT;
 2790                 umtxq_lock(&key);
 2791         }
 2792         umtxq_unbusy(&key);
 2793         umtxq_unlock(&key);
 2794         umtx_key_release(&key);
 2795         return (error);
 2796 }
 2797 
 2798 static int
 2799 do_cv_broadcast(struct thread *td, struct ucond *cv)
 2800 {
 2801         struct umtx_key key;
 2802         int error;
 2803         uint32_t flags;
 2804 
 2805         error = fueword32(&cv->c_flags, &flags);
 2806         if (error == -1)
 2807                 return (EFAULT);
 2808         if ((error = umtx_key_get(cv, TYPE_CV, GET_SHARE(flags), &key)) != 0)
 2809                 return (error); 
 2810 
 2811         umtxq_lock(&key);
 2812         umtxq_busy(&key);
 2813         umtxq_signal(&key, INT_MAX);
 2814         umtxq_unlock(&key);
 2815 
 2816         error = suword32(&cv->c_has_waiters, 0);
 2817         if (error == -1)
 2818                 error = EFAULT;
 2819 
 2820         umtxq_unbusy_unlocked(&key);
 2821 
 2822         umtx_key_release(&key);
 2823         return (error);
 2824 }
 2825 
 2826 static int
 2827 do_rw_rdlock(struct thread *td, struct urwlock *rwlock, long fflag, struct _umtx_time *timeout)
 2828 {
 2829         struct abs_timeout timo;
 2830         struct umtx_q *uq;
 2831         uint32_t flags, wrflags;
 2832         int32_t state, oldstate;
 2833         int32_t blocked_readers;
 2834         int error, error1, rv;
 2835 
 2836         uq = td->td_umtxq;
 2837         error = fueword32(&rwlock->rw_flags, &flags);
 2838         if (error == -1)
 2839                 return (EFAULT);
 2840         error = umtx_key_get(rwlock, TYPE_RWLOCK, GET_SHARE(flags), &uq->uq_key);
 2841         if (error != 0)
 2842                 return (error);
 2843 
 2844         if (timeout != NULL)
 2845                 abs_timeout_init2(&timo, timeout);
 2846 
 2847         wrflags = URWLOCK_WRITE_OWNER;
 2848         if (!(fflag & URWLOCK_PREFER_READER) && !(flags & URWLOCK_PREFER_READER))
 2849                 wrflags |= URWLOCK_WRITE_WAITERS;
 2850 
 2851         for (;;) {
 2852                 rv = fueword32(&rwlock->rw_state, &state);
 2853                 if (rv == -1) {
 2854                         umtx_key_release(&uq->uq_key);
 2855                         return (EFAULT);
 2856                 }
 2857 
 2858                 /* try to lock it */
 2859                 while (!(state & wrflags)) {
 2860                         if (__predict_false(URWLOCK_READER_COUNT(state) == URWLOCK_MAX_READERS)) {
 2861                                 umtx_key_release(&uq->uq_key);
 2862                                 return (EAGAIN);
 2863                         }
 2864                         rv = casueword32(&rwlock->rw_state, state,
 2865                             &oldstate, state + 1);
 2866                         if (rv == -1) {
 2867                                 umtx_key_release(&uq->uq_key);
 2868                                 return (EFAULT);
 2869                         }
 2870                         if (oldstate == state) {
 2871                                 umtx_key_release(&uq->uq_key);
 2872                                 return (0);
 2873                         }
 2874                         error = umtxq_check_susp(td);
 2875                         if (error != 0)
 2876                                 break;
 2877                         state = oldstate;
 2878                 }
 2879 
 2880                 if (error)
 2881                         break;
 2882 
 2883                 /* grab monitor lock */
 2884                 umtxq_lock(&uq->uq_key);
 2885                 umtxq_busy(&uq->uq_key);
 2886                 umtxq_unlock(&uq->uq_key);
 2887 
 2888                 /*
 2889                  * re-read the state, in case it changed between the try-lock above
 2890                  * and the check below
 2891                  */
 2892                 rv = fueword32(&rwlock->rw_state, &state);
 2893                 if (rv == -1)
 2894                         error = EFAULT;
 2895 
 2896                 /* set read contention bit */
 2897                 while (error == 0 && (state & wrflags) &&
 2898                     !(state & URWLOCK_READ_WAITERS)) {
 2899                         rv = casueword32(&rwlock->rw_state, state,
 2900                             &oldstate, state | URWLOCK_READ_WAITERS);
 2901                         if (rv == -1) {
 2902                                 error = EFAULT;
 2903                                 break;
 2904                         }
 2905                         if (oldstate == state)
 2906                                 goto sleep;
 2907                         state = oldstate;
 2908                         error = umtxq_check_susp(td);
 2909                         if (error != 0)
 2910                                 break;
 2911                 }
 2912                 if (error != 0) {
 2913                         umtxq_unbusy_unlocked(&uq->uq_key);
 2914                         break;
 2915                 }
 2916 
 2917                 /* state is changed while setting flags, restart */
 2918                 if (!(state & wrflags)) {
 2919                         umtxq_unbusy_unlocked(&uq->uq_key);
 2920                         error = umtxq_check_susp(td);
 2921                         if (error != 0)
 2922                                 break;
 2923                         continue;
 2924                 }
 2925 
 2926 sleep:
 2927                 /* contention bit is set, before sleeping, increase read waiter count */
 2928                 rv = fueword32(&rwlock->rw_blocked_readers,
 2929                     &blocked_readers);
 2930                 if (rv == -1) {
 2931                         umtxq_unbusy_unlocked(&uq->uq_key);
 2932                         error = EFAULT;
 2933                         break;
 2934                 }
 2935                 suword32(&rwlock->rw_blocked_readers, blocked_readers+1);
 2936 
 2937                 while (state & wrflags) {
 2938                         umtxq_lock(&uq->uq_key);
 2939                         umtxq_insert(uq);
 2940                         umtxq_unbusy(&uq->uq_key);
 2941 
 2942                         error = umtxq_sleep(uq, "urdlck", timeout == NULL ?
 2943                             NULL : &timo);
 2944 
 2945                         umtxq_busy(&uq->uq_key);
 2946                         umtxq_remove(uq);
 2947                         umtxq_unlock(&uq->uq_key);
 2948                         if (error)
 2949                                 break;
 2950                         rv = fueword32(&rwlock->rw_state, &state);
 2951                         if (rv == -1) {
 2952                                 error = EFAULT;
 2953                                 break;
 2954                         }
 2955                 }
 2956 
 2957                 /* decrease read waiter count, and may clear read contention bit */
 2958                 rv = fueword32(&rwlock->rw_blocked_readers,
 2959                     &blocked_readers);
 2960                 if (rv == -1) {
 2961                         umtxq_unbusy_unlocked(&uq->uq_key);
 2962                         error = EFAULT;
 2963                         break;
 2964                 }
 2965                 suword32(&rwlock->rw_blocked_readers, blocked_readers-1);
 2966                 if (blocked_readers == 1) {
 2967                         rv = fueword32(&rwlock->rw_state, &state);
 2968                         if (rv == -1) {
 2969                                 umtxq_unbusy_unlocked(&uq->uq_key);
 2970                                 error = EFAULT;
 2971                                 break;
 2972                         }
 2973                         for (;;) {
 2974                                 rv = casueword32(&rwlock->rw_state, state,
 2975                                     &oldstate, state & ~URWLOCK_READ_WAITERS);
 2976                                 if (rv == -1) {
 2977                                         error = EFAULT;
 2978                                         break;
 2979                                 }
 2980                                 if (oldstate == state)
 2981                                         break;
 2982                                 state = oldstate;
 2983                                 error1 = umtxq_check_susp(td);
 2984                                 if (error1 != 0) {
 2985                                         if (error == 0)
 2986                                                 error = error1;
 2987                                         break;
 2988                                 }
 2989                         }
 2990                 }
 2991 
 2992                 umtxq_unbusy_unlocked(&uq->uq_key);
 2993                 if (error != 0)
 2994                         break;
 2995         }
 2996         umtx_key_release(&uq->uq_key);
 2997         if (error == ERESTART)
 2998                 error = EINTR;
 2999         return (error);
 3000 }
 3001 
 3002 static int
 3003 do_rw_wrlock(struct thread *td, struct urwlock *rwlock, struct _umtx_time *timeout)
 3004 {
 3005         struct abs_timeout timo;
 3006         struct umtx_q *uq;
 3007         uint32_t flags;
 3008         int32_t state, oldstate;
 3009         int32_t blocked_writers;
 3010         int32_t blocked_readers;
 3011         int error, error1, rv;
 3012 
 3013         uq = td->td_umtxq;
 3014         error = fueword32(&rwlock->rw_flags, &flags);
 3015         if (error == -1)
 3016                 return (EFAULT);
 3017         error = umtx_key_get(rwlock, TYPE_RWLOCK, GET_SHARE(flags), &uq->uq_key);
 3018         if (error != 0)
 3019                 return (error);
 3020 
 3021         if (timeout != NULL)
 3022                 abs_timeout_init2(&timo, timeout);
 3023 
 3024         blocked_readers = 0;
 3025         for (;;) {
 3026                 rv = fueword32(&rwlock->rw_state, &state);
 3027                 if (rv == -1) {
 3028                         umtx_key_release(&uq->uq_key);
 3029                         return (EFAULT);
 3030                 }
 3031                 while (!(state & URWLOCK_WRITE_OWNER) && URWLOCK_READER_COUNT(state) == 0) {
 3032                         rv = casueword32(&rwlock->rw_state, state,
 3033                             &oldstate, state | URWLOCK_WRITE_OWNER);
 3034                         if (rv == -1) {
 3035                                 umtx_key_release(&uq->uq_key);
 3036                                 return (EFAULT);
 3037                         }
 3038                         if (oldstate == state) {
 3039                                 umtx_key_release(&uq->uq_key);
 3040                                 return (0);
 3041                         }
 3042                         state = oldstate;
 3043                         error = umtxq_check_susp(td);
 3044                         if (error != 0)
 3045                                 break;
 3046                 }
 3047 
 3048                 if (error) {
 3049                         if (!(state & (URWLOCK_WRITE_OWNER|URWLOCK_WRITE_WAITERS)) &&
 3050                             blocked_readers != 0) {
 3051                                 umtxq_lock(&uq->uq_key);
 3052                                 umtxq_busy(&uq->uq_key);
 3053                                 umtxq_signal_queue(&uq->uq_key, INT_MAX, UMTX_SHARED_QUEUE);
 3054                                 umtxq_unbusy(&uq->uq_key);
 3055                                 umtxq_unlock(&uq->uq_key);
 3056                         }
 3057 
 3058                         break;
 3059                 }
 3060 
 3061                 /* grab monitor lock */
 3062                 umtxq_lock(&uq->uq_key);
 3063                 umtxq_busy(&uq->uq_key);
 3064                 umtxq_unlock(&uq->uq_key);
 3065 
 3066                 /*
 3067                  * re-read the state, in case it changed between the try-lock above
 3068                  * and the check below
 3069                  */
 3070                 rv = fueword32(&rwlock->rw_state, &state);
 3071                 if (rv == -1)
 3072                         error = EFAULT;
 3073 
 3074                 while (error == 0 && ((state & URWLOCK_WRITE_OWNER) ||
 3075                     URWLOCK_READER_COUNT(state) != 0) &&
 3076                     (state & URWLOCK_WRITE_WAITERS) == 0) {
 3077                         rv = casueword32(&rwlock->rw_state, state,
 3078                             &oldstate, state | URWLOCK_WRITE_WAITERS);
 3079                         if (rv == -1) {
 3080                                 error = EFAULT;
 3081                                 break;
 3082                         }
 3083                         if (oldstate == state)
 3084                                 goto sleep;
 3085                         state = oldstate;
 3086                         error = umtxq_check_susp(td);
 3087                         if (error != 0)
 3088                                 break;
 3089                 }
 3090                 if (error != 0) {
 3091                         umtxq_unbusy_unlocked(&uq->uq_key);
 3092                         break;
 3093                 }
 3094 
 3095                 if (!(state & URWLOCK_WRITE_OWNER) && URWLOCK_READER_COUNT(state) == 0) {
 3096                         umtxq_unbusy_unlocked(&uq->uq_key);
 3097                         error = umtxq_check_susp(td);
 3098                         if (error != 0)
 3099                                 break;
 3100                         continue;
 3101                 }
 3102 sleep:
 3103                 rv = fueword32(&rwlock->rw_blocked_writers,
 3104                     &blocked_writers);
 3105                 if (rv == -1) {
 3106                         umtxq_unbusy_unlocked(&uq->uq_key);
 3107                         error = EFAULT;
 3108                         break;
 3109                 }
 3110                 suword32(&rwlock->rw_blocked_writers, blocked_writers+1);
 3111 
 3112                 while ((state & URWLOCK_WRITE_OWNER) || URWLOCK_READER_COUNT(state) != 0) {
 3113                         umtxq_lock(&uq->uq_key);
 3114                         umtxq_insert_queue(uq, UMTX_EXCLUSIVE_QUEUE);
 3115                         umtxq_unbusy(&uq->uq_key);
 3116 
 3117                         error = umtxq_sleep(uq, "uwrlck", timeout == NULL ?
 3118                             NULL : &timo);
 3119 
 3120                         umtxq_busy(&uq->uq_key);
 3121                         umtxq_remove_queue(uq, UMTX_EXCLUSIVE_QUEUE);
 3122                         umtxq_unlock(&uq->uq_key);
 3123                         if (error)
 3124                                 break;
 3125                         rv = fueword32(&rwlock->rw_state, &state);
 3126                         if (rv == -1) {
 3127                                 error = EFAULT;
 3128                                 break;
 3129                         }
 3130                 }
 3131 
 3132                 rv = fueword32(&rwlock->rw_blocked_writers,
 3133                     &blocked_writers);
 3134                 if (rv == -1) {
 3135                         umtxq_unbusy_unlocked(&uq->uq_key);
 3136                         error = EFAULT;
 3137                         break;
 3138                 }
 3139                 suword32(&rwlock->rw_blocked_writers, blocked_writers-1);
 3140                 if (blocked_writers == 1) {
 3141                         rv = fueword32(&rwlock->rw_state, &state);
 3142                         if (rv == -1) {
 3143                                 umtxq_unbusy_unlocked(&uq->uq_key);
 3144                                 error = EFAULT;
 3145                                 break;
 3146                         }
 3147                         for (;;) {
 3148                                 rv = casueword32(&rwlock->rw_state, state,
 3149                                     &oldstate, state & ~URWLOCK_WRITE_WAITERS);
 3150                                 if (rv == -1) {
 3151                                         error = EFAULT;
 3152                                         break;
 3153                                 }
 3154                                 if (oldstate == state)
 3155                                         break;
 3156                                 state = oldstate;
 3157                                 error1 = umtxq_check_susp(td);
 3158                                 /*
 3159                                  * We are leaving the URWLOCK_WRITE_WAITERS
 3160                                  * behind, but this should not harm the
 3161                                  * correctness.
 3162                                  */
 3163                                 if (error1 != 0) {
 3164                                         if (error == 0)
 3165                                                 error = error1;
 3166                                         break;
 3167                                 }
 3168                         }
 3169                         rv = fueword32(&rwlock->rw_blocked_readers,
 3170                             &blocked_readers);
 3171                         if (rv == -1) {
 3172                                 umtxq_unbusy_unlocked(&uq->uq_key);
 3173                                 error = EFAULT;
 3174                                 break;
 3175                         }
 3176                 } else
 3177                         blocked_readers = 0;
 3178 
 3179                 umtxq_unbusy_unlocked(&uq->uq_key);
 3180         }
 3181 
 3182         umtx_key_release(&uq->uq_key);
 3183         if (error == ERESTART)
 3184                 error = EINTR;
 3185         return (error);
 3186 }
 3187 
 3188 static int
 3189 do_rw_unlock(struct thread *td, struct urwlock *rwlock)
 3190 {
 3191         struct umtx_q *uq;
 3192         uint32_t flags;
 3193         int32_t state, oldstate;
 3194         int error, rv, q, count;
 3195 
 3196         uq = td->td_umtxq;
 3197         error = fueword32(&rwlock->rw_flags, &flags);
 3198         if (error == -1)
 3199                 return (EFAULT);
 3200         error = umtx_key_get(rwlock, TYPE_RWLOCK, GET_SHARE(flags), &uq->uq_key);
 3201         if (error != 0)
 3202                 return (error);
 3203 
 3204         error = fueword32(&rwlock->rw_state, &state);
 3205         if (error == -1) {
 3206                 error = EFAULT;
 3207                 goto out;
 3208         }
 3209         if (state & URWLOCK_WRITE_OWNER) {
 3210                 for (;;) {
 3211                         rv = casueword32(&rwlock->rw_state, state, 
 3212                             &oldstate, state & ~URWLOCK_WRITE_OWNER);
 3213                         if (rv == -1) {
 3214                                 error = EFAULT;
 3215                                 goto out;
 3216                         }
 3217                         if (oldstate != state) {
 3218                                 state = oldstate;
 3219                                 if (!(oldstate & URWLOCK_WRITE_OWNER)) {
 3220                                         error = EPERM;
 3221                                         goto out;
 3222                                 }
 3223                                 error = umtxq_check_susp(td);
 3224                                 if (error != 0)
 3225                                         goto out;
 3226                         } else
 3227                                 break;
 3228                 }
 3229         } else if (URWLOCK_READER_COUNT(state) != 0) {
 3230                 for (;;) {
 3231                         rv = casueword32(&rwlock->rw_state, state,
 3232                             &oldstate, state - 1);
 3233                         if (rv == -1) {
 3234                                 error = EFAULT;
 3235                                 goto out;
 3236                         }
 3237                         if (oldstate != state) {
 3238                                 state = oldstate;
 3239                                 if (URWLOCK_READER_COUNT(oldstate) == 0) {
 3240                                         error = EPERM;
 3241                                         goto out;
 3242                                 }
 3243                                 error = umtxq_check_susp(td);
 3244                                 if (error != 0)
 3245                                         goto out;
 3246                         } else
 3247                                 break;
 3248                 }
 3249         } else {
 3250                 error = EPERM;
 3251                 goto out;
 3252         }
 3253 
 3254         count = 0;
 3255 
 3256         if (!(flags & URWLOCK_PREFER_READER)) {
 3257                 if (state & URWLOCK_WRITE_WAITERS) {
 3258                         count = 1;
 3259                         q = UMTX_EXCLUSIVE_QUEUE;
 3260                 } else if (state & URWLOCK_READ_WAITERS) {
 3261                         count = INT_MAX;
 3262                         q = UMTX_SHARED_QUEUE;
 3263                 }
 3264         } else {
 3265                 if (state & URWLOCK_READ_WAITERS) {
 3266                         count = INT_MAX;
 3267                         q = UMTX_SHARED_QUEUE;
 3268                 } else if (state & URWLOCK_WRITE_WAITERS) {
 3269                         count = 1;
 3270                         q = UMTX_EXCLUSIVE_QUEUE;
 3271                 }
 3272         }
 3273 
 3274         if (count) {
 3275                 umtxq_lock(&uq->uq_key);
 3276                 umtxq_busy(&uq->uq_key);
 3277                 umtxq_signal_queue(&uq->uq_key, count, q);
 3278                 umtxq_unbusy(&uq->uq_key);
 3279                 umtxq_unlock(&uq->uq_key);
 3280         }
 3281 out:
 3282         umtx_key_release(&uq->uq_key);
 3283         return (error);
 3284 }
 3285 
 3286 static int
 3287 do_sem_wait(struct thread *td, struct _usem *sem, struct _umtx_time *timeout)
 3288 {
 3289         struct abs_timeout timo;
 3290         struct umtx_q *uq;
 3291         uint32_t flags, count, count1;
 3292         int error, rv;
 3293 
 3294         uq = td->td_umtxq;
 3295         error = fueword32(&sem->_flags, &flags);
 3296         if (error == -1)
 3297                 return (EFAULT);
 3298         error = umtx_key_get(sem, TYPE_SEM, GET_SHARE(flags), &uq->uq_key);
 3299         if (error != 0)
 3300                 return (error);
 3301 
 3302         if (timeout != NULL)
 3303                 abs_timeout_init2(&timo, timeout);
 3304 
 3305         umtxq_lock(&uq->uq_key);
 3306         umtxq_busy(&uq->uq_key);
 3307         umtxq_insert(uq);
 3308         umtxq_unlock(&uq->uq_key);
 3309         rv = casueword32(&sem->_has_waiters, 0, &count1, 1);
 3310         if (rv == 0)
 3311                 rv = fueword32(&sem->_count, &count);
 3312         if (rv == -1 || count != 0) {
 3313                 umtxq_lock(&uq->uq_key);
 3314                 umtxq_unbusy(&uq->uq_key);
 3315                 umtxq_remove(uq);
 3316                 umtxq_unlock(&uq->uq_key);
 3317                 umtx_key_release(&uq->uq_key);
 3318                 return (rv == -1 ? EFAULT : 0);
 3319         }
 3320         umtxq_lock(&uq->uq_key);
 3321         umtxq_unbusy(&uq->uq_key);
 3322 
 3323         error = umtxq_sleep(uq, "usem", timeout == NULL ? NULL : &timo);
 3324 
 3325         if ((uq->uq_flags & UQF_UMTXQ) == 0)
 3326                 error = 0;
 3327         else {
 3328                 umtxq_remove(uq);
 3329                 /* A relative timeout cannot be restarted. */
 3330                 if (error == ERESTART && timeout != NULL &&
 3331                     (timeout->_flags & UMTX_ABSTIME) == 0)
 3332                         error = EINTR;
 3333         }
 3334         umtxq_unlock(&uq->uq_key);
 3335         umtx_key_release(&uq->uq_key);
 3336         return (error);
 3337 }
 3338 
 3339 /*
 3340  * Signal a userland condition variable.
 3341  */
 3342 static int
 3343 do_sem_wake(struct thread *td, struct _usem *sem)
 3344 {
 3345         struct umtx_key key;
 3346         int error, cnt;
 3347         uint32_t flags;
 3348 
 3349         error = fueword32(&sem->_flags, &flags);
 3350         if (error == -1)
 3351                 return (EFAULT);
 3352         if ((error = umtx_key_get(sem, TYPE_SEM, GET_SHARE(flags), &key)) != 0)
 3353                 return (error); 
 3354         umtxq_lock(&key);
 3355         umtxq_busy(&key);
 3356         cnt = umtxq_count(&key);
 3357         if (cnt > 0) {
 3358                 /*
 3359                  * Check if count is greater than 0, this means the memory is
 3360                  * still being referenced by user code, so we can safely
 3361                  * update _has_waiters flag.
 3362                  */
 3363                 if (cnt == 1) {
 3364                         umtxq_unlock(&key);
 3365                         error = suword32(&sem->_has_waiters, 0);
 3366                         umtxq_lock(&key);
 3367                         if (error == -1)
 3368                                 error = EFAULT;
 3369                 }
 3370                 umtxq_signal(&key, 1);
 3371         }
 3372         umtxq_unbusy(&key);
 3373         umtxq_unlock(&key);
 3374         umtx_key_release(&key);
 3375         return (error);
 3376 }
 3377 
 3378 int
 3379 sys__umtx_lock(struct thread *td, struct _umtx_lock_args *uap)
 3380     /* struct umtx *umtx */
 3381 {
 3382         return do_lock_umtx(td, uap->umtx, td->td_tid, 0);
 3383 }
 3384 
 3385 int
 3386 sys__umtx_unlock(struct thread *td, struct _umtx_unlock_args *uap)
 3387     /* struct umtx *umtx */
 3388 {
 3389         return do_unlock_umtx(td, uap->umtx, td->td_tid);
 3390 }
 3391 
 3392 inline int
 3393 umtx_copyin_timeout(const void *addr, struct timespec *tsp)
 3394 {
 3395         int error;
 3396 
 3397         error = copyin(addr, tsp, sizeof(struct timespec));
 3398         if (error == 0) {
 3399                 if (tsp->tv_sec < 0 ||
 3400                     tsp->tv_nsec >= 1000000000 ||
 3401                     tsp->tv_nsec < 0)
 3402                         error = EINVAL;
 3403         }
 3404         return (error);
 3405 }
 3406 
 3407 static inline int
 3408 umtx_copyin_umtx_time(const void *addr, size_t size, struct _umtx_time *tp)
 3409 {
 3410         int error;
 3411         
 3412         if (size <= sizeof(struct timespec)) {
 3413                 tp->_clockid = CLOCK_REALTIME;
 3414                 tp->_flags = 0;
 3415                 error = copyin(addr, &tp->_timeout, sizeof(struct timespec));
 3416         } else 
 3417                 error = copyin(addr, tp, sizeof(struct _umtx_time));
 3418         if (error != 0)
 3419                 return (error);
 3420         if (tp->_timeout.tv_sec < 0 ||
 3421             tp->_timeout.tv_nsec >= 1000000000 || tp->_timeout.tv_nsec < 0)
 3422                 return (EINVAL);
 3423         return (0);
 3424 }
 3425 
 3426 static int
 3427 __umtx_op_lock_umtx(struct thread *td, struct _umtx_op_args *uap)
 3428 {
 3429         struct timespec *ts, timeout;
 3430         int error;
 3431 
 3432         /* Allow a null timespec (wait forever). */
 3433         if (uap->uaddr2 == NULL)
 3434                 ts = NULL;
 3435         else {
 3436                 error = umtx_copyin_timeout(uap->uaddr2, &timeout);
 3437                 if (error != 0)
 3438                         return (error);
 3439                 ts = &timeout;
 3440         }
 3441         return (do_lock_umtx(td, uap->obj, uap->val, ts));
 3442 }
 3443 
 3444 static int
 3445 __umtx_op_unlock_umtx(struct thread *td, struct _umtx_op_args *uap)
 3446 {
 3447         return (do_unlock_umtx(td, uap->obj, uap->val));
 3448 }
 3449 
 3450 static int
 3451 __umtx_op_wait(struct thread *td, struct _umtx_op_args *uap)
 3452 {
 3453         struct _umtx_time timeout, *tm_p;
 3454         int error;
 3455 
 3456         if (uap->uaddr2 == NULL)
 3457                 tm_p = NULL;
 3458         else {
 3459                 error = umtx_copyin_umtx_time(
 3460                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3461                 if (error != 0)
 3462                         return (error);
 3463                 tm_p = &timeout;
 3464         }
 3465         return do_wait(td, uap->obj, uap->val, tm_p, 0, 0);
 3466 }
 3467 
 3468 static int
 3469 __umtx_op_wait_uint(struct thread *td, struct _umtx_op_args *uap)
 3470 {
 3471         struct _umtx_time timeout, *tm_p;
 3472         int error;
 3473 
 3474         if (uap->uaddr2 == NULL)
 3475                 tm_p = NULL;
 3476         else {
 3477                 error = umtx_copyin_umtx_time(
 3478                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3479                 if (error != 0)
 3480                         return (error);
 3481                 tm_p = &timeout;
 3482         }
 3483         return do_wait(td, uap->obj, uap->val, tm_p, 1, 0);
 3484 }
 3485 
 3486 static int
 3487 __umtx_op_wait_uint_private(struct thread *td, struct _umtx_op_args *uap)
 3488 {
 3489         struct _umtx_time *tm_p, timeout;
 3490         int error;
 3491 
 3492         if (uap->uaddr2 == NULL)
 3493                 tm_p = NULL;
 3494         else {
 3495                 error = umtx_copyin_umtx_time(
 3496                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3497                 if (error != 0)
 3498                         return (error);
 3499                 tm_p = &timeout;
 3500         }
 3501         return do_wait(td, uap->obj, uap->val, tm_p, 1, 1);
 3502 }
 3503 
 3504 static int
 3505 __umtx_op_wake(struct thread *td, struct _umtx_op_args *uap)
 3506 {
 3507         return (kern_umtx_wake(td, uap->obj, uap->val, 0));
 3508 }
 3509 
 3510 #define BATCH_SIZE      128
 3511 static int
 3512 __umtx_op_nwake_private(struct thread *td, struct _umtx_op_args *uap)
 3513 {
 3514         int count = uap->val;
 3515         void *uaddrs[BATCH_SIZE];
 3516         char **upp = (char **)uap->obj;
 3517         int tocopy;
 3518         int error = 0;
 3519         int i, pos = 0;
 3520 
 3521         while (count > 0) {
 3522                 tocopy = count;
 3523                 if (tocopy > BATCH_SIZE)
 3524                         tocopy = BATCH_SIZE;
 3525                 error = copyin(upp+pos, uaddrs, tocopy * sizeof(char *));
 3526                 if (error != 0)
 3527                         break;
 3528                 for (i = 0; i < tocopy; ++i)
 3529                         kern_umtx_wake(td, uaddrs[i], INT_MAX, 1);
 3530                 count -= tocopy;
 3531                 pos += tocopy;
 3532         }
 3533         return (error);
 3534 }
 3535 
 3536 static int
 3537 __umtx_op_wake_private(struct thread *td, struct _umtx_op_args *uap)
 3538 {
 3539         return (kern_umtx_wake(td, uap->obj, uap->val, 1));
 3540 }
 3541 
 3542 static int
 3543 __umtx_op_lock_umutex(struct thread *td, struct _umtx_op_args *uap)
 3544 {
 3545         struct _umtx_time *tm_p, timeout;
 3546         int error;
 3547 
 3548         /* Allow a null timespec (wait forever). */
 3549         if (uap->uaddr2 == NULL)
 3550                 tm_p = NULL;
 3551         else {
 3552                 error = umtx_copyin_umtx_time(
 3553                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3554                 if (error != 0)
 3555                         return (error);
 3556                 tm_p = &timeout;
 3557         }
 3558         return do_lock_umutex(td, uap->obj, tm_p, 0);
 3559 }
 3560 
 3561 static int
 3562 __umtx_op_trylock_umutex(struct thread *td, struct _umtx_op_args *uap)
 3563 {
 3564         return do_lock_umutex(td, uap->obj, NULL, _UMUTEX_TRY);
 3565 }
 3566 
 3567 static int
 3568 __umtx_op_wait_umutex(struct thread *td, struct _umtx_op_args *uap)
 3569 {
 3570         struct _umtx_time *tm_p, timeout;
 3571         int error;
 3572 
 3573         /* Allow a null timespec (wait forever). */
 3574         if (uap->uaddr2 == NULL)
 3575                 tm_p = NULL;
 3576         else {
 3577                 error = umtx_copyin_umtx_time(
 3578                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3579                 if (error != 0)
 3580                         return (error);
 3581                 tm_p = &timeout;
 3582         }
 3583         return do_lock_umutex(td, uap->obj, tm_p, _UMUTEX_WAIT);
 3584 }
 3585 
 3586 static int
 3587 __umtx_op_wake_umutex(struct thread *td, struct _umtx_op_args *uap)
 3588 {
 3589         return do_wake_umutex(td, uap->obj);
 3590 }
 3591 
 3592 static int
 3593 __umtx_op_unlock_umutex(struct thread *td, struct _umtx_op_args *uap)
 3594 {
 3595         return do_unlock_umutex(td, uap->obj);
 3596 }
 3597 
 3598 static int
 3599 __umtx_op_set_ceiling(struct thread *td, struct _umtx_op_args *uap)
 3600 {
 3601         return do_set_ceiling(td, uap->obj, uap->val, uap->uaddr1);
 3602 }
 3603 
 3604 static int
 3605 __umtx_op_cv_wait(struct thread *td, struct _umtx_op_args *uap)
 3606 {
 3607         struct timespec *ts, timeout;
 3608         int error;
 3609 
 3610         /* Allow a null timespec (wait forever). */
 3611         if (uap->uaddr2 == NULL)
 3612                 ts = NULL;
 3613         else {
 3614                 error = umtx_copyin_timeout(uap->uaddr2, &timeout);
 3615                 if (error != 0)
 3616                         return (error);
 3617                 ts = &timeout;
 3618         }
 3619         return (do_cv_wait(td, uap->obj, uap->uaddr1, ts, uap->val));
 3620 }
 3621 
 3622 static int
 3623 __umtx_op_cv_signal(struct thread *td, struct _umtx_op_args *uap)
 3624 {
 3625         return do_cv_signal(td, uap->obj);
 3626 }
 3627 
 3628 static int
 3629 __umtx_op_cv_broadcast(struct thread *td, struct _umtx_op_args *uap)
 3630 {
 3631         return do_cv_broadcast(td, uap->obj);
 3632 }
 3633 
 3634 static int
 3635 __umtx_op_rw_rdlock(struct thread *td, struct _umtx_op_args *uap)
 3636 {
 3637         struct _umtx_time timeout;
 3638         int error;
 3639 
 3640         /* Allow a null timespec (wait forever). */
 3641         if (uap->uaddr2 == NULL) {
 3642                 error = do_rw_rdlock(td, uap->obj, uap->val, 0);
 3643         } else {
 3644                 error = umtx_copyin_umtx_time(uap->uaddr2,
 3645                    (size_t)uap->uaddr1, &timeout);
 3646                 if (error != 0)
 3647                         return (error);
 3648                 error = do_rw_rdlock(td, uap->obj, uap->val, &timeout);
 3649         }
 3650         return (error);
 3651 }
 3652 
 3653 static int
 3654 __umtx_op_rw_wrlock(struct thread *td, struct _umtx_op_args *uap)
 3655 {
 3656         struct _umtx_time timeout;
 3657         int error;
 3658 
 3659         /* Allow a null timespec (wait forever). */
 3660         if (uap->uaddr2 == NULL) {
 3661                 error = do_rw_wrlock(td, uap->obj, 0);
 3662         } else {
 3663                 error = umtx_copyin_umtx_time(uap->uaddr2, 
 3664                    (size_t)uap->uaddr1, &timeout);
 3665                 if (error != 0)
 3666                         return (error);
 3667 
 3668                 error = do_rw_wrlock(td, uap->obj, &timeout);
 3669         }
 3670         return (error);
 3671 }
 3672 
 3673 static int
 3674 __umtx_op_rw_unlock(struct thread *td, struct _umtx_op_args *uap)
 3675 {
 3676         return do_rw_unlock(td, uap->obj);
 3677 }
 3678 
 3679 static int
 3680 __umtx_op_sem_wait(struct thread *td, struct _umtx_op_args *uap)
 3681 {
 3682         struct _umtx_time *tm_p, timeout;
 3683         int error;
 3684 
 3685         /* Allow a null timespec (wait forever). */
 3686         if (uap->uaddr2 == NULL)
 3687                 tm_p = NULL;
 3688         else {
 3689                 error = umtx_copyin_umtx_time(
 3690                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3691                 if (error != 0)
 3692                         return (error);
 3693                 tm_p = &timeout;
 3694         }
 3695         return (do_sem_wait(td, uap->obj, tm_p));
 3696 }
 3697 
 3698 static int
 3699 __umtx_op_sem_wake(struct thread *td, struct _umtx_op_args *uap)
 3700 {
 3701         return do_sem_wake(td, uap->obj);
 3702 }
 3703 
 3704 static int
 3705 __umtx_op_wake2_umutex(struct thread *td, struct _umtx_op_args *uap)
 3706 {
 3707         return do_wake2_umutex(td, uap->obj, uap->val);
 3708 }
 3709 
 3710 typedef int (*_umtx_op_func)(struct thread *td, struct _umtx_op_args *uap);
 3711 
 3712 static _umtx_op_func op_table[] = {
 3713         __umtx_op_lock_umtx,            /* UMTX_OP_LOCK */
 3714         __umtx_op_unlock_umtx,          /* UMTX_OP_UNLOCK */
 3715         __umtx_op_wait,                 /* UMTX_OP_WAIT */
 3716         __umtx_op_wake,                 /* UMTX_OP_WAKE */
 3717         __umtx_op_trylock_umutex,       /* UMTX_OP_MUTEX_TRYLOCK */
 3718         __umtx_op_lock_umutex,          /* UMTX_OP_MUTEX_LOCK */
 3719         __umtx_op_unlock_umutex,        /* UMTX_OP_MUTEX_UNLOCK */
 3720         __umtx_op_set_ceiling,          /* UMTX_OP_SET_CEILING */
 3721         __umtx_op_cv_wait,              /* UMTX_OP_CV_WAIT*/
 3722         __umtx_op_cv_signal,            /* UMTX_OP_CV_SIGNAL */
 3723         __umtx_op_cv_broadcast,         /* UMTX_OP_CV_BROADCAST */
 3724         __umtx_op_wait_uint,            /* UMTX_OP_WAIT_UINT */
 3725         __umtx_op_rw_rdlock,            /* UMTX_OP_RW_RDLOCK */
 3726         __umtx_op_rw_wrlock,            /* UMTX_OP_RW_WRLOCK */
 3727         __umtx_op_rw_unlock,            /* UMTX_OP_RW_UNLOCK */
 3728         __umtx_op_wait_uint_private,    /* UMTX_OP_WAIT_UINT_PRIVATE */
 3729         __umtx_op_wake_private,         /* UMTX_OP_WAKE_PRIVATE */
 3730         __umtx_op_wait_umutex,          /* UMTX_OP_UMUTEX_WAIT */
 3731         __umtx_op_wake_umutex,          /* UMTX_OP_UMUTEX_WAKE */
 3732         __umtx_op_sem_wait,             /* UMTX_OP_SEM_WAIT */
 3733         __umtx_op_sem_wake,             /* UMTX_OP_SEM_WAKE */
 3734         __umtx_op_nwake_private,        /* UMTX_OP_NWAKE_PRIVATE */
 3735         __umtx_op_wake2_umutex          /* UMTX_OP_UMUTEX_WAKE2 */
 3736 };
 3737 
 3738 int
 3739 sys__umtx_op(struct thread *td, struct _umtx_op_args *uap)
 3740 {
 3741         if ((unsigned)uap->op < UMTX_OP_MAX)
 3742                 return (*op_table[uap->op])(td, uap);
 3743         return (EINVAL);
 3744 }
 3745 
 3746 #ifdef COMPAT_FREEBSD32
 3747 int
 3748 freebsd32_umtx_lock(struct thread *td, struct freebsd32_umtx_lock_args *uap)
 3749     /* struct umtx *umtx */
 3750 {
 3751         return (do_lock_umtx32(td, (uint32_t *)uap->umtx, td->td_tid, NULL));
 3752 }
 3753 
 3754 int
 3755 freebsd32_umtx_unlock(struct thread *td, struct freebsd32_umtx_unlock_args *uap)
 3756     /* struct umtx *umtx */
 3757 {
 3758         return (do_unlock_umtx32(td, (uint32_t *)uap->umtx, td->td_tid));
 3759 }
 3760 
 3761 struct timespec32 {
 3762         int32_t tv_sec;
 3763         int32_t tv_nsec;
 3764 };
 3765 
 3766 struct umtx_time32 {
 3767         struct  timespec32      timeout;
 3768         uint32_t                flags;
 3769         uint32_t                clockid;
 3770 };
 3771 
 3772 static inline int
 3773 umtx_copyin_timeout32(void *addr, struct timespec *tsp)
 3774 {
 3775         struct timespec32 ts32;
 3776         int error;
 3777 
 3778         error = copyin(addr, &ts32, sizeof(struct timespec32));
 3779         if (error == 0) {
 3780                 if (ts32.tv_sec < 0 ||
 3781                     ts32.tv_nsec >= 1000000000 ||
 3782                     ts32.tv_nsec < 0)
 3783                         error = EINVAL;
 3784                 else {
 3785                         tsp->tv_sec = ts32.tv_sec;
 3786                         tsp->tv_nsec = ts32.tv_nsec;
 3787                 }
 3788         }
 3789         return (error);
 3790 }
 3791 
 3792 static inline int
 3793 umtx_copyin_umtx_time32(const void *addr, size_t size, struct _umtx_time *tp)
 3794 {
 3795         struct umtx_time32 t32;
 3796         int error;
 3797         
 3798         t32.clockid = CLOCK_REALTIME;
 3799         t32.flags   = 0;
 3800         if (size <= sizeof(struct timespec32))
 3801                 error = copyin(addr, &t32.timeout, sizeof(struct timespec32));
 3802         else 
 3803                 error = copyin(addr, &t32, sizeof(struct umtx_time32));
 3804         if (error != 0)
 3805                 return (error);
 3806         if (t32.timeout.tv_sec < 0 ||
 3807             t32.timeout.tv_nsec >= 1000000000 || t32.timeout.tv_nsec < 0)
 3808                 return (EINVAL);
 3809         tp->_timeout.tv_sec = t32.timeout.tv_sec;
 3810         tp->_timeout.tv_nsec = t32.timeout.tv_nsec;
 3811         tp->_flags = t32.flags;
 3812         tp->_clockid = t32.clockid;
 3813         return (0);
 3814 }
 3815 
 3816 static int
 3817 __umtx_op_lock_umtx_compat32(struct thread *td, struct _umtx_op_args *uap)
 3818 {
 3819         struct timespec *ts, timeout;
 3820         int error;
 3821 
 3822         /* Allow a null timespec (wait forever). */
 3823         if (uap->uaddr2 == NULL)
 3824                 ts = NULL;
 3825         else {
 3826                 error = umtx_copyin_timeout32(uap->uaddr2, &timeout);
 3827                 if (error != 0)
 3828                         return (error);
 3829                 ts = &timeout;
 3830         }
 3831         return (do_lock_umtx32(td, uap->obj, uap->val, ts));
 3832 }
 3833 
 3834 static int
 3835 __umtx_op_unlock_umtx_compat32(struct thread *td, struct _umtx_op_args *uap)
 3836 {
 3837         return (do_unlock_umtx32(td, uap->obj, (uint32_t)uap->val));
 3838 }
 3839 
 3840 static int
 3841 __umtx_op_wait_compat32(struct thread *td, struct _umtx_op_args *uap)
 3842 {
 3843         struct _umtx_time *tm_p, timeout;
 3844         int error;
 3845 
 3846         if (uap->uaddr2 == NULL)
 3847                 tm_p = NULL;
 3848         else {
 3849                 error = umtx_copyin_umtx_time32(uap->uaddr2,
 3850                         (size_t)uap->uaddr1, &timeout);
 3851                 if (error != 0)
 3852                         return (error);
 3853                 tm_p = &timeout;
 3854         }
 3855         return do_wait(td, uap->obj, uap->val, tm_p, 1, 0);
 3856 }
 3857 
 3858 static int
 3859 __umtx_op_lock_umutex_compat32(struct thread *td, struct _umtx_op_args *uap)
 3860 {
 3861         struct _umtx_time *tm_p, timeout;
 3862         int error;
 3863 
 3864         /* Allow a null timespec (wait forever). */
 3865         if (uap->uaddr2 == NULL)
 3866                 tm_p = NULL;
 3867         else {
 3868                 error = umtx_copyin_umtx_time(uap->uaddr2,
 3869                             (size_t)uap->uaddr1, &timeout);
 3870                 if (error != 0)
 3871                         return (error);
 3872                 tm_p = &timeout;
 3873         }
 3874         return do_lock_umutex(td, uap->obj, tm_p, 0);
 3875 }
 3876 
 3877 static int
 3878 __umtx_op_wait_umutex_compat32(struct thread *td, struct _umtx_op_args *uap)
 3879 {
 3880         struct _umtx_time *tm_p, timeout;
 3881         int error;
 3882 
 3883         /* Allow a null timespec (wait forever). */
 3884         if (uap->uaddr2 == NULL)
 3885                 tm_p = NULL;
 3886         else {
 3887                 error = umtx_copyin_umtx_time32(uap->uaddr2, 
 3888                     (size_t)uap->uaddr1, &timeout);
 3889                 if (error != 0)
 3890                         return (error);
 3891                 tm_p = &timeout;
 3892         }
 3893         return do_lock_umutex(td, uap->obj, tm_p, _UMUTEX_WAIT);
 3894 }
 3895 
 3896 static int
 3897 __umtx_op_cv_wait_compat32(struct thread *td, struct _umtx_op_args *uap)
 3898 {
 3899         struct timespec *ts, timeout;
 3900         int error;
 3901 
 3902         /* Allow a null timespec (wait forever). */
 3903         if (uap->uaddr2 == NULL)
 3904                 ts = NULL;
 3905         else {
 3906                 error = umtx_copyin_timeout32(uap->uaddr2, &timeout);
 3907                 if (error != 0)
 3908                         return (error);
 3909                 ts = &timeout;
 3910         }
 3911         return (do_cv_wait(td, uap->obj, uap->uaddr1, ts, uap->val));
 3912 }
 3913 
 3914 static int
 3915 __umtx_op_rw_rdlock_compat32(struct thread *td, struct _umtx_op_args *uap)
 3916 {
 3917         struct _umtx_time timeout;
 3918         int error;
 3919 
 3920         /* Allow a null timespec (wait forever). */
 3921         if (uap->uaddr2 == NULL) {
 3922                 error = do_rw_rdlock(td, uap->obj, uap->val, 0);
 3923         } else {
 3924                 error = umtx_copyin_umtx_time32(uap->uaddr2,
 3925                     (size_t)uap->uaddr1, &timeout);
 3926                 if (error != 0)
 3927                         return (error);
 3928                 error = do_rw_rdlock(td, uap->obj, uap->val, &timeout);
 3929         }
 3930         return (error);
 3931 }
 3932 
 3933 static int
 3934 __umtx_op_rw_wrlock_compat32(struct thread *td, struct _umtx_op_args *uap)
 3935 {
 3936         struct _umtx_time timeout;
 3937         int error;
 3938 
 3939         /* Allow a null timespec (wait forever). */
 3940         if (uap->uaddr2 == NULL) {
 3941                 error = do_rw_wrlock(td, uap->obj, 0);
 3942         } else {
 3943                 error = umtx_copyin_umtx_time32(uap->uaddr2,
 3944                     (size_t)uap->uaddr1, &timeout);
 3945                 if (error != 0)
 3946                         return (error);
 3947                 error = do_rw_wrlock(td, uap->obj, &timeout);
 3948         }
 3949         return (error);
 3950 }
 3951 
 3952 static int
 3953 __umtx_op_wait_uint_private_compat32(struct thread *td, struct _umtx_op_args *uap)
 3954 {
 3955         struct _umtx_time *tm_p, timeout;
 3956         int error;
 3957 
 3958         if (uap->uaddr2 == NULL)
 3959                 tm_p = NULL;
 3960         else {
 3961                 error = umtx_copyin_umtx_time32(
 3962                     uap->uaddr2, (size_t)uap->uaddr1,&timeout);
 3963                 if (error != 0)
 3964                         return (error);
 3965                 tm_p = &timeout;
 3966         }
 3967         return do_wait(td, uap->obj, uap->val, tm_p, 1, 1);
 3968 }
 3969 
 3970 static int
 3971 __umtx_op_sem_wait_compat32(struct thread *td, struct _umtx_op_args *uap)
 3972 {
 3973         struct _umtx_time *tm_p, timeout;
 3974         int error;
 3975 
 3976         /* Allow a null timespec (wait forever). */
 3977         if (uap->uaddr2 == NULL)
 3978                 tm_p = NULL;
 3979         else {
 3980                 error = umtx_copyin_umtx_time32(uap->uaddr2,
 3981                     (size_t)uap->uaddr1, &timeout);
 3982                 if (error != 0)
 3983                         return (error);
 3984                 tm_p = &timeout;
 3985         }
 3986         return (do_sem_wait(td, uap->obj, tm_p));
 3987 }
 3988 
 3989 static int
 3990 __umtx_op_nwake_private32(struct thread *td, struct _umtx_op_args *uap)
 3991 {
 3992         int count = uap->val;
 3993         uint32_t uaddrs[BATCH_SIZE];
 3994         uint32_t **upp = (uint32_t **)uap->obj;
 3995         int tocopy;
 3996         int error = 0;
 3997         int i, pos = 0;
 3998 
 3999         while (count > 0) {
 4000                 tocopy = count;
 4001                 if (tocopy > BATCH_SIZE)
 4002                         tocopy = BATCH_SIZE;
 4003                 error = copyin(upp+pos, uaddrs, tocopy * sizeof(uint32_t));
 4004                 if (error != 0)
 4005                         break;
 4006                 for (i = 0; i < tocopy; ++i)
 4007                         kern_umtx_wake(td, (void *)(intptr_t)uaddrs[i],
 4008                                 INT_MAX, 1);
 4009                 count -= tocopy;
 4010                 pos += tocopy;
 4011         }
 4012         return (error);
 4013 }
 4014 
 4015 static _umtx_op_func op_table_compat32[] = {
 4016         __umtx_op_lock_umtx_compat32,   /* UMTX_OP_LOCK */
 4017         __umtx_op_unlock_umtx_compat32, /* UMTX_OP_UNLOCK */
 4018         __umtx_op_wait_compat32,        /* UMTX_OP_WAIT */
 4019         __umtx_op_wake,                 /* UMTX_OP_WAKE */
 4020         __umtx_op_trylock_umutex,       /* UMTX_OP_MUTEX_LOCK */
 4021         __umtx_op_lock_umutex_compat32, /* UMTX_OP_MUTEX_TRYLOCK */
 4022         __umtx_op_unlock_umutex,        /* UMTX_OP_MUTEX_UNLOCK */
 4023         __umtx_op_set_ceiling,          /* UMTX_OP_SET_CEILING */
 4024         __umtx_op_cv_wait_compat32,     /* UMTX_OP_CV_WAIT*/
 4025         __umtx_op_cv_signal,            /* UMTX_OP_CV_SIGNAL */
 4026         __umtx_op_cv_broadcast,         /* UMTX_OP_CV_BROADCAST */
 4027         __umtx_op_wait_compat32,        /* UMTX_OP_WAIT_UINT */
 4028         __umtx_op_rw_rdlock_compat32,   /* UMTX_OP_RW_RDLOCK */
 4029         __umtx_op_rw_wrlock_compat32,   /* UMTX_OP_RW_WRLOCK */
 4030         __umtx_op_rw_unlock,            /* UMTX_OP_RW_UNLOCK */
 4031         __umtx_op_wait_uint_private_compat32,   /* UMTX_OP_WAIT_UINT_PRIVATE */
 4032         __umtx_op_wake_private,         /* UMTX_OP_WAKE_PRIVATE */
 4033         __umtx_op_wait_umutex_compat32, /* UMTX_OP_UMUTEX_WAIT */
 4034         __umtx_op_wake_umutex,          /* UMTX_OP_UMUTEX_WAKE */
 4035         __umtx_op_sem_wait_compat32,    /* UMTX_OP_SEM_WAIT */
 4036         __umtx_op_sem_wake,             /* UMTX_OP_SEM_WAKE */
 4037         __umtx_op_nwake_private32,      /* UMTX_OP_NWAKE_PRIVATE */
 4038         __umtx_op_wake2_umutex          /* UMTX_OP_UMUTEX_WAKE2 */
 4039 };
 4040 
 4041 int
 4042 freebsd32_umtx_op(struct thread *td, struct freebsd32_umtx_op_args *uap)
 4043 {
 4044         if ((unsigned)uap->op < UMTX_OP_MAX)
 4045                 return (*op_table_compat32[uap->op])(td,
 4046                         (struct _umtx_op_args *)uap);
 4047         return (EINVAL);
 4048 }
 4049 #endif
 4050 
 4051 void
 4052 umtx_thread_init(struct thread *td)
 4053 {
 4054         td->td_umtxq = umtxq_alloc();
 4055         td->td_umtxq->uq_thread = td;
 4056 }
 4057 
 4058 void
 4059 umtx_thread_fini(struct thread *td)
 4060 {
 4061         umtxq_free(td->td_umtxq);
 4062 }
 4063 
 4064 /*
 4065  * It will be called when new thread is created, e.g fork().
 4066  */
 4067 void
 4068 umtx_thread_alloc(struct thread *td)
 4069 {
 4070         struct umtx_q *uq;
 4071 
 4072         uq = td->td_umtxq;
 4073         uq->uq_inherited_pri = PRI_MAX;
 4074 
 4075         KASSERT(uq->uq_flags == 0, ("uq_flags != 0"));
 4076         KASSERT(uq->uq_thread == td, ("uq_thread != td"));
 4077         KASSERT(uq->uq_pi_blocked == NULL, ("uq_pi_blocked != NULL"));
 4078         KASSERT(TAILQ_EMPTY(&uq->uq_pi_contested), ("uq_pi_contested is not empty"));
 4079 }
 4080 
 4081 /*
 4082  * exec() hook.
 4083  */
 4084 static void
 4085 umtx_exec_hook(void *arg __unused, struct proc *p __unused,
 4086         struct image_params *imgp __unused)
 4087 {
 4088         umtx_thread_cleanup(curthread);
 4089 }
 4090 
 4091 /*
 4092  * thread_exit() hook.
 4093  */
 4094 void
 4095 umtx_thread_exit(struct thread *td)
 4096 {
 4097         umtx_thread_cleanup(td);
 4098 }
 4099 
 4100 /*
 4101  * clean up umtx data.
 4102  */
 4103 static void
 4104 umtx_thread_cleanup(struct thread *td)
 4105 {
 4106         struct umtx_q *uq;
 4107         struct umtx_pi *pi;
 4108 
 4109         if ((uq = td->td_umtxq) == NULL)
 4110                 return;
 4111 
 4112         mtx_lock(&umtx_lock);
 4113         uq->uq_inherited_pri = PRI_MAX;
 4114         while ((pi = TAILQ_FIRST(&uq->uq_pi_contested)) != NULL) {
 4115                 pi->pi_owner = NULL;
 4116                 TAILQ_REMOVE(&uq->uq_pi_contested, pi, pi_link);
 4117         }
 4118         mtx_unlock(&umtx_lock);
 4119         thread_lock(td);
 4120         sched_lend_user_prio(td, PRI_MAX);
 4121         thread_unlock(td);
 4122 }

Cache object: f115ea023c1e072907573b2e24bd1e50


[ source navigation ] [ diff markup ] [ identifier search ] [ freetext search ] [ file search ] [ list types ] [ track identifier ]


This page is part of the FreeBSD/Linux Linux Kernel Cross-Reference, and was automatically generated using a modified version of the LXR engine.