--- zzzz-none-000/linux-4.4.271/drivers/irqchip/irq-gic.c 2021-06-03 06:22:09.000000000 +0000 +++ maple-fsgw-759/linux-4.4.271/drivers/irqchip/irq-gic.c 2023-12-20 10:37:14.000000000 +0000 @@ -20,6 +20,12 @@ * As such, the enable set/clear, pending set/clear and active bit * registers are banked per-cpu for these sources. */ + +#ifdef CONFIG_AVM_FASTIRQ +#include +#define CLIENT_FIQ_PRIO FIQ_PRIO_MONITOR +#endif + #include #include #include @@ -44,12 +50,17 @@ #include #include + #include #include #include #include "irq-gic-common.h" +#include +#include +#include + #ifdef CONFIG_ARM64 #include @@ -63,6 +74,13 @@ #define gic_check_cpu_features() do { } while(0) #endif +#if defined(CONFIG_AVM_FASTIRQ) +#include +#include +#include +#include +#endif + union gic_base { void __iomem *common_base; void __percpu * __iomem *percpu_base; @@ -87,7 +105,8 @@ #endif }; -static DEFINE_RAW_SPINLOCK(irq_controller_lock); +DEFINE_RAW_SPINLOCK(irq_controller_lock); +EXPORT_SYMBOL(irq_controller_lock); /* * The GIC mapping of CPU interfaces does not necessarily match @@ -137,6 +156,37 @@ #define gic_set_base_accessor(d, f) #endif +#ifndef CONFIG_AVM_FASTIRQ_TZ +void __iomem * get_dist_base( unsigned int gic_nr ) +{ + struct gic_chip_data *gic; + + BUG_ON(gic_nr >= MAX_GIC_NR); + + gic = &gic_data[gic_nr]; + + return(gic_data_dist_base(gic)); +} + +void __iomem * get_cpu_base( unsigned int gic_nr ) +{ + struct gic_chip_data *gic; + + BUG_ON(gic_nr >= MAX_GIC_NR); + + gic = &gic_data[gic_nr]; + + return(gic_data_cpu_base(gic)); +} +#endif + +struct irq_domain *get_irq_domain( void ) +{ + struct gic_chip_data *gic = &gic_data[0]; + return( gic->domain ); +} + + static inline void __iomem *gic_dist_base(struct irq_data *d) { struct gic_chip_data *gic_data = irq_data_get_irq_chip_data(d); @@ -182,6 +232,9 @@ static void gic_mask_irq(struct irq_data *d) { +#if defined(CONFIG_AVM_FASTIRQ) + BUG_ON(avm_gic_fiq_is(gic_irq(d))); +#endif/*--- #if defined(CONFIG_AVM_FASTIRQ) ---*/ gic_poke_irq(d, GIC_DIST_ENABLE_CLEAR); } @@ -205,9 +258,16 @@ gic_poke_irq(d, GIC_DIST_ENABLE_SET); } +#if defined(CONFIG_AVM_FASTIRQ) && !defined(CONFIG_AVM_FASTIRQ_TZ) +unsigned int avm_secmon_gate(unsigned int wr, unsigned int addr, unsigned int val); +#endif static void gic_eoi_irq(struct irq_data *d) { +#if defined(CONFIG_AVM_FASTIRQ) && !defined(CONFIG_AVM_FASTIRQ_TZ) + (void)avm_secmon_gate(1, (IO_ADDRESS(gic_cpu_base(d)) + GIC_CPU_EOI), gic_irq(d)); +#else writel_relaxed(gic_irq(d), gic_cpu_base(d) + GIC_CPU_EOI); +#endif } static void gic_eoimode1_eoi_irq(struct irq_data *d) @@ -315,35 +375,129 @@ if (cpu >= NR_GIC_CPU_IF || cpu >= nr_cpu_ids) return -EINVAL; - raw_spin_lock_irqsave(&irq_controller_lock, flags); + rte_raw_spin_lock_irqsave(&irq_controller_lock, flags); mask = 0xff << shift; bit = gic_cpu_map[cpu] << shift; val = readl_relaxed(reg) & ~mask; writel_relaxed(val | bit, reg); - raw_spin_unlock_irqrestore(&irq_controller_lock, flags); + rte_raw_spin_unlock_irqrestore(&irq_controller_lock, flags); return IRQ_SET_MASK_OK; } #endif +#if defined(CONFIG_AVM_FASTIRQ) && !defined(CONFIG_AVM_FASTIRQ_TZ) +/*--------------------------------------------------------------------------------*\ + * Code, um aus dem IRQ in den FIQ zu wechseln und den FIQ-Handler anzuspringen * +\*--------------------------------------------------------------------------------*/ + +static void noinline handle_FIQ(unsigned long flags) +{ + unsigned long addr; + unsigned long flags_save; + + asm volatile( \ + "mrs %1, cpsr \n" \ + "bic %1, %1, #0x1f \n" \ + "orr %1, %1, #0x11 \n" \ + "msr cpsr_fsxc, %1 \n" \ + "isb \n" \ + "msr spsr_fsxc, %2 \n" \ + "movw lr, #:lower16:2f \n" \ + "movt lr, #:upper16:2f \n" \ + "ldr %0, =avm_fiq_handler_begin \n" \ + "bx %0 \n" \ + "1: mov pc, lr \n" \ + "2: \n" \ + : "=r" (addr), "=&r" (flags_save) \ + : "r" (flags) \ + : "cc" \ + ); +} +#endif + static void __exception_irq_entry gic_handle_irq(struct pt_regs *regs) { - u32 irqstat, irqnr; + u32 irq_handled, irqstat, irqnr; struct gic_chip_data *gic = &gic_data[0]; void __iomem *cpu_base = gic_data_cpu_base(gic); +#if !defined(CONFIG_AVM_FASTIRQ_TZ) && defined(CONFIG_AVM_FASTIRQ) + unsigned long flags; +#endif + + avm_cpu_wait_end(); /*auch wenn es wait_irqoff gibt: trotzdem aufrufen, um system-load-Ausgabe zu triggern */ +#if !defined(CONFIG_AVM_FASTIRQ) + /* trace code only if no fastirq-profiler exist */ + avm_simple_profiling_enter_irq_context(regs->ARM_pc, regs->ARM_lr); +#endif + irq_handled = 0; do { + dsb(); + +#if defined(CONFIG_AVM_FASTIRQ) + + /* NON-SECURE - Zugriff zum Auslesen des Interrupts ... */ +#ifndef CONFIG_AVM_FASTIRQ_TZ + flags = avm_raw_local_fiq_and_iq_disable(); + + irqstat = avm_secmon_gate(0, (IO_ADDRESS(cpu_base) + GIC_CPU_INTACK), 0); +#else + irqstat = readl_relaxed(cpu_base + GIC_CPU_INTACK); +#endif + irqnr = irqstat & GICC_IAR_INT_ID_MASK; + +#ifndef CONFIG_AVM_FASTIRQ_TZ + if (unlikely(irqnr >= 1020))//(unlikely(avm_gic_fiq_is(irqnr))) + { + irqstat = readl_relaxed(cpu_base + OFFSET_ICCHPIR); + irqnr = irqstat & GICC_IAR_INT_ID_MASK; + + if (likely(avm_gic_fiq_is(irqnr))) + { + /* SECURE interrupt, passiert ungluecklicherweise ab und zu ... */ + handle_FIQ(flags); // avm_raw_local_fiq_and_iq_enable(flags) implizit bei Ruecksprung + // mit Modewechsel fuer flags + break; + } + else + { + avm_raw_local_fiq_and_iq_enable(flags); + + /* Interrupt Nr 1020, 1021, 1022 und Nr > 1023 fuer NON-SECURE nicht definiert, einfach Beenden ... */ + /* Spurious Interrupts zählen */ + if(!irq_handled) { + __this_cpu_write(spurious_count, (__this_cpu_read(spurious_count) + 1)); + } + + break; + } + } + + avm_raw_local_fiq_and_iq_enable(flags); +#endif + +#else irqstat = readl_relaxed(cpu_base + GIC_CPU_INTACK); irqnr = irqstat & GICC_IAR_INT_ID_MASK; +#endif if (likely(irqnr > 15 && irqnr < 1020)) { if (static_key_true(&supports_deactivate)) writel_relaxed(irqstat, cpu_base + GIC_CPU_EOI); + avm_simple_profiling_log(avm_profile_data_type_hw_irq_begin, (unsigned int)(irq_to_desc(irqnr)), irqnr); handle_domain_irq(gic->domain, irqnr, regs); + avm_simple_profiling_log(avm_profile_data_type_hw_irq_end, (unsigned int)(irq_to_desc(irqnr)), irqnr); + irq_handled = 1; continue; } - if (irqnr < 16) { + if (likely(irqnr < 16)) { + /* NON-SECURE IPI */ +#if defined(CONFIG_AVM_FASTIRQ) && !defined(CONFIG_AVM_FASTIRQ_TZ) + (void)avm_secmon_gate(1, (IO_ADDRESS(cpu_base) + GIC_CPU_EOI), irqstat); +#else writel_relaxed(irqstat, cpu_base + GIC_CPU_EOI); +#endif if (static_key_true(&supports_deactivate)) writel_relaxed(irqstat, cpu_base + GIC_CPU_DEACTIVATE); #ifdef CONFIG_SMP @@ -355,11 +509,23 @@ * Pairs with the write barrier in gic_raise_softirq */ smp_rmb(); + avm_simple_profiling_log(avm_profile_data_type_hw_irq_begin, (unsigned int)(irq_to_desc(irqnr)), irqnr); handle_IPI(irqnr, regs); + avm_simple_profiling_log(avm_profile_data_type_hw_irq_end, (unsigned int)(irq_to_desc(irqnr)), irqnr); #endif + irq_handled = 1; continue; } + +#if defined(CONFIG_AVM_FASTIRQ) + /* Interrupt Nr 1020, 1021, 1022 und Nr > 1023 fuer NON-SECURE nicht definiert, einfach Beenden ... */ + /* Spurious Interrupts zählen */ + if(!irq_handled) { + __this_cpu_write(spurious_count, (__this_cpu_read(spurious_count) + 1)); + } +#endif break; + } while (1); } @@ -372,9 +538,13 @@ chained_irq_enter(chip, desc); - raw_spin_lock(&irq_controller_lock); + rte_raw_spin_lock(&irq_controller_lock); +#if defined(CONFIG_AVM_FASTIRQ) && !defined(CONFIG_AVM_FASTIRQ_TZ) + status = avm_secmon_gate(0, (IO_ADDRESS(gic_data_cpu_base(chip_data)) + GIC_CPU_INTACK), 0); +#else status = readl_relaxed(gic_data_cpu_base(chip_data) + GIC_CPU_INTACK); - raw_spin_unlock(&irq_controller_lock); +#endif + rte_raw_spin_unlock(&irq_controller_lock); gic_irq = (status & GICC_IAR_INT_ID_MASK); if (gic_irq == GICC_INT_SPURIOUS) @@ -771,7 +941,7 @@ int cpu; unsigned long flags, map = 0; - raw_spin_lock_irqsave(&irq_controller_lock, flags); + rte_raw_spin_lock_irqsave(&irq_controller_lock, flags); /* Convert our logical CPU mask into a physical one. */ for_each_cpu(cpu, mask) @@ -784,9 +954,13 @@ dmb(ishst); /* this always happens on GIC0 */ +#ifdef CONFIG_AVM_FASTIRQ_TZ writel_relaxed(map << 16 | irq, gic_data_dist_base(&gic_data[0]) + GIC_DIST_SOFTINT); +#else + writel_relaxed(map << 16 | (1 << 15) | irq, gic_data_dist_base(&gic_data[0]) + GIC_DIST_SOFTINT); +#endif - raw_spin_unlock_irqrestore(&irq_controller_lock, flags); + rte_raw_spin_unlock_irqrestore(&irq_controller_lock, flags); } #endif @@ -855,7 +1029,7 @@ cur_target_mask = 0x01010101 << cur_cpu_id; ror_val = (cur_cpu_id - new_cpu_id) & 31; - raw_spin_lock(&irq_controller_lock); + rte_raw_spin_lock(&irq_controller_lock); /* Update the target interface for this logical CPU */ gic_cpu_map[cpu] = 1 << new_cpu_id; @@ -875,7 +1049,7 @@ } } - raw_spin_unlock(&irq_controller_lock); + rte_raw_spin_unlock(&irq_controller_lock); /* * Now let's migrate and clear any potential SGIs that might be @@ -992,6 +1166,24 @@ return -EINVAL; } +void gic_trigger_irq(unsigned int intr, unsigned int set) +{ + struct irq_desc *desc = irq_to_desc(intr); + struct irq_data *d = 0; + u32 mask = 0; + + if (desc == NULL) return; + d = &desc->irq_data; + mask = 1 << (d->hwirq % 32); + + if (set) { + writel_relaxed(mask, gic_dist_base(d) + GIC_DIST_PENDING_SET + (d->hwirq/ 32) * 4); + } else { + writel_relaxed(mask, gic_dist_base(d) + GIC_DIST_PENDING_CLEAR + (d->hwirq / 32) * 4); + } +} +EXPORT_SYMBOL(gic_trigger_irq); + #ifdef CONFIG_SMP static int gic_secondary_init(struct notifier_block *nfb, unsigned long action, void *hcpu)