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