[mirror_frr.git] / lib / seqlock.c

/*
 * "Sequence" lock primitive
 *
 * Copyright (C) 2015  David Lamparter <equinox@diac24.net>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
 * Boston, MA  02110-1301  USA
 */

#define _GNU_SOURCE

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <string.h>
#include <unistd.h>
#include <limits.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/time.h>
#include <pthread.h>
#include <assert.h>

#include "seqlock.h"

#ifdef HAVE_SYNC_LINUX_FUTEX
/* Linux-specific - sys_futex() */
#include <sys/syscall.h>
#include <linux/futex.h>

static long sys_futex(void *addr1, int op, int val1,
		      const struct timespec *timeout, void *addr2, int val3)
{
	return syscall(SYS_futex, addr1, op, val1, timeout, addr2, val3);
}

#define wait_once(sqlo, val)	\
	sys_futex((int *)&sqlo->pos, FUTEX_WAIT, (int)val, NULL, NULL, 0)
#define wait_time(sqlo, val, time, reltime)	\
	sys_futex((int *)&sqlo->pos, FUTEX_WAIT_BITSET, (int)val, time, \
		  NULL, ~0U)
#define wait_poke(sqlo)		\
	sys_futex((int *)&sqlo->pos, FUTEX_WAKE, INT_MAX, NULL, NULL, 0)

#elif defined(HAVE_SYNC_OPENBSD_FUTEX)
/* OpenBSD variant of the above. */
#include <sys/syscall.h>
#include <sys/futex.h>

#define TIME_RELATIVE		1

#define wait_once(sqlo, val)	\
	futex((int *)&sqlo->pos, FUTEX_WAIT, (int)val, NULL, NULL, 0)
#define wait_time(sqlo, val, time, reltime)	\
	futex((int *)&sqlo->pos, FUTEX_WAIT, (int)val, reltime, NULL, 0)
#define wait_poke(sqlo)		\
	futex((int *)&sqlo->pos, FUTEX_WAKE, INT_MAX, NULL, NULL, 0)

#elif defined(HAVE_SYNC_UMTX_OP)
/* FreeBSD-specific: umtx_op() */
#include <sys/umtx.h>

#define wait_once(sqlo, val)	\
	_umtx_op((void *)&sqlo->pos, UMTX_OP_WAIT_UINT, val, NULL, NULL)
static int wait_time(struct seqlock *sqlo, uint32_t val,
		      const struct timespec *abstime,
		      const struct timespec *reltime)
{
	struct _umtx_time t;
	t._flags = UMTX_ABSTIME;
	t._clockid = CLOCK_MONOTONIC;
	memcpy(&t._timeout, abstime, sizeof(t._timeout));
	return _umtx_op((void *)&sqlo->pos, UMTX_OP_WAIT_UINT, val,
		 (void *)(uintptr_t) sizeof(t), &t);
}
#define wait_poke(sqlo)		\
	_umtx_op((void *)&sqlo->pos, UMTX_OP_WAKE, INT_MAX, NULL, NULL)

#else
/* generic version.  used on *BSD, Solaris and OSX.
 */

#define TIME_ABS_REALTIME	1

#define wait_init(sqlo)		do { \
		pthread_mutex_init(&sqlo->lock, NULL); \
		pthread_cond_init(&sqlo->wake, NULL); \
	} while (0)
#define wait_prep(sqlo)		pthread_mutex_lock(&sqlo->lock)
#define wait_once(sqlo, val)	pthread_cond_wait(&sqlo->wake, &sqlo->lock)
#define wait_time(sqlo, val, time, reltime) \
				pthread_cond_timedwait(&sqlo->wake, \
						       &sqlo->lock, time);
#define wait_done(sqlo)		pthread_mutex_unlock(&sqlo->lock)
#define wait_poke(sqlo)		do { \
		pthread_mutex_lock(&sqlo->lock); \
		pthread_cond_broadcast(&sqlo->wake); \
		pthread_mutex_unlock(&sqlo->lock); \
	} while (0)

#endif

#ifndef wait_init
#define wait_init(sqlo)		/**/
#define wait_prep(sqlo)		/**/
#define wait_done(sqlo)		/**/
#endif /* wait_init */


void seqlock_wait(struct seqlock *sqlo, seqlock_val_t val)
{
	seqlock_val_t cur, cal;

	seqlock_assert_valid(val);

	wait_prep(sqlo);
	cur = atomic_load_explicit(&sqlo->pos, memory_order_relaxed);

	while (cur & SEQLOCK_HELD) {
		cal = SEQLOCK_VAL(cur) - val - 1;
		assert(cal < 0x40000000 || cal > 0xc0000000);
		if (cal < 0x80000000)
			break;

		if ((cur & SEQLOCK_WAITERS)
		    || atomic_compare_exchange_weak_explicit(
				&sqlo->pos, &cur, cur | SEQLOCK_WAITERS,
				memory_order_relaxed, memory_order_relaxed)) {
			wait_once(sqlo, cur | SEQLOCK_WAITERS);
			cur = atomic_load_explicit(&sqlo->pos,
				memory_order_relaxed);
		}
		/* else: we failed to swap in cur because it just changed */
	}
	wait_done(sqlo);
}

bool seqlock_timedwait(struct seqlock *sqlo, seqlock_val_t val,
		       const struct timespec *abs_monotime_limit)
{
#if TIME_ABS_REALTIME
#define time_arg1 &abs_rt
#define time_arg2 NULL
#define time_prep
	struct timespec curmono, abs_rt;

	clock_gettime(CLOCK_MONOTONIC, &curmono);
	clock_gettime(CLOCK_REALTIME, &abs_rt);

	abs_rt.tv_nsec += abs_monotime_limit->tv_nsec - curmono.tv_nsec;
	if (abs_rt.tv_nsec < 0) {
		abs_rt.tv_sec--;
		abs_rt.tv_nsec += 1000000000;
	} else if (abs_rt.tv_nsec >= 1000000000) {
		abs_rt.tv_sec++;
		abs_rt.tv_nsec -= 1000000000;
	}
	abs_rt.tv_sec += abs_monotime_limit->tv_sec - curmono.tv_sec;

#elif TIME_RELATIVE
	struct timespec reltime;

#define time_arg1 abs_monotime_limit
#define time_arg2 &reltime
#define time_prep \
	clock_gettime(CLOCK_MONOTONIC, &reltime);                              \
	reltime.tv_sec = abs_monotime_limit.tv_sec - reltime.tv_sec;           \
	reltime.tv_nsec = abs_monotime_limit.tv_nsec - reltime.tv_nsec;        \
	if (reltime.tv_nsec < 0) {                                             \
		reltime.tv_sec--;                                              \
		reltime.tv_nsec += 1000000000;                                 \
	}
#else
#define time_arg1 abs_monotime_limit
#define time_arg2 NULL
#define time_prep
#endif

	bool ret = true;
	seqlock_val_t cur, cal;

	seqlock_assert_valid(val);

	wait_prep(sqlo);
	cur = atomic_load_explicit(&sqlo->pos, memory_order_relaxed);

	while (cur & SEQLOCK_HELD) {
		cal = SEQLOCK_VAL(cur) - val - 1;
		assert(cal < 0x40000000 || cal > 0xc0000000);
		if (cal < 0x80000000)
			break;

		if ((cur & SEQLOCK_WAITERS)
		    || atomic_compare_exchange_weak_explicit(
				&sqlo->pos, &cur, cur | SEQLOCK_WAITERS,
				memory_order_relaxed, memory_order_relaxed)) {
			int rv;

			time_prep

			rv = wait_time(sqlo, cur | SEQLOCK_WAITERS, time_arg1,
				       time_arg2);
			if (rv) {
				ret = false;
				break;
			}
			cur = atomic_load_explicit(&sqlo->pos,
				memory_order_relaxed);
		}
	}
	wait_done(sqlo);

	return ret;
}

bool seqlock_check(struct seqlock *sqlo, seqlock_val_t val)
{
	seqlock_val_t cur;

	seqlock_assert_valid(val);

	cur = atomic_load_explicit(&sqlo->pos, memory_order_relaxed);
	if (!(cur & SEQLOCK_HELD))
		return 1;
	cur = SEQLOCK_VAL(cur) - val - 1;
	assert(cur < 0x40000000 || cur > 0xc0000000);
	return cur < 0x80000000;
}

void seqlock_acquire_val(struct seqlock *sqlo, seqlock_val_t val)
{
	seqlock_val_t prev;

	seqlock_assert_valid(val);

	prev = atomic_exchange_explicit(&sqlo->pos, val, memory_order_relaxed);
	if (prev & SEQLOCK_WAITERS)
		wait_poke(sqlo);
}

void seqlock_release(struct seqlock *sqlo)
{
	seqlock_val_t prev;

	prev = atomic_exchange_explicit(&sqlo->pos, 0, memory_order_relaxed);
	if (prev & SEQLOCK_WAITERS)
		wait_poke(sqlo);
}

void seqlock_init(struct seqlock *sqlo)
{
	sqlo->pos = 0;
	wait_init(sqlo);
}


seqlock_val_t seqlock_cur(struct seqlock *sqlo)
{
	return SEQLOCK_VAL(atomic_load_explicit(&sqlo->pos,
						memory_order_relaxed));
}

seqlock_val_t seqlock_bump(struct seqlock *sqlo)
{
	seqlock_val_t val, cur;

	cur = atomic_load_explicit(&sqlo->pos, memory_order_relaxed);
	seqlock_assert_valid(cur);

	do {
		val = SEQLOCK_VAL(cur) + SEQLOCK_INCR;
	} while (!atomic_compare_exchange_weak_explicit(&sqlo->pos, &cur, val,
			memory_order_relaxed, memory_order_relaxed));

	if (cur & SEQLOCK_WAITERS)
		wait_poke(sqlo);
	return val;
}
Commit	Line	Data
440d5faa DL	1	/*
	2	* "Sequence" lock primitive
	3	*
	4	* Copyright (C) 2015 David Lamparter <equinox@diac24.net>
	5	*
	6	* This library is free software; you can redistribute it and/or
	7	* modify it under the terms of the GNU Lesser General Public
	8	* License as published by the Free Software Foundation; either
	9	* version 2.1 of the License, or (at your option) any later version.
	10	*
	11	* This library is distributed in the hope that it will be useful,
	12	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	13	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
	14	* Lesser General Public License for more details.
	15	*
	16	* You should have received a copy of the GNU Lesser General Public
	17	* License along with this library; if not, write to the
	18	* Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
	19	* Boston, MA 02110-1301 USA
	20	*/
	21
	22	#define _GNU_SOURCE
	23
	24	#ifdef HAVE_CONFIG_H
	25	#include "config.h"
	26	#endif
	27
2a5e6235	28	#include <string.h>
440d5faa DL	29	#include <unistd.h>
	30	#include <limits.h>
	31	#include <errno.h>
	32	#include <sys/types.h>
	33	#include <sys/time.h>
	34	#include <pthread.h>
	35	#include <assert.h>
	36
	37	#include "seqlock.h"
	38
	39	#ifdef HAVE_SYNC_LINUX_FUTEX
	40	/* Linux-specific - sys_futex() */
	41	#include <sys/syscall.h>
	42	#include <linux/futex.h>
	43
2a5e6235 DL	44	static long sys_futex(void *addr1, int op, int val1,
2a5e6235 DL	45	const struct timespec timeout, void addr2, int val3)
440d5faa DL	46	{
	47	return syscall(SYS_futex, addr1, op, val1, timeout, addr2, val3);
	48	}
	49
	50	#define wait_once(sqlo, val) \
	51	sys_futex((int *)&sqlo->pos, FUTEX_WAIT, (int)val, NULL, NULL, 0)
2a5e6235 DL	52	#define wait_time(sqlo, val, time, reltime) \
	53	sys_futex((int *)&sqlo->pos, FUTEX_WAIT_BITSET, (int)val, time, \
	54	NULL, ~0U)
440d5faa DL	55	#define wait_poke(sqlo) \
	56	sys_futex((int *)&sqlo->pos, FUTEX_WAKE, INT_MAX, NULL, NULL, 0)
	57
	58	#elif defined(HAVE_SYNC_OPENBSD_FUTEX)
2a5e6235	59	/* OpenBSD variant of the above. */
440d5faa DL	60	#include <sys/syscall.h>
	61	#include <sys/futex.h>
	62
2a5e6235 DL	63	#define TIME_RELATIVE 1
2a5e6235 DL	64
440d5faa DL	65	#define wait_once(sqlo, val) \
440d5faa DL	66	futex((int *)&sqlo->pos, FUTEX_WAIT, (int)val, NULL, NULL, 0)
2a5e6235 DL	67	#define wait_time(sqlo, val, time, reltime) \
2a5e6235 DL	68	futex((int *)&sqlo->pos, FUTEX_WAIT, (int)val, reltime, NULL, 0)
440d5faa DL	69	#define wait_poke(sqlo) \
	70	futex((int *)&sqlo->pos, FUTEX_WAKE, INT_MAX, NULL, NULL, 0)
	71
	72	#elif defined(HAVE_SYNC_UMTX_OP)
	73	/* FreeBSD-specific: umtx_op() */
	74	#include <sys/umtx.h>
	75
	76	#define wait_once(sqlo, val) \
	77	_umtx_op((void *)&sqlo->pos, UMTX_OP_WAIT_UINT, val, NULL, NULL)
2a5e6235 DL	78	static int wait_time(struct seqlock *sqlo, uint32_t val,
	79	const struct timespec *abstime,
	80	const struct timespec *reltime)
	81	{
	82	struct _umtx_time t;
	83	t._flags = UMTX_ABSTIME;
	84	t._clockid = CLOCK_MONOTONIC;
	85	memcpy(&t._timeout, abstime, sizeof(t._timeout));
	86	return _umtx_op((void *)&sqlo->pos, UMTX_OP_WAIT_UINT, val,
	87	(void *)(uintptr_t) sizeof(t), &t);
	88	}
440d5faa DL	89	#define wait_poke(sqlo) \
	90	_umtx_op((void *)&sqlo->pos, UMTX_OP_WAKE, INT_MAX, NULL, NULL)
	91
	92	#else
	93	/* generic version. used on *BSD, Solaris and OSX.
	94	*/
	95
2a5e6235 DL	96	#define TIME_ABS_REALTIME 1
2a5e6235 DL	97
440d5faa DL	98	#define wait_init(sqlo) do { \
	99	pthread_mutex_init(&sqlo->lock, NULL); \
	100	pthread_cond_init(&sqlo->wake, NULL); \
	101	} while (0)
	102	#define wait_prep(sqlo) pthread_mutex_lock(&sqlo->lock)
	103	#define wait_once(sqlo, val) pthread_cond_wait(&sqlo->wake, &sqlo->lock)
2a5e6235 DL	104	#define wait_time(sqlo, val, time, reltime) \
	105	pthread_cond_timedwait(&sqlo->wake, \
	106	&sqlo->lock, time);
440d5faa DL	107	#define wait_done(sqlo) pthread_mutex_unlock(&sqlo->lock)
	108	#define wait_poke(sqlo) do { \
	109	pthread_mutex_lock(&sqlo->lock); \
	110	pthread_cond_broadcast(&sqlo->wake); \
	111	pthread_mutex_unlock(&sqlo->lock); \
	112	} while (0)
	113
	114	#endif
	115
	116	#ifndef wait_init
	117	#define wait_init(sqlo) /**/
	118	#define wait_prep(sqlo) /**/
	119	#define wait_done(sqlo) /**/
	120	#endif /* wait_init */
	121
	122
	123	void seqlock_wait(struct seqlock *sqlo, seqlock_val_t val)
	124	{
	125	seqlock_val_t cur, cal;
	126
	127	seqlock_assert_valid(val);
	128
	129	wait_prep(sqlo);
6046b690 DL	130	cur = atomic_load_explicit(&sqlo->pos, memory_order_relaxed);
	131
	132	while (cur & SEQLOCK_HELD) {
	133	cal = SEQLOCK_VAL(cur) - val - 1;
440d5faa DL	134	assert(cal < 0x40000000 \|\| cal > 0xc0000000);
	135	if (cal < 0x80000000)
	136	break;
	137
6046b690 DL	138	if ((cur & SEQLOCK_WAITERS)
	139	\|\| atomic_compare_exchange_weak_explicit(
	140	&sqlo->pos, &cur, cur \| SEQLOCK_WAITERS,
	141	memory_order_relaxed, memory_order_relaxed)) {
	142	wait_once(sqlo, cur \| SEQLOCK_WAITERS);
	143	cur = atomic_load_explicit(&sqlo->pos,
	144	memory_order_relaxed);
	145	}
	146	/* else: we failed to swap in cur because it just changed */
440d5faa DL	147	}
	148	wait_done(sqlo);
	149	}
	150
2a5e6235 DL	151	bool seqlock_timedwait(struct seqlock *sqlo, seqlock_val_t val,
	152	const struct timespec *abs_monotime_limit)
	153	{
	154	#if TIME_ABS_REALTIME
	155	#define time_arg1 &abs_rt
	156	#define time_arg2 NULL
	157	#define time_prep
	158	struct timespec curmono, abs_rt;
	159
	160	clock_gettime(CLOCK_MONOTONIC, &curmono);
	161	clock_gettime(CLOCK_REALTIME, &abs_rt);
	162
	163	abs_rt.tv_nsec += abs_monotime_limit->tv_nsec - curmono.tv_nsec;
	164	if (abs_rt.tv_nsec < 0) {
	165	abs_rt.tv_sec--;
	166	abs_rt.tv_nsec += 1000000000;
	167	} else if (abs_rt.tv_nsec >= 1000000000) {
	168	abs_rt.tv_sec++;
	169	abs_rt.tv_nsec -= 1000000000;
	170	}
	171	abs_rt.tv_sec += abs_monotime_limit->tv_sec - curmono.tv_sec;
	172
	173	#elif TIME_RELATIVE
	174	struct timespec reltime;
	175
	176	#define time_arg1 abs_monotime_limit
	177	#define time_arg2 &reltime
	178	#define time_prep \
	179	clock_gettime(CLOCK_MONOTONIC, &reltime); \
	180	reltime.tv_sec = abs_monotime_limit.tv_sec - reltime.tv_sec; \
	181	reltime.tv_nsec = abs_monotime_limit.tv_nsec - reltime.tv_nsec; \
	182	if (reltime.tv_nsec < 0) { \
	183	reltime.tv_sec--; \
	184	reltime.tv_nsec += 1000000000; \
	185	}
	186	#else
	187	#define time_arg1 abs_monotime_limit
	188	#define time_arg2 NULL
	189	#define time_prep
	190	#endif
	191
	192	bool ret = true;
	193	seqlock_val_t cur, cal;
	194
	195	seqlock_assert_valid(val);
	196
	197	wait_prep(sqlo);
	198	cur = atomic_load_explicit(&sqlo->pos, memory_order_relaxed);
	199
	200	while (cur & SEQLOCK_HELD) {
	201	cal = SEQLOCK_VAL(cur) - val - 1;
	202	assert(cal < 0x40000000 \|\| cal > 0xc0000000);
	203	if (cal < 0x80000000)
	204	break;
	205
	206	if ((cur & SEQLOCK_WAITERS)
	207	\|\| atomic_compare_exchange_weak_explicit(
	208	&sqlo->pos, &cur, cur \| SEQLOCK_WAITERS,
	209	memory_order_relaxed, memory_order_relaxed)) {
	210	int rv;
	211
	212	time_prep
	213
	214	rv = wait_time(sqlo, cur \| SEQLOCK_WAITERS, time_arg1,
215	time_arg2);
216	if (rv) {
217	ret = false;
218	break;
219	}
220	cur = atomic_load_explicit(&sqlo->pos,
221	memory_order_relaxed);
222	}
223	}
224	wait_done(sqlo);
225
226	return ret;
227	}
228
440d5faa DL	229	bool seqlock_check(struct seqlock *sqlo, seqlock_val_t val)
	230	{
	231	seqlock_val_t cur;
	232
	233	seqlock_assert_valid(val);
	234
6046b690 DL	235	cur = atomic_load_explicit(&sqlo->pos, memory_order_relaxed);
6046b690 DL	236	if (!(cur & SEQLOCK_HELD))
440d5faa	237	return 1;
6046b690	238	cur = SEQLOCK_VAL(cur) - val - 1;
440d5faa DL	239	assert(cur < 0x40000000 \|\| cur > 0xc0000000);
	240	return cur < 0x80000000;
	241	}
	242
	243	void seqlock_acquire_val(struct seqlock *sqlo, seqlock_val_t val)
	244	{
6046b690 DL	245	seqlock_val_t prev;
6046b690 DL	246
440d5faa DL	247	seqlock_assert_valid(val);
440d5faa DL	248
6046b690 DL	249	prev = atomic_exchange_explicit(&sqlo->pos, val, memory_order_relaxed);
	250	if (prev & SEQLOCK_WAITERS)
	251	wait_poke(sqlo);
440d5faa DL	252	}
	253
	254	void seqlock_release(struct seqlock *sqlo)
	255	{
6046b690 DL	256	seqlock_val_t prev;
	257
	258	prev = atomic_exchange_explicit(&sqlo->pos, 0, memory_order_relaxed);
	259	if (prev & SEQLOCK_WAITERS)
	260	wait_poke(sqlo);
440d5faa DL	261	}
	262
	263	void seqlock_init(struct seqlock *sqlo)
	264	{
	265	sqlo->pos = 0;
	266	wait_init(sqlo);
	267	}
	268
	269
	270	seqlock_val_t seqlock_cur(struct seqlock *sqlo)
	271	{
6046b690 DL	272	return SEQLOCK_VAL(atomic_load_explicit(&sqlo->pos,
6046b690 DL	273	memory_order_relaxed));
440d5faa DL	274	}
	275
	276	seqlock_val_t seqlock_bump(struct seqlock *sqlo)
	277	{
6046b690 DL	278	seqlock_val_t val, cur;
	279
	280	cur = atomic_load_explicit(&sqlo->pos, memory_order_relaxed);
	281	seqlock_assert_valid(cur);
	282
	283	do {
	284	val = SEQLOCK_VAL(cur) + SEQLOCK_INCR;
	285	} while (!atomic_compare_exchange_weak_explicit(&sqlo->pos, &cur, val,
	286	memory_order_relaxed, memory_order_relaxed));
440d5faa	287
6046b690 DL	288	if (cur & SEQLOCK_WAITERS)
6046b690 DL	289	wait_poke(sqlo);
440d5faa DL	290	return val;
440d5faa DL	291	}