csky/kernel/ftrace.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 // Copyright (C) 2018 Hangzhou C-SKY Microsystems co.,ltd.
0003
0004 #include <linux/ftrace.h>
0005 #include <linux/uaccess.h>
0006 #include <linux/stop_machine.h>
0007 #include <asm/cacheflush.h>
0008
0009 #ifdef CONFIG_DYNAMIC_FTRACE
0010
0011 #define NOP     0x4000
0012 #define NOP32_HI    0xc400
0013 #define NOP32_LO    0x4820
0014 #define PUSH_LR     0x14d0
0015 #define MOVIH_LINK  0xea3a
0016 #define ORI_LINK    0xef5a
0017 #define JSR_LINK    0xe8fa
0018 #define BSR_LINK    0xe000
0019
0020 /*
0021  * Gcc-csky with -pg will insert stub in function prologue:
0022  *  push    lr
0023  *  jbsr    _mcount
0024  *  nop32
0025  *  nop32
0026  *
0027  * If the (callee - current_pc) is less then 64MB, we'll use bsr:
0028  *  push    lr
0029  *  bsr _mcount
0030  *  nop32
0031  *  nop32
0032  * else we'll use (movih + ori + jsr):
0033  *  push    lr
0034  *  movih   r26, ...
0035  *  ori r26, ...
0036  *  jsr r26
0037  *
0038  * (r26 is our reserved link-reg)
0039  *
0040  */
0041 static inline void make_jbsr(unsigned long callee, unsigned long pc,
0042                  uint16_t *call, bool nolr)
0043 {
0044     long offset;
0045
0046     call[0] = nolr ? NOP : PUSH_LR;
0047
0048     offset = (long) callee - (long) pc;
0049
0050     if (unlikely(offset < -67108864 || offset > 67108864)) {
0051         call[1] = MOVIH_LINK;
0052         call[2] = callee >> 16;
0053         call[3] = ORI_LINK;
0054         call[4] = callee & 0xffff;
0055         call[5] = JSR_LINK;
0056         call[6] = 0;
0057     } else {
0058         offset = offset >> 1;
0059
0060         call[1] = BSR_LINK |
0061              ((uint16_t)((unsigned long) offset >> 16) & 0x3ff);
0062         call[2] = (uint16_t)((unsigned long) offset & 0xffff);
0063         call[3] = call[5] = NOP32_HI;
0064         call[4] = call[6] = NOP32_LO;
0065     }
0066 }
0067
0068 static uint16_t nops[7] = {NOP, NOP32_HI, NOP32_LO, NOP32_HI, NOP32_LO,
0069                 NOP32_HI, NOP32_LO};
0070 static int ftrace_check_current_nop(unsigned long hook)
0071 {
0072     uint16_t olds[7];
0073     unsigned long hook_pos = hook - 2;
0074
0075     if (copy_from_kernel_nofault((void *)olds, (void *)hook_pos,
0076             sizeof(nops)))
0077         return -EFAULT;
0078
0079     if (memcmp((void *)nops, (void *)olds, sizeof(nops))) {
0080         pr_err("%p: nop but get (%04x %04x %04x %04x %04x %04x %04x)\n",
0081             (void *)hook_pos,
0082             olds[0], olds[1], olds[2], olds[3], olds[4], olds[5],
0083             olds[6]);
0084
0085         return -EINVAL;
0086     }
0087
0088     return 0;
0089 }
0090
0091 static int ftrace_modify_code(unsigned long hook, unsigned long target,
0092                   bool enable, bool nolr)
0093 {
0094     uint16_t call[7];
0095
0096     unsigned long hook_pos = hook - 2;
0097     int ret = 0;
0098
0099     make_jbsr(target, hook, call, nolr);
0100
0101     ret = copy_to_kernel_nofault((void *)hook_pos, enable ? call : nops,
0102                  sizeof(nops));
0103     if (ret)
0104         return -EPERM;
0105
0106     flush_icache_range(hook_pos, hook_pos + MCOUNT_INSN_SIZE);
0107
0108     return 0;
0109 }
0110
0111 int ftrace_make_call(struct dyn_ftrace *rec, unsigned long addr)
0112 {
0113     int ret = ftrace_check_current_nop(rec->ip);
0114
0115     if (ret)
0116         return ret;
0117
0118     return ftrace_modify_code(rec->ip, addr, true, false);
0119 }
0120
0121 int ftrace_make_nop(struct module *mod, struct dyn_ftrace *rec,
0122             unsigned long addr)
0123 {
0124     return ftrace_modify_code(rec->ip, addr, false, false);
0125 }
0126
0127 int ftrace_update_ftrace_func(ftrace_func_t func)
0128 {
0129     int ret = ftrace_modify_code((unsigned long)&ftrace_call,
0130                 (unsigned long)func, true, true);
0131     if (!ret)
0132         ret = ftrace_modify_code((unsigned long)&ftrace_regs_call,
0133                 (unsigned long)func, true, true);
0134     return ret;
0135 }
0136 #endif /* CONFIG_DYNAMIC_FTRACE */
0137
0138 #ifdef CONFIG_DYNAMIC_FTRACE_WITH_REGS
0139 int ftrace_modify_call(struct dyn_ftrace *rec, unsigned long old_addr,
0140                unsigned long addr)
0141 {
0142     return ftrace_modify_code(rec->ip, addr, true, true);
0143 }
0144 #endif
0145
0146 #ifdef CONFIG_FUNCTION_GRAPH_TRACER
0147 void prepare_ftrace_return(unsigned long *parent, unsigned long self_addr,
0148                unsigned long frame_pointer)
0149 {
0150     unsigned long return_hooker = (unsigned long)&return_to_handler;
0151     unsigned long old;
0152
0153     if (unlikely(atomic_read(&current->tracing_graph_pause)))
0154         return;
0155
0156     old = *parent;
0157
0158     if (!function_graph_enter(old, self_addr,
0159             *(unsigned long *)frame_pointer, parent)) {
0160         /*
0161          * For csky-gcc function has sub-call:
0162          * subi sp, sp, 8
0163          * stw  r8, (sp, 0)
0164          * mov  r8, sp
0165          * st.w r15,    (sp, 0x4)
0166          * push r15
0167          * jl   _mcount
0168          * We only need set *parent for resume
0169          *
0170          * For csky-gcc function has no sub-call:
0171          * subi sp, sp, 4
0172          * stw  r8, (sp, 0)
0173          * mov  r8, sp
0174          * push r15
0175          * jl   _mcount
0176          * We need set *parent and *(frame_pointer + 4) for resume,
0177          * because lr is resumed twice.
0178          */
0179         *parent = return_hooker;
0180         frame_pointer += 4;
0181         if (*(unsigned long *)frame_pointer == old)
0182             *(unsigned long *)frame_pointer = return_hooker;
0183     }
0184 }
0185
0186 #ifdef CONFIG_DYNAMIC_FTRACE
0187 int ftrace_enable_ftrace_graph_caller(void)
0188 {
0189     return ftrace_modify_code((unsigned long)&ftrace_graph_call,
0190             (unsigned long)&ftrace_graph_caller, true, true);
0191 }
0192
0193 int ftrace_disable_ftrace_graph_caller(void)
0194 {
0195     return ftrace_modify_code((unsigned long)&ftrace_graph_call,
0196             (unsigned long)&ftrace_graph_caller, false, true);
0197 }
0198 #endif /* CONFIG_DYNAMIC_FTRACE */
0199 #endif /* CONFIG_FUNCTION_GRAPH_TRACER */
0200
0201 #ifdef CONFIG_DYNAMIC_FTRACE
0202 #ifndef CONFIG_CPU_HAS_ICACHE_INS
0203 struct ftrace_modify_param {
0204     int command;
0205     atomic_t cpu_count;
0206 };
0207
0208 static int __ftrace_modify_code(void *data)
0209 {
0210     struct ftrace_modify_param *param = data;
0211
0212     if (atomic_inc_return(&param->cpu_count) == 1) {
0213         ftrace_modify_all_code(param->command);
0214         atomic_inc(&param->cpu_count);
0215     } else {
0216         while (atomic_read(&param->cpu_count) <= num_online_cpus())
0217             cpu_relax();
0218         local_icache_inv_all(NULL);
0219     }
0220
0221     return 0;
0222 }
0223
0224 void arch_ftrace_update_code(int command)
0225 {
0226     struct ftrace_modify_param param = { command, ATOMIC_INIT(0) };
0227
0228     stop_machine(__ftrace_modify_code, &param, cpu_online_mask);
0229 }
0230 #endif
0231 #endif /* CONFIG_DYNAMIC_FTRACE */
0232
0233 /* _mcount is defined in abi's mcount.S */
0234 EXPORT_SYMBOL(_mcount);