Idea.

swap_func.cpp

#include <sys/types.h>
#include <sys/mman.h>

#include <assert.h>
#include <err.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>

#include <map>

using namespace std;

struct func_entry {
        unsigned char orig_byte;
        void *new_addr;
};

static std::map<void *,struct func_entry *> func_mapping;

int
swap_func(void *old_func, void *new_func)
{
        unsigned char *ptr;
        int ret;
        struct func_entry *entry;

        entry = (struct func_entry *)calloc(1, sizeof(*entry));
        assert(entry != NULL);

        if (func_mapping.count(old_func) > 0)
            return (1);

        ptr = (unsigned char *)old_func;

        func_mapping[old_func] = entry;
        ret = mprotect(old_func, 1, PROT_READ | PROT_WRITE | PROT_EXEC);
        assert(ret == 0);

        func_mapping[old_func]->orig_byte = ptr[0];
        ptr[0] = 0xcc; /* int3 */

        ret = mprotect(old_func, 1, PROT_READ | PROT_EXEC);
        assert(ret == 0);

        func_mapping[old_func]->new_addr = new_func;
        return (0);
}

int
unswap_func(void *old_func)
{
        unsigned char *ptr;
        int ret;
        struct func_entry *entry;

        if (func_mapping.count(old_func) == 0)
            return (1);

        ptr = (unsigned char *)old_func;
        entry = func_mapping[old_func];
        ret = mprotect(old_func, 1, PROT_READ | PROT_WRITE | PROT_EXEC);
        assert(ret == 0);
        ptr[0] = func_mapping[old_func]->orig_byte;
        ret = mprotect(old_func, 1, PROT_READ | PROT_EXEC);
        assert(ret == 0);
        func_mapping.erase(old_func);
        free(entry);
        return (0);
}

void
sigtrap(int signo, siginfo_t *info, void *addr)
{
        ucontext_t *ctx = (ucontext_t *)addr;
        void *func_addr;

        func_addr = (void *)(ctx->uc_mcontext.mc_rip - 1);
        if (func_mapping.count(func_addr) != 0) {
            ctx->uc_mcontext.mc_rip =
                (register_t)(func_mapping[func_addr]->new_addr);
        }
}

ssize_t
my_read(int fd, void *buffer, size_t nbytes)
{

        printf("dupa!\n");
        return (0);
}

int
main()
{
        char buff[4096];
        struct sigaction action;

        action.sa_sigaction = sigtrap;
        action.sa_flags = 0;
        sigemptyset(&action.sa_mask);
        if (sigaction(SIGTRAP, &action, NULL) != 0)
            err(1, "sigaction(2) failed");

        printf("%p\n", &read);
        printf("%p\n", &my_read);
        assert(swap_func((void *)&read, (void *)&my_read) == 0);
        read(0, buff, sizeof(buff));
        assert(unswap_func((void *)&read) == 0);
        read(0, buff, sizeof(buff));
        return (0);
}