arch/arm/common/fiq_glue_setup.c

   1 /*
   2  * Copyright (C) 2010 Google, Inc.
   3  *
   4  * This software is licensed under the terms of the GNU General Public
   5  * License version 2, as published by the Free Software Foundation, and
   6  * may be copied, distributed, and modified under those terms.
   7  *
   8  * This program is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  */
  13
  14 #include <linux/kernel.h>
  15 #include <linux/percpu.h>
  16 #include <linux/slab.h>
  17 #include <asm/fiq.h>
  18 #include <asm/fiq_glue.h>
  19
  20 extern unsigned char fiq_glue, fiq_glue_end;
  21 extern void fiq_glue_setup(void *func, void *data, void *sp);
  22
  23 static struct fiq_handler fiq_debbuger_fiq_handler = {
  24         .name = "fiq_glue",
  25 };
  26 DEFINE_PER_CPU(void *, fiq_stack);
  27 static struct fiq_glue_handler *current_handler;
  28 static DEFINE_MUTEX(fiq_glue_lock);
  29
  30 static void fiq_glue_setup_helper(void *info)
  31 {
  32         struct fiq_glue_handler *handler = info;
  33         fiq_glue_setup(handler->fiq, handler,
  34                 __get_cpu_var(fiq_stack) + THREAD_START_SP);
  35 }
  36
  37 int fiq_glue_register_handler(struct fiq_glue_handler *handler)
  38 {
  39         int ret;
  40         int cpu;
  41
  42         if (!handler || !handler->fiq)
  43                 return -EINVAL;
  44
  45         mutex_lock(&fiq_glue_lock);
  46         if (fiq_stack) {
  47                 ret = -EBUSY;
  48                 goto err_busy;
  49         }
  50
  51         for_each_possible_cpu(cpu) {
  52                 void *stack;
  53                 stack = (void *)__get_free_pages(GFP_KERNEL, THREAD_SIZE_ORDER);
  54                 if (WARN_ON(!stack)) {
  55                         ret = -ENOMEM;
  56                         goto err_alloc_fiq_stack;
  57                 }
  58                 per_cpu(fiq_stack, cpu) = stack;
  59         }
  60
  61         ret = claim_fiq(&fiq_debbuger_fiq_handler);
  62         if (WARN_ON(ret))
  63                 goto err_claim_fiq;
  64
  65         current_handler = handler;
  66         on_each_cpu(fiq_glue_setup_helper, handler, true);
  67         set_fiq_handler(&fiq_glue, &fiq_glue_end - &fiq_glue);
  68
  69         mutex_unlock(&fiq_glue_lock);
  70         return 0;
  71
  72 err_claim_fiq:
  73 err_alloc_fiq_stack:
  74         for_each_possible_cpu(cpu) {
  75                 __free_pages(per_cpu(fiq_stack, cpu), THREAD_SIZE_ORDER);
  76                 per_cpu(fiq_stack, cpu) = NULL;
  77         }
  78 err_busy:
  79         mutex_unlock(&fiq_glue_lock);
  80         return ret;
  81 }
  82
  83 /**
  84  * fiq_glue_resume - Restore fiqs after suspend or low power idle states
  85  *
  86  * This must be called before calling local_fiq_enable after returning from a
  87  * power state where the fiq mode registers were lost. If a driver provided
  88  * a resume hook when it registered the handler it will be called.
  89  */
  90
  91 void fiq_glue_resume(void)
  92 {
  93         if (!current_handler)
  94                 return;
  95         fiq_glue_setup(current_handler->fiq, current_handler,
  96                 __get_cpu_var(fiq_stack) + THREAD_START_SP);
  97         if (current_handler->resume)
  98                 current_handler->resume(current_handler);
  99 }
 100