sys/kern/kern_umtx.c

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