soc: rockchip: amp: support config amp os irqs

Signed-off-by: Tony Xie <tony.xie@rock-chips.com>
Change-Id: I628a5c4a863e4ed6d51a71ed9243d04993a04e43

drivers/soc/rockchip/rockchip_amp.c

@@ -6,6 +6,7 @@
  * Author: Tony Xie <tony.xie@rock-chips.com>
  */
 
+#include <asm/cputype.h>
 #include <linux/clk.h>
 #include <linux/module.h>
 #include <linux/of.h>
@@ -17,6 +18,12 @@
 #define RK_CPU_STATUS_OFF		0
 #define RK_CPU_STATUS_ON		1
 #define RK_CPU_STATUS_BUSY		-1
+#define AMP_AFF_MAX_CLUSTER		4
+#define AMP_AFF_MAX_CPU			8
+#define GPIO_BANK_NUM			16
+#define GPIO_GROUP_PRIO_MAX		3
+
+#define AMP_GIC_DBG(fmt, arg...)	do { if (0) { pr_warn(fmt, ##arg); } } while (0)
 
 enum amp_cpu_ctrl_status {
 	AMP_CPU_STATUS_AMP_DIS = 0,
@@ -44,6 +51,33 @@ static struct {
 	u64 cpu_id;
 } cpu_boot_info[CONFIG_NR_CPUS];
 
+struct amp_gpio_group_s {
+	u32 bank_id;
+	u32 prio;
+	u32 irq_aff[AMP_AFF_MAX_CPU];
+	u32 irq_id[AMP_AFF_MAX_CPU];
+	u32 en[AMP_AFF_MAX_CPU];
+};
+
+struct amp_irq_cfg_s {
+	u32 prio;
+	u32 cpumask;
+	u32 aff;
+	int amp_flag;
+} irqs_cfg[1024];
+
+static struct amp_gic_ctrl_s {
+	struct {
+		u32 aff;
+		u32 cpumask;
+		u32 flag;
+	} aff_to_cpumask[AMP_AFF_MAX_CLUSTER][AMP_AFF_MAX_CPU];
+	struct amp_irq_cfg_s irqs_cfg[1024];
+	u32 validmask[1020 / 32 + 1];
+	struct amp_gpio_group_s gpio_grp[GPIO_BANK_NUM][GPIO_GROUP_PRIO_MAX];
+	u32 gpio_banks;
+} amp_ctrl;
+
 static int get_cpu_boot_info_idx(unsigned long cpu_id)
 {
 	int i;
@@ -220,6 +254,257 @@ static int rockchip_amp_boot_cpus(struct device *dev,
 	return 0;
 }
 
+int rockchip_amp_check_amp_irq(u32 irq)
+{
+	return amp_ctrl.irqs_cfg[irq].amp_flag;
+}
+
+u32 rockchip_amp_get_irq_prio(u32 irq)
+{
+	return amp_ctrl.irqs_cfg[irq].prio;
+}
+
+u32 rockchip_amp_get_irq_cpumask(u32 irq)
+{
+	return amp_ctrl.irqs_cfg[irq].cpumask;
+}
+
+static u32 amp_get_cpumask_bit(u32 aff)
+{
+	u32 aff_cluster, aff_cpu;
+
+	aff_cluster = MPIDR_AFFINITY_LEVEL(aff, 1);
+	aff_cpu = MPIDR_AFFINITY_LEVEL(aff, 0);
+
+	if (aff_cpu >= AMP_AFF_MAX_CPU || aff_cluster >= AMP_AFF_MAX_CLUSTER)
+		return 0;
+
+	AMP_GIC_DBG("%s: aff:%d-%d: %x\n", __func__, aff_cluster, aff_cpu,
+		    amp_ctrl.aff_to_cpumask[aff_cluster][aff_cpu].cpumask);
+
+	return amp_ctrl.aff_to_cpumask[aff_cluster][aff_cpu].cpumask;
+}
+
+static int gic_amp_get_gpio_prio_group_info(struct device_node *np,
+					    struct amp_gic_ctrl_s *amp_ctrl,
+					    int prio_id)
+{
+	u32 gpio_bank, count0, count1, prio, irq_id, irq_aff;
+	int i;
+	struct amp_gpio_group_s *gpio_grp;
+	struct amp_irq_cfg_s *irqs_cfg;
+
+	if (prio_id >= GPIO_GROUP_PRIO_MAX)
+		return -EINVAL;
+
+	if (of_property_read_u32_array(np, "gpio-bank", &gpio_bank, 1))
+		return -EINVAL;
+	if (gpio_bank >= amp_ctrl->gpio_banks)
+		return -EINVAL;
+
+	gpio_grp = &amp_ctrl->gpio_grp[gpio_bank][prio_id];
+
+	if (of_property_read_u32_array(np, "prio", &prio, 1))
+		return -EINVAL;
+
+	if (gpio_bank >= GPIO_BANK_NUM)
+		return -EINVAL;
+
+	AMP_GIC_DBG("%s: gpio-%d, group prio:%d-%x\n",
+		    __func__, gpio_bank, prio_id, prio);
+
+	count0 = of_property_count_u32_elems(np, "girq-id");
+	count1 = of_property_count_u32_elems(np, "girq-aff");
+
+	if (count0 != count1)
+		return -EINVAL;
+
+	gpio_grp->prio = prio;
+
+	for (i = 0; i < count0; i++) {
+		of_property_read_u32_index(np, "girq-id", i, &irq_id);
+		gpio_grp->irq_id[i] = irq_id;
+		of_property_read_u32_index(np, "girq-aff", i, &irq_aff);
+
+		gpio_grp->irq_aff[i] = irq_aff;
+
+		of_property_read_u32_index(np, "girq-en", i, &gpio_grp->en[i]);
+
+		irqs_cfg = &amp_ctrl->irqs_cfg[irq_id];
+
+		AMP_GIC_DBG(" %s: group cpu-%d, irq-%d: prio-%x, aff-%x en-%d\n",
+			    __func__, i, gpio_grp->irq_id[i], gpio_grp->prio,
+			    gpio_grp->irq_aff[i], gpio_grp->en[i]);
+
+		if (gpio_grp->en[i]) {
+			irqs_cfg->prio = gpio_grp->prio;
+			irqs_cfg->aff = irq_aff;
+			irqs_cfg->cpumask = amp_get_cpumask_bit(irq_aff);
+			irqs_cfg->amp_flag = 1;
+		}
+
+		AMP_GIC_DBG("  %s: irqs_cfg prio-%x aff-%x cpumaks-%x en-%d\n",
+			    __func__, irqs_cfg->prio, irqs_cfg->aff,
+			    irqs_cfg->cpumask, irqs_cfg->amp_flag);
+	}
+
+	return 0;
+}
+
+static int gic_amp_gpio_group_get_info(struct device_node *group_node,
+				       struct amp_gic_ctrl_s *amp_ctrl,
+				       int idx)
+{
+	int i = 0;
+	struct device_node *node;
+
+	if (group_node) {
+		for_each_available_child_of_node(group_node, node) {
+			if (i >= GPIO_GROUP_PRIO_MAX)
+				break;
+			if (!gic_amp_get_gpio_prio_group_info(node, amp_ctrl,
+							      i)) {
+				i++;
+			}
+		}
+	}
+	return 0;
+}
+
+static void gic_of_get_gpio_group(struct device_node *np,
+				  struct amp_gic_ctrl_s *amp_ctrl)
+{
+	struct device_node *gpio_group_node, *node;
+	int i = 0;
+
+	if (of_property_read_u32_array(np, "gpio-group-banks",
+				       &amp_ctrl->gpio_banks, 1))
+		return;
+
+	gpio_group_node = of_get_child_by_name(np, "gpio-group");
+	if (gpio_group_node) {
+		for_each_available_child_of_node(gpio_group_node, node) {
+			if (i >= amp_ctrl->gpio_banks)
+				break;
+			if (!gic_amp_gpio_group_get_info(node, amp_ctrl, i))
+				i++;
+		}
+	}
+
+	of_node_put(gpio_group_node);
+}
+
+static int amp_gic_get_cpumask(struct device_node *np, struct amp_gic_ctrl_s *amp_ctrl)
+{
+	const struct property *prop;
+	int count, i;
+	u32 cluster, aff_cpu, aff, cpumask;
+
+	prop = of_find_property(np, "amp-cpu-aff-maskbits", NULL);
+	if (!prop)
+		return -1;
+
+	if (!prop->value)
+		return -1;
+
+	count = of_property_count_u32_elems(np, "amp-cpu-aff-maskbits");
+	if (count % 2)
+		return -1;
+
+	for (i = 0; i < count / 2; i++) {
+		of_property_read_u32_index(np, "amp-cpu-aff-maskbits",
+					   2 * i, &aff);
+		cluster = MPIDR_AFFINITY_LEVEL(aff, 1);
+		aff_cpu = MPIDR_AFFINITY_LEVEL(aff, 0);
+		amp_ctrl->aff_to_cpumask[cluster][aff_cpu].aff = aff;
+
+		of_property_read_u32_index(np, "amp-cpu-aff-maskbits",
+					   2 * i + 1, &cpumask);
+
+		amp_ctrl->aff_to_cpumask[cluster][aff_cpu].cpumask = cpumask;
+
+		AMP_GIC_DBG("cpumask: %d-%d: aff-%d cpumask-%d\n",
+			    cluster, aff_cpu, aff, cpumask);
+
+		if (!cpumask)
+			return -1;
+	}
+
+	return 0;
+}
+
+static void amp_gic_get_irqs_config(struct device_node *np,
+				    struct amp_gic_ctrl_s *amp_ctrl)
+{
+	const struct property *prop;
+	int count, i;
+	u32 irq, prio, aff;
+
+	prop = of_find_property(np, "amp-irqs", NULL);
+	if (!prop)
+		return;
+
+	if (!prop->value)
+		return;
+
+	count = of_property_count_u32_elems(np, "amp-irqs");
+
+	if (count < 0 || count % 3)
+		return;
+
+	for (i = 0; i < count / 3; i++) {
+		of_property_read_u32_index(np, "amp-irqs", 3 * i, &irq);
+
+		if (irq > 1020)
+			break;
+
+		of_property_read_u32_index(np, "amp-irqs", 3 * i + 1, &prio);
+		of_property_read_u32_index(np, "amp-irqs", 3 * i + 2, &aff);
+
+		AMP_GIC_DBG("%s: irq-%d aff-%d prio-%x\n",
+			    __func__, irq, aff, prio);
+
+		amp_ctrl->irqs_cfg[irq].prio = prio;
+		amp_ctrl->irqs_cfg[irq].aff = aff;
+		amp_ctrl->irqs_cfg[irq].cpumask = amp_get_cpumask_bit(aff);
+
+		if (!amp_ctrl->irqs_cfg[irq].cpumask) {
+			AMP_GIC_DBG("%s: get cpumask error\n", __func__);
+			break;
+		}
+
+		if (!amp_ctrl->irqs_cfg[irq].aff &&
+		    !amp_ctrl->irqs_cfg[irq].prio)
+			break;
+
+		amp_ctrl->irqs_cfg[irq].amp_flag = 1;
+
+		AMP_GIC_DBG("%s: irq-%d aff-%d cpumask-%d pri-%x\n",
+			    __func__, irq, amp_ctrl->irqs_cfg[irq].aff,
+			    amp_ctrl->irqs_cfg[irq].cpumask,
+			    amp_ctrl->irqs_cfg[irq].prio);
+	}
+}
+
+void rockchip_amp_get_gic_info(void)
+{
+	struct device_node *np;
+
+	np = of_find_node_by_name(NULL, "rockchip-amp");
+	if (!np)
+		return;
+
+	if (amp_gic_get_cpumask(np, &amp_ctrl)) {
+		pr_err("%s: get amp gic cpu mask error\n", __func__);
+		goto exit;
+	}
+	gic_of_get_gpio_group(np, &amp_ctrl);
+	amp_gic_get_irqs_config(np, &amp_ctrl);
+
+exit:
+	of_node_put(np);
+}
+
 static int rockchip_amp_probe(struct platform_device *pdev)
 {
 	struct rkamp_device *rkamp_dev = NULL;
@@ -273,6 +558,7 @@ static int rockchip_amp_probe(struct platform_device *pdev)
 				idx++;
 			}
 		}
+		of_node_put(cpus_node);
 	}
 
 	rk_amp_kobj = kobject_create_and_add("rk_amp", NULL);

include/soc/rockchip/rockchip_amp.h

@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Rockchip AMP support.
+ *
+ * Copyright (c) 2023 Rockchip Electronics Co. Ltd.
+ * Author: Tony Xie <tony.xie@rock-chips.com>
+ */
+
+#ifndef _ROCKCHIP_AMP
+#define _ROCKCHIP_AMP
+
+#if IS_REACHABLE(CONFIG_ROCKCHIP_AMP)
+void rockchip_amp_get_gic_info(void);
+int rockchip_amp_check_amp_irq(u32 irq);
+u32 rockchip_amp_get_irq_prio(u32 irq);
+u32 rockchip_amp_get_irq_cpumask(u32 irq);
+#else
+#include <linux/irqchip/arm-gic-common.h>
+
+static inline void rockchip_amp_get_gic_info(void)
+{
+}
+
+static inline int rockchip_amp_check_amp_irq(u32 irq)
+{
+	return 0;
+}
+
+static inline u32 rockchip_amp_get_irq_prio(u32 irq)
+{
+	return GICD_INT_DEF_PRI;
+}
+
+static inline u32 rockchip_amp_get_irq_cpumask(u32 irq)
+{
+	return 0;
+}
+#endif /* CONFIG_ROCKCHIP_AMP */
+#endif /* _ROCKCHIP_AMP */