[apple/libc.git] / gen / nanosleep.c

/*
 * Copyright (c) 1999, 2003, 2006, 2007, 2010 Apple Inc. All rights reserved.
 *
 * @APPLE_LICENSE_HEADER_START@
 * 
 * This file contains Original Code and/or Modifications of Original Code
 * as defined in and that are subject to the Apple Public Source License
 * Version 2.0 (the 'License'). You may not use this file except in
 * compliance with the License. Please obtain a copy of the License at
 * http://www.opensource.apple.com/apsl/ and read it before using this
 * file.
 * 
 * The Original Code and all software distributed under the License are
 * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
 * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
 * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
 * Please see the License for the specific language governing rights and
 * limitations under the License.
 * 
 * @APPLE_LICENSE_HEADER_END@
 */

#include <errno.h>
#include <sys/time.h>
#include <mach/mach_error.h>
#include <mach/mach_time.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <TargetConditionals.h>

#if __DARWIN_UNIX03
#include <mach/clock.h>
#include <pthread.h>
#include <mach/mach.h>
#include <mach/mach_error.h>

#if !defined(BUILDING_VARIANT)
semaphore_t	clock_sem = MACH_PORT_NULL;
mach_port_t	clock_port = MACH_PORT_NULL;

void _init_clock_port(void);

void _init_clock_port(void) {
	kern_return_t kr;
	mach_port_t host = mach_host_self();
	
	/* Get the clock service port for nanosleep */
	kr = host_get_clock_service(host, SYSTEM_CLOCK, &clock_port);
	if (kr != KERN_SUCCESS) {
		abort();
	}
	
	kr = semaphore_create(mach_task_self(), &clock_sem, SYNC_POLICY_FIFO, 0);
	if (kr != KERN_SUCCESS) {
		abort();
	}
	mach_port_deallocate(mach_task_self(), host);
}
#else
extern semaphore_t clock_sem;
extern mach_port_t clock_port;
#endif /* !BUILDING_VARIANT */

extern int __unix_conforming;
#ifdef VARIANT_CANCELABLE
extern int __semwait_signal(int cond_sem, int mutex_sem, int timeout, int relative, __int64_t tv_sec, __int32_t tv_nsec);
#define	SEMWAIT_SIGNAL	__semwait_signal
#else /* !VARIANT_CANCELABLE */
extern int __semwait_signal_nocancel(int cond_sem, int mutex_sem, int timeout, int relative, __int64_t tv_sec, __int32_t tv_nsec);
#define	SEMWAIT_SIGNAL	__semwait_signal_nocancel
#endif /* VARIANT_CANCELABLE */

int
nanosleep(const struct timespec *requested_time, struct timespec *remaining_time) {
    kern_return_t kret;
    int ret;
    mach_timespec_t current;
    mach_timespec_t completion;
   
	if (__unix_conforming == 0)
		__unix_conforming = 1;

#ifdef VARIANT_CANCELABLE
    pthread_testcancel();
#endif /* VARIANT_CANCELABLE */

    if ((requested_time == NULL) || (requested_time->tv_sec < 0) || (requested_time->tv_nsec >= NSEC_PER_SEC)) {
        errno = EINVAL;
        return -1;
    }


    if (remaining_time != NULL) {
	/* once we add requested_time, this will be the completion time */
        kret = clock_get_time(clock_port, &completion);
        if (kret != KERN_SUCCESS) {
            fprintf(stderr, "clock_get_time() failed: %s\n", mach_error_string(kret));
            errno = EINVAL;
            return -1;
        }
    }
    ret = SEMWAIT_SIGNAL(clock_sem, MACH_PORT_NULL, 1, 1, (int64_t)requested_time->tv_sec, (int32_t)requested_time->tv_nsec);    
    if (ret < 0) {
        if (errno == ETIMEDOUT) {
		return 0;
        } else if (errno == EINTR) {
            if (remaining_time != NULL) {
                ret = clock_get_time(clock_port, &current);
                if (ret != KERN_SUCCESS) {
                    fprintf(stderr, "clock_get_time() failed: %s\n", mach_error_string(ret));
                    return -1;
                }
                /* This depends on the layout of a mach_timespec_t and timespec_t being equivalent */
                ADD_MACH_TIMESPEC(&completion, requested_time);
		/* We have to compare first, since mach_timespect_t contains unsigned integers */
		if(CMP_MACH_TIMESPEC(&completion, &current) > 0) {
		    SUB_MACH_TIMESPEC(&completion, &current);
		    remaining_time->tv_sec = completion.tv_sec;
		    remaining_time->tv_nsec = completion.tv_nsec;
		} else {
		    bzero(remaining_time, sizeof(*remaining_time));
		}
            }
        } else {
            errno = EINVAL;
	}
    }
    return -1;
}


#else /* !__DARWIN_UNIX03 */

typedef struct {
    uint64_t high;
    uint64_t low;
} uint128_t;

/* 128-bit addition: acc += add */
static inline void
add128_128(uint128_t *acc, uint128_t *add)
{
    acc->high += add->high;
    acc->low += add->low;
    if(acc->low < add->low)
	acc->high++; // carry
}

/* 128-bit subtraction: acc -= sub */
static inline void
sub128_128(uint128_t *acc, uint128_t *sub)
{
    acc->high -= sub->high;
    if(acc->low < sub->low)
	acc->high--; // borrow
    acc->low -= sub->low;
}

#define TWO64	(((double)(1ULL << 32)) * ((double)(1ULL << 32)))

static inline double
uint128_double(uint128_t *u)
{
    return TWO64 * u->high + u->low; // may loses precision
}

/* 64x64 -> 128 bit multiplication */
static inline void
mul64x64(uint64_t x, uint64_t y, uint128_t *prod)
{
    uint128_t add;
    /*
     * Split the two 64-bit multiplicands into 32-bit parts:
     * x => 2^32 * x1 + x2
     * y => 2^32 * y1 + y2
     */
    uint32_t x1 = (uint32_t)(x >> 32);
    uint32_t x2 = (uint32_t)x;
    uint32_t y1 = (uint32_t)(y >> 32);
    uint32_t y2 = (uint32_t)y;
    /*
     * direct multiplication:
     * x * y => 2^64 * (x1 * y1) + 2^32 (x1 * y2 + x2 * y1) + (x2 * y2)
     * The first and last terms are direct assignmenet into the uint128_t
     * structure.  Then we add the middle two terms separately, to avoid
     * 64-bit overflow.  (We could use the Karatsuba algorithm to save
     * one multiply, but it is harder to deal with 64-bit overflows.)
     */
    prod->high = (uint64_t)x1 * (uint64_t)y1;
    prod->low = (uint64_t)x2 * (uint64_t)y2;
    add.low = (uint64_t)x1 * (uint64_t)y2;
    add.high = (add.low >> 32);
    add.low <<= 32;
    add128_128(prod, &add);
    add.low = (uint64_t)x2 * (uint64_t)y1;
    add.high = (add.low >> 32);
    add.low <<= 32;
    add128_128(prod, &add);
}

/* calculate (x * y / divisor), using 128-bit internal calculations */
static int
muldiv128(uint64_t x, uint64_t y, uint64_t divisor, uint64_t *res)
{
    uint128_t temp;
    uint128_t divisor128 = {0, divisor};
    uint64_t result = 0;
    double recip;

    /* calculate (x * y) */
    mul64x64(x, y, &temp);
    /*
     * Now divide by the divisor.  We use floating point to calculate an
     * approximate answer and update the results.  Then we iterate and
     * calculate a correction from the difference.
     */
    recip = 1.0 / ((double)divisor);
    while(temp.high || temp.low >= divisor) {
	uint128_t backmul;
	uint64_t uapprox;
	double approx = uint128_double(&temp) * recip;

	if(approx > __LONG_LONG_MAX__)
	    return 0; // answer overflows 64-bits
	uapprox = (uint64_t)approx;
	mul64x64(uapprox, divisor, &backmul);
	/*
	 * Because we are using unsigned integers, we need to approach the
	 * answer from the lesser side.  So if our estimate is too large
	 * we need to decrease it until it is smaller.
	 */
	while(backmul.high > temp.high || (backmul.high == temp.high && backmul.low > temp.low)) {
	    sub128_128(&backmul, &divisor128);
	    uapprox--;
	}
	sub128_128(&temp, &backmul);
	result += uapprox;
    }
    *res = result;
    return 1;
}

int
nanosleep(const struct timespec *requested_time, struct timespec *remaining_time) {
    kern_return_t ret;
    uint64_t end, units;
    static struct mach_timebase_info info = {0, 0};
    static int unity;
    
    if ((requested_time == NULL) || (requested_time->tv_sec < 0) || (requested_time->tv_nsec > NSEC_PER_SEC)) {
        errno = EINVAL;
        return -1;
    }

    if (info.denom == 0) {
        ret = mach_timebase_info(&info);
        if (ret != KERN_SUCCESS) {
            fprintf(stderr, "mach_timebase_info() failed: %s\n", mach_error_string(ret));
            errno = EAGAIN;
            return -1;
        }
	/* If numer == denom == 1 (as in intel), no conversion needed */
	unity = (info.numer == info.denom);
    }

    if(unity)
	units = (uint64_t)requested_time->tv_sec * NSEC_PER_SEC;
    else if(!muldiv128((uint64_t)info.denom * NSEC_PER_SEC,
		       (uint64_t)requested_time->tv_sec,
		       (uint64_t)info.numer,
		       &units))
    {
	errno = EINVAL;
	return -1;
    }
    end = mach_absolute_time()
	+ units
	+ (uint64_t)info.denom * requested_time->tv_nsec / info.numer;
    ret = mach_wait_until(end);
    if (ret != KERN_SUCCESS) {
        if (ret == KERN_ABORTED) {
            errno = EINTR;
            if (remaining_time != NULL) {
                uint64_t now = mach_absolute_time();
                if (now >= end) {
		    remaining_time->tv_sec = 0;
		    remaining_time->tv_nsec = 0;
		} else {
		    if(unity)
			units = (end - now);
		    else
			muldiv128((uint64_t)info.numer,
				  (end - now),
				  (uint64_t)info.denom,
				  &units); // this can't overflow
		    remaining_time->tv_sec = units / NSEC_PER_SEC;
		    remaining_time->tv_nsec = units % NSEC_PER_SEC;
		}
            }
        } else {
            errno = EINVAL;
        }
        return -1;
    }
    return 0;
}


#endif /* __DARWIN_UNIX03 */
Commit	Line	Data
e9ce8d39	1	/*
1f2f436a	2	* Copyright (c) 1999, 2003, 2006, 2007, 2010 Apple Inc. All rights reserved.
e9ce8d39 A	3	*
	4	* @APPLE_LICENSE_HEADER_START@
	5	*
734aad71 A	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
e9ce8d39 A	15	* EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
e9ce8d39 A	16	* INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
734aad71 A	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.
e9ce8d39 A	20	*
	21	* @APPLE_LICENSE_HEADER_END@
	22	*/
	23
	24	#include <errno.h>
	25	#include <sys/time.h>
e9ce8d39	26	#include <mach/mach_error.h>
59e0d9fe	27	#include <mach/mach_time.h>
e9ce8d39	28	#include <stdio.h>
6465356a	29	#include <stdlib.h>
1f2f436a	30	#include <string.h>
6465356a	31	#include <TargetConditionals.h>
3d9156a7	32
224c7076	33	#if __DARWIN_UNIX03
224c7076	34	#include <mach/clock.h>
6465356a A	35	#include <pthread.h>
	36	#include <mach/mach.h>
	37	#include <mach/mach_error.h>
3d9156a7	38
23e20b00	39	#if !defined(BUILDING_VARIANT)
1f2f436a A	40	semaphore_t clock_sem = MACH_PORT_NULL;
	41	mach_port_t clock_port = MACH_PORT_NULL;
	42
6465356a A	43	void _init_clock_port(void);
	44
	45	void _init_clock_port(void) {
1f2f436a A	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
b061a43b	55	kr = semaphore_create(mach_task_self(), &clock_sem, SYNC_POLICY_FIFO, 0);
1f2f436a A	56	if (kr != KERN_SUCCESS) {
	57	abort();
	58	}
	59	mach_port_deallocate(mach_task_self(), host);
	60	}
	61	#else
3d9156a7	62	extern semaphore_t clock_sem;
1f2f436a A	63	extern mach_port_t clock_port;
	64	#endif /* !BUILDING_VARIANT */
	65
	66	extern int __unix_conforming;
224c7076	67	#ifdef VARIANT_CANCELABLE
34e8f829	68	extern int __semwait_signal(int cond_sem, int mutex_sem, int timeout, int relative, __int64_t tv_sec, __int32_t tv_nsec);
224c7076 A	69	#define SEMWAIT_SIGNAL __semwait_signal
224c7076 A	70	#else /* !VARIANT_CANCELABLE */
34e8f829	71	extern int __semwait_signal_nocancel(int cond_sem, int mutex_sem, int timeout, int relative, __int64_t tv_sec, __int32_t tv_nsec);
224c7076 A	72	#define SEMWAIT_SIGNAL __semwait_signal_nocancel
224c7076 A	73	#endif /* VARIANT_CANCELABLE */
3d9156a7 A	74
	75	int
	76	nanosleep(const struct timespec requested_time, struct timespec remaining_time) {
	77	kern_return_t kret;
	78	int ret;
3d9156a7	79	mach_timespec_t current;
1f2f436a	80	mach_timespec_t completion;
3d9156a7 A	81
	82	if (__unix_conforming == 0)
	83	__unix_conforming = 1;
224c7076 A	84
224c7076 A	85	#ifdef VARIANT_CANCELABLE
6465356a	86	pthread_testcancel();
224c7076 A	87	#endif /* VARIANT_CANCELABLE */
224c7076 A	88
3d9156a7 A	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
224c7076	94
3d9156a7	95	if (remaining_time != NULL) {
1f2f436a A	96	/* once we add requested_time, this will be the completion time */
1f2f436a A	97	kret = clock_get_time(clock_port, &completion);
3d9156a7	98	if (kret != KERN_SUCCESS) {
224c7076	99	fprintf(stderr, "clock_get_time() failed: %s\n", mach_error_string(kret));
1f2f436a	100	errno = EINVAL;
3d9156a7 A	101	return -1;
	102	}
	103	}
34e8f829	104	ret = SEMWAIT_SIGNAL(clock_sem, MACH_PORT_NULL, 1, 1, (int64_t)requested_time->tv_sec, (int32_t)requested_time->tv_nsec);
3d9156a7 A	105	if (ret < 0) {
	106	if (errno == ETIMEDOUT) {
	107	return 0;
	108	} else if (errno == EINTR) {
	109	if (remaining_time != NULL) {
1f2f436a	110	ret = clock_get_time(clock_port, &current);
3d9156a7 A	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 */
1f2f436a A	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	}
3d9156a7 A	125	}
	126	} else {
	127	errno = EINVAL;
	128	}
	129	}
	130	return -1;
	131	}
	132
	133
224c7076 A	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	*/
6465356a	234	while(backmul.high > temp.high \|\| (backmul.high == temp.high && backmul.low > temp.low)) {
224c7076 A	235	sub128_128(&backmul, &divisor128);
	236	uapprox--;
	237	}
	238	sub128_128(&temp, &backmul);
	239	result += uapprox;
	240	}
	241	*res = result;
	242	return 1;
	243	}
3d9156a7	244
e9ce8d39 A	245	int
	246	nanosleep(const struct timespec requested_time, struct timespec remaining_time) {
	247	kern_return_t ret;
224c7076 A	248	uint64_t end, units;
	249	static struct mach_timebase_info info = {0, 0};
	250	static int unity;
e9ce8d39 A	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
224c7076	257	if (info.denom == 0) {
59e0d9fe A	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	}
224c7076 A	264	/* If numer == denom == 1 (as in intel), no conversion needed */
224c7076 A	265	unity = (info.numer == info.denom);
e9ce8d39	266	}
59e0d9fe	267
224c7076 A	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;
59e0d9fe	281	ret = mach_wait_until(end);
e9ce8d39 A	282	if (ret != KERN_SUCCESS) {
	283	if (ret == KERN_ABORTED) {
	284	errno = EINTR;
	285	if (remaining_time != NULL) {
59e0d9fe	286	uint64_t now = mach_absolute_time();
224c7076 A	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	}
e9ce8d39 A	301	}
	302	} else {
	303	errno = EINVAL;
	304	}
	305	return -1;
	306	}
	307	return 0;
	308	}
3d9156a7 A	309
3d9156a7 A	310
224c7076	311	#endif /* __DARWIN_UNIX03 */