PM: EM: Implement em_nl_get_pd_table_doit()

When a userspace requests EM_CMD_GET_PD_TABLE with an ID of a performance
domain, the kernel reports back the energy model table of the specified
performance domain. The message format of the response is as follows:

EM_A_PD_TABLE_PD_ID (NLA_U32)
EM_A_PD_TABLE_PS (NLA_NESTED)*
    EM_A_PS_PERFORMANCE (NLA_U64)
    EM_A_PS_FREQUENCY (NLA_U64)
    EM_A_PS_POWER (NLA_U64)
    EM_A_PS_COST (NLA_U64)
    EM_A_PS_FLAGS (NLA_U64)

where EM_A_PD_TABLE_PS can be repeated as many times as there are
performance states (struct em_perf_state).

Signed-off-by: Changwoo Min <changwoo@igalia.com>
Reviewed-by: Lukasz Luba <lukasz.luba@arm.com>
Link: https://patch.msgid.link/20251020220914.320832-8-changwoo@igalia.com
Signed-off-by: Rafael J. Wysocki <rafael.j.wysocki@intel.com>

diff --git a/kernel/power/em_netlink.c b/kernel/power/em_netlink.c
index 16bdcf47f4d89b3e3d361e35a1edc8edd534627d..e144624f03353ad79cb467a74f25b1a5cfee0cd3 100644 (file)
--- a/kernel/power/em_netlink.c
+++ b/kernel/power/em_netlink.c
@@ -102,9 +102,115 @@ out_free_msg:
        return ret;
 }
 
+static struct em_perf_domain *__em_nl_get_pd_table_id(struct nlattr **attrs)
+{
+       struct em_perf_domain *pd;
+       int id;
+
+       if (!attrs[EM_A_PD_TABLE_PD_ID])
+               return NULL;
+
+       id = nla_get_u32(attrs[EM_A_PD_TABLE_PD_ID]);
+       pd = em_perf_domain_get_by_id(id);
+       return pd;
+}
+
+static int __em_nl_get_pd_table_size(const struct em_perf_domain *pd)
+{
+       int id_sz, ps_sz;
+
+       id_sz = nla_total_size(sizeof(u32));            /* EM_A_PD_TABLE_PD_ID */
+       ps_sz = nla_total_size(0) +                     /* EM_A_PD_TABLE_PS */
+               nla_total_size_64bit(sizeof(u64)) +     /* EM_A_PS_PERFORMANCE */
+               nla_total_size_64bit(sizeof(u64)) +     /* EM_A_PS_FREQUENCY */
+               nla_total_size_64bit(sizeof(u64)) +     /* EM_A_PS_POWER */
+               nla_total_size_64bit(sizeof(u64)) +     /* EM_A_PS_COST */
+               nla_total_size_64bit(sizeof(u64));      /* EM_A_PS_FLAGS */
+       ps_sz *= pd->nr_perf_states;
+
+       return nlmsg_total_size(genlmsg_msg_size(id_sz + ps_sz));
+}
+
+static int __em_nl_get_pd_table(struct sk_buff *msg, const struct em_perf_domain *pd)
+{
+       struct em_perf_state *table, *ps;
+       struct nlattr *entry;
+       int i;
+
+       if (nla_put_u32(msg, EM_A_PD_TABLE_PD_ID, pd->id))
+               goto out_err;
+
+       rcu_read_lock();
+       table = em_perf_state_from_pd((struct em_perf_domain *)pd);
+
+       for (i = 0; i < pd->nr_perf_states; i++) {
+               ps = &table[i];
+
+               entry = nla_nest_start(msg, EM_A_PD_TABLE_PS);
+               if (!entry)
+                       goto out_unlock_ps;
+
+               if (nla_put_u64_64bit(msg, EM_A_PS_PERFORMANCE,
+                                     ps->performance, EM_A_PS_PAD))
+                       goto out_cancel_ps_nest;
+               if (nla_put_u64_64bit(msg, EM_A_PS_FREQUENCY,
+                                     ps->frequency, EM_A_PS_PAD))
+                       goto out_cancel_ps_nest;
+               if (nla_put_u64_64bit(msg, EM_A_PS_POWER,
+                                     ps->power, EM_A_PS_PAD))
+                       goto out_cancel_ps_nest;
+               if (nla_put_u64_64bit(msg, EM_A_PS_COST,
+                                     ps->cost, EM_A_PS_PAD))
+                       goto out_cancel_ps_nest;
+               if (nla_put_u64_64bit(msg, EM_A_PS_FLAGS,
+                                     ps->flags, EM_A_PS_PAD))
+                       goto out_cancel_ps_nest;
+
+               nla_nest_end(msg, entry);
+       }
+       rcu_read_unlock();
+       return 0;
+
+out_cancel_ps_nest:
+       nla_nest_cancel(msg, entry);
+out_unlock_ps:
+       rcu_read_unlock();
+out_err:
+       return -EMSGSIZE;
+}
+
 int em_nl_get_pd_table_doit(struct sk_buff *skb, struct genl_info *info)
 {
-       return -EOPNOTSUPP;
+       int cmd = info->genlhdr->cmd;
+       int msg_sz, ret = -EMSGSIZE;
+       struct em_perf_domain *pd;
+       struct sk_buff *msg;
+       void *hdr;
+
+       pd = __em_nl_get_pd_table_id(info->attrs);
+       if (!pd)
+               return -EINVAL;
+
+       msg_sz = __em_nl_get_pd_table_size(pd);
+
+       msg = genlmsg_new(msg_sz, GFP_KERNEL);
+       if (!msg)
+               return -ENOMEM;
+
+       hdr = genlmsg_put_reply(msg, info, &em_nl_family, 0, cmd);
+       if (!hdr)
+               goto out_free_msg;
+
+       ret = __em_nl_get_pd_table(msg, pd);
+       if (ret)
+               goto out_free_msg;
+
+       genlmsg_end(msg, hdr);
+       return genlmsg_reply(msg, info);
+
+out_free_msg:
+       nlmsg_free(msg);
+       return ret;
 }
 
 static int __init em_netlink_init(void)