PS2 hook pad working

#include <thbase.h>
#include <thsemap.h>
#include <sifcmd.h>
#include <sifman.h>
#include <loadcore.h>
#include <string.h>
#include <intrman.h>
#include <ioman.h>
#include <iop2eedebug.h>

#define debug(...) \
    do { \
        char buf[IOP2EEDEBUG_CMD_STR_MAX_SIZE]; \
        sprintf(buf, __VA_ARGS__); \
        iop2eedebug_print(buf); \
    } while (0)

typedef struct
{
    u32 padEnd;
    s32 eventflag;
    u32 init;
    u32 stopTransfer;
    s32 tid_1;
    s32 tid_2;
} vblankData_t;

struct padButtonStatus
{
    unsigned char ok;
    unsigned char mode;
    unsigned short btns;
    // joysticks
    unsigned char rjoy_h;
    unsigned char rjoy_v;
    unsigned char ljoy_h;
    unsigned char ljoy_v;
    // pressure mode
    unsigned char right_p;
    unsigned char left_p;
    unsigned char up_p;
    unsigned char down_p;
    unsigned char triangle_p;
    unsigned char circle_p;
    unsigned char cross_p;
    unsigned char square_p;
    unsigned char l1_p;
    unsigned char r1_p;
    unsigned char l2_p;
    unsigned char r2_p;
    unsigned char unkn16[12];
} __attribute__((packed));


typedef struct
{
    u8 data[32];
    u32 actDirData[2];
    u32 actAlignData[2];
    u32 actData[8];
    u32 modeTable[2];
    u32 frame;
    u32 findPadRetries;
    u32 length;
    u8 modeConfig;
    u8 modeCurId;
    u8 model;
    u8 buttonDataReady;
    u8 nrOfModes;
    u8 modeCurOffs;
    u8 nrOfActuators;
    u8 numActComb;
    u8 val_c6;
    u8 mode;
    u8 lock;
    u8 actDirSize;
    u8 state;
    u8 reqState;
    u8 currentTask;
    u8 runTask;
    u8 stat70bit;
    u8 padding[11];
} ee_paddata_t;


typedef struct
{
    ee_paddata_t ee_pdata __attribute__((aligned(4)));
    u8 modeConfig;
    u8 modeCurId;
    u8 model;
    u8 numModes;
    u8 modeCurOffs;
    u8 numActuators;
    u8 numActComb;
    u8 disconnected;
    u32 actData[4];
    u32 combData[4];
    u32 modeTable[2];
    u8 buttonInfo[4];
    u8 buttonMask[4];
    u8 vrefParam[12];
    u16 val_c6; // unused
    u8 inbuffer[32];
    u8 outbuffer[32];
    u8 buttonStatus[32];
    u32 buttonDataReady;
    u8 mode;
    u8 lock;
    u8 ee_actDirectData[8] __attribute__((aligned(4)));
    s16 ee_actDirectSize;
    u8 ee_actAlignData[8] __attribute__((aligned(4)));
    u16 state;
    u16 reqState;
    u32 frame;
    u32 padarea_ee_addr;
    u32 findPadRetries;
    s32 updatepadTid;
    s32 querypadTid;
    s32 unused158; // probably unused thread id
    s32 setmainmodeTid;
    s32 setactalignTid;
    s32 setbuttoninfoTid;
    s32 setvrefparamTid;
    u32 eventflag;
    u16 port;
    u16 slot;
    u32 currentTask;
    u32 runTask;
    u32 taskTid;
    u32 stat70bit;
    u32 val_184; // unused
} padState_t;


//Grabbed from the PSPSDK
#define MIPS_JAL(f)        (0x0C000000 | (((u32)(f) >> 2) & 0x03ffffff))
#define MIPS_J(f)          (0x08000000 | (((u32)(f) & 0x0ffffffc) >> 2))
#define MAKE_CALL(a, f) _sw(0x0C000000 | (((u32)(f) >> 2) & 0x03ffffff), (u32)a)
#define MAKE_JUMP(a, f) _sw(0x08000000 | (((u32)(f) & 0x0ffffffc) >> 2), (u32)a)

void *GetExportTable(char *libname, int version);
void *GetExportEntry(void *table, u32 entry);
void *HookExportEntry(void *table, u32 entry, void *func);
u32 GetExportTableSize(void *table);

#define padPortOpen_import_num  6
#define padInit_import_num      4
#define sceSifSetDma_import_num 7

#define bkpt() asm volatile("break\n\tnop\n\t")

volatile s32 (*padPortOpen_orig)(s32 port, s32 slot, s32 pad_area_ee_addr, u32 *buf) = NULL;
volatile s32 (*padInit_orig)(void * ee_addr) = NULL;

volatile int (*sceSifSetDma_orig)(SifDmaTransfer_t *dmat, int count) = NULL;

volatile s32 _pad_area_ee_addr = 0;
volatile u32 *_buf = NULL;

s32 padInit_hook(void *ee_addr)
{
    return padInit_orig(ee_addr);
}

s32 padPortOpen_hook(s32 port, s32 slot, s32 pad_area_ee_addr, u32 *buf)
{
    _pad_area_ee_addr = pad_area_ee_addr;
    _buf = buf;
    return padPortOpen_orig(port, slot, pad_area_ee_addr, buf);
}

int i = 0;

int sceSifSetDma_hook(SifDmaTransfer_t *dmat, int count)
{
    ee_paddata_t *e = (ee_paddata_t *)dmat->src;
    struct padButtonStatus *but = (struct padButtonStatus *)&e->data[2];
    if (dmat->size == 128) {
        debug("0x%04X\n",((u16*)e->data)[0x12/2]);
        //but->btns |= 0x0010;
    }
        //e->data[0] = 0xEA;
    //padState_t *p = dmat->src;
    //p->buttonStatus[0] = 0xA0A0;
    //i++;
    return sceSifSetDma_orig(dmat, count);
}

static void thread_func(void *arg)
{
    //Hook padman's sceSifSetDma import
    while (1) {
        register void *exp = GetExportTable("sifman", 0x101);
        if (exp) {
            int ctx;
            CpuSuspendIntr(&ctx);

            debug("Found sifman export table at: 0x%08X\n", exp);
            iop_library_t *lib = (iop_library_t *)((u32)exp - 20);

            debug("\tname: %s\n", lib->name);
            debug("\tversion: 0x%04X\n", lib->version);
            debug("\tflags: 0x%04X\n", lib->flags);

            struct irx_import_table *table = lib->caller;

            while (table) {
                debug("\t\ttable name: %s\n", table->name);
                struct irx_import_stub *stub = (struct irx_import_stub *)table->stubs;
                while (stub->jump) {
                    if (stub->fno == sceSifSetDma_import_num) {
                        debug("\t\t\tpatching: %i\n", stub->fno);
                        sceSifSetDma_orig = ((void**)exp)[stub->fno];
                        //((void**)exp)[stub->fno] = sceSifSetDma_hook;
                        MAKE_JUMP(&stub->jump, sceSifSetDma_hook);
                    }
                    stub++;
                }
                table = table->next;
            }
            FlushDcache();
            FlushIcache();
            CpuResumeIntr(ctx);
            while (1) {
                //debug("%i\n", i);
                DelayThread(1000);
            }

            /*struct irx_import_stub *stub;
            int j = 0;
            for (table = lib->caller; table != NULL; table = table->next) {
                j++;
                int i;
                //debug("%i : %s", j, table->name);
                for (i = 0; table->name[i] != '\0'; i++) {
                    if (table->name[i] > 'd') {
                        //bkpt();
                        stub = (struct irx_import_stub *)table->stubs;
                        sceSifSetDma_orig = (void*)stub;
                        //MAKE_JUMP(&(stub[sceSifSetDma_import_num].jump), sceSifSetDma_hook);
                    }
                }
            }*/
        }
        DelayThread(1000);
    }

    //Hook padPortOpen
    /*while (1) {
        register void *exp = GetExportTable("padman", 0x102);
        if (exp) {
            int ctx;
            CpuSuspendIntr(&ctx);

            u32 *exptable = exp;
            padPortOpen_orig = (void *)exptable[padPortOpen_import_num];
            u32 jal_padPortOpen = MIPS_JAL(padPortOpen_orig);
            u32 *addr = exptable - 0x10000;
            for (; addr < exptable + 0x10000; addr++) {
                if (*addr == jal_padPortOpen) {
                    MAKE_CALL(addr, padPortOpen_hook);
                    FlushDcache();
                    FlushIcache();
                    CpuResumeIntr(ctx);
                    break;
                }
            }
            CpuResumeIntr(ctx);
        }
        DelayThread(1000);
    }*/

    while (1) {
        if (_pad_area_ee_addr) {
            //memset((void*)_pad_area_ee_addr, 0xAE, 256);
        }
        DelayThread(1000);
    }
}

int _start()
{
    iop_thread_t th = {
        .attr      = TH_C,
        .thread    = thread_func,
        .priority  = 40,
        .stacksize = 0x400,
        .option    = 0
    };

    int thid = CreateThread(&th);
    if (thid > 0) {
        StartThread(thid, NULL);
        return 0;
    }

    return 1;
}

u32 GetExportTableSize(void *table)
{
    register void **exp;
    register u32 size;
    exp = (void**)table;
    size = 0;

    if (exp != NULL)
        while (*exp ++ != NULL)
            size ++;

    return size;
}
void *GetExportTable(char *libname, int version)
{
    if (libname != NULL) {
        iop_library_t lib;
        register int i;
        register char *psrc;

        memset(&lib, 0, sizeof(iop_library_t));
        lib.version = version;

        for (i = 0, psrc = libname; (i < 8) && (*psrc); i ++, psrc ++) lib.name[i] = *psrc;

        return QueryLibraryEntryTable(&lib);
    }
    return NULL;
}
void *GetExportEntry(void *table, u32 entry)
{
    if (entry < GetExportTableSize(table)) {
        register void **exp;
        exp = (void**)table;
        return exp[entry];
    }
    return NULL;
}
void *HookExportEntry(void *table, u32 entry, void *func)
{
    if (entry < GetExportTableSize(table)) {
        int oldstate;
        register void **exp, *temp;

        exp = (void**)table;
        exp = &exp[entry];

        CpuSuspendIntr(&oldstate);
        temp = *exp;
        *exp = func;
        func = temp;
        CpuResumeIntr(oldstate);

        return func;
    }
    return NULL;
}

void fix_imports(iop_library_t *lib)
{
    struct irx_import_table *table;
    struct irx_import_stub *stub;

    FlushDcache();

    // go through each table that imports the library
    for(table = lib->caller; table != NULL; table = table->next)
    {
        // go through each import in the table
        for(stub = (struct irx_import_stub *) table->stubs; stub->jump != 0; stub++)
        {
            // patch the stub to jump to the address specified in the library export table for "fno"
            stub->jump = 0x08000000 | (((u32) lib->exports[stub->fno] << 4) >> 6);
        }
    }

    FlushIcache();
}