1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38#define pr_fmt(fmt) "ii_dev: " fmt
39
40#include <linux/cpu.h>
41#include <linux/hrtimer.h>
42#include <linux/kthread.h>
43#include <linux/sched.h>
44#include <linux/slab.h>
45#include <linux/smpboot.h>
46
47#include <uapi/linux/sched/types.h>
48
49
50
51
52
53
54struct idle_inject_thread {
55 struct task_struct *tsk;
56 int should_run;
57};
58
59
60
61
62
63
64
65
66
67struct idle_inject_device {
68 struct hrtimer timer;
69 unsigned int idle_duration_us;
70 unsigned int run_duration_us;
71 unsigned int latency_us;
72 unsigned long cpumask[];
73};
74
75static DEFINE_PER_CPU(struct idle_inject_thread, idle_inject_thread);
76static DEFINE_PER_CPU(struct idle_inject_device *, idle_inject_device);
77
78
79
80
81
82
83
84
85static void idle_inject_wakeup(struct idle_inject_device *ii_dev)
86{
87 struct idle_inject_thread *iit;
88 unsigned int cpu;
89
90 for_each_cpu_and(cpu, to_cpumask(ii_dev->cpumask), cpu_online_mask) {
91 iit = per_cpu_ptr(&idle_inject_thread, cpu);
92 iit->should_run = 1;
93 wake_up_process(iit->tsk);
94 }
95}
96
97
98
99
100
101
102
103
104
105
106
107static enum hrtimer_restart idle_inject_timer_fn(struct hrtimer *timer)
108{
109 unsigned int duration_us;
110 struct idle_inject_device *ii_dev =
111 container_of(timer, struct idle_inject_device, timer);
112
113 duration_us = READ_ONCE(ii_dev->run_duration_us);
114 duration_us += READ_ONCE(ii_dev->idle_duration_us);
115
116 idle_inject_wakeup(ii_dev);
117
118 hrtimer_forward_now(timer, ns_to_ktime(duration_us * NSEC_PER_USEC));
119
120 return HRTIMER_RESTART;
121}
122
123
124
125
126
127
128
129
130static void idle_inject_fn(unsigned int cpu)
131{
132 struct idle_inject_device *ii_dev;
133 struct idle_inject_thread *iit;
134
135 ii_dev = per_cpu(idle_inject_device, cpu);
136 iit = per_cpu_ptr(&idle_inject_thread, cpu);
137
138
139
140
141 iit->should_run = 0;
142
143 play_idle_precise(READ_ONCE(ii_dev->idle_duration_us) * NSEC_PER_USEC,
144 READ_ONCE(ii_dev->latency_us) * NSEC_PER_USEC);
145}
146
147
148
149
150
151
152void idle_inject_set_duration(struct idle_inject_device *ii_dev,
153 unsigned int run_duration_us,
154 unsigned int idle_duration_us)
155{
156 if (run_duration_us && idle_duration_us) {
157 WRITE_ONCE(ii_dev->run_duration_us, run_duration_us);
158 WRITE_ONCE(ii_dev->idle_duration_us, idle_duration_us);
159 }
160}
161
162
163
164
165
166
167void idle_inject_get_duration(struct idle_inject_device *ii_dev,
168 unsigned int *run_duration_us,
169 unsigned int *idle_duration_us)
170{
171 *run_duration_us = READ_ONCE(ii_dev->run_duration_us);
172 *idle_duration_us = READ_ONCE(ii_dev->idle_duration_us);
173}
174
175
176
177
178
179void idle_inject_set_latency(struct idle_inject_device *ii_dev,
180 unsigned int latency_us)
181{
182 WRITE_ONCE(ii_dev->latency_us, latency_us);
183}
184
185
186
187
188
189
190
191
192
193
194
195int idle_inject_start(struct idle_inject_device *ii_dev)
196{
197 unsigned int idle_duration_us = READ_ONCE(ii_dev->idle_duration_us);
198 unsigned int run_duration_us = READ_ONCE(ii_dev->run_duration_us);
199
200 if (!idle_duration_us || !run_duration_us)
201 return -EINVAL;
202
203 pr_debug("Starting injecting idle cycles on CPUs '%*pbl'\n",
204 cpumask_pr_args(to_cpumask(ii_dev->cpumask)));
205
206 idle_inject_wakeup(ii_dev);
207
208 hrtimer_start(&ii_dev->timer,
209 ns_to_ktime((idle_duration_us + run_duration_us) *
210 NSEC_PER_USEC),
211 HRTIMER_MODE_REL);
212
213 return 0;
214}
215
216
217
218
219
220
221
222
223
224
225
226
227void idle_inject_stop(struct idle_inject_device *ii_dev)
228{
229 struct idle_inject_thread *iit;
230 unsigned int cpu;
231
232 pr_debug("Stopping idle injection on CPUs '%*pbl'\n",
233 cpumask_pr_args(to_cpumask(ii_dev->cpumask)));
234
235 hrtimer_cancel(&ii_dev->timer);
236
237
238
239
240
241
242
243
244 cpu_hotplug_disable();
245
246
247
248
249
250
251
252 for_each_cpu(cpu, to_cpumask(ii_dev->cpumask)) {
253 iit = per_cpu_ptr(&idle_inject_thread, cpu);
254 iit->should_run = 0;
255
256 wait_task_inactive(iit->tsk, 0);
257 }
258
259 cpu_hotplug_enable();
260}
261
262
263
264
265
266
267
268
269static void idle_inject_setup(unsigned int cpu)
270{
271 sched_set_fifo(current);
272}
273
274
275
276
277
278
279
280static int idle_inject_should_run(unsigned int cpu)
281{
282 struct idle_inject_thread *iit =
283 per_cpu_ptr(&idle_inject_thread, cpu);
284
285 return iit->should_run;
286}
287
288
289
290
291
292
293
294
295
296
297
298
299struct idle_inject_device *idle_inject_register(struct cpumask *cpumask)
300{
301 struct idle_inject_device *ii_dev;
302 int cpu, cpu_rb;
303
304 ii_dev = kzalloc(sizeof(*ii_dev) + cpumask_size(), GFP_KERNEL);
305 if (!ii_dev)
306 return NULL;
307
308 cpumask_copy(to_cpumask(ii_dev->cpumask), cpumask);
309 hrtimer_init(&ii_dev->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
310 ii_dev->timer.function = idle_inject_timer_fn;
311 ii_dev->latency_us = UINT_MAX;
312
313 for_each_cpu(cpu, to_cpumask(ii_dev->cpumask)) {
314
315 if (per_cpu(idle_inject_device, cpu)) {
316 pr_err("cpu%d is already registered\n", cpu);
317 goto out_rollback;
318 }
319
320 per_cpu(idle_inject_device, cpu) = ii_dev;
321 }
322
323 return ii_dev;
324
325out_rollback:
326 for_each_cpu(cpu_rb, to_cpumask(ii_dev->cpumask)) {
327 if (cpu == cpu_rb)
328 break;
329 per_cpu(idle_inject_device, cpu_rb) = NULL;
330 }
331
332 kfree(ii_dev);
333
334 return NULL;
335}
336
337
338
339
340
341
342
343
344
345void idle_inject_unregister(struct idle_inject_device *ii_dev)
346{
347 unsigned int cpu;
348
349 idle_inject_stop(ii_dev);
350
351 for_each_cpu(cpu, to_cpumask(ii_dev->cpumask))
352 per_cpu(idle_inject_device, cpu) = NULL;
353
354 kfree(ii_dev);
355}
356
357static struct smp_hotplug_thread idle_inject_threads = {
358 .store = &idle_inject_thread.tsk,
359 .setup = idle_inject_setup,
360 .thread_fn = idle_inject_fn,
361 .thread_comm = "idle_inject/%u",
362 .thread_should_run = idle_inject_should_run,
363};
364
365static int __init idle_inject_init(void)
366{
367 return smpboot_register_percpu_thread(&idle_inject_threads);
368}
369early_initcall(idle_inject_init);
370