1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2024, Linaro Limited
4 */
5
6 #include <initcall.h>
7 #include <kernel/callout.h>
8 #include <kernel/notif.h>
9 #include <kernel/panic.h>
10 #include <kernel/tee_time.h>
11 #include <kernel/virtualization.h>
12 #include <types_ext.h>
13
14 #define TEST_WD_TIMER_PERIOD_MS 1000
15
16 struct wd_data {
17 bool pending;
18 bool enabled;
19 uint16_t guest_id;
20 unsigned int timeout_count;
21 unsigned int call_count;
22 struct callout callout;
23 };
24
25 static struct wd_data default_wd_data;
26 static unsigned int wd_data_id __nex_bss;
27
get_wd_data(struct guest_partition * prtn)28 static struct wd_data *get_wd_data(struct guest_partition *prtn)
29 {
30 if (IS_ENABLED(CFG_NS_VIRTUALIZATION)) {
31 assert(prtn);
32 return virt_get_guest_spec_data(prtn, wd_data_id);
33 }
34 return &default_wd_data;
35 }
36
test_wd_callback(struct callout * co)37 static bool test_wd_callback(struct callout *co)
38 {
39 struct wd_data *wd = container_of(co, struct wd_data, callout);
40
41 if (wd->pending)
42 wd->timeout_count++;
43 wd->call_count++;
44 if (wd->call_count < 10 || !(wd->call_count % 60) || wd->pending) {
45 if (IS_ENABLED(CFG_NS_VIRTUALIZATION))
46 DMSG("WD %"PRIu16" call_count %u, timeout_count %u",
47 wd->guest_id, wd->call_count, wd->timeout_count);
48 else
49 DMSG("WD call_count %u, timeout_count %u",
50 wd->call_count, wd->timeout_count);
51 }
52 wd->pending = true;
53 notif_send_async(NOTIF_VALUE_DO_BOTTOM_HALF, wd->guest_id);
54
55 return true;
56 }
57
wd_ndrv_atomic_cb(struct notif_driver * ndrv __unused,enum notif_event ev,uint16_t guest_id)58 static void wd_ndrv_atomic_cb(struct notif_driver *ndrv __unused,
59 enum notif_event ev, uint16_t guest_id)
60 {
61 if (ev == NOTIF_EVENT_STARTED) {
62 struct guest_partition *prtn = virt_get_guest(guest_id);
63 struct wd_data *wd = get_wd_data(prtn);
64
65 if (!wd->enabled) {
66 wd->guest_id = guest_id;
67 callout_add(&wd->callout, test_wd_callback,
68 TEST_WD_TIMER_PERIOD_MS);
69
70 wd->enabled = true;
71 }
72 virt_put_guest(prtn);
73 }
74 }
75 DECLARE_KEEP_PAGER(wd_ndrv_atomic_cb);
76
wd_ndrv_yielding_cb(struct notif_driver * ndrv __unused,enum notif_event ev)77 static void wd_ndrv_yielding_cb(struct notif_driver *ndrv __unused,
78 enum notif_event ev)
79 {
80 if (ev == NOTIF_EVENT_DO_BOTTOM_HALF) {
81 struct guest_partition *prtn = virt_get_current_guest();
82 struct wd_data *wd = get_wd_data(prtn);
83
84 if (wd->pending && wd->call_count < 10)
85 DMSG("Clearing pending");
86 wd->pending = false;
87 virt_put_guest(prtn);
88 }
89 }
90
91 struct notif_driver wd_ndrv __nex_data = {
92 .atomic_cb = wd_ndrv_atomic_cb,
93 .yielding_cb = wd_ndrv_yielding_cb,
94 };
95
wd_data_destroy(void * data)96 static void wd_data_destroy(void *data)
97 {
98 struct wd_data *wd = data;
99
100 callout_rem(&wd->callout);
101 }
102
nex_init_test_wd(void)103 static TEE_Result nex_init_test_wd(void)
104 {
105 TEE_Result res = TEE_SUCCESS;
106
107 if (IS_ENABLED(CFG_NS_VIRTUALIZATION)) {
108 res = virt_add_guest_spec_data(&wd_data_id,
109 sizeof(struct wd_data),
110 wd_data_destroy);
111 if (res)
112 return res;
113 }
114
115 notif_register_driver(&wd_ndrv);
116
117 return TEE_SUCCESS;
118 }
119
120 nex_early_init(nex_init_test_wd);
121
122 struct periodic_data {
123 unsigned int count;
124 struct callout callout;
125 };
126
periodic_callback(struct callout * co)127 static bool periodic_callback(struct callout *co)
128 {
129 struct periodic_data *d = container_of(co, struct periodic_data,
130 callout);
131 TEE_Time t = { };
132
133 if (tee_time_get_sys_time(&t))
134 panic();
135 d->count++;
136 DMSG("seconds %"PRIu32" millis %"PRIu32" count %u",
137 t.seconds, t.millis, d->count);
138
139 if (d->count > 20) {
140 DMSG("Disabling periodic callout");
141 return false;
142 }
143
144 return true;
145 }
146 DECLARE_KEEP_PAGER(periodic_callback);
147
nex_init_periodic_callback(void)148 static TEE_Result nex_init_periodic_callback(void)
149 {
150 struct periodic_data *d = nex_calloc(1, sizeof(*d));
151
152 if (!d)
153 return TEE_ERROR_OUT_OF_MEMORY;
154
155 DMSG("Adding a periodic callout");
156 callout_add(&d->callout, periodic_callback, TEST_WD_TIMER_PERIOD_MS);
157
158 return TEE_SUCCESS;
159 }
160
161 nex_early_init(nex_init_periodic_callback);
162