gen/nanosleep.c

   1 /*
   2  * Copyright (c) 1999, 2003, 2006, 2007, 2010 Apple Inc. All rights reserved.
   3  *
   4  * @APPLE_LICENSE_HEADER_START@
   5  *
   6  * This file contains Original Code and/or Modifications of Original Code
   7  * as defined in and that are subject to the Apple Public Source License
   8  * Version 2.0 (the 'License'). You may not use this file except in
   9  * compliance with the License. Please obtain a copy of the License at
  10  * http://www.opensource.apple.com/apsl/ and read it before using this
  11  * file.
  12  *
  13  * The Original Code and all software distributed under the License are
  14  * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
  15  * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
  16  * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
  17  * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
  18  * Please see the License for the specific language governing rights and
  19  * limitations under the License.
  20  *
  21  * @APPLE_LICENSE_HEADER_END@
  22  */
  23
  24 #include <errno.h>
  25 #include <sys/time.h>
  26 #include <mach/mach_error.h>
  27 #include <mach/mach_time.h>
  28 #include <stdio.h>
  29 #include <string.h>
  30
  31
  32 #if __DARWIN_UNIX03
  33 #include "pthread_internals.h"
  34 #include <mach/clock.h>
  35
  36 #ifndef BUILDING_VARIANT
  37 semaphore_t     clock_sem = MACH_PORT_NULL;
  38 mach_port_t     clock_port = MACH_PORT_NULL;
  39
  40 void _init_clock_port() {
  41         kern_return_t kr;
  42         mach_port_t host = mach_host_self();
  43
  44         /* Get the clock service port for nanosleep */
  45         kr = host_get_clock_service(host, SYSTEM_CLOCK, &clock_port);
  46         if (kr != KERN_SUCCESS) {
  47                 abort();
  48         }
  49
  50         kr = semaphore_create(mach_task_self_, &clock_sem, SYNC_POLICY_FIFO, 0);
  51         if (kr != KERN_SUCCESS) {
  52                 abort();
  53         }
  54         mach_port_deallocate(mach_task_self(), host);
  55 }
  56 #else
  57 extern semaphore_t clock_sem;
  58 extern mach_port_t clock_port;
  59 #endif /* !BUILDING_VARIANT */
  60
  61 extern int __unix_conforming;
  62 #ifdef VARIANT_CANCELABLE
  63 extern void _pthread_testcancel(pthread_t thread, int isconforming);
  64 extern int __semwait_signal(int cond_sem, int mutex_sem, int timeout, int relative, __int64_t tv_sec, __int32_t tv_nsec);
  65 #define SEMWAIT_SIGNAL  __semwait_signal
  66 #else /* !VARIANT_CANCELABLE */
  67 extern int __semwait_signal_nocancel(int cond_sem, int mutex_sem, int timeout, int relative, __int64_t tv_sec, __int32_t tv_nsec);
  68 #define SEMWAIT_SIGNAL  __semwait_signal_nocancel
  69 #endif /* VARIANT_CANCELABLE */
  70
  71 int
  72 nanosleep(const struct timespec *requested_time, struct timespec *remaining_time) {
  73     kern_return_t kret;
  74     int ret;
  75     mach_timespec_t current;
  76     mach_timespec_t completion;
  77
  78         if (__unix_conforming == 0)
  79                 __unix_conforming = 1;
  80
  81 #ifdef VARIANT_CANCELABLE
  82     _pthread_testcancel(pthread_self(), 1);
  83 #endif /* VARIANT_CANCELABLE */
  84
  85     if ((requested_time == NULL) || (requested_time->tv_sec < 0) || (requested_time->tv_nsec >= NSEC_PER_SEC)) {
  86         errno = EINVAL;
  87         return -1;
  88     }
  89
  90
  91     if (remaining_time != NULL) {
  92         /* once we add requested_time, this will be the completion time */
  93         kret = clock_get_time(clock_port, &completion);
  94         if (kret != KERN_SUCCESS) {
  95             fprintf(stderr, "clock_get_time() failed: %s\n", mach_error_string(kret));
  96             errno = EINVAL;
  97             return -1;
  98         }
  99     }
 100     ret = SEMWAIT_SIGNAL(clock_sem, MACH_PORT_NULL, 1, 1, (int64_t)requested_time->tv_sec, (int32_t)requested_time->tv_nsec);
 101     if (ret < 0) {
 102         if (errno == ETIMEDOUT) {
 103                 return 0;
 104         } else if (errno == EINTR) {
 105             if (remaining_time != NULL) {
 106                 ret = clock_get_time(clock_port, &current);
 107                 if (ret != KERN_SUCCESS) {
 108                     fprintf(stderr, "clock_get_time() failed: %s\n", mach_error_string(ret));
 109                     return -1;
 110                 }
 111                 /* This depends on the layout of a mach_timespec_t and timespec_t being equivalent */
 112                 ADD_MACH_TIMESPEC(&completion, requested_time);
 113                 /* We have to compare first, since mach_timespect_t contains unsigned integers */
 114                 if(CMP_MACH_TIMESPEC(&completion, &current) > 0) {
 115                     SUB_MACH_TIMESPEC(&completion, &current);
 116                     remaining_time->tv_sec = completion.tv_sec;
 117                     remaining_time->tv_nsec = completion.tv_nsec;
 118                 } else {
 119                     bzero(remaining_time, sizeof(*remaining_time));
 120                 }
 121             }
 122         } else {
 123             errno = EINVAL;
 124         }
 125     }
 126     return -1;
 127 }
 128
 129
 130 #else /* !__DARWIN_UNIX03 */
 131
 132 typedef struct {
 133     uint64_t high;
 134     uint64_t low;
 135 } uint128_t;
 136
 137 /* 128-bit addition: acc += add */
 138 static inline void
 139 add128_128(uint128_t *acc, uint128_t *add)
 140 {
 141     acc->high += add->high;
 142     acc->low += add->low;
 143     if(acc->low < add->low)
 144         acc->high++; // carry
 145 }
 146
 147 /* 128-bit subtraction: acc -= sub */
 148 static inline void
 149 sub128_128(uint128_t *acc, uint128_t *sub)
 150 {
 151     acc->high -= sub->high;
 152     if(acc->low < sub->low)
 153         acc->high--; // borrow
 154     acc->low -= sub->low;
 155 }
 156
 157 #define TWO64   (((double)(1ULL << 32)) * ((double)(1ULL << 32)))
 158
 159 static inline double
 160 uint128_double(uint128_t *u)
 161 {
 162     return TWO64 * u->high + u->low; // may loses precision
 163 }
 164
 165 /* 64x64 -> 128 bit multiplication */
 166 static inline void
 167 mul64x64(uint64_t x, uint64_t y, uint128_t *prod)
 168 {
 169     uint128_t add;
 170     /*
 171      * Split the two 64-bit multiplicands into 32-bit parts:
 172      * x => 2^32 * x1 + x2
 173      * y => 2^32 * y1 + y2
 174      */
 175     uint32_t x1 = (uint32_t)(x >> 32);
 176     uint32_t x2 = (uint32_t)x;
 177     uint32_t y1 = (uint32_t)(y >> 32);
 178     uint32_t y2 = (uint32_t)y;
 179     /*
 180      * direct multiplication:
 181      * x * y => 2^64 * (x1 * y1) + 2^32 (x1 * y2 + x2 * y1) + (x2 * y2)
 182      * The first and last terms are direct assignmenet into the uint128_t
 183      * structure.  Then we add the middle two terms separately, to avoid
 184      * 64-bit overflow.  (We could use the Karatsuba algorithm to save
 185      * one multiply, but it is harder to deal with 64-bit overflows.)
 186      */
 187     prod->high = (uint64_t)x1 * (uint64_t)y1;
 188     prod->low = (uint64_t)x2 * (uint64_t)y2;
 189     add.low = (uint64_t)x1 * (uint64_t)y2;
 190     add.high = (add.low >> 32);
 191     add.low <<= 32;
 192     add128_128(prod, &add);
 193     add.low = (uint64_t)x2 * (uint64_t)y1;
 194     add.high = (add.low >> 32);
 195     add.low <<= 32;
 196     add128_128(prod, &add);
 197 }
 198
 199 /* calculate (x * y / divisor), using 128-bit internal calculations */
 200 static int
 201 muldiv128(uint64_t x, uint64_t y, uint64_t divisor, uint64_t *res)
 202 {
 203     uint128_t temp;
 204     uint128_t divisor128 = {0, divisor};
 205     uint64_t result = 0;
 206     double recip;
 207
 208     /* calculate (x * y) */
 209     mul64x64(x, y, &temp);
 210     /*
 211      * Now divide by the divisor.  We use floating point to calculate an
 212      * approximate answer and update the results.  Then we iterate and
 213      * calculate a correction from the difference.
 214      */
 215     recip = 1.0 / ((double)divisor);
 216     while(temp.high || temp.low >= divisor) {
 217         uint128_t backmul;
 218         uint64_t uapprox;
 219         double approx = uint128_double(&temp) * recip;
 220
 221         if(approx > __LONG_LONG_MAX__)
 222             return 0; // answer overflows 64-bits
 223         uapprox = (uint64_t)approx;
 224         mul64x64(uapprox, divisor, &backmul);
 225         /*
 226          * Because we are using unsigned integers, we need to approach the
 227          * answer from the lesser side.  So if our estimate is too large
 228          * we need to decrease it until it is smaller.
 229          */
 230         while(backmul.high > temp.high || backmul.high == temp.high && backmul.low > temp.low) {
 231             sub128_128(&backmul, &divisor128);
 232             uapprox--;
 233         }
 234         sub128_128(&temp, &backmul);
 235         result += uapprox;
 236     }
 237     *res = result;
 238     return 1;
 239 }
 240
 241 int
 242 nanosleep(const struct timespec *requested_time, struct timespec *remaining_time) {
 243     kern_return_t ret;
 244     uint64_t end, units;
 245     static struct mach_timebase_info info = {0, 0};
 246     static int unity;
 247
 248     if ((requested_time == NULL) || (requested_time->tv_sec < 0) || (requested_time->tv_nsec > NSEC_PER_SEC)) {
 249         errno = EINVAL;
 250         return -1;
 251     }
 252
 253     if (info.denom == 0) {
 254         ret = mach_timebase_info(&info);
 255         if (ret != KERN_SUCCESS) {
 256             fprintf(stderr, "mach_timebase_info() failed: %s\n", mach_error_string(ret));
 257             errno = EAGAIN;
 258             return -1;
 259         }
 260         /* If numer == denom == 1 (as in intel), no conversion needed */
 261         unity = (info.numer == info.denom);
 262     }
 263
 264     if(unity)
 265         units = (uint64_t)requested_time->tv_sec * NSEC_PER_SEC;
 266     else if(!muldiv128((uint64_t)info.denom * NSEC_PER_SEC,
 267                        (uint64_t)requested_time->tv_sec,
 268                        (uint64_t)info.numer,
 269                        &units))
 270     {
 271         errno = EINVAL;
 272         return -1;
 273     }
 274     end = mach_absolute_time()
 275         + units
 276         + (uint64_t)info.denom * requested_time->tv_nsec / info.numer;
 277     ret = mach_wait_until(end);
 278     if (ret != KERN_SUCCESS) {
 279         if (ret == KERN_ABORTED) {
 280             errno = EINTR;
 281             if (remaining_time != NULL) {
 282                 uint64_t now = mach_absolute_time();
 283                 if (now >= end) {
 284                     remaining_time->tv_sec = 0;
 285                     remaining_time->tv_nsec = 0;
 286                 } else {
 287                     if(unity)
 288                         units = (end - now);
 289                     else
 290                         muldiv128((uint64_t)info.numer,
 291                                   (end - now),
 292                                   (uint64_t)info.denom,
 293                                   &units); // this can't overflow
 294                     remaining_time->tv_sec = units / NSEC_PER_SEC;
 295                     remaining_time->tv_nsec = units % NSEC_PER_SEC;
 296                 }
 297             }
 298         } else {
 299             errno = EINVAL;
 300         }
 301         return -1;
 302     }
 303     return 0;
 304 }
 305
 306
 307 #endif /* __DARWIN_UNIX03 */