LXR linux/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
  19static const unsigned long NOP[2] = {0xa0fffffe, 0xa0fffffe};
  20static unsigned long movt_and_call_insn[2];
  21
  22static unsigned char *ftrace_nop_replace(void)
  23{
  24        return (char *)&NOP[0];
  25}
  26
  27static 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
  51static 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
  82int 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
  96int 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
 108int 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 */
 120int __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}
 127