arch/metag/kernel/ftrace.c

   1 /*
   2  * Copyright (C) 2008 Imagination Technologies Ltd.
   3  * Licensed under the GPL
   4  *
   5  * Dynamic ftrace support.
   6  */
   7
   8 #include <linux/ftrace.h>
   9 #include <linux/io.h>
  10 #include <linux/uaccess.h>
  11
  12 #include <asm/cacheflush.h>
  13
  14 #define D04_MOVT_TEMPLATE       0x02200005
  15 #define D04_CALL_TEMPLATE       0xAC200005
  16 #define D1RTP_MOVT_TEMPLATE     0x03200005
  17 #define D1RTP_CALL_TEMPLATE     0xAC200006
  18
  19 static const unsigned long NOP[2] = {0xa0fffffe, 0xa0fffffe};
  20 static unsigned long movt_and_call_insn[2];
  21
  22 static unsigned char *ftrace_nop_replace(void)
  23 {
  24         return (char *)&NOP[0];
  25 }
  26
  27 static unsigned char *ftrace_call_replace(unsigned long pc, unsigned long addr)
  28 {
  29         unsigned long hi16, low16;
  30
  31         hi16 = (addr & 0xffff0000) >> 13;
  32         low16 = (addr & 0x0000ffff) << 3;
  33
  34         /*
  35          * The compiler makes the call to mcount_wrapper()
  36          * (Meta's wrapper around mcount()) through the register
  37          * D0.4. So whenever we're patching one of those compiler-generated
  38          * calls we also need to go through D0.4. Otherwise use D1RtP.
  39          */
  40         if (pc == (unsigned long)&ftrace_call) {
  41                 writel(D1RTP_MOVT_TEMPLATE | hi16, &movt_and_call_insn[0]);
  42                 writel(D1RTP_CALL_TEMPLATE | low16, &movt_and_call_insn[1]);
  43         } else {
  44                 writel(D04_MOVT_TEMPLATE | hi16, &movt_and_call_insn[0]);
  45                 writel(D04_CALL_TEMPLATE | low16, &movt_and_call_insn[1]);
  46         }
  47
  48         return (unsigned char *)&movt_and_call_insn[0];
  49 }
  50
  51 static int ftrace_modify_code(unsigned long pc, unsigned char *old_code,
  52                               unsigned char *new_code)
  53 {
  54         unsigned char replaced[MCOUNT_INSN_SIZE];
  55
  56         /*
  57          * Note: Due to modules and __init, code can
  58          *  disappear and change, we need to protect against faulting
  59          *  as well as code changing.
  60          *
  61          * No real locking needed, this code is run through
  62          * kstop_machine.
  63          */
  64
  65         /* read the text we want to modify */
  66         if (probe_kernel_read(replaced, (void *)pc, MCOUNT_INSN_SIZE))
  67                 return -EFAULT;
  68
  69         /* Make sure it is what we expect it to be */
  70         if (memcmp(replaced, old_code, MCOUNT_INSN_SIZE) != 0)
  71                 return -EINVAL;
  72
  73         /* replace the text with the new text */
  74         if (probe_kernel_write((void *)pc, new_code, MCOUNT_INSN_SIZE))
  75                 return -EPERM;
  76
  77         flush_icache_range(pc, pc + MCOUNT_INSN_SIZE);
  78
  79         return 0;
  80 }
  81
  82 int ftrace_update_ftrace_func(ftrace_func_t func)
  83 {
  84         int ret;
  85         unsigned long pc;
  86         unsigned char old[MCOUNT_INSN_SIZE], *new;
  87
  88         pc = (unsigned long)&ftrace_call;
  89         memcpy(old, &ftrace_call, MCOUNT_INSN_SIZE);
  90         new = ftrace_call_replace(pc, (unsigned long)func);
  91         ret = ftrace_modify_code(pc, old, new);
  92
  93         return ret;
  94 }
  95
  96 int ftrace_make_nop(struct module *mod,
  97                     struct dyn_ftrace *rec, unsigned long addr)
  98 {
  99         unsigned char *new, *old;
 100         unsigned long ip = rec->ip;
 101
 102         old = ftrace_call_replace(ip, addr);
 103         new = ftrace_nop_replace();
 104
 105         return ftrace_modify_code(ip, old, new);
 106 }
 107
 108 int ftrace_make_call(struct dyn_ftrace *rec, unsigned long addr)
 109 {
 110         unsigned char *new, *old;
 111         unsigned long ip = rec->ip;
 112
 113         old = ftrace_nop_replace();
 114         new = ftrace_call_replace(ip, addr);
 115
 116         return ftrace_modify_code(ip, old, new);
 117 }
 118
 119 /* run from kstop_machine */
 120 int __init ftrace_dyn_arch_init(void *data)
 121 {
 122         /* The return code is returned via data */
 123         writel(0, data);
 124
 125         return 0;
 126 }