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)
40semaphore_t	clock_sem = MACH_PORT_NULL;
41mach_port_t	clock_port = MACH_PORT_NULL;
42
43void _init_clock_port(void);
44
45void _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
62extern semaphore_t clock_sem;
63extern mach_port_t clock_port;
64#endif /* !BUILDING_VARIANT */
65
66extern int __unix_conforming;
67#ifdef VARIANT_CANCELABLE
68extern 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 */
71extern 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
75int
76nanosleep(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
136typedef struct {
137    uint64_t high;
138    uint64_t low;
139} uint128_t;
140
141/* 128-bit addition: acc += add */
142static inline void
143add128_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 */
152static inline void
153sub128_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
163static inline double
164uint128_double(uint128_t *u)
165{
166    return TWO64 * u->high + u->low; // may loses precision
167}
168
169/* 64x64 -> 128 bit multiplication */
170static inline void
171mul64x64(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 */
204static int
205muldiv128(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
245int
246nanosleep(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 */
312