// SPDX-License-Identifier: GPL-3.0-only

#include "protocol.h"
#include "sdk_shim.h"
#include "net.h"
#include "debug.h"
#include "proc.h"
#include "kern_rw_fast.h"
#include "debug_state.h"

struct server_client {
    uint32_t active;
    int      fd;
    uint32_t debugging;
    uint8_t  client_addr[16];
    uint8_t  pad_1c[4];
    uint8_t  dbgctx[0x358];
};

extern struct server_client g_clients[12];
extern void    *curdbgcli;
extern void    *curdbgctx;
extern uint32_t g_debug_attached;
extern void    *g_server_mutex;
extern void    *g_proc_rw_mutex;

struct dbgctx {
    uint32_t pid;
    int      dbgfd;

};

static int g_cached_app_pid = 0;
static int g_cached_app_id  = 0;

void debug_resume_cache_flush(void) {
    g_cached_app_pid = 0;
    g_cached_app_id  = 0;
}

static int resume_app_via_self_id(int pid) {
    int app_id;
    if (pid != 0 && pid == g_cached_app_pid && g_cached_app_id != 0) {
        app_id = g_cached_app_id;
    } else {
        int rc = _sceApplicationGetAppId(pid, &app_id);
        if (rc != 0) return rc;
        g_cached_app_pid = pid;
        g_cached_app_id  = app_id;
    }
    return sceApplicationContinue(app_id);
}

extern bool fw_uses_kernel_dbreg_path(void);
extern bool is_process_stopped(int pid);
extern bool kern_thread_step_walker(int pid);
extern int  kern_get_dbregs(int pid, int lwpid, void *dbreg_buf);
extern int  kern_set_dbregs(int pid, int lwpid, void *dbreg_buf);
extern int  kern_proc_install_dbregs(int pid, int lwpid, void *fpu_buf);
extern int  kern_get_proc_info_by_pid(int pid, int lwpid, void *regs_buf);
extern int  kern_apply_thread_dbgctx(int pid, int lwpid, void *regs_buf);
extern int  kern_get_lwp_full_state(int pid, int lwpid, void *regs, void *dbreg, void *fpu);
extern void kern_thread_cache_flush(void);

#define REG_BLOB_SIZE     0xB0
#define FPREG_BLOB_SIZE   0x340
#define DBREG_BLOB_SIZE   0x80

struct cmd_debug_lwp_packet {
    uint32_t lwpid;
} __attribute__((packed));

struct cmd_debug_setreg_packet {
    uint32_t lwpid;
    uint32_t length;
} __attribute__((packed));

#define DBGCTX() ((struct dbgctx *)curdbgctx)

int connect_debugger(struct dbgctx *ctx, void *client_sockaddr_in) {
    g_debug_attached = 1;

    struct sockaddr_in addr;
    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_port   = htons(755);

    addr.sin_addr.s_addr = *(uint32_t *)((uint8_t *)client_sockaddr_in + 4);

    int sock = socket(AF_INET, SOCK_STREAM, 0);
    ctx->dbgfd = sock;
    if (sock <= 0) return 1;

    extern void configure_socket(int fd);
    configure_socket(sock);

    if (connect(sock, (struct sockaddr *)&addr, sizeof(addr)) != 0) {
        close(sock);
        ctx->dbgfd = -1;
        return 1;
    }
    return 0;
}

uint32_t g_stepping_lwpid = 0;

extern int sys_proc_vm_map(uint32_t pid, void **out_maps, int *out_count);
extern int gettimeofday(struct timeval *tp, void *tzp);

#define INT3_REWIND_MAX 16
static uint64_t g_recently_disabled_bps[INT3_REWIND_MAX];
static int      g_recently_disabled_count = 0;

static int int3_scan_stuck(int pid, const uint64_t *bp_addrs, int n_bps,
                           int *out_lwpids, int out_cap, int *out_n) {
    if (out_n) *out_n = 0;
    int stuck = 0;
    if (n_bps <= 0) return 0;

    int numlwps = (int)ptrace_raw(PT_GETNUMLWPS, pid, NULL, 0);
    if (numlwps <= 0) return 0;

    int *lwpids = (int *)net_alloc_buffer((unsigned long)(numlwps * 4));
    if (!lwpids) return 0;

    long lr = ptrace_raw(PT_GETLWPLIST, pid, lwpids, numlwps);
    if (lr > 0) {
        uint8_t regs_buf[176];
        for (int i = 0; i < numlwps; i++) {
            errno = 0;
            if (ptrace_raw(PT_GETREGS, lwpids[i], regs_buf, 0) != 0) continue;
            uint64_t rip = *(uint64_t *)(regs_buf + 0x88);
            for (int b = 0; b < n_bps; b++) {
                uint64_t a = bp_addrs[b];
                int hit = 0;
                if (rip == a + 1) {
                    *(uint64_t *)(regs_buf + 0x88) = a;
                    ptrace_raw(PT_SETREGS, lwpids[i], regs_buf, 0);
                    hit = 1;
                } else if (rip == a) {
                    hit = 1;
                } else if (rip > a + 1 && rip < a + 16) {
                    hit = 1;
                }
                if (hit) {
                    if (out_lwpids && stuck < out_cap) out_lwpids[stuck] = lwpids[i];
                    stuck++;
                    break;
                }
            }
        }
    }
    free(lwpids);
    if (out_n) *out_n = (stuck < out_cap) ? stuck : out_cap;
    return stuck;
}

static int int3_lwp_stopped(int lwp) {
    uint8_t rb[176];
    errno = 0;
    if (ptrace_raw(PT_GETREGS, lwp, rb, 0) == 0) return 1;
    return (errno == 16) ? -1 : 0;
}

static void int3_resume_lwps(int pid, const int *lwpids, int n) {
    for (int i = 0; i < n && i < 256; i++) {
        int lw = lwpids[i];
        if (int3_lwp_stopped(lw) == 0) continue;
        for (int tries = 0; tries < 12; tries++) {
            errno = 0;
            ptrace_raw(PT_CONTINUE, lw, (void *)1, 0);
            int e1 = errno;
            if (int3_lwp_stopped(lw) == 0) break;
            errno = 0;
            ptrace_raw(PT_CONTINUE, pid, (void *)1, 0);
            int e2 = errno;
            if (int3_lwp_stopped(lw) == 0) break;
            sceKernelUsleep((e1 == 16 || e2 == 16) ? 1500 : 400);
        }
    }
}

static void int3_iterative_sweep(int pid, const uint64_t *bp_addrs, int n_bps) {
    if (n_bps <= 0) return;
    for (int round = 0; round < 4; round++) {
        int lw[INT3_REWIND_MAX]; int n = 0;
        int stuck = int3_scan_stuck(pid, bp_addrs, n_bps, lw, INT3_REWIND_MAX, &n);
        if (stuck == 0) break;
        int3_resume_lwps(pid, lw, n);
        sceKernelUsleep(5000);
    }
}

void debug_full_teardown(void *svc) {
    if (!g_debug_attached) {
        return;
    }

    g_recently_disabled_count = 0;

    debug_resume_cache_flush();
    kern_proc_cache_flush();
    kern_thread_cache_flush();

    struct elev_state es;
    int es_active = (elev_save_and_set(&es) == 0);

    void *old_cli = curdbgcli;
    curdbgcli = NULL;
    if (old_cli) {
        *(uint32_t *)((char *)old_cli + 8) = 0;
    }
    curdbgctx = NULL;
    g_debug_attached = 0;

    uint32_t prev_state = g_stopgo_mode;
    g_stopgo_resume_signal = 0xFFFFFFFFu;
    g_stopgo_resume_pid  = 0;
    if (prev_state == 2) {
        g_stopgo_mode = 0;
        g_stopgo_last_signal = 0xFFFFFFFFu;
        g_stopgo_target_pid  = 0;
    }

    int      pid   = svc ? *(int *)svc : 0;
    uint32_t dbgfd = svc ? *(uint32_t *)((char *)svc + 4) : 0;

    int   vm_count = 0;
    void *vm_maps  = NULL;
    int   alive_rc = sys_proc_vm_map((uint32_t)pid, &vm_maps, &vm_count);
    if (alive_rc != 0) {
        if (vm_maps) free(vm_maps);
        goto close_socket_and_unlock;
    }
    if (vm_maps) free(vm_maps);

    char *rbx     = (char *)svc + 0x18;
    char *bp_end  = (char *)svc + 0x2E8;
    uint64_t teardown_bp_addrs[INT3_REWIND_MAX];
    int teardown_n_bps = 0;
    while (rbx != bp_end) {
        uint64_t address = *(uint64_t *)(rbx - 8);
        if (address == 0) break;
        sys_proc_rw_w1((uint64_t)pid, address, 1, rbx, 0);
        if (teardown_n_bps < INT3_REWIND_MAX) {
            teardown_bp_addrs[teardown_n_bps++] = address;
        }
        rbx += 0x18;
    }

    int3_iterative_sweep(pid, teardown_bp_addrs, teardown_n_bps);

    char dr_buf[0x100];
    memset(dr_buf, 0, sizeof(dr_buf));
    ptrace_raw(PT_GETDBREGS, pid, dr_buf, 0);

    uint8_t dr7_low = (uint8_t)dr_buf[0x38];
    int     have_active_dr =
        (dr7_low & 0x03) || (dr7_low & 0x0C) ||
        (dr7_low & 0x30) || (dr7_low & 0xC0);

    int *lwpids = NULL;
    int  count  = 0;
    int  we_stopped = 0;

    if (!have_active_dr) {
        int rc = (int)ptrace_raw(PT_GETNUMLWPS, pid, NULL, 0);
        if (rc == -1) {
            if (errno != 16 ) {
                goto teardown_done;
            }
            kill(pid, 17 );
            wait4(pid, NULL, 0, NULL);
            we_stopped = 1;
            ptrace_raw(PT_GETNUMLWPS, pid, NULL, 0);
        }
        goto free_and_detach;
    }

    int rc = (int)ptrace_raw(PT_GETNUMLWPS, pid, NULL, 0);
    if (rc == -1) {
        if (errno != 16) {
            goto teardown_done;
        }
        kill(pid, 17);
        wait4(pid, NULL, 0, NULL);
        we_stopped = 1;
        rc    = (int)ptrace_raw(PT_GETNUMLWPS, pid, NULL, 0);
        count = rc;
        lwpids = (int *)net_alloc_buffer((unsigned long)(count * 4));
        if (!lwpids) {
            resume_app_via_self_id(pid);
            ptrace_raw(PT_CONTINUE, pid, (void *)1, 0);
            goto close_socket_and_unlock;
        }
        if (ptrace_raw(PT_GETLWPLIST, pid, lwpids, count) == -1) {
            resume_app_via_self_id(pid);
            ptrace_raw(PT_CONTINUE, pid, (void *)1, 0);
            goto close_socket_and_unlock;
        }
    } else {
        count = rc;
        lwpids = (int *)net_alloc_buffer((unsigned long)(count * 4));
        if (!lwpids) {
            goto close_socket_and_unlock;
        }
        if (ptrace_raw(PT_GETLWPLIST, pid, lwpids, count) == -1) {
            goto close_socket_and_unlock;
        }
    }

    if (count == 0) {
        goto free_and_detach;
    }

    memset(dr_buf, 0, sizeof(dr_buf));
    bool use_kernel_path = fw_uses_kernel_dbreg_path();
    for (int i = 0; i < count; i++) {
        int lwpid = lwpids[i];
        if (use_kernel_path) {
            if (kern_set_dbregs(pid, lwpid, dr_buf) != 0) {
                ptrace_raw(PT_SETDBREGS, lwpid, dr_buf, 0);
            }
        } else {
            ptrace_raw(PT_SETDBREGS, lwpid, dr_buf, 0);
        }
    }

free_and_detach:
    if (lwpids) free(lwpids);
    if (we_stopped) {
        resume_app_via_self_id(pid);
    }
    ptrace_raw(PT_DETACH, pid, NULL, 0);

close_socket_and_unlock:
    if (dbgfd > 0) {
        close((int)dbgfd);
    }

teardown_done:
    if (es_active) elev_restore(&es);
}

static const uint32_t k_stopgo_signal_table[3] = { 19u , 17u , 9u  };

int debug_stopgo_handle(int pid, char action) {
    scePthreadMutexLock(&g_server_mutex);
    g_stopgo_target_pid = (uint32_t)pid;

    if (g_debug_attached) {
        if (action == 0) {
            g_stopgo_resume_signal = 0;
            g_stopgo_resume_pid  = (uint32_t)pid;
            g_stopgo_last_signal = 0;
            g_stopgo_mode = 2;
        } else if (action == 1) {
            g_stopgo_last_signal = 0x11;
            kill(pid, 0x11);
            wait4(pid, NULL, 0, NULL);
            g_stopgo_mode = 2;
        } else if (action == 2) {
            g_stopgo_resume_signal = 9;
            g_stopgo_resume_pid  = (uint32_t)pid;
            g_stopgo_last_signal = 9;
            g_stopgo_mode = 2;
        }
    } else {
        if ((unsigned char)action <= 2) {
            uint32_t sig = k_stopgo_signal_table[(unsigned char)action];
            g_stopgo_mode = 1;
            g_stopgo_last_signal = sig;
            kill(pid, (int)sig);
            wait4(pid, NULL, 0, NULL);
            if (action == 0 && g_debug_attached) {
                resume_app_via_self_id((int)g_stopgo_resume_pid);
            }
        } else {
            kill(pid, 0);
            wait4(pid, NULL, 0, NULL);
        }
    }

    scePthreadMutexUnlock(&g_server_mutex);
    return 0;
}

int debug_process_stop_handle(int fd, struct cmd_packet *packet) {
    void *data = packet->data;
    if (!data) { net_send_int32(fd, CMD_DATA_NULL); return 1; }

    unsigned char action = ((unsigned char *)data)[4];
    if (action > 2) { net_send_int32(fd, CMD_ERROR); return 1; }

    int pid = *(int *)data;
    if (pid == 0)   { net_send_int32(fd, CMD_ERROR); return 1; }

    debug_stopgo_handle(pid, (char)action);
    net_send_int32(fd, CMD_SUCCESS);
    return 0;
}

extern long ptrace_raw(int op, int pid, void *addr, int data);
extern int *__error(void);

int debug_attach_handle(int fd, struct cmd_packet *packet) {
    if (g_debug_attached) {
        net_send_int32(fd, CMD_ALREADY_DEBUG);
        return 1;
    }

    struct cmd_debug_attach_packet *ap =
        (struct cmd_debug_attach_packet *)packet->data;
    if (!ap) {
        net_send_int32(fd, CMD_DATA_NULL);
        return 1;
    }

    struct elev_state es;
    if (elev_save_and_set(&es) != 0) {
        net_send_int32(fd, CMD_ERROR);
        return 1;
    }

    if (ptrace_raw(PT_ATTACH, ap->pid, NULL, 0) == -1) {
        elev_restore(&es);
        net_send_int32(fd, CMD_ERROR);
        return 1;
    }

    int wait_status = 0;
    *__error() = 0;
    long wait_rc = __crt_syscall(7 , ap->pid, &wait_status, 0, 0);
    if (wait_rc == -1) {
        sceKernelUsleep(20000);
    }

    resume_app_via_self_id(ap->pid);
    if (ptrace_raw(PT_CONTINUE, ap->pid, (void *)1, 0) != 0) {
        elev_restore(&es);
        net_send_int32(fd, CMD_ERROR);
        return 1;
    }

    elev_restore(&es);

    void *evt_client_sockaddr = (char *)curdbgcli + 0x0C;
    if (connect_debugger(DBGCTX(), evt_client_sockaddr) != 0) {
        net_send_int32(fd, CMD_ERROR);
        return 1;
    }

    DBGCTX()->pid = ap->pid;
    *(uint32_t *)((char *)curdbgcli + 8) = 1;
    net_send_int32(fd, CMD_SUCCESS);
    return 0;
}

int debug_detach_handle(int fd, struct cmd_packet *packet) {
    (void)packet;
    debug_full_teardown(curdbgctx);
    net_send_int32(fd, CMD_SUCCESS);
    return 0;
}

#define DBG_BP_SLOT_SIZE 0x18

int debug_set_breakpoint_handle(int fd, struct cmd_packet *packet) {
    if (!g_debug_attached) { net_send_int32(fd, CMD_ERROR); return 1; }
    int pid = DBGCTX()->pid;
    if (pid == 0) { net_send_int32(fd, CMD_ERROR); return 1; }

    struct cmd_debug_breakpt_packet *bp =
        (struct cmd_debug_breakpt_packet *)packet->data;
    if (!bp) { net_send_int32(fd, CMD_DATA_NULL); return 1; }
    if (bp->index > MAX_BREAKPOINTS - 1) {
        net_send_int32(fd, CMD_INVALID_INDEX);
        return 1;
    }

    char *bp_entry  = (char *)curdbgctx + bp->index * DBG_BP_SLOT_SIZE;
    char *saved_byte_slot = bp_entry + DBG_BP_SLOT_SIZE;

    if (bp->enabled) {
        *(uint32_t *)(bp_entry + 0x08) = 1;
        *(uint64_t *)(bp_entry + 0x10) = bp->address;

        sys_proc_rw_w0((uint64_t)pid, bp->address, 1, saved_byte_slot, 0);

        unsigned char int3 = 0xCC;
        sys_proc_rw_w1((uint64_t)pid, bp->address, 1, &int3, 0);
    } else {

        uint32_t was_enabled = *(uint32_t *)(bp_entry + 0x08);
        uint64_t bp_addr     = *(uint64_t *)(bp_entry + 0x10);

        if (was_enabled && bp_addr) {
            struct elev_state es;
            int es_active = (elev_save_and_set(&es) == 0);

            int stuck_lw[INT3_REWIND_MAX]; int stuck_n = 0;
            int stuck = int3_scan_stuck(pid, &bp_addr, 1, stuck_lw, INT3_REWIND_MAX, &stuck_n);

            sys_proc_rw_w1((uint64_t)pid, bp_addr, 1, saved_byte_slot, 0);

            if (stuck > 0) {
                int3_resume_lwps(pid, stuck_lw, stuck_n);
            }
            ptrace_raw(PT_CONTINUE, pid, (void *)1, 0);

            if (es_active) elev_restore(&es);

            if (g_recently_disabled_count < INT3_REWIND_MAX) {
                g_recently_disabled_bps[g_recently_disabled_count++] = bp_addr;
            }
        }

        *(uint32_t *)(bp_entry + 0x08) = 0;
        *(uint64_t *)(bp_entry + 0x10) = 0;
    }

    net_send_int32(fd, CMD_SUCCESS);
    return 0;
}

int debug_set_watchpoint_handle(int fd, struct cmd_packet *packet) {
    if (!g_debug_attached) { net_send_int32(fd, CMD_ERROR); return 1; }
    int pid = DBGCTX()->pid;
    if (pid == 0) { net_send_int32(fd, CMD_ERROR); return 1; }

    struct cmd_debug_watchpt_packet *wp =
        (struct cmd_debug_watchpt_packet *)packet->data;
    if (!wp) { net_send_int32(fd, CMD_DATA_NULL); return 1; }
    if (wp->index > 3) { net_send_int32(fd, CMD_INVALID_INDEX); return 1; }

    struct elev_state es;
    if (elev_save_and_set(&es) != 0) {
        net_send_int32(fd, CMD_ERROR);
        return 1;
    }

    int  count;
    int *lwpids = NULL;
    int  we_stopped;
    int  ret_rc;

    int rc = (int)ptrace_raw(PT_GETNUMLWPS, pid, NULL, 0);
    if (rc != -1) {
        count = rc; we_stopped = 0;
        lwpids = (int *)net_alloc_buffer((unsigned long)(count * 4));
        if (!lwpids) goto wp_err_no_resume;
        if (ptrace_raw(PT_GETLWPLIST, pid, lwpids, count) == -1) {
            free(lwpids); goto wp_err_no_resume;
        }
    } else {
        if (errno != 16 ) goto wp_err_no_resume;
        kill(pid, 17); wait4(pid, NULL, 0, NULL);
        count = (int)ptrace_raw(PT_GETNUMLWPS, pid, NULL, 0);
        we_stopped = 1;
        lwpids = (int *)net_alloc_buffer((unsigned long)(count * 4));
        if (!lwpids) goto wp_resume_and_err;
        if (ptrace_raw(PT_GETLWPLIST, pid, lwpids, count) == -1) {
            free(lwpids); goto wp_resume_and_err;
        }
    }

    char     *ctx     = (char *)curdbgctx;
    uint64_t *dr_addr = (uint64_t *)(ctx + 0x2D8);
    uint64_t *dr7     = (uint64_t *)(ctx + 0x310);

    int idx = (int)wp->index;
    int cl1 = idx * 2;
    int cl2 = idx * 4 + 16;

    uint64_t mask = ((uint64_t)0xFu << cl2)
                  | (uint64_t)(int64_t)(int32_t)(3 << cl1);
    *dr7 &= ~mask;

    if (wp->enabled) {
        dr_addr[idx] = wp->address;
        uint64_t low_bits  = (uint64_t)(int64_t)(int32_t)(3 << cl1);
        uint64_t high_bits = (((uint64_t)wp->length << 2) | (uint64_t)wp->breaktype) << cl2;
        *dr7 |= low_bits | high_bits;
    } else {
        dr_addr[idx] = 0;
    }

    void *dr_block = (void *)(ctx + 0x2D8);
    for (int i = 0; i < count; i++) {
        if (ptrace_raw(PT_SETDBREGS, lwpids[i], dr_block, 0) == -1) {
            if (errno != 0) {
                free(lwpids);
                if (we_stopped) goto wp_resume_and_err;
                goto wp_err_no_resume;
            }
        }
    }

    if (we_stopped) {
        resume_app_via_self_id(pid);
        ptrace_raw(PT_CONTINUE, pid, (void *)1, 0);
    }
    net_send_int32(fd, CMD_SUCCESS);
    free(lwpids);
    ret_rc = 0;
    goto wp_done;

wp_resume_and_err:
    resume_app_via_self_id(pid);
    ptrace_raw(PT_CONTINUE, pid, (void *)1, 0);
wp_err_no_resume:
    net_send_int32(fd, CMD_ERROR);
    ret_rc = 1;
wp_done:
    elev_restore(&es);
    return ret_rc;
}

int debug_get_thread_list_handle(int fd, struct cmd_packet *packet) {
    (void)packet;
    if (!g_debug_attached) { net_send_int32(fd, CMD_ERROR); return 1; }
    int pid = DBGCTX()->pid;
    if (pid == 0) { net_send_int32(fd, CMD_ERROR); return 1; }

    int count = (int)ptrace_elev(PT_GETNUMLWPS, pid, NULL, 0);
    int we_stopped = 0;

    if (count == -1) {
        if (errno != 16 ) { net_send_int32(fd, CMD_ERROR); return 1; }
        kill(pid, 17 );
        wait4(pid, NULL, 0, NULL);
        count = (int)ptrace_elev(PT_GETNUMLWPS, pid, NULL, 0);
        we_stopped = 1;
    }

    int   size = count * 4;
    int  *buf  = (int *)net_alloc_buffer((unsigned long)size);
    if (!buf) goto err;
    if (ptrace_elev(PT_GETLWPLIST, pid, buf, count) == -1) goto err;

    if (we_stopped) {
        resume_app_via_self_id(pid);
        ptrace_elev(PT_CONTINUE, pid, (void *)1, 0);
    }

    net_send_int32(fd, CMD_SUCCESS);
    net_send_all(fd, &count, 4);
    net_send_all(fd, buf, size);
    free(buf);
    return 0;

err:
    if (we_stopped) ptrace_elev(PT_CONTINUE, pid, (void *)1, 0);
    if (buf) free(buf);
    net_send_int32(fd, CMD_ERROR);
    return 1;
}

int debug_suspend_thread_handle(int fd, struct cmd_packet *packet) {
    void *data = packet->data;
    if (DBGCTX()->pid == 0) { net_send_int32(fd, CMD_ERROR);     return 1; }
    if (!data)              { net_send_int32(fd, CMD_DATA_NULL); return 1; }

    if (ptrace_elev(PT_SUSPEND, *(int *)data, NULL, 0) == -1) {
        net_send_int32(fd, CMD_ERROR);
        return 0;
    }
    net_send_int32(fd, CMD_SUCCESS);
    return 0;
}

int debug_resume_thread_handle(int fd, struct cmd_packet *packet) {
    void *data = packet->data;
    if (DBGCTX()->pid == 0) { net_send_int32(fd, CMD_ERROR);     return 1; }
    if (!data)              { net_send_int32(fd, CMD_DATA_NULL); return 1; }

    if (ptrace_elev(PT_RESUME, *(int *)data, NULL, 0) == -1) {
        net_send_int32(fd, CMD_ERROR);
        return 0;
    }
    net_send_int32(fd, CMD_SUCCESS);
    return 0;
}

int debug_getregs_handle(int fd, struct cmd_packet *packet) {
    char buf[REG_BLOB_SIZE];
    if (!g_debug_attached) goto err;
    int pid = DBGCTX()->pid;
    if (pid == 0) goto err;

    struct cmd_debug_lwp_packet *gp = (struct cmd_debug_lwp_packet *)packet->data;
    if (!gp) { net_send_int32(fd, CMD_DATA_NULL); return 1; }

    memset(buf, 0, REG_BLOB_SIZE);
    if (ptrace_elev(PT_GETREGS, gp->lwpid, buf, 0) == -1 && errno != 0) goto err;

    net_send_int32(fd, CMD_SUCCESS);
    net_send_all(fd, buf, REG_BLOB_SIZE);
    return 0;
err:
    net_send_int32(fd, CMD_ERROR);
    return 1;
}

int debug_setregs_handle(int fd, struct cmd_packet *packet) {
    char buf[REG_BLOB_SIZE];
    if (!g_debug_attached) goto err;
    int pid = DBGCTX()->pid;
    if (pid == 0) goto err;

    struct cmd_debug_setreg_packet *sp = (struct cmd_debug_setreg_packet *)packet->data;
    if (!sp) goto err;

    net_send_int32(fd, CMD_SUCCESS);
    net_recv_all(fd, buf, sp->length, 1);

    if (ptrace_elev(PT_SETREGS, pid, buf, 0) == -1 && errno != 0) goto err;

    net_send_int32(fd, CMD_SUCCESS);
    return 0;
err:
    net_send_int32(fd, CMD_ERROR);
    return 1;
}

int debug_getfpregs_handle(int fd, struct cmd_packet *packet) {
    char buf[FPREG_BLOB_SIZE] __attribute__((aligned(64)));
    if (!g_debug_attached) goto err;
    int pid = DBGCTX()->pid;
    if (pid == 0) goto err;

    struct cmd_debug_lwp_packet *gp = (struct cmd_debug_lwp_packet *)packet->data;
    if (!gp) { net_send_int32(fd, CMD_DATA_NULL); return 1; }

    memset(buf, 0, FPREG_BLOB_SIZE);
    if (ptrace_elev(PT_GETFPREGS, gp->lwpid, buf, 0) == -1 && errno != 0) goto err;

    net_send_int32(fd, CMD_SUCCESS);
    net_send_all(fd, buf, FPREG_BLOB_SIZE);
    return 0;
err:
    net_send_int32(fd, CMD_ERROR);
    return 1;
}

int debug_setfpregs_handle(int fd, struct cmd_packet *packet) {
    char buf[FPREG_BLOB_SIZE] __attribute__((aligned(64)));
    if (!g_debug_attached) goto err;
    int pid = DBGCTX()->pid;
    if (pid == 0) goto err;

    struct cmd_debug_setreg_packet *sp = (struct cmd_debug_setreg_packet *)packet->data;
    if (!sp) goto err;

    net_send_int32(fd, CMD_SUCCESS);
    net_recv_all(fd, buf, sp->length, 1);

    if (ptrace_elev(PT_SETFPREGS, pid, buf, 0) == -1 && errno != 0) goto err;

    net_send_int32(fd, CMD_SUCCESS);
    return 0;
err:
    net_send_int32(fd, CMD_ERROR);
    return 1;
}

int debug_getdbregs_handle(int fd, struct cmd_packet *packet) {
    char buf[DBREG_BLOB_SIZE];
    if (!g_debug_attached) goto err;
    int pid = DBGCTX()->pid;
    if (pid == 0) goto err;

    struct cmd_debug_lwp_packet *gp = (struct cmd_debug_lwp_packet *)packet->data;
    if (!gp) { net_send_int32(fd, CMD_DATA_NULL); return 1; }

    int we_stopped = 0;
    if (is_process_stopped(pid) == 0) {
        kill(pid, 17 );
        wait4(pid, NULL, 0, NULL);
        we_stopped = 1;
    }

    memset(buf, 0, DBREG_BLOB_SIZE);

    if (fw_uses_kernel_dbreg_path()) {
        if (kern_get_dbregs(pid, gp->lwpid, buf) != 0) goto err;
    } else {
        if (ptrace_elev(PT_GETDBREGS, gp->lwpid, buf, 0) == -1 && errno != 0) goto err;
    }

    if (we_stopped) {
        resume_app_via_self_id(pid);
        ptrace_elev(PT_CONTINUE, pid, (void *)1, 0);
    }

    net_send_int32(fd, CMD_SUCCESS);
    net_send_all(fd, buf, DBREG_BLOB_SIZE);
    return 0;
err:
    net_send_int32(fd, CMD_ERROR);
    return 1;
}

int debug_setdbregs_handle(int fd, struct cmd_packet *packet) {
    char buf[DBREG_BLOB_SIZE];
    if (!g_debug_attached) goto err;
    int pid = DBGCTX()->pid;
    if (pid == 0) goto err;

    struct cmd_debug_setreg_packet *sp = (struct cmd_debug_setreg_packet *)packet->data;
    if (!sp) goto err;

    net_send_int32(fd, CMD_SUCCESS);
    net_recv_all(fd, buf, sp->length, 1);

    ptrace_elev(PT_CONTINUE, pid, (void *)1, 0);
    kill(pid, 17 );
    wait4(pid, NULL, 0, NULL);

    if (ptrace_elev(PT_SETDBREGS, sp->lwpid, buf, 0) == -1 && errno != 0) goto err;

    resume_app_via_self_id(pid);
    ptrace_elev(PT_CONTINUE, pid, (void *)1, 0);

    net_send_int32(fd, CMD_SUCCESS);
    return 0;
err:
    net_send_int32(fd, CMD_ERROR);
    return 1;
}

int debug_continue_handle(int fd, struct cmd_packet *packet) {
    int pid = DBGCTX()->pid;
    if (pid == 0) { net_send_int32(fd, CMD_ERROR); return 1; }

    void *data = packet->data;
    if (!data) { net_send_int32(fd, CMD_DATA_NULL); return 1; }

    debug_stopgo_handle(pid, *(char *)data);
    net_send_int32(fd, CMD_SUCCESS);
    return 0;
}

struct dbg_thrinfo_response {
    uint32_t lwpid;
    uint32_t priority;
    char     tdname[32];
} __attribute__((packed));

static int kern_get_thread_info(int pid, uint32_t *thrinfo_out)
{
    if (pid == 0)        return 1;
    if (!thrinfo_out)    return 1;

    intptr_t kproc = kernel_get_proc_fast(pid);
    if (!kproc)          return 1;

    uint8_t  chain_hdr[0x30];
    if (kernel_copyout_fast(kproc + 0x10, chain_hdr, 0x30) != 0) return 1;

    uint64_t kthread = *(uint64_t *)chain_hdr;
    if (!kthread)        return 1;

    uint8_t tbuf[0x680];
    for (;;) {
        if (kernel_copyout_fast((intptr_t)kthread, tbuf, 0x680) != 0) return 1;
        if (*(uint32_t *)(tbuf + 0x9c) == thrinfo_out[0]) {
            thrinfo_out[1] = *(uint16_t *)(tbuf + 0x3c0);
            memcpy((char *)thrinfo_out + 8, tbuf + 0x294, 0x20);
            return 0;
        }
        kthread = *(uint64_t *)(tbuf + 0x10);
        if (!kthread)    return 1;
    }
}

int debug_thread_info_handle(int fd, struct cmd_packet *packet) {
    if (!g_debug_attached) { net_send_int32(fd, CMD_ERROR); return 1; }
    int pid = DBGCTX()->pid;
    if (pid == 0) { net_send_int32(fd, CMD_ERROR); return 1; }

    struct cmd_debug_lwp_packet *gp = (struct cmd_debug_lwp_packet *)packet->data;
    if (!gp) { net_send_int32(fd, CMD_DATA_NULL); return 1; }

    uint32_t thrinfo[10];
    memset(thrinfo, 0, sizeof(thrinfo));
    thrinfo[0] = gp->lwpid;
    if (kern_get_thread_info(pid, thrinfo) != 0) {
        net_send_int32(fd, CMD_ERROR);
        return 1;
    }

    struct dbg_thrinfo_response resp;
    memset(&resp, 0, sizeof(resp));
    resp.lwpid    = thrinfo[0];
    resp.priority = thrinfo[1];
    memcpy(resp.tdname, (char *)thrinfo + 8, 32);
    resp.tdname[31] = 0;

    net_send_int32(fd, CMD_SUCCESS);
    net_send_all(fd, &resp, sizeof(resp));
    return 0;
}

int debug_step_handle(int fd, struct cmd_packet *packet) {
    (void)packet;
    if (!g_debug_attached) { net_send_int32(fd, CMD_ERROR); return 1; }
    int pid = DBGCTX()->pid;
    if (pid == 0) {
        net_send_int32(fd, CMD_ERROR);
        return 1;
    }
    if (ptrace_elev(PT_STEP, pid, (void *)1, 0) != 0) {
        net_send_int32(fd, CMD_ERROR);
        return 1;
    }
    net_send_int32(fd, CMD_SUCCESS);
    return 0;
}

int debug_step_thread_handle(int fd, struct cmd_packet *packet) {
    if (!g_debug_attached) { net_send_int32(fd, CMD_ERROR); return 1; }
    int pid = DBGCTX()->pid;
    if (pid == 0) { net_send_int32(fd, CMD_ERROR); return 1; }

    struct cmd_debug_lwp_packet *gp = (struct cmd_debug_lwp_packet *)packet->data;

    long rc;
    if (gp) {
        scePthreadMutexLock(&g_server_mutex);
        g_stepping_lwpid = gp->lwpid;
        rc = ptrace_elev(PT_STEP, (int)gp->lwpid, (void *)1, 0);
        scePthreadMutexUnlock(&g_server_mutex);
    } else {
        rc = ptrace_elev(PT_STEP, pid, (void *)1, 0);
    }

    if (rc != 0) { net_send_int32(fd, CMD_ERROR); return 1; }
    net_send_int32(fd, CMD_SUCCESS);
    return 0;
}

static void debug_handle_breakpoint_resume(void) {
    if (!g_debug_attached) return;
    int sig = (int)g_stopgo_resume_signal;
    if (sig == -1) return;

    int pid = (int)g_stopgo_resume_pid;

    if (sig == 0 && g_recently_disabled_count > 0) {
        int3_iterative_sweep(pid, g_recently_disabled_bps, g_recently_disabled_count);
        g_recently_disabled_count = 0;
    }

    if (sig == 0) {
        resume_app_via_self_id(pid);
        sig = (int)g_stopgo_resume_signal;
    }

    ptrace_raw(PT_CONTINUE, pid, (void *)1, sig);

    if ((int)g_stopgo_resume_signal == 17) {
        wait4(pid, NULL, 0, NULL);
    }

    g_stopgo_resume_signal = 0xFFFFFFFFu;
    g_stopgo_resume_pid  = 0;
}

static uint32_t g_last_alive_check = 0;

int dispatch_debug_events(void) {

    struct elev_state es;
    if (elev_save_and_set(&es) != 0) return 1;
    int rc = 0;
    #define DDE_RETURN(v) do { rc = (v); goto dde_done; } while (0)

    debug_handle_breakpoint_resume();

    struct { uint64_t tv_sec; uint64_t tv_usec; } now = {0,0};

    gettimeofday((struct timeval *)&now, NULL);
    if (((uint32_t)now.tv_sec - g_last_alive_check) > 4) {
        if (!kern_thread_step_walker((int)DBGCTX()->pid)) {
            DDE_RETURN(1);
        }
        g_last_alive_check = (uint32_t)now.tv_sec;
    }

    int status = 0;
    int wait_rc = wait4((int)DBGCTX()->pid, &status, 1 , NULL);
    if (wait_rc == 0) DDE_RETURN(0);

    uint8_t sig = (uint8_t)((status >> 8) & 0xFF);
    if (sig == 17) DDE_RETURN(0);

    if (sig == 9) {
        debug_full_teardown(curdbgctx);
        ptrace_raw(PT_CONTINUE, (int)DBGCTX()->pid, (void *)1, 9);
        DDE_RETURN(0);
    }

    uint8_t lwpi[160];
    if (ptrace_raw(PT_LWPINFO, (int)DBGCTX()->pid, lwpi, 160) != 0) {
        resume_app_via_self_id((int)DBGCTX()->pid);
        DDE_RETURN(1);
    }

    char pkt[1184];
    memset(pkt, 0, 1184);
    *(uint32_t *)(pkt + 0x000) = *(uint32_t *)lwpi;
    *(uint32_t *)(pkt + 0x004) = (uint32_t)status;
    memcpy(pkt + 0x008, lwpi + 0x80, 24);

    uint8_t regs_buf[176];
    uint8_t fpu_buf[832];
    uint8_t dbreg_buf[128];
    int  pid   = (int)DBGCTX()->pid;
    int  lwpid = (int)*(uint32_t *)lwpi;
    bool got_dbreg = false;

    memset(regs_buf,  0, 176);
    memset(dbreg_buf, 0, 128);

    bool use_kernel = fw_uses_kernel_dbreg_path();
    if (use_kernel) {
        if (kern_get_lwp_full_state(pid, lwpid, regs_buf, dbreg_buf, fpu_buf) == 0) {
            got_dbreg = true;
        } else {
            if (kern_get_proc_info_by_pid(pid, lwpid, regs_buf) != 0
                && ptrace_raw(PT_GETREGS, lwpid, regs_buf, 0) != 0) {
                resume_app_via_self_id(pid); DDE_RETURN(1);
            }
            if (kern_proc_install_dbregs(pid, lwpid, fpu_buf) != 0
                && ptrace_raw(PT_GETFPREGS, lwpid, fpu_buf, 0) != 0) {
                resume_app_via_self_id(pid); DDE_RETURN(1);
            }
            if (kern_get_dbregs(pid, lwpid, dbreg_buf) == 0) {
                got_dbreg = true;
            }
        }
    } else {
        if (ptrace_raw(PT_GETREGS, lwpid, regs_buf, 0) != 0) {
            resume_app_via_self_id(pid); DDE_RETURN(1);
        }
        memset(fpu_buf, 0, 832);
        if (ptrace_raw(PT_GETFPREGS, lwpid, fpu_buf, 0) != 0) {
            resume_app_via_self_id(pid); DDE_RETURN(1);
        }
    }

    if (!got_dbreg) {
        if (ptrace_raw(PT_GETDBREGS, lwpid, dbreg_buf, 0) != 0) {
            resume_app_via_self_id(pid); DDE_RETURN(1);
        }
    }

    memcpy(pkt + 0x030, regs_buf,  176);
    memcpy(pkt + 0x0E0, fpu_buf,   832);
    memcpy(pkt + 0x420, dbreg_buf, 128);

    uint64_t pkt_rip = *(uint64_t *)(pkt + 0x030 + 0x88);
    uint64_t rip_minus_1 = pkt_rip - 1;
    char *bp_addrs_base = (char *)curdbgctx + 0x10;
    char *matched_bp_v34 = NULL;
    for (int i = 0; i < 30; i++) {
        uint64_t addr = *(uint64_t *)(bp_addrs_base + i * 0x18);
        if (addr != 0 && addr == rip_minus_1) {
            matched_bp_v34 = (char *)curdbgctx + i * 0x18 + 8;
            break;
        }
    }

    int matched_wp = -1;
    uint64_t *pkt_dr = (uint64_t *)(pkt + 0x420);
    for (int i = 0; i < 4; i++) {
        if (pkt_dr[i] != 0 && pkt_dr[i] == pkt_rip) { matched_wp = i; break; }
    }

    uint8_t length_buf[176];

    if (matched_wp >= 0) {
        int      cl1 = matched_wp * 2;
        int      cl2 = matched_wp * 4 + 16;
        uint64_t saved_dr7 = pkt_dr[7];
        uint64_t rw_bits   = (saved_dr7 >> cl2) & 3;
        uint64_t clear_mask = ~(((uint64_t)0xF << cl2) | ((uint64_t)3 << cl1));

        int numlwps = (int)ptrace_raw(PT_GETNUMLWPS, pid, NULL, 0);
        if (numlwps > 0) {
            uint32_t *lwpids = (uint32_t *)net_alloc_buffer((uint64_t)numlwps * 4);
            if (lwpids) {
                if (ptrace_raw(PT_GETLWPLIST, pid, lwpids, numlwps) > 0) {
                    for (int i = 0; i < numlwps; i++) {
                        int lwp = (int)lwpids[i];
                        if (lwp == lwpid) {
                            if (rw_bits == 0) {
                                uint64_t saved_pkt_dr7 = pkt_dr[7];
                                pkt_dr[7] &= clear_mask;
                                if (use_kernel) {
                                    if (kern_set_dbregs(pid, lwpid, pkt + 0x420) != 0) {
                                        ptrace_raw(PT_SETDBREGS, lwpid, pkt + 0x420, 0);
                                    }
                                } else {
                                    ptrace_raw(PT_SETDBREGS, lwpid, pkt + 0x420, 0);
                                }
                                ptrace_raw(PT_STEP, lwpid, (void *)1, 0);
                                int s2 = 0;
                                while (wait4(pid, &s2, 1, NULL) == 0) {
                                    sceKernelUsleep(100);
                                }
                                pkt_dr[7] = saved_pkt_dr7;
                                if (use_kernel) {
                                    kern_set_dbregs(pid, lwpid, pkt + 0x420);
                                } else {
                                    ptrace_raw(PT_SETDBREGS, lwpid, pkt + 0x420, 0);
                                }
                            }
                            continue;
                        }
                        if (use_kernel) {
                            if (kern_get_proc_info_by_pid(pid, lwp, length_buf) != 0) {
                                ptrace_raw(PT_GETREGS, lwp, length_buf, 0);
                            }
                        } else {
                            ptrace_raw(PT_GETREGS, lwp, length_buf, 0);
                        }
                        uint64_t thr_rip = *(uint64_t *)(length_buf + 0x88);
                        if (thr_rip != pkt_rip) continue;
                        uint8_t small_dbreg[128];
                        ptrace_raw(PT_GETDBREGS, lwp, small_dbreg, 0);
                        uint64_t thr_dr7 = *(uint64_t *)(small_dbreg + 0x38);
                        if (((thr_dr7 >> cl2) & 3) != 0) continue;
                        uint64_t saved_thr_dr7 = thr_dr7;
                        *(uint64_t *)(small_dbreg + 0x38) = thr_dr7 & clear_mask;
                        ptrace_raw(PT_SETDBREGS, lwp, small_dbreg, 0);
                        ptrace_raw(PT_STEP, lwp, (void *)1, 0);
                        int s3 = 0;
                        while (wait4(pid, &s3, 1, NULL) == 0) {
                            sceKernelUsleep(100);
                        }
                        *(uint64_t *)(small_dbreg + 0x38) = saved_thr_dr7;
                        if (use_kernel) {
                            kern_set_dbregs(pid, lwp, small_dbreg);
                        } else {
                            ptrace_raw(PT_SETDBREGS, lwp, small_dbreg, 0);
                        }
                    }
                }
                free(lwpids);
            }
        } else if (rw_bits == 0) {
            uint64_t saved_pkt_dr7 = pkt_dr[7];
            pkt_dr[7] = saved_pkt_dr7 & clear_mask;
            if (use_kernel) {
                if (kern_set_dbregs(pid, lwpid, pkt + 0x420) != 0) {
                    ptrace_raw(PT_SETDBREGS, lwpid, pkt + 0x420, 0);
                }
            } else {
                ptrace_raw(PT_SETDBREGS, lwpid, pkt + 0x420, 0);
            }
            ptrace_raw(PT_STEP, lwpid, (void *)1, 0);
            int s4 = 0;
            while (wait4(pid, &s4, 1, NULL) == 0) sceKernelUsleep(100);
            pkt_dr[7] = saved_pkt_dr7;
            if (use_kernel) {
                kern_set_dbregs(pid, lwpid, pkt + 0x420);
            } else {
                ptrace_raw(PT_SETDBREGS, lwpid, pkt + 0x420, 0);
            }
        }
    }

    if (matched_bp_v34 != NULL) {
        uint64_t bp_addr        = *(uint64_t *)(matched_bp_v34 + 8);
        char    *saved_byte_ptr =  matched_bp_v34 + 16;
        uint8_t  int3           = 0xCC;
        sys_proc_rw_w1((uint64_t)pid, bp_addr, 1, saved_byte_ptr, 0);
        *(uint64_t *)(pkt + 0x030 + 0x88) -= 1;
        if (use_kernel) {
            if (kern_apply_thread_dbgctx(pid, lwpid, pkt + 0x030) != 0) {
                ptrace_raw(PT_SETREGS, lwpid, pkt + 0x030, 0);
            }
        } else {
            ptrace_raw(PT_SETREGS, lwpid, pkt + 0x030, 0);
        }
        ptrace_raw(PT_STEP, lwpid, (void *)1, 0);
        int s_step = 0;
        while (wait4(pid, &s_step, 1, NULL) == 0) sceKernelUsleep(100);
        {
            uint8_t li2[160];
            errno = 0;
            if (ptrace_raw(PT_LWPINFO, pid, li2, 160) == 0) {
                int who_lwp = (int)*(uint32_t *)li2;
                if (who_lwp != lwpid && who_lwp != 0) {
                    uint8_t srb[176];
                    if (ptrace_raw(PT_GETREGS, who_lwp, srb, 0) == 0 &&
                        *(uint64_t *)(srb + 0x88) == bp_addr + 1) {
                        *(uint64_t *)(srb + 0x88) = bp_addr;
                        ptrace_raw(PT_SETREGS, who_lwp, srb, 0);
                    }
                }
            }
        }
        sys_proc_rw_w1((uint64_t)pid, bp_addr, 1, &int3, 0);
    }

    uint32_t pkt_lwpid = *(uint32_t *)pkt;
    int stepping = (int)g_stepping_lwpid;
    if (stepping > 0) {
        if ((int)pkt_lwpid != stepping) {
            ptrace_raw(PT_CONTINUE, (int)pkt_lwpid, (void *)1, 0);
            DDE_RETURN(0);
        }
        *(uint64_t *)(pkt + 0x420 + 0x30) = 0;
    }
    g_stepping_lwpid = 0;

    net_send_all(DBGCTX()->dbgfd, pkt, 1184);
    resume_app_via_self_id((int)DBGCTX()->pid);
    DDE_RETURN(0);

dde_done:
    elev_restore(&es);
    #undef DDE_RETURN
    return rc;
}

int debug_handle(int fd, struct cmd_packet *packet) {
    switch (packet->cmd) {
    case 0xBDBB0500u: return debug_process_stop_handle(fd, packet);
    case 0xBDBB0001u: return debug_attach_handle(fd, packet);
    case 0xBDBB0002u: return debug_detach_handle(fd, packet);
    case 0xBDBB0003u: return debug_set_breakpoint_handle(fd, packet);
    case 0xBDBB0004u: return debug_set_watchpoint_handle(fd, packet);
    case 0xBDBB0005u: return debug_get_thread_list_handle(fd, packet);
    case 0xBDBB0006u: return debug_suspend_thread_handle(fd, packet);
    case 0xBDBB0007u: return debug_resume_thread_handle(fd, packet);
    case 0xBDBB0008u: return debug_getregs_handle(fd, packet);
    case 0xBDBB0009u: return debug_setregs_handle(fd, packet);
    case 0xBDBB000Au: return debug_getfpregs_handle(fd, packet);
    case 0xBDBB000Bu: return debug_setfpregs_handle(fd, packet);
    case 0xBDBB000Cu: return debug_getdbregs_handle(fd, packet);
    case 0xBDBB000Du: return debug_setdbregs_handle(fd, packet);
    case 0xBDBB0010u: return debug_continue_handle(fd, packet);
    case 0xBDBB0011u: return debug_thread_info_handle(fd, packet);
    case 0xBDBB0012u: return debug_step_handle(fd, packet);
    case 0xBDBB0013u: return debug_step_thread_handle(fd, packet);
    }
    net_send_int32(fd, CMD_ERROR);
    return 0;
}