target/linux/cns3xxx/patches-3.3/460-cns3xxx_fiq_support.patch

   1 --- a/arch/arm/Kconfig
   2 +++ b/arch/arm/Kconfig
   3 @@ -375,6 +375,7 @@ config ARCH_CNS3XXX
   4         select PCI_DOMAINS if PCI
   5         select HAVE_ARM_TWD
   6         select HAVE_SMP
   7 +       select FIQ
   8         help
   9           Support for Cavium Networks CNS3XXX platform.
  10
  11 --- a/arch/arm/kernel/fiq.c
  12 +++ b/arch/arm/kernel/fiq.c
  13 @@ -49,6 +49,8 @@
  14
  15  static unsigned long no_fiq_insn;
  16
  17 +unsigned int fiq_number[2] = {0, 0};
  18 +
  19  /* Default reacquire function
  20   * - we always relinquish FIQ control
  21   * - we always reacquire FIQ control
  22 @@ -70,9 +72,12 @@ static struct fiq_handler *current_fiq =
  23
  24  int show_fiq_list(struct seq_file *p, int prec)
  25  {
  26 -       if (current_fiq != &default_owner)
  27 -               seq_printf(p, "%*s:              %s\n", prec, "FIQ",
  28 -                       current_fiq->name);
  29 +       if (current_fiq != &default_owner) {
  30 +               seq_printf(p, "%*s: ", prec, "FIQ");
  31 +               seq_printf(p, "%10u ", fiq_number[0]);
  32 +               seq_printf(p, "%10u ", fiq_number[1]);
  33 +               seq_printf(p, "      %s\n", current_fiq->name);
  34 +       }
  35
  36         return 0;
  37  }
  38 --- a/arch/arm/kernel/smp.c
  39 +++ b/arch/arm/kernel/smp.c
  40 @@ -400,13 +400,13 @@ void show_ipi_list(struct seq_file *p, i
  41         unsigned int cpu, i;
  42
  43         for (i = 0; i < NR_IPI; i++) {
  44 -               seq_printf(p, "%*s%u: ", prec - 1, "IPI", i);
  45 +               seq_printf(p, "%*s%u:", prec - 1, "IPI", i);
  46
  47                 for_each_present_cpu(cpu)
  48                         seq_printf(p, "%10u ",
  49                                    __get_irq_stat(cpu, ipi_irqs[i]));
  50
  51 -               seq_printf(p, " %s\n", ipi_types[i]);
  52 +               seq_printf(p, "      %s\n", ipi_types[i]);
  53         }
  54  }
  55
  56 --- a/arch/arm/mach-cns3xxx/Makefile
  57 +++ b/arch/arm/mach-cns3xxx/Makefile
  58 @@ -2,6 +2,6 @@ obj-$(CONFIG_ARCH_CNS3XXX)              += core.o pm
  59  obj-$(CONFIG_PCI)                      += pcie.o
  60  obj-$(CONFIG_MACH_CNS3420VB)           += cns3420vb.o
  61  obj-$(CONFIG_MACH_GW2388)              += laguna.o
  62 -obj-$(CONFIG_SMP)                      += platsmp.o headsmp.o
  63 +obj-$(CONFIG_SMP)                      += platsmp.o headsmp.o cns3xxx_fiq.o
  64  obj-$(CONFIG_HOTPLUG_CPU)              += hotplug.o
  65  obj-$(CONFIG_LOCAL_TIMERS)             += localtimer.o
  66 --- /dev/null
  67 +++ b/arch/arm/mach-cns3xxx/cns3xxx_fiq.S
  68 @@ -0,0 +1,96 @@
  69 +/*
  70 + *  Copyright (C) 2012 Gateworks Corporation
  71 + *      Chris Lang <clang@gateworks.com>
  72 + *
  73 + * This program is free software; you can redistribute it and/or modify
  74 + * it under the terms of the GNU General Public License version 2 as
  75 + * published by the Free Software Foundation.
  76 + */
  77 +#include <linux/linkage.h>
  78 +#include <asm/assembler.h>
  79 +#include <asm/asm-offsets.h>
  80 +
  81 +#define D_CACHE_LINE_SIZE 32
  82 +
  83 +       .text
  84 +
  85 +/*
  86 + * R8  - DMA Start Address
  87 + * R9  - DMA Length
  88 + * R10 - DMA Direction
  89 + * R11 - DMA type
  90 + * R12 - fiq_buffer Address
  91 + * R13 - DMA type Address
  92 +*/
  93 +
  94 +       .global cns3xxx_fiq_end
  95 +ENTRY(cns3xxx_fiq_start)
  96 +       mov r8, #0
  97 +       str r8, [r13]
  98 +
  99 +       ldr r9, [r12]
 100 +       ldr r8, [r9]
 101 +       add r8, r8, #1
 102 +       str r8, [r9]
 103 +
 104 +       ldmib r12, {r8, r9, r10}
 105 +       and r11, r10, #0x3000000
 106 +       and r10, r10, #0xff
 107 +
 108 +       teq r11, #0x1000000
 109 +       beq cns3xxx_dma_map_area
 110 +       teq r11, #0x2000000
 111 +       beq cns3xxx_dma_unmap_area
 112 +       b cns3xxx_dma_flush_range
 113 +
 114 +cns3xxx_fiq_exit:
 115 +       mov r8, #0
 116 +       str r8, [r12, #12]
 117 +       mcr p15, 0, r8, c7, c10, 4    @ drain write buffer
 118 +       subs pc, lr, #4
 119 +
 120 +cns3xxx_dma_map_area:
 121 +       add r9, r9, r8
 122 +       teq r10, #DMA_FROM_DEVICE
 123 +       beq cns3xxx_dma_inv_range
 124 +       b cns3xxx_dma_clean_range
 125 +
 126 +cns3xxx_dma_unmap_area:
 127 +       add r9, r9, r8
 128 +       teq r10, #DMA_TO_DEVICE
 129 +       bne cns3xxx_dma_inv_range
 130 +       b cns3xxx_fiq_exit
 131 +
 132 +cns3xxx_dma_flush_range:
 133 +       bic r8, r8, #D_CACHE_LINE_SIZE - 1
 134 +1:
 135 +       mcr p15, 0, r8, c7, c14, 1   @ clean & invalidate D line
 136 +       add r8, r8, #D_CACHE_LINE_SIZE
 137 +       cmp r8, r9
 138 +       blo 1b
 139 +       b cns3xxx_fiq_exit
 140 +
 141 +cns3xxx_dma_clean_range:
 142 +       bic r8, r8, #D_CACHE_LINE_SIZE - 1
 143 +1:
 144 +       mcr p15, 0, r8, c7, c10, 1    @ clean D line
 145 +       add r8, r8, #D_CACHE_LINE_SIZE
 146 +       cmp r8, r9
 147 +       blo 1b
 148 +       b cns3xxx_fiq_exit
 149 +
 150 +cns3xxx_dma_inv_range:
 151 +       tst r8, #D_CACHE_LINE_SIZE - 1
 152 +       bic r8, r8, #D_CACHE_LINE_SIZE - 1
 153 +       mcrne p15, 0, r8, c7, c10, 1    @ clean D line
 154 +       tst r9, #D_CACHE_LINE_SIZE - 1
 155 +       bic r9, r9, #D_CACHE_LINE_SIZE - 1
 156 +       mcrne p15, 0, r9, c7, c14, 1    @ clean & invalidate D line
 157 +1:
 158 +       mcr p15, 0, r8, c7, c6, 1   @ invalidate D line
 159 +       add r8, r8, #D_CACHE_LINE_SIZE
 160 +       cmp r8, r9
 161 +       blo 1b
 162 +       b cns3xxx_fiq_exit
 163 +
 164 +cns3xxx_fiq_end:
 165 --- a/arch/arm/mach-cns3xxx/include/mach/cns3xxx.h
 166 +++ b/arch/arm/mach-cns3xxx/include/mach/cns3xxx.h
 167 @@ -294,6 +294,7 @@
 168  #define MISC_PCIE_INT_MASK(x)                  MISC_MEM_MAP(0x978 + (x) * 0x100)
 169  #define MISC_PCIE_INT_STATUS(x)                        MISC_MEM_MAP(0x97C + (x) * 0x100)
 170
 171 +#define MISC_FIQ_CPU(x)                                MISC_MEM_MAP(0xA58 - (x) * 0x4)
 172  /*
 173   * Power management and clock control
 174   */
 175 --- a/arch/arm/mach-cns3xxx/include/mach/irqs.h
 176 +++ b/arch/arm/mach-cns3xxx/include/mach/irqs.h
 177 @@ -14,6 +14,7 @@
 178  #define IRQ_LOCALTIMER         29
 179  #define IRQ_LOCALWDOG          30
 180  #define IRQ_TC11MP_GIC_START   32
 181 +#define FIQ_START 0
 182
 183  #include <mach/cns3xxx.h>
 184
 185 --- /dev/null
 186 +++ b/arch/arm/mach-cns3xxx/include/mach/smp.h
 187 @@ -0,0 +1,8 @@
 188 +#ifndef __MACH_SMP_H
 189 +#define __MACH_SMP_H
 190 +
 191 +extern void smp_dma_map_area(const void *, size_t, int);
 192 +extern void smp_dma_unmap_area(const void *, size_t, int);
 193 +extern void smp_dma_flush_range(const void *, const void *);
 194 +
 195 +#endif
 196 --- a/arch/arm/mach-cns3xxx/platsmp.c
 197 +++ b/arch/arm/mach-cns3xxx/platsmp.c
 198 @@ -24,10 +24,27 @@
 199  #include <asm/hardware/gic.h>
 200  #include <asm/smp_scu.h>
 201  #include <asm/unified.h>
 202 -
 203 +#include <asm/fiq.h>
 204 +#include <mach/smp.h>
 205  #include <mach/cns3xxx.h>
 206
 207 +static struct fiq_handler fh = {
 208 +       .name = "cns3xxx-fiq"
 209 +};
 210 +
 211 +static unsigned int fiq_buffer[8];
 212 +
 213 +#define FIQ_ENABLED         0x80000000
 214 +#define FIQ_GENERATE                           0x00010000
 215 +#define CNS3XXX_MAP_AREA    0x01000000
 216 +#define CNS3XXX_UNMAP_AREA  0x02000000
 217 +#define CNS3XXX_FLUSH_RANGE 0x03000000
 218 +
 219  extern void cns3xxx_secondary_startup(void);
 220 +extern unsigned char cns3xxx_fiq_start, cns3xxx_fiq_end;
 221 +extern unsigned int fiq_number[2];
 222 +extern struct cpu_cache_fns cpu_cache;
 223 +struct cpu_cache_fns cpu_cache_save;
 224
 225  #define SCU_CPU_STATUS 0x08
 226  static void __iomem *scu_base;
 227 @@ -38,12 +55,50 @@ static void __iomem *scu_base;
 228   */
 229  volatile int __cpuinitdata pen_release = -1;
 230
 231 +static void __init cns3xxx_set_fiq_regs(void)
 232 +{
 233 +       struct pt_regs FIQ_regs;
 234 +       unsigned int cpu = smp_processor_id();
 235 +
 236 +       if (cpu) {
 237 +               FIQ_regs.ARM_ip = (unsigned int)&fiq_buffer[4];
 238 +               FIQ_regs.ARM_sp = (unsigned int)MISC_FIQ_CPU(0);
 239 +       } else {
 240 +               FIQ_regs.ARM_ip = (unsigned int)&fiq_buffer[0];
 241 +               FIQ_regs.ARM_sp = (unsigned int)MISC_FIQ_CPU(1);
 242 +       }
 243 +       set_fiq_regs(&FIQ_regs);
 244 +}
 245 +
 246 +static void __init cns3xxx_init_fiq(void)
 247 +{
 248 +       void *fiqhandler_start;
 249 +       unsigned int fiqhandler_length;
 250 +       int ret;
 251 +
 252 +       fiqhandler_start = &cns3xxx_fiq_start;
 253 +       fiqhandler_length = &cns3xxx_fiq_end - &cns3xxx_fiq_start;
 254 +
 255 +       ret = claim_fiq(&fh);
 256 +
 257 +       if (ret) {
 258 +               return;
 259 +       }
 260 +
 261 +       set_fiq_handler(fiqhandler_start, fiqhandler_length);
 262 +       fiq_buffer[0] = (unsigned int)&fiq_number[0];
 263 +       fiq_buffer[3] = 0;
 264 +       fiq_buffer[4] = (unsigned int)&fiq_number[1];
 265 +       fiq_buffer[7] = 0;
 266 +}
 267 +
 268 +
 269  /*
 270   * Write pen_release in a way that is guaranteed to be visible to all
 271   * observers, irrespective of whether they're taking part in coherency
 272   * or not.  This is necessary for the hotplug code to work reliably.
 273   */
 274 -static void write_pen_release(int val)
 275 +static void __cpuinit write_pen_release(int val)
 276  {
 277         pen_release = val;
 278         smp_wmb();
 279 @@ -63,12 +118,25 @@ void __cpuinit platform_secondary_init(u
 280         gic_secondary_init(0);
 281
 282         /*
 283 +        * Setup Secondary Core FIQ regs
 284 +        */
 285 +       cns3xxx_set_fiq_regs();
 286 +
 287 +       /*
 288          * let the primary processor know we're out of the
 289          * pen, then head off into the C entry point
 290          */
 291         write_pen_release(-1);
 292
 293         /*
 294 +        * Fixup DMA Operations
 295 +        *
 296 +        */
 297 +       cpu_cache.dma_map_area = (void *)smp_dma_map_area;
 298 +       cpu_cache.dma_unmap_area = (void *)smp_dma_unmap_area;
 299 +       cpu_cache.dma_flush_range = (void *)smp_dma_flush_range;
 300 +
 301 +       /*
 302          * Synchronise with the boot thread.
 303          */
 304         spin_lock(&boot_lock);
 305 @@ -171,4 +239,112 @@ void __init platform_smp_prepare_cpus(un
 306          */
 307         __raw_writel(virt_to_phys(cns3xxx_secondary_startup),
 308                         (void __iomem *)(CNS3XXX_MISC_BASE_VIRT + 0x0600));
 309 +
 310 +       /*
 311 +        * Setup FIQ's for main cpu
 312 +        */
 313 +       cns3xxx_init_fiq();
 314 +       cns3xxx_set_fiq_regs();
 315 +       memcpy((void *)&cpu_cache_save, (void *)&cpu_cache, sizeof(struct cpu_cache_fns));
 316 +}
 317 +
 318 +
 319 +static inline unsigned long cns3xxx_cpu_id(void)
 320 +{
 321 +       unsigned long cpu;
 322 +
 323 +       asm volatile(
 324 +               " mrc p15, 0, %0, c0, c0, 5  @ cns3xxx_cpu_id\n"
 325 +               : "=r" (cpu) : : "memory", "cc");
 326 +       return (cpu & 0xf);
 327 +}
 328 +
 329 +void smp_dma_map_area(const void *addr, size_t size, int dir)
 330 +{
 331 +       unsigned int cpu;
 332 +       unsigned long flags;
 333 +       raw_local_irq_save(flags);
 334 +       cpu = cns3xxx_cpu_id();
 335 +       if (cpu) {
 336 +               fiq_buffer[1] = (unsigned int)addr;
 337 +               fiq_buffer[2] = size;
 338 +               fiq_buffer[3] = dir | CNS3XXX_MAP_AREA | FIQ_ENABLED;
 339 +               smp_mb();
 340 +               __raw_writel(FIQ_GENERATE, MISC_FIQ_CPU(1));
 341 +
 342 +               cpu_cache_save.dma_map_area(addr, size, dir);
 343 +               while ((fiq_buffer[3]) & FIQ_ENABLED) { barrier(); }
 344 +       } else {
 345 +
 346 +               fiq_buffer[5] = (unsigned int)addr;
 347 +               fiq_buffer[6] = size;
 348 +               fiq_buffer[7] = dir | CNS3XXX_MAP_AREA | FIQ_ENABLED;
 349 +               smp_mb();
 350 +               __raw_writel(FIQ_GENERATE, MISC_FIQ_CPU(0));
 351 +
 352 +               cpu_cache_save.dma_map_area(addr, size, dir);
 353 +               while ((fiq_buffer[7]) & FIQ_ENABLED) { barrier(); }
 354 +       }
 355 +       raw_local_irq_restore(flags);
 356 +}
 357 +
 358 +void smp_dma_unmap_area(const void *addr, size_t size, int dir)
 359 +{
 360 +       unsigned int cpu;
 361 +       unsigned long flags;
 362 +
 363 +       raw_local_irq_save(flags);
 364 +       cpu = cns3xxx_cpu_id();
 365 +       if (cpu) {
 366 +
 367 +               fiq_buffer[1] = (unsigned int)addr;
 368 +               fiq_buffer[2] = size;
 369 +               fiq_buffer[3] = dir | CNS3XXX_UNMAP_AREA | FIQ_ENABLED;
 370 +               smp_mb();
 371 +               __raw_writel(FIQ_GENERATE, MISC_FIQ_CPU(1));
 372 +
 373 +               cpu_cache_save.dma_unmap_area(addr, size, dir);
 374 +               while ((fiq_buffer[3]) & FIQ_ENABLED) { barrier(); }
 375 +       } else {
 376 +
 377 +               fiq_buffer[5] = (unsigned int)addr;
 378 +               fiq_buffer[6] = size;
 379 +               fiq_buffer[7] = dir | CNS3XXX_UNMAP_AREA | FIQ_ENABLED;
 380 +               smp_mb();
 381 +               __raw_writel(FIQ_GENERATE, MISC_FIQ_CPU(0));
 382 +
 383 +               cpu_cache_save.dma_unmap_area(addr, size, dir);
 384 +               while ((fiq_buffer[7]) & FIQ_ENABLED) { barrier(); }
 385 +       }
 386 +       raw_local_irq_restore(flags);
 387 +}
 388 +
 389 +void smp_dma_flush_range(const void *start, const void *end)
 390 +{
 391 +       unsigned int cpu;
 392 +       unsigned long flags;
 393 +       raw_local_irq_save(flags);
 394 +       cpu = cns3xxx_cpu_id();
 395 +       if (cpu) {
 396 +
 397 +               fiq_buffer[1] = (unsigned int)start;
 398 +               fiq_buffer[2] = (unsigned int)end;
 399 +               fiq_buffer[3] = CNS3XXX_FLUSH_RANGE | FIQ_ENABLED;
 400 +               smp_mb();
 401 +               __raw_writel(FIQ_GENERATE, MISC_FIQ_CPU(1));
 402 +
 403 +               cpu_cache_save.dma_flush_range(start, end);
 404 +               while ((fiq_buffer[3]) & FIQ_ENABLED) { barrier(); }
 405 +       } else {
 406 +
 407 +               fiq_buffer[5] = (unsigned int)start;
 408 +               fiq_buffer[6] = (unsigned int)end;
 409 +               fiq_buffer[7] = CNS3XXX_FLUSH_RANGE | FIQ_ENABLED;
 410 +               smp_mb();
 411 +               __raw_writel(FIQ_GENERATE, MISC_FIQ_CPU(0));
 412 +
 413 +               cpu_cache_save.dma_flush_range(start, end);
 414 +               while ((fiq_buffer[7]) & FIQ_ENABLED) { barrier(); }
 415 +       }
 416 +       raw_local_irq_restore(flags);
 417  }
 418 --- a/arch/arm/mm/Kconfig
 419 +++ b/arch/arm/mm/Kconfig
 420 @@ -793,7 +793,7 @@ config NEEDS_SYSCALL_FOR_CMPXCHG
 421
 422  config DMA_CACHE_RWFO
 423         bool "Enable read/write for ownership DMA cache maintenance"
 424 -       depends on CPU_V6K && SMP
 425 +       depends on CPU_V6K && SMP && !ARCH_CNS3XXX
 426         default y
 427         help
 428           The Snoop Control Unit on ARM11MPCore does not detect the