gen/nanosleep.c

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