1 /*
2  * Arm SCP/MCP Software
3  * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
4  *
5  * SPDX-License-Identifier: BSD-3-Clause
6  */
7 
8 #include <mod_power_domain.h>
9 #include <mod_scmi_perf.h>
10 #include <mod_traffic_cop.h>
11 
12 #include <fwk_core.h>
13 #include <fwk_id.h>
14 #include <fwk_log.h>
15 #include <fwk_mm.h>
16 #include <fwk_module.h>
17 #include <fwk_notification.h>
18 #include <fwk_status.h>
19 
20 struct mod_tcop_core_ctx {
21     /* Core Identifier */
22     fwk_id_t core_id;
23 
24     /* The core is online */
25     bool online;
26 
27     /* Used to block the PD when transitioning from OFF to ON */
28     bool pd_blocked;
29 
30     /* Cookie to un-block the PD transition from OFF to ON */
31     uint32_t cookie;
32 };
33 
34 struct mod_tcop_domain_ctx {
35     /* Context Domain ID */
36     fwk_id_t domain_id;
37 
38     /* Number of cores to monitor */
39     uint32_t num_cores;
40 
41     /* Number of cores online */
42     uint32_t num_cores_online;
43 
44     /* Latest perf level value as reported by the plugin handler */
45     uint32_t current_perf_level;
46 
47     /* Latest perf limit value required by traffic cop */
48     uint32_t perf_limit;
49 
50     /* Core context */
51     struct mod_tcop_core_ctx *core_ctx;
52 
53     /* Domain configuration */
54     const struct mod_tcop_domain_config *domain_config;
55 };
56 
57 static struct mod_tcop_ctx {
58     /* Number of domains */
59     uint32_t tcop_domain_count;
60 
61     /* Domain context table */
62     struct mod_tcop_domain_ctx *domain_ctx;
63 
64     /* Perf plugin API */
65     struct perf_plugins_handler_api *perf_plugins_handler_api;
66 } tcop_ctx;
67 
get_domain_ctx(fwk_id_t domain_id)68 static struct mod_tcop_domain_ctx *get_domain_ctx(fwk_id_t domain_id)
69 {
70     uint32_t idx = fwk_id_get_element_idx(domain_id);
71 
72     if (idx < tcop_ctx.tcop_domain_count) {
73         return &tcop_ctx.domain_ctx[idx];
74     } else {
75         return NULL;
76     }
77 }
78 
tcop_evaluate_perf_limit(struct mod_tcop_domain_ctx * ctx)79 static uint32_t tcop_evaluate_perf_limit(struct mod_tcop_domain_ctx *ctx)
80 {
81     int pct_idx;
82     size_t pct_size;
83     struct mod_tcop_pct_table *pct_config;
84 
85     /* Parse PCT table from the bottom-up*/
86     pct_config = ctx->domain_config->pct;
87     pct_size = ctx->domain_config->pct_size;
88     /* Start from last table index */
89     for (pct_idx = (pct_size - 1); pct_idx >= 0; pct_idx--) {
90         /* Search for the table entry that matches the number of cores online */
91         if (ctx->num_cores_online <= pct_config[pct_idx].cores_online) {
92             return pct_config[pct_idx].perf_limit;
93         }
94     }
95 
96     FWK_LOG_WARN(
97         "[TRAFFIC_COP] No entry found in the PCT for %ld online cores",
98         (long)ctx->num_cores_online);
99 
100     return 0;
101 }
102 
103 /*
104  * Update function will be called periodically. It needs to maintain the
105  * performance limits.
106  */
tcop_update(struct perf_plugins_perf_update * data)107 static int tcop_update(struct perf_plugins_perf_update *data)
108 {
109     uint32_t domain_idx;
110     struct mod_tcop_domain_ctx *domain_ctx;
111     /*
112      * Get the performance element id from the sub-element provided in the
113      * function argument.
114      */
115     fwk_id_t perf_id = FWK_ID_ELEMENT(
116         FWK_MODULE_IDX_DVFS, fwk_id_get_element_idx(data->domain_id));
117 
118     for (domain_idx = 0; domain_idx < tcop_ctx.tcop_domain_count;
119          domain_idx++) {
120         if (fwk_id_is_equal(
121                 tcop_ctx.domain_ctx[domain_idx].domain_config->perf_id,
122                 perf_id)) {
123             break;
124         }
125     }
126 
127     if (domain_idx == tcop_ctx.tcop_domain_count) {
128         return FWK_E_PARAM;
129     }
130 
131     domain_ctx = &tcop_ctx.domain_ctx[domain_idx];
132 
133     /* Keep the last calculated performance limits. */
134     data->adj_max_limit[0] = domain_ctx->perf_limit;
135 
136     return FWK_SUCCESS;
137 }
138 
tcop_report(struct perf_plugins_perf_report * data)139 static int tcop_report(struct perf_plugins_perf_report *data)
140 {
141     int status;
142     uint32_t core_idx, domain_idx;
143     struct fwk_event resp_notif;
144     struct mod_tcop_domain_ctx *domain_ctx;
145     struct mod_pd_power_state_pre_transition_notification_resp_params
146         *pd_resp_params =
147             (struct mod_pd_power_state_pre_transition_notification_resp_params
148                  *)resp_notif.params;
149     /*
150      * Get the performance element id from the sub-element provided in the
151      * function argument.
152      */
153     fwk_id_t perf_id = FWK_ID_ELEMENT(
154         FWK_MODULE_IDX_DVFS, fwk_id_get_element_idx(data->dep_dom_id));
155 
156     for (domain_idx = 0; domain_idx < tcop_ctx.tcop_domain_count;
157          domain_idx++) {
158         if (fwk_id_is_equal(
159                 tcop_ctx.domain_ctx[domain_idx].domain_config->perf_id,
160                 perf_id)) {
161             break;
162         }
163     }
164 
165     if (domain_idx == tcop_ctx.tcop_domain_count) {
166         return FWK_E_PARAM;
167     }
168 
169     domain_ctx = &tcop_ctx.domain_ctx[domain_idx];
170     domain_ctx->current_perf_level = data->level;
171 
172     /*
173      * If a previous core wake-up sequence was delayed to allow a period of time
174      * for the perf transition to take place, then respond to the power domain
175      * notification so the core can now be turned on.
176      */
177     for (core_idx = 0; core_idx < domain_ctx->num_cores; core_idx++) {
178         if (domain_ctx->core_ctx[core_idx].pd_blocked) {
179             domain_ctx->core_ctx[core_idx].pd_blocked = false;
180 
181             status = fwk_get_delayed_response(
182                 domain_ctx->domain_id,
183                 domain_ctx->core_ctx[core_idx].cookie,
184                 &resp_notif);
185             if (status != FWK_SUCCESS) {
186                 return status;
187             }
188 
189             pd_resp_params->status = FWK_SUCCESS;
190             status = fwk_put_event(&resp_notif);
191             if (status != FWK_SUCCESS) {
192                 return status;
193             }
194         }
195     }
196 
197     return FWK_SUCCESS;
198 }
199 
200 static struct perf_plugins_api perf_plugins_api = {
201     .update = tcop_update,
202     .report = tcop_report,
203 };
204 
205 /*
206  * Framework handlers
207  */
tcop_init(fwk_id_t module_id,unsigned int element_count,const void * data)208 static int tcop_init(
209     fwk_id_t module_id,
210     unsigned int element_count,
211     const void *data)
212 {
213     if (element_count == 0) {
214         return FWK_E_PARAM;
215     }
216 
217     tcop_ctx.tcop_domain_count = element_count;
218     tcop_ctx.domain_ctx =
219         fwk_mm_calloc(element_count, sizeof(struct mod_tcop_domain_ctx));
220 
221     return FWK_SUCCESS;
222 }
223 
tcop_element_init(fwk_id_t domain_id,unsigned int sub_element_count,const void * data)224 static int tcop_element_init(
225     fwk_id_t domain_id,
226     unsigned int sub_element_count,
227     const void *data)
228 {
229     struct mod_tcop_domain_ctx *domain_ctx;
230     struct mod_tcop_core_ctx *core_ctx;
231     struct mod_tcop_core_config const *core_config;
232     uint32_t core_idx;
233 
234     if (sub_element_count == 0) {
235         return FWK_E_PARAM;
236     }
237 
238     domain_ctx = get_domain_ctx(domain_id);
239     domain_ctx->domain_id = domain_id;
240     domain_ctx->num_cores = sub_element_count;
241 
242     /* Initialize the configuration */
243     domain_ctx->domain_config = data;
244     fwk_assert(domain_ctx->domain_config->pct != NULL);
245 
246     domain_ctx->core_ctx =
247         fwk_mm_calloc(sub_element_count, sizeof(struct mod_tcop_core_ctx));
248 
249     /* Initialize each core */
250     for (core_idx = 0; core_idx < domain_ctx->num_cores; core_idx++) {
251         core_ctx = &domain_ctx->core_ctx[core_idx];
252         core_config = &domain_ctx->domain_config->core_config[core_idx];
253 
254         if (core_config->core_starts_online) {
255             domain_ctx->num_cores_online++;
256             core_ctx->online = true;
257         }
258     }
259 
260     return FWK_SUCCESS;
261 }
262 
tcop_bind(fwk_id_t id,unsigned int round)263 static int tcop_bind(fwk_id_t id, unsigned int round)
264 {
265     /* Bind in the second round */
266     if ((round == 0) || (!fwk_module_is_valid_module_id(id))) {
267         return FWK_SUCCESS;
268     }
269 
270     return fwk_module_bind(
271         FWK_ID_MODULE(FWK_MODULE_IDX_SCMI_PERF),
272         FWK_ID_API(FWK_MODULE_IDX_SCMI_PERF, MOD_SCMI_PERF_PLUGINS_API),
273         &tcop_ctx.perf_plugins_handler_api);
274 }
275 
tcop_process_bind_request(fwk_id_t source_id,fwk_id_t target_id,fwk_id_t api_id,const void ** api)276 static int tcop_process_bind_request(
277     fwk_id_t source_id,
278     fwk_id_t target_id,
279     fwk_id_t api_id,
280     const void **api)
281 {
282     if (fwk_id_is_equal(source_id, FWK_ID_MODULE(FWK_MODULE_IDX_SCMI_PERF))) {
283         *api = &perf_plugins_api;
284     } else {
285         return FWK_E_ACCESS;
286     }
287 
288     return FWK_SUCCESS;
289 }
290 
tcop_start(fwk_id_t id)291 static int tcop_start(fwk_id_t id)
292 {
293     int status;
294     uint32_t i;
295     struct mod_tcop_domain_ctx *domain_ctx;
296 
297     if (fwk_module_is_valid_module_id(id)) {
298         return FWK_SUCCESS;
299     }
300 
301     /* Subscribe to core power state transition */
302     domain_ctx = get_domain_ctx(id);
303 
304     for (i = 0; i < domain_ctx->num_cores; i++) {
305         status = fwk_notification_subscribe(
306             mod_pd_notification_id_power_state_pre_transition,
307             domain_ctx->domain_config->core_config[i].pd_id,
308             domain_ctx->domain_id);
309         if (status != FWK_SUCCESS) {
310             return status;
311         }
312 
313         status = fwk_notification_subscribe(
314             mod_pd_notification_id_power_state_transition,
315             domain_ctx->domain_config->core_config[i].pd_id,
316             domain_ctx->domain_id);
317         if (status != FWK_SUCCESS) {
318             return status;
319         }
320     }
321 
322     /*
323      * Evaluate limits right now to initialise the limits accordingly with the
324      * known initial power status of cores.
325      */
326     domain_ctx->perf_limit = tcop_evaluate_perf_limit(domain_ctx);
327 
328     return FWK_SUCCESS;
329 }
330 
tcop_process_notification(const struct fwk_event * event,struct fwk_event * resp_event)331 static int tcop_process_notification(
332     const struct fwk_event *event,
333     struct fwk_event *resp_event)
334 {
335     struct mod_pd_power_state_pre_transition_notification_params
336         *pre_state_params;
337     struct mod_pd_power_state_transition_notification_params *post_state_params;
338     struct mod_pd_power_state_pre_transition_notification_resp_params
339         *pd_resp_params =
340             (struct mod_pd_power_state_pre_transition_notification_resp_params
341                  *)resp_event->params;
342     struct mod_tcop_domain_ctx *domain_ctx;
343     uint32_t core_idx;
344 
345     fwk_assert(fwk_module_is_valid_element_id(event->target_id));
346     domain_ctx = get_domain_ctx(event->target_id);
347     if (domain_ctx == NULL) {
348         return FWK_E_PARAM;
349     }
350 
351     /* Find the corresponding core */
352     for (core_idx = 0; core_idx < domain_ctx->num_cores; core_idx++) {
353         if (fwk_id_is_equal(
354                 domain_ctx->domain_config->core_config[core_idx].pd_id,
355                 event->source_id)) {
356             break;
357         }
358     }
359 
360     if (core_idx >= domain_ctx->num_cores) {
361         return FWK_E_PARAM;
362     }
363 
364     if (fwk_id_is_equal(
365             event->id, mod_pd_notification_id_power_state_pre_transition)) {
366         pre_state_params =
367             (struct mod_pd_power_state_pre_transition_notification_params *)
368                 event->params;
369         pd_resp_params->status = FWK_SUCCESS;
370         if (pre_state_params->target_state == MOD_PD_STATE_ON) {
371             /* The core is transitioning to online */
372             domain_ctx->num_cores_online++;
373             domain_ctx->core_ctx[core_idx].online = true;
374             domain_ctx->perf_limit = tcop_evaluate_perf_limit(domain_ctx);
375 
376             /* Set the new limits */
377             struct plugin_limits_req plugin_limit_req = {
378                 .domain_id = domain_ctx->domain_config->perf_id,
379                 .max_limit = domain_ctx->perf_limit,
380             };
381 
382             tcop_ctx.perf_plugins_handler_api->plugin_set_limits(
383                 &plugin_limit_req);
384 
385             /*
386              * If the perf limit requested will not trigger a dvfs change, there
387              * is no need to block the power domain state transition.
388              */
389             if (domain_ctx->perf_limit >= domain_ctx->current_perf_level) {
390                 return FWK_SUCCESS;
391             }
392 
393             /* Block the power domain until the new level is applied */
394             domain_ctx->core_ctx[core_idx].pd_blocked = true;
395             domain_ctx->core_ctx[core_idx].cookie = event->cookie;
396             resp_event->is_delayed_response = true;
397         }
398 
399     } else if (fwk_id_is_equal(
400                    event->id, mod_pd_notification_id_power_state_transition)) {
401         post_state_params =
402             (struct mod_pd_power_state_transition_notification_params *)
403                 event->params;
404         if (post_state_params->state != MOD_PD_STATE_ON) {
405             /* The core transitioned to offline */
406             domain_ctx->num_cores_online--;
407             domain_ctx->core_ctx[core_idx].online = false;
408             domain_ctx->perf_limit = tcop_evaluate_perf_limit(domain_ctx);
409             if (domain_ctx->perf_limit == 0) {
410                 return FWK_E_PARAM;
411             }
412         }
413     }
414 
415     return FWK_SUCCESS;
416 }
417 
418 const struct fwk_module module_traffic_cop = {
419     .type = FWK_MODULE_TYPE_SERVICE,
420     .api_count = 1,
421     .init = tcop_init,
422     .element_init = tcop_element_init,
423     .start = tcop_start,
424     .bind = tcop_bind,
425     .process_bind_request = tcop_process_bind_request,
426     .process_notification = tcop_process_notification,
427 };
428