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.2/sys/kern/kern_umtx.c 280309 2015-03-21 01:39:44Z 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         ret = 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_ower != 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, *uq_owner;
 1833 
 1834         uq_owner = owner->td_umtxq;
 1835         mtx_lock(&umtx_lock);
 1836         if (pi->pi_owner == owner) {
 1837                 mtx_unlock(&umtx_lock);
 1838                 return (0);
 1839         }
 1840 
 1841         if (pi->pi_owner != NULL) {
 1842                 /*
 1843                  * userland may have already messed the mutex, sigh.
 1844                  */
 1845                 mtx_unlock(&umtx_lock);
 1846                 return (EPERM);
 1847         }
 1848         umtx_pi_setowner(pi, owner);
 1849         uq = TAILQ_FIRST(&pi->pi_blocked);
 1850         if (uq != NULL) {
 1851                 int pri;
 1852 
 1853                 pri = UPRI(uq->uq_thread);
 1854                 thread_lock(owner);
 1855                 if (pri < UPRI(owner))
 1856                         sched_lend_user_prio(owner, pri);
 1857                 thread_unlock(owner);
 1858         }
 1859         mtx_unlock(&umtx_lock);
 1860         return (0);
 1861 }
 1862 
 1863 /*
 1864  * Adjust a thread's order position in its blocked PI mutex,
 1865  * this may result new priority propagating process.
 1866  */
 1867 void
 1868 umtx_pi_adjust(struct thread *td, u_char oldpri)
 1869 {
 1870         struct umtx_q *uq;
 1871         struct umtx_pi *pi;
 1872 
 1873         uq = td->td_umtxq;
 1874         mtx_lock(&umtx_lock);
 1875         /*
 1876          * Pick up the lock that td is blocked on.
 1877          */
 1878         pi = uq->uq_pi_blocked;
 1879         if (pi != NULL) {
 1880                 umtx_pi_adjust_thread(pi, td);
 1881                 umtx_repropagate_priority(pi);
 1882         }
 1883         mtx_unlock(&umtx_lock);
 1884 }
 1885 
 1886 /*
 1887  * Sleep on a PI mutex.
 1888  */
 1889 static int
 1890 umtxq_sleep_pi(struct umtx_q *uq, struct umtx_pi *pi,
 1891         uint32_t owner, const char *wmesg, struct abs_timeout *timo)
 1892 {
 1893         struct umtxq_chain *uc;
 1894         struct thread *td, *td1;
 1895         struct umtx_q *uq1;
 1896         int pri;
 1897         int error = 0;
 1898 
 1899         td = uq->uq_thread;
 1900         KASSERT(td == curthread, ("inconsistent uq_thread"));
 1901         uc = umtxq_getchain(&uq->uq_key);
 1902         UMTXQ_LOCKED_ASSERT(uc);
 1903         KASSERT(uc->uc_busy != 0, ("umtx chain is not busy"));
 1904         umtxq_insert(uq);
 1905         mtx_lock(&umtx_lock);
 1906         if (pi->pi_owner == NULL) {
 1907                 mtx_unlock(&umtx_lock);
 1908                 /* XXX Only look up thread in current process. */
 1909                 td1 = tdfind(owner, curproc->p_pid);
 1910                 mtx_lock(&umtx_lock);
 1911                 if (td1 != NULL) {
 1912                         if (pi->pi_owner == NULL)
 1913                                 umtx_pi_setowner(pi, td1);
 1914                         PROC_UNLOCK(td1->td_proc);
 1915                 }
 1916         }
 1917 
 1918         TAILQ_FOREACH(uq1, &pi->pi_blocked, uq_lockq) {
 1919                 pri = UPRI(uq1->uq_thread);
 1920                 if (pri > UPRI(td))
 1921                         break;
 1922         }
 1923 
 1924         if (uq1 != NULL)
 1925                 TAILQ_INSERT_BEFORE(uq1, uq, uq_lockq);
 1926         else
 1927                 TAILQ_INSERT_TAIL(&pi->pi_blocked, uq, uq_lockq);
 1928 
 1929         uq->uq_pi_blocked = pi;
 1930         thread_lock(td);
 1931         td->td_flags |= TDF_UPIBLOCKED;
 1932         thread_unlock(td);
 1933         umtx_propagate_priority(td);
 1934         mtx_unlock(&umtx_lock);
 1935         umtxq_unbusy(&uq->uq_key);
 1936 
 1937         error = umtxq_sleep(uq, wmesg, timo);
 1938         umtxq_remove(uq);
 1939 
 1940         mtx_lock(&umtx_lock);
 1941         uq->uq_pi_blocked = NULL;
 1942         thread_lock(td);
 1943         td->td_flags &= ~TDF_UPIBLOCKED;
 1944         thread_unlock(td);
 1945         TAILQ_REMOVE(&pi->pi_blocked, uq, uq_lockq);
 1946         umtx_repropagate_priority(pi);
 1947         mtx_unlock(&umtx_lock);
 1948         umtxq_unlock(&uq->uq_key);
 1949 
 1950         return (error);
 1951 }
 1952 
 1953 /*
 1954  * Add reference count for a PI mutex.
 1955  */
 1956 static void
 1957 umtx_pi_ref(struct umtx_pi *pi)
 1958 {
 1959         struct umtxq_chain *uc;
 1960 
 1961         uc = umtxq_getchain(&pi->pi_key);
 1962         UMTXQ_LOCKED_ASSERT(uc);
 1963         pi->pi_refcount++;
 1964 }
 1965 
 1966 /*
 1967  * Decrease reference count for a PI mutex, if the counter
 1968  * is decreased to zero, its memory space is freed.
 1969  */ 
 1970 static void
 1971 umtx_pi_unref(struct umtx_pi *pi)
 1972 {
 1973         struct umtxq_chain *uc;
 1974 
 1975         uc = umtxq_getchain(&pi->pi_key);
 1976         UMTXQ_LOCKED_ASSERT(uc);
 1977         KASSERT(pi->pi_refcount > 0, ("invalid reference count"));
 1978         if (--pi->pi_refcount == 0) {
 1979                 mtx_lock(&umtx_lock);
 1980                 if (pi->pi_owner != NULL) {
 1981                         TAILQ_REMOVE(&pi->pi_owner->td_umtxq->uq_pi_contested,
 1982                                 pi, pi_link);
 1983                         pi->pi_owner = NULL;
 1984                 }
 1985                 KASSERT(TAILQ_EMPTY(&pi->pi_blocked),
 1986                         ("blocked queue not empty"));
 1987                 mtx_unlock(&umtx_lock);
 1988                 TAILQ_REMOVE(&uc->uc_pi_list, pi, pi_hashlink);
 1989                 umtx_pi_free(pi);
 1990         }
 1991 }
 1992 
 1993 /*
 1994  * Find a PI mutex in hash table.
 1995  */
 1996 static struct umtx_pi *
 1997 umtx_pi_lookup(struct umtx_key *key)
 1998 {
 1999         struct umtxq_chain *uc;
 2000         struct umtx_pi *pi;
 2001 
 2002         uc = umtxq_getchain(key);
 2003         UMTXQ_LOCKED_ASSERT(uc);
 2004 
 2005         TAILQ_FOREACH(pi, &uc->uc_pi_list, pi_hashlink) {
 2006                 if (umtx_key_match(&pi->pi_key, key)) {
 2007                         return (pi);
 2008                 }
 2009         }
 2010         return (NULL);
 2011 }
 2012 
 2013 /*
 2014  * Insert a PI mutex into hash table.
 2015  */
 2016 static inline void
 2017 umtx_pi_insert(struct umtx_pi *pi)
 2018 {
 2019         struct umtxq_chain *uc;
 2020 
 2021         uc = umtxq_getchain(&pi->pi_key);
 2022         UMTXQ_LOCKED_ASSERT(uc);
 2023         TAILQ_INSERT_TAIL(&uc->uc_pi_list, pi, pi_hashlink);
 2024 }
 2025 
 2026 /*
 2027  * Lock a PI mutex.
 2028  */
 2029 static int
 2030 do_lock_pi(struct thread *td, struct umutex *m, uint32_t flags,
 2031     struct _umtx_time *timeout, int try)
 2032 {
 2033         struct abs_timeout timo;
 2034         struct umtx_q *uq;
 2035         struct umtx_pi *pi, *new_pi;
 2036         uint32_t id, owner, old;
 2037         int error, rv;
 2038 
 2039         id = td->td_tid;
 2040         uq = td->td_umtxq;
 2041 
 2042         if ((error = umtx_key_get(m, TYPE_PI_UMUTEX, GET_SHARE(flags),
 2043             &uq->uq_key)) != 0)
 2044                 return (error);
 2045 
 2046         if (timeout != NULL)
 2047                 abs_timeout_init2(&timo, timeout);
 2048 
 2049         umtxq_lock(&uq->uq_key);
 2050         pi = umtx_pi_lookup(&uq->uq_key);
 2051         if (pi == NULL) {
 2052                 new_pi = umtx_pi_alloc(M_NOWAIT);
 2053                 if (new_pi == NULL) {
 2054                         umtxq_unlock(&uq->uq_key);
 2055                         new_pi = umtx_pi_alloc(M_WAITOK);
 2056                         umtxq_lock(&uq->uq_key);
 2057                         pi = umtx_pi_lookup(&uq->uq_key);
 2058                         if (pi != NULL) {
 2059                                 umtx_pi_free(new_pi);
 2060                                 new_pi = NULL;
 2061                         }
 2062                 }
 2063                 if (new_pi != NULL) {
 2064                         new_pi->pi_key = uq->uq_key;
 2065                         umtx_pi_insert(new_pi);
 2066                         pi = new_pi;
 2067                 }
 2068         }
 2069         umtx_pi_ref(pi);
 2070         umtxq_unlock(&uq->uq_key);
 2071 
 2072         /*
 2073          * Care must be exercised when dealing with umtx structure.  It
 2074          * can fault on any access.
 2075          */
 2076         for (;;) {
 2077                 /*
 2078                  * Try the uncontested case.  This should be done in userland.
 2079                  */
 2080                 rv = casueword32(&m->m_owner, UMUTEX_UNOWNED, &owner, id);
 2081                 /* The address was invalid. */
 2082                 if (rv == -1) {
 2083                         error = EFAULT;
 2084                         break;
 2085                 }
 2086 
 2087                 /* The acquire succeeded. */
 2088                 if (owner == UMUTEX_UNOWNED) {
 2089                         error = 0;
 2090                         break;
 2091                 }
 2092 
 2093                 /* If no one owns it but it is contested try to acquire it. */
 2094                 if (owner == UMUTEX_CONTESTED) {
 2095                         rv = casueword32(&m->m_owner,
 2096                             UMUTEX_CONTESTED, &owner, id | UMUTEX_CONTESTED);
 2097                         /* The address was invalid. */
 2098                         if (rv == -1) {
 2099                                 error = EFAULT;
 2100                                 break;
 2101                         }
 2102 
 2103                         if (owner == UMUTEX_CONTESTED) {
 2104                                 umtxq_lock(&uq->uq_key);
 2105                                 umtxq_busy(&uq->uq_key);
 2106                                 error = umtx_pi_claim(pi, td);
 2107                                 umtxq_unbusy(&uq->uq_key);
 2108                                 umtxq_unlock(&uq->uq_key);
 2109                                 if (error != 0) {
 2110                                         /*
 2111                                          * Since we're going to return an
 2112                                          * error, restore the m_owner to its
 2113                                          * previous, unowned state to avoid
 2114                                          * compounding the problem.
 2115                                          */
 2116                                         (void)casuword32(&m->m_owner,
 2117                                             id | UMUTEX_CONTESTED,
 2118                                             UMUTEX_CONTESTED);
 2119                                 }
 2120                                 break;
 2121                         }
 2122 
 2123                         error = umtxq_check_susp(td);
 2124                         if (error != 0)
 2125                                 break;
 2126 
 2127                         /* If this failed the lock has changed, restart. */
 2128                         continue;
 2129                 }
 2130 
 2131                 if ((owner & ~UMUTEX_CONTESTED) == id) {
 2132                         error = EDEADLK;
 2133                         break;
 2134                 }
 2135 
 2136                 if (try != 0) {
 2137                         error = EBUSY;
 2138                         break;
 2139                 }
 2140 
 2141                 /*
 2142                  * If we caught a signal, we have retried and now
 2143                  * exit immediately.
 2144                  */
 2145                 if (error != 0)
 2146                         break;
 2147                         
 2148                 umtxq_lock(&uq->uq_key);
 2149                 umtxq_busy(&uq->uq_key);
 2150                 umtxq_unlock(&uq->uq_key);
 2151 
 2152                 /*
 2153                  * Set the contested bit so that a release in user space
 2154                  * knows to use the system call for unlock.  If this fails
 2155                  * either some one else has acquired the lock or it has been
 2156                  * released.
 2157                  */
 2158                 rv = casueword32(&m->m_owner, owner, &old,
 2159                     owner | UMUTEX_CONTESTED);
 2160 
 2161                 /* The address was invalid. */
 2162                 if (rv == -1) {
 2163                         umtxq_unbusy_unlocked(&uq->uq_key);
 2164                         error = EFAULT;
 2165                         break;
 2166                 }
 2167 
 2168                 umtxq_lock(&uq->uq_key);
 2169                 /*
 2170                  * We set the contested bit, sleep. Otherwise the lock changed
 2171                  * and we need to retry or we lost a race to the thread
 2172                  * unlocking the umtx.
 2173                  */
 2174                 if (old == owner) {
 2175                         error = umtxq_sleep_pi(uq, pi, owner & ~UMUTEX_CONTESTED,
 2176                             "umtxpi", timeout == NULL ? NULL : &timo);
 2177                         if (error != 0)
 2178                                 continue;
 2179                 } else {
 2180                         umtxq_unbusy(&uq->uq_key);
 2181                         umtxq_unlock(&uq->uq_key);
 2182                 }
 2183 
 2184                 error = umtxq_check_susp(td);
 2185                 if (error != 0)
 2186                         break;
 2187         }
 2188 
 2189         umtxq_lock(&uq->uq_key);
 2190         umtx_pi_unref(pi);
 2191         umtxq_unlock(&uq->uq_key);
 2192 
 2193         umtx_key_release(&uq->uq_key);
 2194         return (error);
 2195 }
 2196 
 2197 /*
 2198  * Unlock a PI mutex.
 2199  */
 2200 static int
 2201 do_unlock_pi(struct thread *td, struct umutex *m, uint32_t flags)
 2202 {
 2203         struct umtx_key key;
 2204         struct umtx_q *uq_first, *uq_first2, *uq_me;
 2205         struct umtx_pi *pi, *pi2;
 2206         uint32_t owner, old, id;
 2207         int error;
 2208         int count;
 2209         int pri;
 2210 
 2211         id = td->td_tid;
 2212         /*
 2213          * Make sure we own this mtx.
 2214          */
 2215         error = fueword32(&m->m_owner, &owner);
 2216         if (error == -1)
 2217                 return (EFAULT);
 2218 
 2219         if ((owner & ~UMUTEX_CONTESTED) != id)
 2220                 return (EPERM);
 2221 
 2222         /* This should be done in userland */
 2223         if ((owner & UMUTEX_CONTESTED) == 0) {
 2224                 error = casueword32(&m->m_owner, owner, &old, UMUTEX_UNOWNED);
 2225                 if (error == -1)
 2226                         return (EFAULT);
 2227                 if (old == owner)
 2228                         return (0);
 2229                 owner = old;
 2230         }
 2231 
 2232         /* We should only ever be in here for contested locks */
 2233         if ((error = umtx_key_get(m, TYPE_PI_UMUTEX, GET_SHARE(flags),
 2234             &key)) != 0)
 2235                 return (error);
 2236 
 2237         umtxq_lock(&key);
 2238         umtxq_busy(&key);
 2239         count = umtxq_count_pi(&key, &uq_first);
 2240         if (uq_first != NULL) {
 2241                 mtx_lock(&umtx_lock);
 2242                 pi = uq_first->uq_pi_blocked;
 2243                 KASSERT(pi != NULL, ("pi == NULL?"));
 2244                 if (pi->pi_owner != curthread) {
 2245                         mtx_unlock(&umtx_lock);
 2246                         umtxq_unbusy(&key);
 2247                         umtxq_unlock(&key);
 2248                         umtx_key_release(&key);
 2249                         /* userland messed the mutex */
 2250                         return (EPERM);
 2251                 }
 2252                 uq_me = curthread->td_umtxq;
 2253                 umtx_pi_disown(pi);
 2254                 /* get highest priority thread which is still sleeping. */
 2255                 uq_first = TAILQ_FIRST(&pi->pi_blocked);
 2256                 while (uq_first != NULL && 
 2257                        (uq_first->uq_flags & UQF_UMTXQ) == 0) {
 2258                         uq_first = TAILQ_NEXT(uq_first, uq_lockq);
 2259                 }
 2260                 pri = PRI_MAX;
 2261                 TAILQ_FOREACH(pi2, &uq_me->uq_pi_contested, pi_link) {
 2262                         uq_first2 = TAILQ_FIRST(&pi2->pi_blocked);
 2263                         if (uq_first2 != NULL) {
 2264                                 if (pri > UPRI(uq_first2->uq_thread))
 2265                                         pri = UPRI(uq_first2->uq_thread);
 2266                         }
 2267                 }
 2268                 thread_lock(curthread);
 2269                 sched_lend_user_prio(curthread, pri);
 2270                 thread_unlock(curthread);
 2271                 mtx_unlock(&umtx_lock);
 2272                 if (uq_first)
 2273                         umtxq_signal_thread(uq_first);
 2274         } else {
 2275                 pi = umtx_pi_lookup(&key);
 2276                 /*
 2277                  * A umtx_pi can exist if a signal or timeout removed the
 2278                  * last waiter from the umtxq, but there is still
 2279                  * a thread in do_lock_pi() holding the umtx_pi.
 2280                  */
 2281                 if (pi != NULL) {
 2282                         /*
 2283                          * The umtx_pi can be unowned, such as when a thread
 2284                          * has just entered do_lock_pi(), allocated the
 2285                          * umtx_pi, and unlocked the umtxq.
 2286                          * If the current thread owns it, it must disown it.
 2287                          */
 2288                         mtx_lock(&umtx_lock);
 2289                         if (pi->pi_owner == td)
 2290                                 umtx_pi_disown(pi);
 2291                         mtx_unlock(&umtx_lock);
 2292                 }
 2293         }
 2294         umtxq_unlock(&key);
 2295 
 2296         /*
 2297          * When unlocking the umtx, it must be marked as unowned if
 2298          * there is zero or one thread only waiting for it.
 2299          * Otherwise, it must be marked as contested.
 2300          */
 2301         error = casueword32(&m->m_owner, owner, &old,
 2302             count <= 1 ? UMUTEX_UNOWNED : UMUTEX_CONTESTED);
 2303 
 2304         umtxq_unbusy_unlocked(&key);
 2305         umtx_key_release(&key);
 2306         if (error == -1)
 2307                 return (EFAULT);
 2308         if (old != owner)
 2309                 return (EINVAL);
 2310         return (0);
 2311 }
 2312 
 2313 /*
 2314  * Lock a PP mutex.
 2315  */
 2316 static int
 2317 do_lock_pp(struct thread *td, struct umutex *m, uint32_t flags,
 2318     struct _umtx_time *timeout, int try)
 2319 {
 2320         struct abs_timeout timo;
 2321         struct umtx_q *uq, *uq2;
 2322         struct umtx_pi *pi;
 2323         uint32_t ceiling;
 2324         uint32_t owner, id;
 2325         int error, pri, old_inherited_pri, su, rv;
 2326 
 2327         id = td->td_tid;
 2328         uq = td->td_umtxq;
 2329         if ((error = umtx_key_get(m, TYPE_PP_UMUTEX, GET_SHARE(flags),
 2330             &uq->uq_key)) != 0)
 2331                 return (error);
 2332 
 2333         if (timeout != NULL)
 2334                 abs_timeout_init2(&timo, timeout);
 2335 
 2336         su = (priv_check(td, PRIV_SCHED_RTPRIO) == 0);
 2337         for (;;) {
 2338                 old_inherited_pri = uq->uq_inherited_pri;
 2339                 umtxq_lock(&uq->uq_key);
 2340                 umtxq_busy(&uq->uq_key);
 2341                 umtxq_unlock(&uq->uq_key);
 2342 
 2343                 rv = fueword32(&m->m_ceilings[0], &ceiling);
 2344                 if (rv == -1) {
 2345                         error = EFAULT;
 2346                         goto out;
 2347                 }
 2348                 ceiling = RTP_PRIO_MAX - ceiling;
 2349                 if (ceiling > RTP_PRIO_MAX) {
 2350                         error = EINVAL;
 2351                         goto out;
 2352                 }
 2353 
 2354                 mtx_lock(&umtx_lock);
 2355                 if (UPRI(td) < PRI_MIN_REALTIME + ceiling) {
 2356                         mtx_unlock(&umtx_lock);
 2357                         error = EINVAL;
 2358                         goto out;
 2359                 }
 2360                 if (su && PRI_MIN_REALTIME + ceiling < uq->uq_inherited_pri) {
 2361                         uq->uq_inherited_pri = PRI_MIN_REALTIME + ceiling;
 2362                         thread_lock(td);
 2363                         if (uq->uq_inherited_pri < UPRI(td))
 2364                                 sched_lend_user_prio(td, uq->uq_inherited_pri);
 2365                         thread_unlock(td);
 2366                 }
 2367                 mtx_unlock(&umtx_lock);
 2368 
 2369                 rv = casueword32(&m->m_owner,
 2370                     UMUTEX_CONTESTED, &owner, id | UMUTEX_CONTESTED);
 2371                 /* The address was invalid. */
 2372                 if (rv == -1) {
 2373                         error = EFAULT;
 2374                         break;
 2375                 }
 2376 
 2377                 if (owner == UMUTEX_CONTESTED) {
 2378                         error = 0;
 2379                         break;
 2380                 }
 2381 
 2382                 if ((flags & UMUTEX_ERROR_CHECK) != 0 &&
 2383                     (owner & ~UMUTEX_CONTESTED) == id) {
 2384                         error = EDEADLK;
 2385                         break;
 2386                 }
 2387 
 2388                 if (try != 0) {
 2389                         error = EBUSY;
 2390                         break;
 2391                 }
 2392 
 2393                 /*
 2394                  * If we caught a signal, we have retried and now
 2395                  * exit immediately.
 2396                  */
 2397                 if (error != 0)
 2398                         break;
 2399 
 2400                 umtxq_lock(&uq->uq_key);
 2401                 umtxq_insert(uq);
 2402                 umtxq_unbusy(&uq->uq_key);
 2403                 error = umtxq_sleep(uq, "umtxpp", timeout == NULL ?
 2404                     NULL : &timo);
 2405                 umtxq_remove(uq);
 2406                 umtxq_unlock(&uq->uq_key);
 2407 
 2408                 mtx_lock(&umtx_lock);
 2409                 uq->uq_inherited_pri = old_inherited_pri;
 2410                 pri = PRI_MAX;
 2411                 TAILQ_FOREACH(pi, &uq->uq_pi_contested, pi_link) {
 2412                         uq2 = TAILQ_FIRST(&pi->pi_blocked);
 2413                         if (uq2 != NULL) {
 2414                                 if (pri > UPRI(uq2->uq_thread))
 2415                                         pri = UPRI(uq2->uq_thread);
 2416                         }
 2417                 }
 2418                 if (pri > uq->uq_inherited_pri)
 2419                         pri = uq->uq_inherited_pri;
 2420                 thread_lock(td);
 2421                 sched_lend_user_prio(td, pri);
 2422                 thread_unlock(td);
 2423                 mtx_unlock(&umtx_lock);
 2424         }
 2425 
 2426         if (error != 0) {
 2427                 mtx_lock(&umtx_lock);
 2428                 uq->uq_inherited_pri = old_inherited_pri;
 2429                 pri = PRI_MAX;
 2430                 TAILQ_FOREACH(pi, &uq->uq_pi_contested, pi_link) {
 2431                         uq2 = TAILQ_FIRST(&pi->pi_blocked);
 2432                         if (uq2 != NULL) {
 2433                                 if (pri > UPRI(uq2->uq_thread))
 2434                                         pri = UPRI(uq2->uq_thread);
 2435                         }
 2436                 }
 2437                 if (pri > uq->uq_inherited_pri)
 2438                         pri = uq->uq_inherited_pri;
 2439                 thread_lock(td);
 2440                 sched_lend_user_prio(td, pri);
 2441                 thread_unlock(td);
 2442                 mtx_unlock(&umtx_lock);
 2443         }
 2444 
 2445 out:
 2446         umtxq_unbusy_unlocked(&uq->uq_key);
 2447         umtx_key_release(&uq->uq_key);
 2448         return (error);
 2449 }
 2450 
 2451 /*
 2452  * Unlock a PP mutex.
 2453  */
 2454 static int
 2455 do_unlock_pp(struct thread *td, struct umutex *m, uint32_t flags)
 2456 {
 2457         struct umtx_key key;
 2458         struct umtx_q *uq, *uq2;
 2459         struct umtx_pi *pi;
 2460         uint32_t owner, id;
 2461         uint32_t rceiling;
 2462         int error, pri, new_inherited_pri, su;
 2463 
 2464         id = td->td_tid;
 2465         uq = td->td_umtxq;
 2466         su = (priv_check(td, PRIV_SCHED_RTPRIO) == 0);
 2467 
 2468         /*
 2469          * Make sure we own this mtx.
 2470          */
 2471         error = fueword32(&m->m_owner, &owner);
 2472         if (error == -1)
 2473                 return (EFAULT);
 2474 
 2475         if ((owner & ~UMUTEX_CONTESTED) != id)
 2476                 return (EPERM);
 2477 
 2478         error = copyin(&m->m_ceilings[1], &rceiling, sizeof(uint32_t));
 2479         if (error != 0)
 2480                 return (error);
 2481 
 2482         if (rceiling == -1)
 2483                 new_inherited_pri = PRI_MAX;
 2484         else {
 2485                 rceiling = RTP_PRIO_MAX - rceiling;
 2486                 if (rceiling > RTP_PRIO_MAX)
 2487                         return (EINVAL);
 2488                 new_inherited_pri = PRI_MIN_REALTIME + rceiling;
 2489         }
 2490 
 2491         if ((error = umtx_key_get(m, TYPE_PP_UMUTEX, GET_SHARE(flags),
 2492             &key)) != 0)
 2493                 return (error);
 2494         umtxq_lock(&key);
 2495         umtxq_busy(&key);
 2496         umtxq_unlock(&key);
 2497         /*
 2498          * For priority protected mutex, always set unlocked state
 2499          * to UMUTEX_CONTESTED, so that userland always enters kernel
 2500          * to lock the mutex, it is necessary because thread priority
 2501          * has to be adjusted for such mutex.
 2502          */
 2503         error = suword32(&m->m_owner, UMUTEX_CONTESTED);
 2504 
 2505         umtxq_lock(&key);
 2506         if (error == 0)
 2507                 umtxq_signal(&key, 1);
 2508         umtxq_unbusy(&key);
 2509         umtxq_unlock(&key);
 2510 
 2511         if (error == -1)
 2512                 error = EFAULT;
 2513         else {
 2514                 mtx_lock(&umtx_lock);
 2515                 if (su != 0)
 2516                         uq->uq_inherited_pri = new_inherited_pri;
 2517                 pri = PRI_MAX;
 2518                 TAILQ_FOREACH(pi, &uq->uq_pi_contested, pi_link) {
 2519                         uq2 = TAILQ_FIRST(&pi->pi_blocked);
 2520                         if (uq2 != NULL) {
 2521                                 if (pri > UPRI(uq2->uq_thread))
 2522                                         pri = UPRI(uq2->uq_thread);
 2523                         }
 2524                 }
 2525                 if (pri > uq->uq_inherited_pri)
 2526                         pri = uq->uq_inherited_pri;
 2527                 thread_lock(td);
 2528                 sched_lend_user_prio(td, pri);
 2529                 thread_unlock(td);
 2530                 mtx_unlock(&umtx_lock);
 2531         }
 2532         umtx_key_release(&key);
 2533         return (error);
 2534 }
 2535 
 2536 static int
 2537 do_set_ceiling(struct thread *td, struct umutex *m, uint32_t ceiling,
 2538         uint32_t *old_ceiling)
 2539 {
 2540         struct umtx_q *uq;
 2541         uint32_t save_ceiling;
 2542         uint32_t owner, id;
 2543         uint32_t flags;
 2544         int error, rv;
 2545 
 2546         error = fueword32(&m->m_flags, &flags);
 2547         if (error == -1)
 2548                 return (EFAULT);
 2549         if ((flags & UMUTEX_PRIO_PROTECT) == 0)
 2550                 return (EINVAL);
 2551         if (ceiling > RTP_PRIO_MAX)
 2552                 return (EINVAL);
 2553         id = td->td_tid;
 2554         uq = td->td_umtxq;
 2555         if ((error = umtx_key_get(m, TYPE_PP_UMUTEX, GET_SHARE(flags),
 2556            &uq->uq_key)) != 0)
 2557                 return (error);
 2558         for (;;) {
 2559                 umtxq_lock(&uq->uq_key);
 2560                 umtxq_busy(&uq->uq_key);
 2561                 umtxq_unlock(&uq->uq_key);
 2562 
 2563                 rv = fueword32(&m->m_ceilings[0], &save_ceiling);
 2564                 if (rv == -1) {
 2565                         error = EFAULT;
 2566                         break;
 2567                 }
 2568 
 2569                 rv = casueword32(&m->m_owner,
 2570                     UMUTEX_CONTESTED, &owner, id | UMUTEX_CONTESTED);
 2571                 if (rv == -1) {
 2572                         error = EFAULT;
 2573                         break;
 2574                 }
 2575 
 2576                 if (owner == UMUTEX_CONTESTED) {
 2577                         suword32(&m->m_ceilings[0], ceiling);
 2578                         suword32(&m->m_owner, UMUTEX_CONTESTED);
 2579                         error = 0;
 2580                         break;
 2581                 }
 2582 
 2583                 if ((owner & ~UMUTEX_CONTESTED) == id) {
 2584                         suword32(&m->m_ceilings[0], ceiling);
 2585                         error = 0;
 2586                         break;
 2587                 }
 2588 
 2589                 /*
 2590                  * If we caught a signal, we have retried and now
 2591                  * exit immediately.
 2592                  */
 2593                 if (error != 0)
 2594                         break;
 2595 
 2596                 /*
 2597                  * We set the contested bit, sleep. Otherwise the lock changed
 2598                  * and we need to retry or we lost a race to the thread
 2599                  * unlocking the umtx.
 2600                  */
 2601                 umtxq_lock(&uq->uq_key);
 2602                 umtxq_insert(uq);
 2603                 umtxq_unbusy(&uq->uq_key);
 2604                 error = umtxq_sleep(uq, "umtxpp", NULL);
 2605                 umtxq_remove(uq);
 2606                 umtxq_unlock(&uq->uq_key);
 2607         }
 2608         umtxq_lock(&uq->uq_key);
 2609         if (error == 0)
 2610                 umtxq_signal(&uq->uq_key, INT_MAX);
 2611         umtxq_unbusy(&uq->uq_key);
 2612         umtxq_unlock(&uq->uq_key);
 2613         umtx_key_release(&uq->uq_key);
 2614         if (error == 0 && old_ceiling != NULL)
 2615                 suword32(old_ceiling, save_ceiling);
 2616         return (error);
 2617 }
 2618 
 2619 /*
 2620  * Lock a userland POSIX mutex.
 2621  */
 2622 static int
 2623 do_lock_umutex(struct thread *td, struct umutex *m,
 2624     struct _umtx_time *timeout, int mode)
 2625 {
 2626         uint32_t flags;
 2627         int error;
 2628 
 2629         error = fueword32(&m->m_flags, &flags);
 2630         if (error == -1)
 2631                 return (EFAULT);
 2632 
 2633         switch(flags & (UMUTEX_PRIO_INHERIT | UMUTEX_PRIO_PROTECT)) {
 2634         case 0:
 2635                 error = do_lock_normal(td, m, flags, timeout, mode);
 2636                 break;
 2637         case UMUTEX_PRIO_INHERIT:
 2638                 error = do_lock_pi(td, m, flags, timeout, mode);
 2639                 break;
 2640         case UMUTEX_PRIO_PROTECT:
 2641                 error = do_lock_pp(td, m, flags, timeout, mode);
 2642                 break;
 2643         default:
 2644                 return (EINVAL);
 2645         }
 2646         if (timeout == NULL) {
 2647                 if (error == EINTR && mode != _UMUTEX_WAIT)
 2648                         error = ERESTART;
 2649         } else {
 2650                 /* Timed-locking is not restarted. */
 2651                 if (error == ERESTART)
 2652                         error = EINTR;
 2653         }
 2654         return (error);
 2655 }
 2656 
 2657 /*
 2658  * Unlock a userland POSIX mutex.
 2659  */
 2660 static int
 2661 do_unlock_umutex(struct thread *td, struct umutex *m)
 2662 {
 2663         uint32_t flags;
 2664         int error;
 2665 
 2666         error = fueword32(&m->m_flags, &flags);
 2667         if (error == -1)
 2668                 return (EFAULT);
 2669 
 2670         switch(flags & (UMUTEX_PRIO_INHERIT | UMUTEX_PRIO_PROTECT)) {
 2671         case 0:
 2672                 return (do_unlock_normal(td, m, flags));
 2673         case UMUTEX_PRIO_INHERIT:
 2674                 return (do_unlock_pi(td, m, flags));
 2675         case UMUTEX_PRIO_PROTECT:
 2676                 return (do_unlock_pp(td, m, flags));
 2677         }
 2678 
 2679         return (EINVAL);
 2680 }
 2681 
 2682 static int
 2683 do_cv_wait(struct thread *td, struct ucond *cv, struct umutex *m,
 2684         struct timespec *timeout, u_long wflags)
 2685 {
 2686         struct abs_timeout timo;
 2687         struct umtx_q *uq;
 2688         uint32_t flags, clockid, hasw;
 2689         int error;
 2690 
 2691         uq = td->td_umtxq;
 2692         error = fueword32(&cv->c_flags, &flags);
 2693         if (error == -1)
 2694                 return (EFAULT);
 2695         error = umtx_key_get(cv, TYPE_CV, GET_SHARE(flags), &uq->uq_key);
 2696         if (error != 0)
 2697                 return (error);
 2698 
 2699         if ((wflags & CVWAIT_CLOCKID) != 0) {
 2700                 error = fueword32(&cv->c_clockid, &clockid);
 2701                 if (error == -1) {
 2702                         umtx_key_release(&uq->uq_key);
 2703                         return (EFAULT);
 2704                 }
 2705                 if (clockid < CLOCK_REALTIME ||
 2706                     clockid >= CLOCK_THREAD_CPUTIME_ID) {
 2707                         /* hmm, only HW clock id will work. */
 2708                         umtx_key_release(&uq->uq_key);
 2709                         return (EINVAL);
 2710                 }
 2711         } else {
 2712                 clockid = CLOCK_REALTIME;
 2713         }
 2714 
 2715         umtxq_lock(&uq->uq_key);
 2716         umtxq_busy(&uq->uq_key);
 2717         umtxq_insert(uq);
 2718         umtxq_unlock(&uq->uq_key);
 2719 
 2720         /*
 2721          * Set c_has_waiters to 1 before releasing user mutex, also
 2722          * don't modify cache line when unnecessary.
 2723          */
 2724         error = fueword32(&cv->c_has_waiters, &hasw);
 2725         if (error == 0 && hasw == 0)
 2726                 suword32(&cv->c_has_waiters, 1);
 2727 
 2728         umtxq_unbusy_unlocked(&uq->uq_key);
 2729 
 2730         error = do_unlock_umutex(td, m);
 2731 
 2732         if (timeout != NULL)
 2733                 abs_timeout_init(&timo, clockid, ((wflags & CVWAIT_ABSTIME) != 0),
 2734                         timeout);
 2735         
 2736         umtxq_lock(&uq->uq_key);
 2737         if (error == 0) {
 2738                 error = umtxq_sleep(uq, "ucond", timeout == NULL ?
 2739                     NULL : &timo);
 2740         }
 2741 
 2742         if ((uq->uq_flags & UQF_UMTXQ) == 0)
 2743                 error = 0;
 2744         else {
 2745                 /*
 2746                  * This must be timeout,interrupted by signal or
 2747                  * surprious wakeup, clear c_has_waiter flag when
 2748                  * necessary.
 2749                  */
 2750                 umtxq_busy(&uq->uq_key);
 2751                 if ((uq->uq_flags & UQF_UMTXQ) != 0) {
 2752                         int oldlen = uq->uq_cur_queue->length;
 2753                         umtxq_remove(uq);
 2754                         if (oldlen == 1) {
 2755                                 umtxq_unlock(&uq->uq_key);
 2756                                 suword32(&cv->c_has_waiters, 0);
 2757                                 umtxq_lock(&uq->uq_key);
 2758                         }
 2759                 }
 2760                 umtxq_unbusy(&uq->uq_key);
 2761                 if (error == ERESTART)
 2762                         error = EINTR;
 2763         }
 2764 
 2765         umtxq_unlock(&uq->uq_key);
 2766         umtx_key_release(&uq->uq_key);
 2767         return (error);
 2768 }
 2769 
 2770 /*
 2771  * Signal a userland condition variable.
 2772  */
 2773 static int
 2774 do_cv_signal(struct thread *td, struct ucond *cv)
 2775 {
 2776         struct umtx_key key;
 2777         int error, cnt, nwake;
 2778         uint32_t flags;
 2779 
 2780         error = fueword32(&cv->c_flags, &flags);
 2781         if (error == -1)
 2782                 return (EFAULT);
 2783         if ((error = umtx_key_get(cv, TYPE_CV, GET_SHARE(flags), &key)) != 0)
 2784                 return (error); 
 2785         umtxq_lock(&key);
 2786         umtxq_busy(&key);
 2787         cnt = umtxq_count(&key);
 2788         nwake = umtxq_signal(&key, 1);
 2789         if (cnt <= nwake) {
 2790                 umtxq_unlock(&key);
 2791                 error = suword32(&cv->c_has_waiters, 0);
 2792                 if (error == -1)
 2793                         error = EFAULT;
 2794                 umtxq_lock(&key);
 2795         }
 2796         umtxq_unbusy(&key);
 2797         umtxq_unlock(&key);
 2798         umtx_key_release(&key);
 2799         return (error);
 2800 }
 2801 
 2802 static int
 2803 do_cv_broadcast(struct thread *td, struct ucond *cv)
 2804 {
 2805         struct umtx_key key;
 2806         int error;
 2807         uint32_t flags;
 2808 
 2809         error = fueword32(&cv->c_flags, &flags);
 2810         if (error == -1)
 2811                 return (EFAULT);
 2812         if ((error = umtx_key_get(cv, TYPE_CV, GET_SHARE(flags), &key)) != 0)
 2813                 return (error); 
 2814 
 2815         umtxq_lock(&key);
 2816         umtxq_busy(&key);
 2817         umtxq_signal(&key, INT_MAX);
 2818         umtxq_unlock(&key);
 2819 
 2820         error = suword32(&cv->c_has_waiters, 0);
 2821         if (error == -1)
 2822                 error = EFAULT;
 2823 
 2824         umtxq_unbusy_unlocked(&key);
 2825 
 2826         umtx_key_release(&key);
 2827         return (error);
 2828 }
 2829 
 2830 static int
 2831 do_rw_rdlock(struct thread *td, struct urwlock *rwlock, long fflag, struct _umtx_time *timeout)
 2832 {
 2833         struct abs_timeout timo;
 2834         struct umtx_q *uq;
 2835         uint32_t flags, wrflags;
 2836         int32_t state, oldstate;
 2837         int32_t blocked_readers;
 2838         int error, rv;
 2839 
 2840         uq = td->td_umtxq;
 2841         error = fueword32(&rwlock->rw_flags, &flags);
 2842         if (error == -1)
 2843                 return (EFAULT);
 2844         error = umtx_key_get(rwlock, TYPE_RWLOCK, GET_SHARE(flags), &uq->uq_key);
 2845         if (error != 0)
 2846                 return (error);
 2847 
 2848         if (timeout != NULL)
 2849                 abs_timeout_init2(&timo, timeout);
 2850 
 2851         wrflags = URWLOCK_WRITE_OWNER;
 2852         if (!(fflag & URWLOCK_PREFER_READER) && !(flags & URWLOCK_PREFER_READER))
 2853                 wrflags |= URWLOCK_WRITE_WAITERS;
 2854 
 2855         for (;;) {
 2856                 rv = fueword32(&rwlock->rw_state, &state);
 2857                 if (rv == -1) {
 2858                         umtx_key_release(&uq->uq_key);
 2859                         return (EFAULT);
 2860                 }
 2861 
 2862                 /* try to lock it */
 2863                 while (!(state & wrflags)) {
 2864                         if (__predict_false(URWLOCK_READER_COUNT(state) == URWLOCK_MAX_READERS)) {
 2865                                 umtx_key_release(&uq->uq_key);
 2866                                 return (EAGAIN);
 2867                         }
 2868                         rv = casueword32(&rwlock->rw_state, state,
 2869                             &oldstate, state + 1);
 2870                         if (rv == -1) {
 2871                                 umtx_key_release(&uq->uq_key);
 2872                                 return (EFAULT);
 2873                         }
 2874                         if (oldstate == state) {
 2875                                 umtx_key_release(&uq->uq_key);
 2876                                 return (0);
 2877                         }
 2878                         error = umtxq_check_susp(td);
 2879                         if (error != 0)
 2880                                 break;
 2881                         state = oldstate;
 2882                 }
 2883 
 2884                 if (error)
 2885                         break;
 2886 
 2887                 /* grab monitor lock */
 2888                 umtxq_lock(&uq->uq_key);
 2889                 umtxq_busy(&uq->uq_key);
 2890                 umtxq_unlock(&uq->uq_key);
 2891 
 2892                 /*
 2893                  * re-read the state, in case it changed between the try-lock above
 2894                  * and the check below
 2895                  */
 2896                 rv = fueword32(&rwlock->rw_state, &state);
 2897                 if (rv == -1)
 2898                         error = EFAULT;
 2899 
 2900                 /* set read contention bit */
 2901                 while (error == 0 && (state & wrflags) &&
 2902                     !(state & URWLOCK_READ_WAITERS)) {
 2903                         rv = casueword32(&rwlock->rw_state, state,
 2904                             &oldstate, state | URWLOCK_READ_WAITERS);
 2905                         if (rv == -1) {
 2906                                 error = EFAULT;
 2907                                 break;
 2908                         }
 2909                         if (oldstate == state)
 2910                                 goto sleep;
 2911                         state = oldstate;
 2912                         error = umtxq_check_susp(td);
 2913                         if (error != 0)
 2914                                 break;
 2915                 }
 2916                 if (error != 0) {
 2917                         umtxq_unbusy_unlocked(&uq->uq_key);
 2918                         break;
 2919                 }
 2920 
 2921                 /* state is changed while setting flags, restart */
 2922                 if (!(state & wrflags)) {
 2923                         umtxq_unbusy_unlocked(&uq->uq_key);
 2924                         error = umtxq_check_susp(td);
 2925                         if (error != 0)
 2926                                 break;
 2927                         continue;
 2928                 }
 2929 
 2930 sleep:
 2931                 /* contention bit is set, before sleeping, increase read waiter count */
 2932                 rv = fueword32(&rwlock->rw_blocked_readers,
 2933                     &blocked_readers);
 2934                 if (rv == -1) {
 2935                         umtxq_unbusy_unlocked(&uq->uq_key);
 2936                         error = EFAULT;
 2937                         break;
 2938                 }
 2939                 suword32(&rwlock->rw_blocked_readers, blocked_readers+1);
 2940 
 2941                 while (state & wrflags) {
 2942                         umtxq_lock(&uq->uq_key);
 2943                         umtxq_insert(uq);
 2944                         umtxq_unbusy(&uq->uq_key);
 2945 
 2946                         error = umtxq_sleep(uq, "urdlck", timeout == NULL ?
 2947                             NULL : &timo);
 2948 
 2949                         umtxq_busy(&uq->uq_key);
 2950                         umtxq_remove(uq);
 2951                         umtxq_unlock(&uq->uq_key);
 2952                         if (error)
 2953                                 break;
 2954                         rv = fueword32(&rwlock->rw_state, &state);
 2955                         if (rv == -1) {
 2956                                 error = EFAULT;
 2957                                 break;
 2958                         }
 2959                 }
 2960 
 2961                 /* decrease read waiter count, and may clear read contention bit */
 2962                 rv = fueword32(&rwlock->rw_blocked_readers,
 2963                     &blocked_readers);
 2964                 if (rv == -1) {
 2965                         umtxq_unbusy_unlocked(&uq->uq_key);
 2966                         error = EFAULT;
 2967                         break;
 2968                 }
 2969                 suword32(&rwlock->rw_blocked_readers, blocked_readers-1);
 2970                 if (blocked_readers == 1) {
 2971                         rv = fueword32(&rwlock->rw_state, &state);
 2972                         if (rv == -1)
 2973                                 error = EFAULT;
 2974                         while (error == 0) {
 2975                                 rv = casueword32(&rwlock->rw_state, state,
 2976                                     &oldstate, state & ~URWLOCK_READ_WAITERS);
 2977                                 if (rv == -1) {
 2978                                         error = EFAULT;
 2979                                         break;
 2980                                 }
 2981                                 if (oldstate == state)
 2982                                         break;
 2983                                 state = oldstate;
 2984                                 error = umtxq_check_susp(td);
 2985                         }
 2986                 }
 2987 
 2988                 umtxq_unbusy_unlocked(&uq->uq_key);
 2989                 if (error != 0)
 2990                         break;
 2991         }
 2992         umtx_key_release(&uq->uq_key);
 2993         if (error == ERESTART)
 2994                 error = EINTR;
 2995         return (error);
 2996 }
 2997 
 2998 static int
 2999 do_rw_wrlock(struct thread *td, struct urwlock *rwlock, struct _umtx_time *timeout)
 3000 {
 3001         struct abs_timeout timo;
 3002         struct umtx_q *uq;
 3003         uint32_t flags;
 3004         int32_t state, oldstate;
 3005         int32_t blocked_writers;
 3006         int32_t blocked_readers;
 3007         int error, rv;
 3008 
 3009         uq = td->td_umtxq;
 3010         error = fueword32(&rwlock->rw_flags, &flags);
 3011         if (error == -1)
 3012                 return (EFAULT);
 3013         error = umtx_key_get(rwlock, TYPE_RWLOCK, GET_SHARE(flags), &uq->uq_key);
 3014         if (error != 0)
 3015                 return (error);
 3016 
 3017         if (timeout != NULL)
 3018                 abs_timeout_init2(&timo, timeout);
 3019 
 3020         blocked_readers = 0;
 3021         for (;;) {
 3022                 rv = fueword32(&rwlock->rw_state, &state);
 3023                 if (rv == -1) {
 3024                         umtx_key_release(&uq->uq_key);
 3025                         return (EFAULT);
 3026                 }
 3027                 while (!(state & URWLOCK_WRITE_OWNER) && URWLOCK_READER_COUNT(state) == 0) {
 3028                         rv = casueword32(&rwlock->rw_state, state,
 3029                             &oldstate, state | URWLOCK_WRITE_OWNER);
 3030                         if (rv == -1) {
 3031                                 umtx_key_release(&uq->uq_key);
 3032                                 return (EFAULT);
 3033                         }
 3034                         if (oldstate == state) {
 3035                                 umtx_key_release(&uq->uq_key);
 3036                                 return (0);
 3037                         }
 3038                         state = oldstate;
 3039                         error = umtxq_check_susp(td);
 3040                         if (error != 0)
 3041                                 break;
 3042                 }
 3043 
 3044                 if (error) {
 3045                         if (!(state & (URWLOCK_WRITE_OWNER|URWLOCK_WRITE_WAITERS)) &&
 3046                             blocked_readers != 0) {
 3047                                 umtxq_lock(&uq->uq_key);
 3048                                 umtxq_busy(&uq->uq_key);
 3049                                 umtxq_signal_queue(&uq->uq_key, INT_MAX, UMTX_SHARED_QUEUE);
 3050                                 umtxq_unbusy(&uq->uq_key);
 3051                                 umtxq_unlock(&uq->uq_key);
 3052                         }
 3053 
 3054                         break;
 3055                 }
 3056 
 3057                 /* grab monitor lock */
 3058                 umtxq_lock(&uq->uq_key);
 3059                 umtxq_busy(&uq->uq_key);
 3060                 umtxq_unlock(&uq->uq_key);
 3061 
 3062                 /*
 3063                  * re-read the state, in case it changed between the try-lock above
 3064                  * and the check below
 3065                  */
 3066                 rv = fueword32(&rwlock->rw_state, &state);
 3067                 if (rv == -1)
 3068                         error = EFAULT;
 3069 
 3070                 while (error == 0 && ((state & URWLOCK_WRITE_OWNER) ||
 3071                     URWLOCK_READER_COUNT(state) != 0) &&
 3072                     (state & URWLOCK_WRITE_WAITERS) == 0) {
 3073                         rv = casueword32(&rwlock->rw_state, state,
 3074                             &oldstate, state | URWLOCK_WRITE_WAITERS);
 3075                         if (rv == -1) {
 3076                                 error = EFAULT;
 3077                                 break;
 3078                         }
 3079                         if (oldstate == state)
 3080                                 goto sleep;
 3081                         state = oldstate;
 3082                         error = umtxq_check_susp(td);
 3083                         if (error != 0)
 3084                                 break;
 3085                 }
 3086                 if (error != 0) {
 3087                         umtxq_unbusy_unlocked(&uq->uq_key);
 3088                         break;
 3089                 }
 3090 
 3091                 if (!(state & URWLOCK_WRITE_OWNER) && URWLOCK_READER_COUNT(state) == 0) {
 3092                         umtxq_unbusy_unlocked(&uq->uq_key);
 3093                         error = umtxq_check_susp(td);
 3094                         if (error != 0)
 3095                                 break;
 3096                         continue;
 3097                 }
 3098 sleep:
 3099                 rv = fueword32(&rwlock->rw_blocked_writers,
 3100                     &blocked_writers);
 3101                 if (rv == -1) {
 3102                         umtxq_unbusy_unlocked(&uq->uq_key);
 3103                         error = EFAULT;
 3104                         break;
 3105                 }
 3106                 suword32(&rwlock->rw_blocked_writers, blocked_writers+1);
 3107 
 3108                 while ((state & URWLOCK_WRITE_OWNER) || URWLOCK_READER_COUNT(state) != 0) {
 3109                         umtxq_lock(&uq->uq_key);
 3110                         umtxq_insert_queue(uq, UMTX_EXCLUSIVE_QUEUE);
 3111                         umtxq_unbusy(&uq->uq_key);
 3112 
 3113                         error = umtxq_sleep(uq, "uwrlck", timeout == NULL ?
 3114                             NULL : &timo);
 3115 
 3116                         umtxq_busy(&uq->uq_key);
 3117                         umtxq_remove_queue(uq, UMTX_EXCLUSIVE_QUEUE);
 3118                         umtxq_unlock(&uq->uq_key);
 3119                         if (error)
 3120                                 break;
 3121                         rv = fueword32(&rwlock->rw_state, &state);
 3122                         if (rv == -1) {
 3123                                 error = EFAULT;
 3124                                 break;
 3125                         }
 3126                 }
 3127 
 3128                 rv = fueword32(&rwlock->rw_blocked_writers,
 3129                     &blocked_writers);
 3130                 if (rv == -1) {
 3131                         umtxq_unbusy_unlocked(&uq->uq_key);
 3132                         error = EFAULT;
 3133                         break;
 3134                 }
 3135                 suword32(&rwlock->rw_blocked_writers, blocked_writers-1);
 3136                 if (blocked_writers == 1) {
 3137                         rv = fueword32(&rwlock->rw_state, &state);
 3138                         if (rv == -1) {
 3139                                 umtxq_unbusy_unlocked(&uq->uq_key);
 3140                                 error = EFAULT;
 3141                                 break;
 3142                         }
 3143                         for (;;) {
 3144                                 rv = casueword32(&rwlock->rw_state, state,
 3145                                     &oldstate, state & ~URWLOCK_WRITE_WAITERS);
 3146                                 if (rv == -1) {
 3147                                         error = EFAULT;
 3148                                         break;
 3149                                 }
 3150                                 if (oldstate == state)
 3151                                         break;
 3152                                 state = oldstate;
 3153                                 error = umtxq_check_susp(td);
 3154                                 /*
 3155                                  * We are leaving the URWLOCK_WRITE_WAITERS
 3156                                  * behind, but this should not harm the
 3157                                  * correctness.
 3158                                  */
 3159                                 if (error != 0)
 3160                                         break;
 3161                         }
 3162                         rv = fueword32(&rwlock->rw_blocked_readers,
 3163                             &blocked_readers);
 3164                         if (rv == -1) {
 3165                                 umtxq_unbusy_unlocked(&uq->uq_key);
 3166                                 error = EFAULT;
 3167                                 break;
 3168                         }
 3169                 } else
 3170                         blocked_readers = 0;
 3171 
 3172                 umtxq_unbusy_unlocked(&uq->uq_key);
 3173         }
 3174 
 3175         umtx_key_release(&uq->uq_key);
 3176         if (error == ERESTART)
 3177                 error = EINTR;
 3178         return (error);
 3179 }
 3180 
 3181 static int
 3182 do_rw_unlock(struct thread *td, struct urwlock *rwlock)
 3183 {
 3184         struct umtx_q *uq;
 3185         uint32_t flags;
 3186         int32_t state, oldstate;
 3187         int error, rv, q, count;
 3188 
 3189         uq = td->td_umtxq;
 3190         error = fueword32(&rwlock->rw_flags, &flags);
 3191         if (error == -1)
 3192                 return (EFAULT);
 3193         error = umtx_key_get(rwlock, TYPE_RWLOCK, GET_SHARE(flags), &uq->uq_key);
 3194         if (error != 0)
 3195                 return (error);
 3196 
 3197         error = fueword32(&rwlock->rw_state, &state);
 3198         if (error == -1) {
 3199                 error = EFAULT;
 3200                 goto out;
 3201         }
 3202         if (state & URWLOCK_WRITE_OWNER) {
 3203                 for (;;) {
 3204                         rv = casueword32(&rwlock->rw_state, state, 
 3205                             &oldstate, state & ~URWLOCK_WRITE_OWNER);
 3206                         if (rv == -1) {
 3207                                 error = EFAULT;
 3208                                 goto out;
 3209                         }
 3210                         if (oldstate != state) {
 3211                                 state = oldstate;
 3212                                 if (!(oldstate & URWLOCK_WRITE_OWNER)) {
 3213                                         error = EPERM;
 3214                                         goto out;
 3215                                 }
 3216                                 error = umtxq_check_susp(td);
 3217                                 if (error != 0)
 3218                                         goto out;
 3219                         } else
 3220                                 break;
 3221                 }
 3222         } else if (URWLOCK_READER_COUNT(state) != 0) {
 3223                 for (;;) {
 3224                         rv = casueword32(&rwlock->rw_state, state,
 3225                             &oldstate, state - 1);
 3226                         if (rv == -1) {
 3227                                 error = EFAULT;
 3228                                 goto out;
 3229                         }
 3230                         if (oldstate != state) {
 3231                                 state = oldstate;
 3232                                 if (URWLOCK_READER_COUNT(oldstate) == 0) {
 3233                                         error = EPERM;
 3234                                         goto out;
 3235                                 }
 3236                                 error = umtxq_check_susp(td);
 3237                                 if (error != 0)
 3238                                         goto out;
 3239                         } else
 3240                                 break;
 3241                 }
 3242         } else {
 3243                 error = EPERM;
 3244                 goto out;
 3245         }
 3246 
 3247         count = 0;
 3248 
 3249         if (!(flags & URWLOCK_PREFER_READER)) {
 3250                 if (state & URWLOCK_WRITE_WAITERS) {
 3251                         count = 1;
 3252                         q = UMTX_EXCLUSIVE_QUEUE;
 3253                 } else if (state & URWLOCK_READ_WAITERS) {
 3254                         count = INT_MAX;
 3255                         q = UMTX_SHARED_QUEUE;
 3256                 }
 3257         } else {
 3258                 if (state & URWLOCK_READ_WAITERS) {
 3259                         count = INT_MAX;
 3260                         q = UMTX_SHARED_QUEUE;
 3261                 } else if (state & URWLOCK_WRITE_WAITERS) {
 3262                         count = 1;
 3263                         q = UMTX_EXCLUSIVE_QUEUE;
 3264                 }
 3265         }
 3266 
 3267         if (count) {
 3268                 umtxq_lock(&uq->uq_key);
 3269                 umtxq_busy(&uq->uq_key);
 3270                 umtxq_signal_queue(&uq->uq_key, count, q);
 3271                 umtxq_unbusy(&uq->uq_key);
 3272                 umtxq_unlock(&uq->uq_key);
 3273         }
 3274 out:
 3275         umtx_key_release(&uq->uq_key);
 3276         return (error);
 3277 }
 3278 
 3279 static int
 3280 do_sem_wait(struct thread *td, struct _usem *sem, struct _umtx_time *timeout)
 3281 {
 3282         struct abs_timeout timo;
 3283         struct umtx_q *uq;
 3284         uint32_t flags, count, count1;
 3285         int error, rv;
 3286 
 3287         uq = td->td_umtxq;
 3288         error = fueword32(&sem->_flags, &flags);
 3289         if (error == -1)
 3290                 return (EFAULT);
 3291         error = umtx_key_get(sem, TYPE_SEM, GET_SHARE(flags), &uq->uq_key);
 3292         if (error != 0)
 3293                 return (error);
 3294 
 3295         if (timeout != NULL)
 3296                 abs_timeout_init2(&timo, timeout);
 3297 
 3298         umtxq_lock(&uq->uq_key);
 3299         umtxq_busy(&uq->uq_key);
 3300         umtxq_insert(uq);
 3301         umtxq_unlock(&uq->uq_key);
 3302         rv = casueword32(&sem->_has_waiters, 0, &count1, 1);
 3303         if (rv == 0)
 3304                 rv = fueword32(&sem->_count, &count);
 3305         if (rv == -1 || count != 0) {
 3306                 umtxq_lock(&uq->uq_key);
 3307                 umtxq_unbusy(&uq->uq_key);
 3308                 umtxq_remove(uq);
 3309                 umtxq_unlock(&uq->uq_key);
 3310                 umtx_key_release(&uq->uq_key);
 3311                 return (rv == -1 ? EFAULT : 0);
 3312         }
 3313         umtxq_lock(&uq->uq_key);
 3314         umtxq_unbusy(&uq->uq_key);
 3315 
 3316         error = umtxq_sleep(uq, "usem", timeout == NULL ? NULL : &timo);
 3317 
 3318         if ((uq->uq_flags & UQF_UMTXQ) == 0)
 3319                 error = 0;
 3320         else {
 3321                 umtxq_remove(uq);
 3322                 /* A relative timeout cannot be restarted. */
 3323                 if (error == ERESTART && timeout != NULL &&
 3324                     (timeout->_flags & UMTX_ABSTIME) == 0)
 3325                         error = EINTR;
 3326         }
 3327         umtxq_unlock(&uq->uq_key);
 3328         umtx_key_release(&uq->uq_key);
 3329         return (error);
 3330 }
 3331 
 3332 /*
 3333  * Signal a userland condition variable.
 3334  */
 3335 static int
 3336 do_sem_wake(struct thread *td, struct _usem *sem)
 3337 {
 3338         struct umtx_key key;
 3339         int error, cnt;
 3340         uint32_t flags;
 3341 
 3342         error = fueword32(&sem->_flags, &flags);
 3343         if (error == -1)
 3344                 return (EFAULT);
 3345         if ((error = umtx_key_get(sem, TYPE_SEM, GET_SHARE(flags), &key)) != 0)
 3346                 return (error); 
 3347         umtxq_lock(&key);
 3348         umtxq_busy(&key);
 3349         cnt = umtxq_count(&key);
 3350         if (cnt > 0) {
 3351                 umtxq_signal(&key, 1);
 3352                 /*
 3353                  * Check if count is greater than 0, this means the memory is
 3354                  * still being referenced by user code, so we can safely
 3355                  * update _has_waiters flag.
 3356                  */
 3357                 if (cnt == 1) {
 3358                         umtxq_unlock(&key);
 3359                         error = suword32(&sem->_has_waiters, 0);
 3360                         umtxq_lock(&key);
 3361                         if (error == -1)
 3362                                 error = EFAULT;
 3363                 }
 3364         }
 3365         umtxq_unbusy(&key);
 3366         umtxq_unlock(&key);
 3367         umtx_key_release(&key);
 3368         return (error);
 3369 }
 3370 
 3371 int
 3372 sys__umtx_lock(struct thread *td, struct _umtx_lock_args *uap)
 3373     /* struct umtx *umtx */
 3374 {
 3375         return do_lock_umtx(td, uap->umtx, td->td_tid, 0);
 3376 }
 3377 
 3378 int
 3379 sys__umtx_unlock(struct thread *td, struct _umtx_unlock_args *uap)
 3380     /* struct umtx *umtx */
 3381 {
 3382         return do_unlock_umtx(td, uap->umtx, td->td_tid);
 3383 }
 3384 
 3385 inline int
 3386 umtx_copyin_timeout(const void *addr, struct timespec *tsp)
 3387 {
 3388         int error;
 3389 
 3390         error = copyin(addr, tsp, sizeof(struct timespec));
 3391         if (error == 0) {
 3392                 if (tsp->tv_sec < 0 ||
 3393                     tsp->tv_nsec >= 1000000000 ||
 3394                     tsp->tv_nsec < 0)
 3395                         error = EINVAL;
 3396         }
 3397         return (error);
 3398 }
 3399 
 3400 static inline int
 3401 umtx_copyin_umtx_time(const void *addr, size_t size, struct _umtx_time *tp)
 3402 {
 3403         int error;
 3404         
 3405         if (size <= sizeof(struct timespec)) {
 3406                 tp->_clockid = CLOCK_REALTIME;
 3407                 tp->_flags = 0;
 3408                 error = copyin(addr, &tp->_timeout, sizeof(struct timespec));
 3409         } else 
 3410                 error = copyin(addr, tp, sizeof(struct _umtx_time));
 3411         if (error != 0)
 3412                 return (error);
 3413         if (tp->_timeout.tv_sec < 0 ||
 3414             tp->_timeout.tv_nsec >= 1000000000 || tp->_timeout.tv_nsec < 0)
 3415                 return (EINVAL);
 3416         return (0);
 3417 }
 3418 
 3419 static int
 3420 __umtx_op_lock_umtx(struct thread *td, struct _umtx_op_args *uap)
 3421 {
 3422         struct timespec *ts, timeout;
 3423         int error;
 3424 
 3425         /* Allow a null timespec (wait forever). */
 3426         if (uap->uaddr2 == NULL)
 3427                 ts = NULL;
 3428         else {
 3429                 error = umtx_copyin_timeout(uap->uaddr2, &timeout);
 3430                 if (error != 0)
 3431                         return (error);
 3432                 ts = &timeout;
 3433         }
 3434         return (do_lock_umtx(td, uap->obj, uap->val, ts));
 3435 }
 3436 
 3437 static int
 3438 __umtx_op_unlock_umtx(struct thread *td, struct _umtx_op_args *uap)
 3439 {
 3440         return (do_unlock_umtx(td, uap->obj, uap->val));
 3441 }
 3442 
 3443 static int
 3444 __umtx_op_wait(struct thread *td, struct _umtx_op_args *uap)
 3445 {
 3446         struct _umtx_time timeout, *tm_p;
 3447         int error;
 3448 
 3449         if (uap->uaddr2 == NULL)
 3450                 tm_p = NULL;
 3451         else {
 3452                 error = umtx_copyin_umtx_time(
 3453                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3454                 if (error != 0)
 3455                         return (error);
 3456                 tm_p = &timeout;
 3457         }
 3458         return do_wait(td, uap->obj, uap->val, tm_p, 0, 0);
 3459 }
 3460 
 3461 static int
 3462 __umtx_op_wait_uint(struct thread *td, struct _umtx_op_args *uap)
 3463 {
 3464         struct _umtx_time timeout, *tm_p;
 3465         int error;
 3466 
 3467         if (uap->uaddr2 == NULL)
 3468                 tm_p = NULL;
 3469         else {
 3470                 error = umtx_copyin_umtx_time(
 3471                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3472                 if (error != 0)
 3473                         return (error);
 3474                 tm_p = &timeout;
 3475         }
 3476         return do_wait(td, uap->obj, uap->val, tm_p, 1, 0);
 3477 }
 3478 
 3479 static int
 3480 __umtx_op_wait_uint_private(struct thread *td, struct _umtx_op_args *uap)
 3481 {
 3482         struct _umtx_time *tm_p, timeout;
 3483         int error;
 3484 
 3485         if (uap->uaddr2 == NULL)
 3486                 tm_p = NULL;
 3487         else {
 3488                 error = umtx_copyin_umtx_time(
 3489                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3490                 if (error != 0)
 3491                         return (error);
 3492                 tm_p = &timeout;
 3493         }
 3494         return do_wait(td, uap->obj, uap->val, tm_p, 1, 1);
 3495 }
 3496 
 3497 static int
 3498 __umtx_op_wake(struct thread *td, struct _umtx_op_args *uap)
 3499 {
 3500         return (kern_umtx_wake(td, uap->obj, uap->val, 0));
 3501 }
 3502 
 3503 #define BATCH_SIZE      128
 3504 static int
 3505 __umtx_op_nwake_private(struct thread *td, struct _umtx_op_args *uap)
 3506 {
 3507         int count = uap->val;
 3508         void *uaddrs[BATCH_SIZE];
 3509         char **upp = (char **)uap->obj;
 3510         int tocopy;
 3511         int error = 0;
 3512         int i, pos = 0;
 3513 
 3514         while (count > 0) {
 3515                 tocopy = count;
 3516                 if (tocopy > BATCH_SIZE)
 3517                         tocopy = BATCH_SIZE;
 3518                 error = copyin(upp+pos, uaddrs, tocopy * sizeof(char *));
 3519                 if (error != 0)
 3520                         break;
 3521                 for (i = 0; i < tocopy; ++i)
 3522                         kern_umtx_wake(td, uaddrs[i], INT_MAX, 1);
 3523                 count -= tocopy;
 3524                 pos += tocopy;
 3525         }
 3526         return (error);
 3527 }
 3528 
 3529 static int
 3530 __umtx_op_wake_private(struct thread *td, struct _umtx_op_args *uap)
 3531 {
 3532         return (kern_umtx_wake(td, uap->obj, uap->val, 1));
 3533 }
 3534 
 3535 static int
 3536 __umtx_op_lock_umutex(struct thread *td, struct _umtx_op_args *uap)
 3537 {
 3538         struct _umtx_time *tm_p, timeout;
 3539         int error;
 3540 
 3541         /* Allow a null timespec (wait forever). */
 3542         if (uap->uaddr2 == NULL)
 3543                 tm_p = NULL;
 3544         else {
 3545                 error = umtx_copyin_umtx_time(
 3546                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3547                 if (error != 0)
 3548                         return (error);
 3549                 tm_p = &timeout;
 3550         }
 3551         return do_lock_umutex(td, uap->obj, tm_p, 0);
 3552 }
 3553 
 3554 static int
 3555 __umtx_op_trylock_umutex(struct thread *td, struct _umtx_op_args *uap)
 3556 {
 3557         return do_lock_umutex(td, uap->obj, NULL, _UMUTEX_TRY);
 3558 }
 3559 
 3560 static int
 3561 __umtx_op_wait_umutex(struct thread *td, struct _umtx_op_args *uap)
 3562 {
 3563         struct _umtx_time *tm_p, timeout;
 3564         int error;
 3565 
 3566         /* Allow a null timespec (wait forever). */
 3567         if (uap->uaddr2 == NULL)
 3568                 tm_p = NULL;
 3569         else {
 3570                 error = umtx_copyin_umtx_time(
 3571                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3572                 if (error != 0)
 3573                         return (error);
 3574                 tm_p = &timeout;
 3575         }
 3576         return do_lock_umutex(td, uap->obj, tm_p, _UMUTEX_WAIT);
 3577 }
 3578 
 3579 static int
 3580 __umtx_op_wake_umutex(struct thread *td, struct _umtx_op_args *uap)
 3581 {
 3582         return do_wake_umutex(td, uap->obj);
 3583 }
 3584 
 3585 static int
 3586 __umtx_op_unlock_umutex(struct thread *td, struct _umtx_op_args *uap)
 3587 {
 3588         return do_unlock_umutex(td, uap->obj);
 3589 }
 3590 
 3591 static int
 3592 __umtx_op_set_ceiling(struct thread *td, struct _umtx_op_args *uap)
 3593 {
 3594         return do_set_ceiling(td, uap->obj, uap->val, uap->uaddr1);
 3595 }
 3596 
 3597 static int
 3598 __umtx_op_cv_wait(struct thread *td, struct _umtx_op_args *uap)
 3599 {
 3600         struct timespec *ts, timeout;
 3601         int error;
 3602 
 3603         /* Allow a null timespec (wait forever). */
 3604         if (uap->uaddr2 == NULL)
 3605                 ts = NULL;
 3606         else {
 3607                 error = umtx_copyin_timeout(uap->uaddr2, &timeout);
 3608                 if (error != 0)
 3609                         return (error);
 3610                 ts = &timeout;
 3611         }
 3612         return (do_cv_wait(td, uap->obj, uap->uaddr1, ts, uap->val));
 3613 }
 3614 
 3615 static int
 3616 __umtx_op_cv_signal(struct thread *td, struct _umtx_op_args *uap)
 3617 {
 3618         return do_cv_signal(td, uap->obj);
 3619 }
 3620 
 3621 static int
 3622 __umtx_op_cv_broadcast(struct thread *td, struct _umtx_op_args *uap)
 3623 {
 3624         return do_cv_broadcast(td, uap->obj);
 3625 }
 3626 
 3627 static int
 3628 __umtx_op_rw_rdlock(struct thread *td, struct _umtx_op_args *uap)
 3629 {
 3630         struct _umtx_time timeout;
 3631         int error;
 3632 
 3633         /* Allow a null timespec (wait forever). */
 3634         if (uap->uaddr2 == NULL) {
 3635                 error = do_rw_rdlock(td, uap->obj, uap->val, 0);
 3636         } else {
 3637                 error = umtx_copyin_umtx_time(uap->uaddr2,
 3638                    (size_t)uap->uaddr1, &timeout);
 3639                 if (error != 0)
 3640                         return (error);
 3641                 error = do_rw_rdlock(td, uap->obj, uap->val, &timeout);
 3642         }
 3643         return (error);
 3644 }
 3645 
 3646 static int
 3647 __umtx_op_rw_wrlock(struct thread *td, struct _umtx_op_args *uap)
 3648 {
 3649         struct _umtx_time timeout;
 3650         int error;
 3651 
 3652         /* Allow a null timespec (wait forever). */
 3653         if (uap->uaddr2 == NULL) {
 3654                 error = do_rw_wrlock(td, uap->obj, 0);
 3655         } else {
 3656                 error = umtx_copyin_umtx_time(uap->uaddr2, 
 3657                    (size_t)uap->uaddr1, &timeout);
 3658                 if (error != 0)
 3659                         return (error);
 3660 
 3661                 error = do_rw_wrlock(td, uap->obj, &timeout);
 3662         }
 3663         return (error);
 3664 }
 3665 
 3666 static int
 3667 __umtx_op_rw_unlock(struct thread *td, struct _umtx_op_args *uap)
 3668 {
 3669         return do_rw_unlock(td, uap->obj);
 3670 }
 3671 
 3672 static int
 3673 __umtx_op_sem_wait(struct thread *td, struct _umtx_op_args *uap)
 3674 {
 3675         struct _umtx_time *tm_p, timeout;
 3676         int error;
 3677 
 3678         /* Allow a null timespec (wait forever). */
 3679         if (uap->uaddr2 == NULL)
 3680                 tm_p = NULL;
 3681         else {
 3682                 error = umtx_copyin_umtx_time(
 3683                     uap->uaddr2, (size_t)uap->uaddr1, &timeout);
 3684                 if (error != 0)
 3685                         return (error);
 3686                 tm_p = &timeout;
 3687         }
 3688         return (do_sem_wait(td, uap->obj, tm_p));
 3689 }
 3690 
 3691 static int
 3692 __umtx_op_sem_wake(struct thread *td, struct _umtx_op_args *uap)
 3693 {
 3694         return do_sem_wake(td, uap->obj);
 3695 }
 3696 
 3697 static int
 3698 __umtx_op_wake2_umutex(struct thread *td, struct _umtx_op_args *uap)
 3699 {
 3700         return do_wake2_umutex(td, uap->obj, uap->val);
 3701 }
 3702 
 3703 typedef int (*_umtx_op_func)(struct thread *td, struct _umtx_op_args *uap);
 3704 
 3705 static _umtx_op_func op_table[] = {
 3706         __umtx_op_lock_umtx,            /* UMTX_OP_LOCK */
 3707         __umtx_op_unlock_umtx,          /* UMTX_OP_UNLOCK */
 3708         __umtx_op_wait,                 /* UMTX_OP_WAIT */
 3709         __umtx_op_wake,                 /* UMTX_OP_WAKE */
 3710         __umtx_op_trylock_umutex,       /* UMTX_OP_MUTEX_TRYLOCK */
 3711         __umtx_op_lock_umutex,          /* UMTX_OP_MUTEX_LOCK */
 3712         __umtx_op_unlock_umutex,        /* UMTX_OP_MUTEX_UNLOCK */
 3713         __umtx_op_set_ceiling,          /* UMTX_OP_SET_CEILING */
 3714         __umtx_op_cv_wait,              /* UMTX_OP_CV_WAIT*/
 3715         __umtx_op_cv_signal,            /* UMTX_OP_CV_SIGNAL */
 3716         __umtx_op_cv_broadcast,         /* UMTX_OP_CV_BROADCAST */
 3717         __umtx_op_wait_uint,            /* UMTX_OP_WAIT_UINT */
 3718         __umtx_op_rw_rdlock,            /* UMTX_OP_RW_RDLOCK */
 3719         __umtx_op_rw_wrlock,            /* UMTX_OP_RW_WRLOCK */
 3720         __umtx_op_rw_unlock,            /* UMTX_OP_RW_UNLOCK */
 3721         __umtx_op_wait_uint_private,    /* UMTX_OP_WAIT_UINT_PRIVATE */
 3722         __umtx_op_wake_private,         /* UMTX_OP_WAKE_PRIVATE */
 3723         __umtx_op_wait_umutex,          /* UMTX_OP_UMUTEX_WAIT */
 3724         __umtx_op_wake_umutex,          /* UMTX_OP_UMUTEX_WAKE */
 3725         __umtx_op_sem_wait,             /* UMTX_OP_SEM_WAIT */
 3726         __umtx_op_sem_wake,             /* UMTX_OP_SEM_WAKE */
 3727         __umtx_op_nwake_private,        /* UMTX_OP_NWAKE_PRIVATE */
 3728         __umtx_op_wake2_umutex          /* UMTX_OP_UMUTEX_WAKE2 */
 3729 };
 3730 
 3731 int
 3732 sys__umtx_op(struct thread *td, struct _umtx_op_args *uap)
 3733 {
 3734         if ((unsigned)uap->op < UMTX_OP_MAX)
 3735                 return (*op_table[uap->op])(td, uap);
 3736         return (EINVAL);
 3737 }
 3738 
 3739 #ifdef COMPAT_FREEBSD32
 3740 int
 3741 freebsd32_umtx_lock(struct thread *td, struct freebsd32_umtx_lock_args *uap)
 3742     /* struct umtx *umtx */
 3743 {
 3744         return (do_lock_umtx32(td, (uint32_t *)uap->umtx, td->td_tid, NULL));
 3745 }
 3746 
 3747 int
 3748 freebsd32_umtx_unlock(struct thread *td, struct freebsd32_umtx_unlock_args *uap)
 3749     /* struct umtx *umtx */
 3750 {
 3751         return (do_unlock_umtx32(td, (uint32_t *)uap->umtx, td->td_tid));
 3752 }
 3753 
 3754 struct timespec32 {
 3755         int32_t tv_sec;
 3756         int32_t tv_nsec;
 3757 };
 3758 
 3759 struct umtx_time32 {
 3760         struct  timespec32      timeout;
 3761         uint32_t                flags;
 3762         uint32_t                clockid;
 3763 };
 3764 
 3765 static inline int
 3766 umtx_copyin_timeout32(void *addr, struct timespec *tsp)
 3767 {
 3768         struct timespec32 ts32;
 3769         int error;
 3770 
 3771         error = copyin(addr, &ts32, sizeof(struct timespec32));
 3772         if (error == 0) {
 3773                 if (ts32.tv_sec < 0 ||
 3774                     ts32.tv_nsec >= 1000000000 ||
 3775                     ts32.tv_nsec < 0)
 3776                         error = EINVAL;
 3777                 else {
 3778                         tsp->tv_sec = ts32.tv_sec;
 3779                         tsp->tv_nsec = ts32.tv_nsec;
 3780                 }
 3781         }
 3782         return (error);
 3783 }
 3784 
 3785 static inline int
 3786 umtx_copyin_umtx_time32(const void *addr, size_t size, struct _umtx_time *tp)
 3787 {
 3788         struct umtx_time32 t32;
 3789         int error;
 3790         
 3791         t32.clockid = CLOCK_REALTIME;
 3792         t32.flags   = 0;
 3793         if (size <= sizeof(struct timespec32))
 3794                 error = copyin(addr, &t32.timeout, sizeof(struct timespec32));
 3795         else 
 3796                 error = copyin(addr, &t32, sizeof(struct umtx_time32));
 3797         if (error != 0)
 3798                 return (error);
 3799         if (t32.timeout.tv_sec < 0 ||
 3800             t32.timeout.tv_nsec >= 1000000000 || t32.timeout.tv_nsec < 0)
 3801                 return (EINVAL);
 3802         tp->_timeout.tv_sec = t32.timeout.tv_sec;
 3803         tp->_timeout.tv_nsec = t32.timeout.tv_nsec;
 3804         tp->_flags = t32.flags;
 3805         tp->_clockid = t32.clockid;
 3806         return (0);
 3807 }
 3808 
 3809 static int
 3810 __umtx_op_lock_umtx_compat32(struct thread *td, struct _umtx_op_args *uap)
 3811 {
 3812         struct timespec *ts, timeout;
 3813         int error;
 3814 
 3815         /* Allow a null timespec (wait forever). */
 3816         if (uap->uaddr2 == NULL)
 3817                 ts = NULL;
 3818         else {
 3819                 error = umtx_copyin_timeout32(uap->uaddr2, &timeout);
 3820                 if (error != 0)
 3821                         return (error);
 3822                 ts = &timeout;
 3823         }
 3824         return (do_lock_umtx32(td, uap->obj, uap->val, ts));
 3825 }
 3826 
 3827 static int
 3828 __umtx_op_unlock_umtx_compat32(struct thread *td, struct _umtx_op_args *uap)
 3829 {
 3830         return (do_unlock_umtx32(td, uap->obj, (uint32_t)uap->val));
 3831 }
 3832 
 3833 static int
 3834 __umtx_op_wait_compat32(struct thread *td, struct _umtx_op_args *uap)
 3835 {
 3836         struct _umtx_time *tm_p, timeout;
 3837         int error;
 3838 
 3839         if (uap->uaddr2 == NULL)
 3840                 tm_p = NULL;
 3841         else {
 3842                 error = umtx_copyin_umtx_time32(uap->uaddr2,
 3843                         (size_t)uap->uaddr1, &timeout);
 3844                 if (error != 0)
 3845                         return (error);
 3846                 tm_p = &timeout;
 3847         }
 3848         return do_wait(td, uap->obj, uap->val, tm_p, 1, 0);
 3849 }
 3850 
 3851 static int
 3852 __umtx_op_lock_umutex_compat32(struct thread *td, struct _umtx_op_args *uap)
 3853 {
 3854         struct _umtx_time *tm_p, timeout;
 3855         int error;
 3856 
 3857         /* Allow a null timespec (wait forever). */
 3858         if (uap->uaddr2 == NULL)
 3859                 tm_p = NULL;
 3860         else {
 3861                 error = umtx_copyin_umtx_time(uap->uaddr2,
 3862                             (size_t)uap->uaddr1, &timeout);
 3863                 if (error != 0)
 3864                         return (error);
 3865                 tm_p = &timeout;
 3866         }
 3867         return do_lock_umutex(td, uap->obj, tm_p, 0);
 3868 }
 3869 
 3870 static int
 3871 __umtx_op_wait_umutex_compat32(struct thread *td, struct _umtx_op_args *uap)
 3872 {
 3873         struct _umtx_time *tm_p, timeout;
 3874         int error;
 3875 
 3876         /* Allow a null timespec (wait forever). */
 3877         if (uap->uaddr2 == NULL)
 3878                 tm_p = NULL;
 3879         else {
 3880                 error = umtx_copyin_umtx_time32(uap->uaddr2, 
 3881                     (size_t)uap->uaddr1, &timeout);
 3882                 if (error != 0)
 3883                         return (error);
 3884                 tm_p = &timeout;
 3885         }
 3886         return do_lock_umutex(td, uap->obj, tm_p, _UMUTEX_WAIT);
 3887 }
 3888 
 3889 static int
 3890 __umtx_op_cv_wait_compat32(struct thread *td, struct _umtx_op_args *uap)
 3891 {
 3892         struct timespec *ts, timeout;
 3893         int error;
 3894 
 3895         /* Allow a null timespec (wait forever). */
 3896         if (uap->uaddr2 == NULL)
 3897                 ts = NULL;
 3898         else {
 3899                 error = umtx_copyin_timeout32(uap->uaddr2, &timeout);
 3900                 if (error != 0)
 3901                         return (error);
 3902                 ts = &timeout;
 3903         }
 3904         return (do_cv_wait(td, uap->obj, uap->uaddr1, ts, uap->val));
 3905 }
 3906 
 3907 static int
 3908 __umtx_op_rw_rdlock_compat32(struct thread *td, struct _umtx_op_args *uap)
 3909 {
 3910         struct _umtx_time timeout;
 3911         int error;
 3912 
 3913         /* Allow a null timespec (wait forever). */
 3914         if (uap->uaddr2 == NULL) {
 3915                 error = do_rw_rdlock(td, uap->obj, uap->val, 0);
 3916         } else {
 3917                 error = umtx_copyin_umtx_time32(uap->uaddr2,
 3918                     (size_t)uap->uaddr1, &timeout);
 3919                 if (error != 0)
 3920                         return (error);
 3921                 error = do_rw_rdlock(td, uap->obj, uap->val, &timeout);
 3922         }
 3923         return (error);
 3924 }
 3925 
 3926 static int
 3927 __umtx_op_rw_wrlock_compat32(struct thread *td, struct _umtx_op_args *uap)
 3928 {
 3929         struct _umtx_time timeout;
 3930         int error;
 3931 
 3932         /* Allow a null timespec (wait forever). */
 3933         if (uap->uaddr2 == NULL) {
 3934                 error = do_rw_wrlock(td, uap->obj, 0);
 3935         } else {
 3936                 error = umtx_copyin_umtx_time32(uap->uaddr2,
 3937                     (size_t)uap->uaddr1, &timeout);
 3938                 if (error != 0)
 3939                         return (error);
 3940                 error = do_rw_wrlock(td, uap->obj, &timeout);
 3941         }
 3942         return (error);
 3943 }
 3944 
 3945 static int
 3946 __umtx_op_wait_uint_private_compat32(struct thread *td, struct _umtx_op_args *uap)
 3947 {
 3948         struct _umtx_time *tm_p, timeout;
 3949         int error;
 3950 
 3951         if (uap->uaddr2 == NULL)
 3952                 tm_p = NULL;
 3953         else {
 3954                 error = umtx_copyin_umtx_time32(
 3955                     uap->uaddr2, (size_t)uap->uaddr1,&timeout);
 3956                 if (error != 0)
 3957                         return (error);
 3958                 tm_p = &timeout;
 3959         }
 3960         return do_wait(td, uap->obj, uap->val, tm_p, 1, 1);
 3961 }
 3962 
 3963 static int
 3964 __umtx_op_sem_wait_compat32(struct thread *td, struct _umtx_op_args *uap)
 3965 {
 3966         struct _umtx_time *tm_p, timeout;
 3967         int error;
 3968 
 3969         /* Allow a null timespec (wait forever). */
 3970         if (uap->uaddr2 == NULL)
 3971                 tm_p = NULL;
 3972         else {
 3973                 error = umtx_copyin_umtx_time32(uap->uaddr2,
 3974                     (size_t)uap->uaddr1, &timeout);
 3975                 if (error != 0)
 3976                         return (error);
 3977                 tm_p = &timeout;
 3978         }
 3979         return (do_sem_wait(td, uap->obj, tm_p));
 3980 }
 3981 
 3982 static int
 3983 __umtx_op_nwake_private32(struct thread *td, struct _umtx_op_args *uap)
 3984 {
 3985         int count = uap->val;
 3986         uint32_t uaddrs[BATCH_SIZE];
 3987         uint32_t **upp = (uint32_t **)uap->obj;
 3988         int tocopy;
 3989         int error = 0;
 3990         int i, pos = 0;
 3991 
 3992         while (count > 0) {
 3993                 tocopy = count;
 3994                 if (tocopy > BATCH_SIZE)
 3995                         tocopy = BATCH_SIZE;
 3996                 error = copyin(upp+pos, uaddrs, tocopy * sizeof(uint32_t));
 3997                 if (error != 0)
 3998                         break;
 3999                 for (i = 0; i < tocopy; ++i)
 4000                         kern_umtx_wake(td, (void *)(intptr_t)uaddrs[i],
 4001                                 INT_MAX, 1);
 4002                 count -= tocopy;
 4003                 pos += tocopy;
 4004         }
 4005         return (error);
 4006 }
 4007 
 4008 static _umtx_op_func op_table_compat32[] = {
 4009         __umtx_op_lock_umtx_compat32,   /* UMTX_OP_LOCK */
 4010         __umtx_op_unlock_umtx_compat32, /* UMTX_OP_UNLOCK */
 4011         __umtx_op_wait_compat32,        /* UMTX_OP_WAIT */
 4012         __umtx_op_wake,                 /* UMTX_OP_WAKE */
 4013         __umtx_op_trylock_umutex,       /* UMTX_OP_MUTEX_LOCK */
 4014         __umtx_op_lock_umutex_compat32, /* UMTX_OP_MUTEX_TRYLOCK */
 4015         __umtx_op_unlock_umutex,        /* UMTX_OP_MUTEX_UNLOCK */
 4016         __umtx_op_set_ceiling,          /* UMTX_OP_SET_CEILING */
 4017         __umtx_op_cv_wait_compat32,     /* UMTX_OP_CV_WAIT*/
 4018         __umtx_op_cv_signal,            /* UMTX_OP_CV_SIGNAL */
 4019         __umtx_op_cv_broadcast,         /* UMTX_OP_CV_BROADCAST */
 4020         __umtx_op_wait_compat32,        /* UMTX_OP_WAIT_UINT */
 4021         __umtx_op_rw_rdlock_compat32,   /* UMTX_OP_RW_RDLOCK */
 4022         __umtx_op_rw_wrlock_compat32,   /* UMTX_OP_RW_WRLOCK */
 4023         __umtx_op_rw_unlock,            /* UMTX_OP_RW_UNLOCK */
 4024         __umtx_op_wait_uint_private_compat32,   /* UMTX_OP_WAIT_UINT_PRIVATE */
 4025         __umtx_op_wake_private,         /* UMTX_OP_WAKE_PRIVATE */
 4026         __umtx_op_wait_umutex_compat32, /* UMTX_OP_UMUTEX_WAIT */
 4027         __umtx_op_wake_umutex,          /* UMTX_OP_UMUTEX_WAKE */
 4028         __umtx_op_sem_wait_compat32,    /* UMTX_OP_SEM_WAIT */
 4029         __umtx_op_sem_wake,             /* UMTX_OP_SEM_WAKE */
 4030         __umtx_op_nwake_private32,      /* UMTX_OP_NWAKE_PRIVATE */
 4031         __umtx_op_wake2_umutex          /* UMTX_OP_UMUTEX_WAKE2 */
 4032 };
 4033 
 4034 int
 4035 freebsd32_umtx_op(struct thread *td, struct freebsd32_umtx_op_args *uap)
 4036 {
 4037         if ((unsigned)uap->op < UMTX_OP_MAX)
 4038                 return (*op_table_compat32[uap->op])(td,
 4039                         (struct _umtx_op_args *)uap);
 4040         return (EINVAL);
 4041 }
 4042 #endif
 4043 
 4044 void
 4045 umtx_thread_init(struct thread *td)
 4046 {
 4047         td->td_umtxq = umtxq_alloc();
 4048         td->td_umtxq->uq_thread = td;
 4049 }
 4050 
 4051 void
 4052 umtx_thread_fini(struct thread *td)
 4053 {
 4054         umtxq_free(td->td_umtxq);
 4055 }
 4056 
 4057 /*
 4058  * It will be called when new thread is created, e.g fork().
 4059  */
 4060 void
 4061 umtx_thread_alloc(struct thread *td)
 4062 {
 4063         struct umtx_q *uq;
 4064 
 4065         uq = td->td_umtxq;
 4066         uq->uq_inherited_pri = PRI_MAX;
 4067 
 4068         KASSERT(uq->uq_flags == 0, ("uq_flags != 0"));
 4069         KASSERT(uq->uq_thread == td, ("uq_thread != td"));
 4070         KASSERT(uq->uq_pi_blocked == NULL, ("uq_pi_blocked != NULL"));
 4071         KASSERT(TAILQ_EMPTY(&uq->uq_pi_contested), ("uq_pi_contested is not empty"));
 4072 }
 4073 
 4074 /*
 4075  * exec() hook.
 4076  */
 4077 static void
 4078 umtx_exec_hook(void *arg __unused, struct proc *p __unused,
 4079         struct image_params *imgp __unused)
 4080 {
 4081         umtx_thread_cleanup(curthread);
 4082 }
 4083 
 4084 /*
 4085  * thread_exit() hook.
 4086  */
 4087 void
 4088 umtx_thread_exit(struct thread *td)
 4089 {
 4090         umtx_thread_cleanup(td);
 4091 }
 4092 
 4093 /*
 4094  * clean up umtx data.
 4095  */
 4096 static void
 4097 umtx_thread_cleanup(struct thread *td)
 4098 {
 4099         struct umtx_q *uq;
 4100         struct umtx_pi *pi;
 4101 
 4102         if ((uq = td->td_umtxq) == NULL)
 4103                 return;
 4104 
 4105         mtx_lock(&umtx_lock);
 4106         uq->uq_inherited_pri = PRI_MAX;
 4107         while ((pi = TAILQ_FIRST(&uq->uq_pi_contested)) != NULL) {
 4108                 pi->pi_owner = NULL;
 4109                 TAILQ_REMOVE(&uq->uq_pi_contested, pi, pi_link);
 4110         }
 4111         mtx_unlock(&umtx_lock);
 4112         thread_lock(td);
 4113         sched_lend_user_prio(td, PRI_MAX);
 4114         thread_unlock(td);
 4115 }

Cache object: 96421f0851e668e372e5dd5c7c6dbd87


[ 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.