diff --git a/drivers/gpio/gpio-rockchip.c b/drivers/gpio/gpio-rockchip.c index d38553dfad26..be27f2949081 100644 --- a/drivers/gpio/gpio-rockchip.c +++ b/drivers/gpio/gpio-rockchip.c @@ -25,6 +25,7 @@ #include #include #include +#include #include "../pinctrl/core.h" #include "../pinctrl/pinctrl-rockchip.h" @@ -36,6 +37,18 @@ #define GPIO_MAX_PINS (32) +struct rockchip_gpio_irq_affinity { + int irq; + int cpu; + int bank_num; + struct list_head list; +}; + +static DEFINE_MUTEX(irq_affinity_mutex); +static LIST_HEAD(irq_affinity_list); +static bool cpuhp_state_initialized; +static enum cpuhp_state gpio_hp_state = CPUHP_INVALID; + #define ROCKCHIP_PIN_EXP_IRQ_DEMUX(_N) \ static void rockchip_single_pin_exp_irq_demux##_N(struct irq_desc *desc) \ { \ @@ -47,7 +60,7 @@ static void rockchip_single_pin_exp_irq_demux##_N(struct irq_desc *desc) \ chained_irq_exit(chip, desc); \ } \ \ -static void rockchip_multi_pin_exp_irq_demux##_N(struct irq_desc *desc) \ +static void rockchip_dual_pin_exp_irq_demux##_N(struct irq_desc *desc) \ { \ struct irq_chip *chip = irq_desc_get_chip(desc); \ struct rockchip_pin_bank *bank = irq_desc_get_handler_data(desc); \ @@ -318,7 +331,7 @@ static int rockchip_gpio_set_debounce(struct gpio_chip *gc, else clk_disable_unprepare(bank->db_clk); } else { - return -ENOTSUPP; + return -EOPNOTSUPP; } return 0; @@ -369,12 +382,30 @@ static int rockchip_gpio_to_irq(struct gpio_chip *gc, unsigned int offset) { struct rockchip_pin_bank *bank = gpiochip_get_data(gc); unsigned int virq; + int i; + struct rockchip_gpio_irq_affinity *irq_affinity; if (!bank->domain) return -ENXIO; virq = irq_create_mapping(bank->domain, offset); + for (i = 0; i < RK_GPIO_IRQ_MAX_NUM; i++) { + if (bank->irq_pins[i] & (1 << offset)) { + irq_affinity = kzalloc(sizeof(*irq_affinity), GFP_KERNEL); + if (!irq_affinity) + return -ENOMEM; + irq_affinity->irq = virq; + irq_affinity->cpu = bank->cpu_affinity[i]; + irq_affinity->bank_num = bank->bank_num; + mutex_lock(&irq_affinity_mutex); + list_add(&irq_affinity->list, &irq_affinity_list); + mutex_unlock(&irq_affinity_mutex); + irq_force_affinity(virq, cpumask_of(irq_affinity->cpu)); + break; + } + } + return (virq) ? : -ENXIO; } @@ -453,10 +484,10 @@ static irq_flow_handler_t rockchip_irq_s[RK_GPIO_IRQ_MAX_NUM - 1] = { rockchip_single_pin_exp_irq_demux3 }; -static irq_flow_handler_t rockchip_irq_m[RK_GPIO_IRQ_MAX_NUM - 1] = { - rockchip_multi_pin_exp_irq_demux1, - rockchip_multi_pin_exp_irq_demux2, - rockchip_multi_pin_exp_irq_demux3 +static irq_flow_handler_t rockchip_irq_d[RK_GPIO_IRQ_MAX_NUM - 1] = { + rockchip_dual_pin_exp_irq_demux1, + rockchip_dual_pin_exp_irq_demux2, + rockchip_dual_pin_exp_irq_demux3 }; static int rockchip_irq_set_type(struct irq_data *d, unsigned int type) @@ -589,6 +620,24 @@ static void rockchip_irq_disable(struct irq_data *d) irq_gc_mask_set_bit(d); } +static int rockchip_irq_set_affinity(struct irq_data *data, + const struct cpumask *mask_val, bool force) +{ + unsigned int cpu; + + if (!force) + cpu = cpumask_any_and(mask_val, cpu_online_mask); + else + cpu = cpumask_first(mask_val); + + if (cpu >= nr_cpu_ids) + return -EINVAL; + + irq_data_update_effective_affinity(data, cpumask_of(cpu)); + + return IRQ_SET_MASK_OK; +} + static int rockchip_interrupts_register(struct rockchip_pin_bank *bank) { unsigned int clr = IRQ_NOREQUEST | IRQ_NOPROBE | IRQ_NOAUTOEN; @@ -637,6 +686,7 @@ static int rockchip_interrupts_register(struct rockchip_pin_bank *bank) gc->chip_types[0].chip.irq_release_resources = rockchip_irq_relres; gc->wake_enabled = IRQ_MSK(bank->nr_pins); + gc->chip_types[0].chip.irq_set_affinity = rockchip_irq_set_affinity; /* * Linux assumes that all interrupts start out disabled/masked. * Our driver only uses the concept of masked and always keeps @@ -651,15 +701,21 @@ static int rockchip_interrupts_register(struct rockchip_pin_bank *bank) rockchip_irq_demux, bank); for (int i = 1; i < RK_GPIO_IRQ_MAX_NUM; i++) { + int pin_num = hweight32(bank->irq_pins[i]); + if (!bank->irq_pins[i]) continue; - if (hweight32(bank->irq_pins[i]) == 1) + if (pin_num == 1) irq_set_chained_handler_and_data(bank->irq[i], rockchip_irq_s[i - 1], bank); + else if (pin_num == 2) + irq_set_chained_handler_and_data(bank->irq[i], + rockchip_irq_d[i - 1], + bank); else irq_set_chained_handler_and_data(bank->irq[i], - rockchip_irq_m[i - 1], + rockchip_irq_demux, bank); } return 0; @@ -800,11 +856,33 @@ static bool rockchip_gpio_has_irq_affinity(struct device_node *node) return !!of_find_property(node, "interrupt-affinity", NULL); } -static int rockchip_gpio_set_irq_affinity(const struct device *dev, int index, - struct rockchip_pin_bank *bank) +static bool rockchip_gpio_has_irq_pins(struct device_node *node) +{ + return !!of_find_property(node, "interrupt-pins", NULL); +} + +static int rockchip_gpio_set_irq_affinity(int irq, int cpu) +{ + int ret; + + ret = irq_force_affinity(irq, cpumask_of(cpu)); + if (ret) { + pr_err("unable to set irq affinity (irq=%d, cpu=%u)\n", irq, cpu); + return ret; + } + + return 0; +} + +static int rockchip_gpio_init_irq_affinity(const struct device *dev, int index, + struct rockchip_pin_bank *bank) { int cpu, ret; struct device_node *node = dev->of_node, *dn; + struct rockchip_gpio_irq_affinity *irq_affinity; + + if (!bank->irq_pins[index]) + return 0; dn = of_parse_phandle(node, "interrupt-affinity", index); if (!dn) { @@ -817,46 +895,83 @@ static int rockchip_gpio_set_irq_affinity(const struct device *dev, int index, if (cpu < 0) return cpu; - ret = irq_force_affinity(bank->irq[index], cpumask_of(cpu)); - if (ret) { - dev_err(dev, "unable to set irq affinity (irq=%d, cpu=%u)\n", - bank->irq[index], cpu); + if (cpu == 0) + return 0; + + ret = rockchip_gpio_set_irq_affinity(bank->irq[index], cpu); + if (ret) return ret; - } + + bank->cpu_affinity[index] = cpu; + + irq_affinity = kzalloc(sizeof(*irq_affinity), GFP_KERNEL); + if (!irq_affinity) + return -ENOMEM; + irq_affinity->irq = bank->irq[index]; + irq_affinity->cpu = cpu; + irq_affinity->bank_num = bank->bank_num; + mutex_lock(&irq_affinity_mutex); + list_add(&irq_affinity->list, &irq_affinity_list); + mutex_unlock(&irq_affinity_mutex); return 0; } -static bool rockchip_gpio_has_irq_pins(struct device_node *node) -{ - return !!of_find_property(node, "interrupt-pins", NULL); -} - -static int rockchip_gpio_get_irq_pins(const struct device *dev, int index, +static int rockchip_gpio_set_irq_pins(const struct device *dev, int index, struct rockchip_pin_bank *bank) { - int ret, i; + struct arm_smccc_res res; + int ret = 0; + + if (rockchip_gpio_has_irq_pins(dev->of_node)) { + ret = of_property_read_u32_index(dev->of_node, "interrupt-pins", + index, &bank->irq_pins[index]); + if (ret) { + dev_err(dev, "Failed to read interrupt-pin at index %d\n", index); + return ret; + } + + if (!bank->irq_pins[index]) + return 0; + res = sip_smc_gpio_config(GPIO_SET_GROUP_INFO, bank->bank_num, + index, bank->irq_pins[index]); + + switch (res.a0) { + case SIP_RET_SUCCESS: + return 0; + case SIP_RET_NOT_SUPPORTED: + dev_err(dev, "Failed to set interrupt pins, no support\n"); + return -EOPNOTSUPP; + case SIP_RET_INVALID_PARAMS: + dev_err(dev, "gpio id or group id invalid\n"); + return -EINVAL; + case SIP_RET_DENIED: + dev_warn(dev, "Failed to set interrupt pins, please modify syscfg.dts\n"); + break; + default: + break; + } + } + res = sip_smc_gpio_config(GPIO_GET_GROUP_INFO, bank->bank_num, index, 0); + if (res.a0 == 0) + bank->irq_pins[index] = res.a1; + + return 0; +} + +static int rockchip_gpio_init_irq_pins(const struct device *dev, int index, + struct rockchip_pin_bank *bank) +{ + int i; unsigned int pin; unsigned long pending; - struct device_node *node = dev->of_node; if (index == 0) return 0; for (i = 0; i < RK_GPIO_EXP_IRQ_MAX_PIN_NUM; i++) bank->irq_pin_id[index][i] = -1; - ret = of_property_read_u32_index(node, "interrupt-pins", index, - &bank->irq_pins[index]); - if (ret) { - dev_err(dev, "Failed to read interrupt-pin at index %d\n", index); - return ret; - } - - if (hweight32(bank->irq_pins[index]) > RK_GPIO_EXP_IRQ_MAX_PIN_NUM) { - dev_err(dev, "Interrupt-pin at index %d must not more then %d\n", - index, RK_GPIO_EXP_IRQ_MAX_PIN_NUM); - return -EINVAL; - } + rockchip_gpio_set_irq_pins(dev, index, bank); if (bank->irq_pins[index]) { i = 0; @@ -874,7 +989,6 @@ static int rockchip_gpio_parse_irqs(struct platform_device *pdev, { int num_irqs, i, ret; struct device *dev = &pdev->dev; - bool has_irq_pins = rockchip_gpio_has_irq_pins(dev->of_node); bool has_affinity = rockchip_gpio_has_irq_affinity(dev->of_node); num_irqs = platform_irq_count(pdev); @@ -891,15 +1005,13 @@ static int rockchip_gpio_parse_irqs(struct platform_device *pdev, if (bank->irq[i] < 0) return dev_err_probe(dev, -EINVAL, "failed to get gpio irq %d\n", i); - if (!has_irq_pins) - continue; - ret = rockchip_gpio_get_irq_pins(dev, i, bank); - if (ret) - return ret; - if (!has_affinity) continue; - ret = rockchip_gpio_set_irq_affinity(dev, i, bank); + + ret = rockchip_gpio_init_irq_pins(dev, i, bank); + if (ret) + return ret; + ret = rockchip_gpio_init_irq_affinity(dev, i, bank); if (ret) return ret; } @@ -907,6 +1019,58 @@ static int rockchip_gpio_parse_irqs(struct platform_device *pdev, return 0; } +static int gpio_cpu_on(unsigned int cpu) +{ + struct rockchip_gpio_irq_affinity *entry; + + mutex_lock(&irq_affinity_mutex); + list_for_each_entry(entry, &irq_affinity_list, list) { + if (entry->cpu == cpu) + rockchip_gpio_set_irq_affinity(entry->irq, cpu); + } + mutex_unlock(&irq_affinity_mutex); + + return 0; +} + +static int gpio_cpu_off(unsigned int cpu) +{ + return 0; +} + +static void rockchip_gpio_init_cpuhp(void) +{ + int ret; + + mutex_lock(&irq_affinity_mutex); + if (!cpuhp_state_initialized && !list_empty(&irq_affinity_list)) { + ret = cpuhp_setup_state_nocalls(CPUHP_AP_ONLINE_DYN, "gpio", + gpio_cpu_on, gpio_cpu_off); + if (ret < 0) { + pr_err("ERROR: gpio failed to register hotplug callbacks\n"); + mutex_unlock(&irq_affinity_mutex); + return; + } + cpuhp_state_initialized = true; + gpio_hp_state = ret; + } + + mutex_unlock(&irq_affinity_mutex); +} + +static void rockchip_gpio_remove_cpuhp(void) +{ + mutex_lock(&irq_affinity_mutex); + if (cpuhp_state_initialized && list_empty(&irq_affinity_list)) { + if (gpio_hp_state != CPUHP_INVALID) { + cpuhp_remove_state_nocalls(gpio_hp_state); + gpio_hp_state = CPUHP_INVALID; + cpuhp_state_initialized = false; + } + } + mutex_unlock(&irq_affinity_mutex); +} + static int rockchip_gpio_probe(struct platform_device *pdev) { struct device *dev = &pdev->dev; @@ -914,6 +1078,7 @@ static int rockchip_gpio_probe(struct platform_device *pdev) struct rockchip_pin_bank *bank = NULL; int bank_id = 0; int ret; + struct rockchip_gpio_irq_affinity *entry, *tmp; bank_id = rockchip_gpio_acpi_get_bank_id(dev); if (bank_id < 0) { @@ -950,10 +1115,6 @@ static int rockchip_gpio_probe(struct platform_device *pdev) rockchip_gpio_get_ver(bank); - ret = rockchip_gpio_parse_irqs(pdev, bank); - if (ret < 0) - return ret; - raw_spin_lock_init(&bank->slock); if (!ACPI_COMPANION(dev)) { @@ -977,6 +1138,11 @@ static int rockchip_gpio_probe(struct platform_device *pdev) clk_prepare_enable(bank->clk); clk_prepare_enable(bank->db_clk); + ret = rockchip_gpio_parse_irqs(pdev, bank); + if (ret < 0) + return ret; + rockchip_gpio_init_cpuhp(); + /* * Prevent clashes with a deferred output setting * being added right at this moment. @@ -1036,6 +1202,16 @@ static int rockchip_gpio_probe(struct platform_device *pdev) return 0; err_unlock: + mutex_lock(&irq_affinity_mutex); + list_for_each_entry_safe(entry, tmp, &irq_affinity_list, list) { + if (entry->bank_num == bank->bank_num) { + list_del(&entry->list); + kfree(entry); + } + } + mutex_unlock(&irq_affinity_mutex); + rockchip_gpio_remove_cpuhp(); + mutex_unlock(&bank->deferred_lock); clk_disable_unprepare(bank->clk); clk_disable_unprepare(bank->db_clk); @@ -1046,6 +1222,17 @@ err_unlock: static int rockchip_gpio_remove(struct platform_device *pdev) { struct rockchip_pin_bank *bank = platform_get_drvdata(pdev); + struct rockchip_gpio_irq_affinity *entry, *tmp; + + mutex_lock(&irq_affinity_mutex); + list_for_each_entry_safe(entry, tmp, &irq_affinity_list, list) { + if (entry->bank_num == bank->bank_num) { + list_del(&entry->list); + kfree(entry); + } + } + mutex_unlock(&irq_affinity_mutex); + rockchip_gpio_remove_cpuhp(); clk_disable_unprepare(bank->clk); clk_disable_unprepare(bank->db_clk); diff --git a/drivers/pinctrl/pinctrl-rockchip.h b/drivers/pinctrl/pinctrl-rockchip.h index 8e1173f60d73..110f58dc6602 100644 --- a/drivers/pinctrl/pinctrl-rockchip.h +++ b/drivers/pinctrl/pinctrl-rockchip.h @@ -306,6 +306,7 @@ struct rockchip_drv { * @irq: interrupt of the gpio bank * @irq_pins: masks of the irq pins * @irq_pin_id: pin number of the irq pin + * @cpu_affinity: cpu affinity of the irq pin * @saved_masks: Saved content of GPIO_INTEN at suspend time. * @pin_base: first pin number * @nr_pins: number of pins in this bank @@ -336,6 +337,7 @@ struct rockchip_pin_bank { int irq[RK_GPIO_IRQ_MAX_NUM]; u32 irq_pins[RK_GPIO_IRQ_MAX_NUM]; int irq_pin_id[RK_GPIO_IRQ_MAX_NUM][RK_GPIO_EXP_IRQ_MAX_PIN_NUM]; + int cpu_affinity[RK_GPIO_IRQ_MAX_NUM]; u32 saved_masks; u32 pin_base; u8 nr_pins;