*/
        mod_code_status = probe_kernel_write(mod_code_ip, mod_code_newcode,
                                             MCOUNT_INSN_SIZE);
-
 }
 
 void ftrace_nmi_enter(void)
 
 #ifdef CONFIG_FUNCTION_GRAPH_TRACER
 
-#ifndef CONFIG_DYNAMIC_FTRACE
+#ifdef CONFIG_DYNAMIC_FTRACE
+extern void ftrace_graph_call(void);
+
+static int ftrace_mod_jmp(unsigned long ip,
+                         int old_offset, int new_offset)
+{
+       unsigned char code[MCOUNT_INSN_SIZE];
+
+       if (probe_kernel_read(code, (void *)ip, MCOUNT_INSN_SIZE))
+               return -EFAULT;
+
+       if (code[0] != 0xe9 || old_offset != *(int *)(&code[1]))
+               return -EINVAL;
+
+       *(int *)(&code[1]) = new_offset;
+
+       if (do_ftrace_mod_code(ip, &code))
+               return -EPERM;
+
+       return 0;
+}
+
+int ftrace_enable_ftrace_graph_caller(void)
+{
+       unsigned long ip = (unsigned long)(&ftrace_graph_call);
+       int old_offset, new_offset;
+
+       old_offset = (unsigned long)(&ftrace_stub) - (ip + MCOUNT_INSN_SIZE);
+       new_offset = (unsigned long)(&ftrace_graph_caller) - (ip + MCOUNT_INSN_SIZE);
+
+       return ftrace_mod_jmp(ip, old_offset, new_offset);
+}
+
+int ftrace_disable_ftrace_graph_caller(void)
+{
+       unsigned long ip = (unsigned long)(&ftrace_graph_call);
+       int old_offset, new_offset;
+
+       old_offset = (unsigned long)(&ftrace_graph_caller) - (ip + MCOUNT_INSN_SIZE);
+       new_offset = (unsigned long)(&ftrace_stub) - (ip + MCOUNT_INSN_SIZE);
+
+       return ftrace_mod_jmp(ip, old_offset, new_offset);
+}
+
+#else /* CONFIG_DYNAMIC_FTRACE */
 
 /*
  * These functions are picked from those used on
 {
        atomic_dec(&in_nmi);
 }
+
 #endif /* !CONFIG_DYNAMIC_FTRACE */
 
 /* Add a function return address to the trace stack on thread info.*/
 
        FTRACE_UPDATE_TRACE_FUNC        = (1 << 2),
        FTRACE_ENABLE_MCOUNT            = (1 << 3),
        FTRACE_DISABLE_MCOUNT           = (1 << 4),
+       FTRACE_START_FUNC_RET           = (1 << 5),
+       FTRACE_STOP_FUNC_RET            = (1 << 6),
 };
 
 static int ftrace_filtered;
        unsigned long ip, fl;
        unsigned long ftrace_addr;
 
-#ifdef CONFIG_FUNCTION_GRAPH_TRACER
-       if (ftrace_tracing_type == FTRACE_TYPE_ENTER)
-               ftrace_addr = (unsigned long)ftrace_caller;
-       else
-               ftrace_addr = (unsigned long)ftrace_graph_caller;
-#else
        ftrace_addr = (unsigned long)ftrace_caller;
-#endif
 
        ip = rec->ip;
 
        if (*command & FTRACE_UPDATE_TRACE_FUNC)
                ftrace_update_ftrace_func(ftrace_trace_function);
 
+       if (*command & FTRACE_START_FUNC_RET)
+               ftrace_enable_ftrace_graph_caller();
+       else if (*command & FTRACE_STOP_FUNC_RET)
+               ftrace_disable_ftrace_graph_caller();
+
        return 0;
 }
 
        ftrace_run_update_code(command);
 }
 
-static void ftrace_startup(void)
+static void ftrace_startup(int command)
 {
-       int command = 0;
-
        if (unlikely(ftrace_disabled))
                return;
 
        mutex_unlock(&ftrace_start_lock);
 }
 
-static void ftrace_shutdown(void)
+static void ftrace_shutdown(int command)
 {
-       int command = 0;
-
        if (unlikely(ftrace_disabled))
                return;
 
 
 static inline int ftrace_init_dyn_debugfs(struct dentry *d_tracer) { return 0; }
 static inline void ftrace_startup_enable(int command) { }
-# define ftrace_startup()              do { } while (0)
-# define ftrace_shutdown()             do { } while (0)
+/* Keep as macros so we do not need to define the commands */
+# define ftrace_startup(command)       do { } while (0)
+# define ftrace_shutdown(command)      do { } while (0)
 # define ftrace_startup_sysctl()       do { } while (0)
 # define ftrace_shutdown_sysctl()      do { } while (0)
 #endif /* CONFIG_DYNAMIC_FTRACE */
        }
 
        ret = __register_ftrace_function(ops);
-       ftrace_startup();
+       ftrace_startup(0);
 
 out:
        mutex_unlock(&ftrace_sysctl_lock);
 
        mutex_lock(&ftrace_sysctl_lock);
        ret = __unregister_ftrace_function(ops);
-       ftrace_shutdown();
+       ftrace_shutdown(0);
        mutex_unlock(&ftrace_sysctl_lock);
 
        return ret;
        ftrace_tracing_type = FTRACE_TYPE_RETURN;
        ftrace_graph_return = retfunc;
        ftrace_graph_entry = entryfunc;
-       ftrace_startup();
+       ftrace_startup(FTRACE_START_FUNC_RET);
 
 out:
        mutex_unlock(&ftrace_sysctl_lock);
        atomic_dec(&ftrace_graph_active);
        ftrace_graph_return = (trace_func_graph_ret_t)ftrace_stub;
        ftrace_graph_entry = (trace_func_graph_ent_t)ftrace_stub;
-       ftrace_shutdown();
+       ftrace_shutdown(FTRACE_STOP_FUNC_RET);
        /* Restore normal tracing type */
        ftrace_tracing_type = FTRACE_TYPE_ENTER;