subr_fault.c revision 1.1 
1/*	$NetBSD: subr_fault.c,v 1.1 2020/06/07 09:45:19 maxv Exp $	*/
2
3/*
4 * Copyright (c) 2020 The NetBSD Foundation, Inc.
5 * All rights reserved.
6 *
7 * This code is derived from software contributed to The NetBSD Foundation
8 * by Maxime Villard.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 * 1. Redistributions of source code must retain the above copyright
14 *    notice, this list of conditions and the following disclaimer.
15 * 2. Redistributions in binary form must reproduce the above copyright
16 *    notice, this list of conditions and the following disclaimer in the
17 *    documentation and/or other materials provided with the distribution.
18 *
19 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
20 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
21 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
22 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
23 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 * POSSIBILITY OF SUCH DAMAGE.
30 */
31
32#include <sys/cdefs.h>
33__KERNEL_RCSID(0, "$NetBSD: subr_fault.c,v 1.1 2020/06/07 09:45:19 maxv Exp $");
34
35#include <sys/module.h>
36#include <sys/param.h>
37#include <sys/systm.h>
38#include <sys/kernel.h>
39
40#include <sys/conf.h>
41#include <sys/types.h>
42#include <sys/specificdata.h>
43#include <sys/kmem.h>
44#include <sys/atomic.h>
45#include <sys/ioccom.h>
46#include <sys/lwp.h>
47#include <sys/fault.h>
48
49typedef struct {
50	volatile bool enabled;
51	volatile unsigned long nth;
52	volatile unsigned long cnt;
53	volatile unsigned long nfaults;
54} fault_t;
55
56static fault_t fault_global __cacheline_aligned = {
57	.enabled = false,
58	.nth = 2,
59	.cnt = 0,
60	.nfaults = 0
61};
62
63static kmutex_t fault_global_lock __cacheline_aligned;
64static specificdata_key_t fault_lwp_key;
65
66/* -------------------------------------------------------------------------- */
67
68bool
69fault_inject(void)
70{
71	volatile unsigned long cnt;
72	fault_t *f;
73
74	if (__predict_false(cold))
75		return false;
76
77	if (__predict_false(atomic_load_acquire(&fault_global.enabled))) {
78		f = &fault_global;
79	} else {
80		f = lwp_getspecific(fault_lwp_key);
81		if (__predict_true(f == NULL))
82			return false;
83		if (__predict_false(!f->enabled))
84			return false;
85	}
86
87	cnt = atomic_inc_ulong_nv(&f->cnt);
88	if (__predict_false(cnt % atomic_load_relaxed(&f->nth) == 0)) {
89		atomic_inc_ulong(&f->nfaults);
90		return true;
91	}
92
93	return false;
94}
95
96/* -------------------------------------------------------------------------- */
97
98static int
99fault_open(dev_t dev, int flag, int mode, struct lwp *l)
100{
101	return 0;
102}
103
104static int
105fault_close(dev_t dev, int flag, int mode, struct lwp *l)
106{
107	return 0;
108}
109
110static int
111fault_ioc_enable(struct fault_ioc_enable *args)
112{
113	fault_t *f;
114
115	if (args->mode != FAULT_MODE_NTH)
116		return EINVAL;
117	if (args->nth < 2)
118		return EINVAL;
119
120	switch (args->scope) {
121	case FAULT_SCOPE_GLOBAL:
122		mutex_enter(&fault_global_lock);
123		if (fault_global.enabled) {
124			mutex_exit(&fault_global_lock);
125			return EEXIST;
126		}
127		atomic_store_relaxed(&fault_global.nth, args->nth);
128		fault_global.cnt = 0;
129		fault_global.nfaults = 0;
130		atomic_store_release(&fault_global.enabled, true);
131		mutex_exit(&fault_global_lock);
132		break;
133	case FAULT_SCOPE_LWP:
134		f = lwp_getspecific(fault_lwp_key);
135		if (f != NULL) {
136			if (f->enabled)
137				return EEXIST;
138		} else {
139			f = kmem_zalloc(sizeof(*f), KM_SLEEP);
140			lwp_setspecific(fault_lwp_key, f);
141		}
142		atomic_store_relaxed(&f->nth, args->nth);
143		f->cnt = 0;
144		f->nfaults = 0;
145		atomic_store_release(&f->enabled, true);
146		break;
147	default:
148		return EINVAL;
149	}
150
151	return 0;
152}
153
154static int
155fault_ioc_disable(struct fault_ioc_disable *args)
156{
157	fault_t *f;
158
159	switch (args->scope) {
160	case FAULT_SCOPE_GLOBAL:
161		mutex_enter(&fault_global_lock);
162		if (!fault_global.enabled) {
163			mutex_exit(&fault_global_lock);
164			return ENOENT;
165		}
166		atomic_store_release(&fault_global.enabled, false);
167		mutex_exit(&fault_global_lock);
168		break;
169	case FAULT_SCOPE_LWP:
170		f = lwp_getspecific(fault_lwp_key);
171		if (f == NULL)
172			return ENOENT;
173		if (!f->enabled)
174			return ENOENT;
175		atomic_store_release(&f->enabled, false);
176		break;
177	default:
178		return EINVAL;
179	}
180
181	return 0;
182}
183
184static int
185fault_ioc_getinfo(struct fault_ioc_getinfo *args)
186{
187	fault_t *f;
188
189	switch (args->scope) {
190	case FAULT_SCOPE_GLOBAL:
191		args->nfaults = atomic_load_relaxed(&fault_global.nfaults);
192		break;
193	case FAULT_SCOPE_LWP:
194		f = lwp_getspecific(fault_lwp_key);
195		if (f == NULL)
196			return ENOENT;
197		args->nfaults = atomic_load_relaxed(&f->nfaults);
198		break;
199	default:
200		return EINVAL;
201	}
202
203	return 0;
204}
205
206static int
207fault_ioctl(dev_t dev, u_long cmd, void *addr, int flag, struct lwp *l)
208{
209	switch (cmd) {
210	case FAULT_IOC_ENABLE:
211		return fault_ioc_enable(addr);
212	case FAULT_IOC_DISABLE:
213		return fault_ioc_disable(addr);
214	case FAULT_IOC_GETINFO:
215		return fault_ioc_getinfo(addr);
216	default:
217		return EINVAL;
218	}
219}
220
221const struct cdevsw fault_cdevsw = {
222	.d_open = fault_open,
223	.d_close = fault_close,
224	.d_read = noread,
225	.d_write = nowrite,
226	.d_ioctl = fault_ioctl,
227	.d_stop = nostop,
228	.d_tty = notty,
229	.d_poll = nopoll,
230	.d_mmap = nommap,
231	.d_kqfilter = nokqfilter,
232	.d_discard = nodiscard,
233	.d_flag = D_OTHER | D_MPSAFE
234};
235
236/* -------------------------------------------------------------------------- */
237
238MODULE(MODULE_CLASS_MISC, fault, NULL);
239
240static void
241fault_lwp_free(void *arg)
242{
243	fault_t *f = (fault_t *)arg;
244
245	if (f == NULL) {
246		return;
247	}
248
249	kmem_free(f, sizeof(*f));
250}
251
252static void
253fault_init(void)
254{
255	mutex_init(&fault_global_lock, MUTEX_DEFAULT, IPL_NONE);
256	lwp_specific_key_create(&fault_lwp_key, fault_lwp_free);
257}
258
259static int
260fault_modcmd(modcmd_t cmd, void *arg)
261{
262   	switch (cmd) {
263	case MODULE_CMD_INIT:
264		fault_init();
265		return 0;
266	case MODULE_CMD_FINI:
267		return EINVAL;
268	default:
269		return ENOTTY;
270	}
271}
272