#include <windows.h>
#include "debug.h"
#include "stringutils.h"
#include "args.h"
#include  "kernelmisc.h"

extern "C" BOOL KernelIoControl(DWORD dwCode, void *in, DWORD insize, void *out, DWORD outsize, DWORD *returned);

#define DPLL_CTRL_ADDR 0xb80ecf00
#define DPLL_CTRL (*(volatile DWORD*)DPLL_CTRL_ADDR)
#define setpll(div,mult) DPLL_CTRL = (DPLL_CTRL&~0xfe0)|(div<<5)|(mult<<7)
bool set_cpu_speed(int div, int mult)
{
    setpll(div,mult);
    return true;
}

DwordVector gencode(int n)
{
    DwordVector v;
    v.push_back(0xe2500001); // subs r0,r0,1
    for (int i=0 ; i<n ; i++)
        v.push_back(0xe1a01001+0x1001*(i&7)); // mov ri,ri
    v.push_back(0x1afffffd-n);  // bne loop
    v.push_back(0xe1a0f00e); // mov pc,lr

    return v;
}
typedef std::vector<DWORD> DwordVector;
typedef std::vector<DwordVector> CodeList;
typedef void(*pfn_test)(unsigned int n);

int exceptfilter(EXCEPTION_POINTERS *e)
{
        debug("exception: code=%08lx addr=%08lx: ", e->ExceptionRecord->ExceptionCode, e->ExceptionRecord->ExceptionAddress);
        for (int i=0 ; i<e->ExceptionRecord->NumberParameters ; i++)
            debug(" %08lx", e->ExceptionRecord->ExceptionInformation[i]);
        debug("\n");
        return EXCEPTION_EXECUTE_HANDLER;
}
inline void callfn(pfn_test fn, unsigned int count)
{

    //debug("calling %08lx, %d\n", fn, count);
    __try {
        fn(count);
    }
    __except(exceptfilter(GetExceptionInformation()))
    {

        DWORD *pdw= reinterpret_cast<DWORD*>(fn);
        debug("fn %08lx:", fn);
        if (!IsBadReadPtr(pdw, 7*4))
            debug(" %08lx %08lx %08lx %08lx %08lx %08lx %08lx\n", pdw[0], pdw[1], pdw[2], pdw[3], pdw[4], pdw[5], pdw[6]);
        else
            debug(" ?\n");
    }
}
double calc_mhz(DWORD t, unsigned int count)
{
    return (double)count/t/1000;
}
void run_cpu_test(int n, unsigned int count)
{
    debugt("running test, n=%d, count=0x%x\n", n, count);
    CodeList tst;
    for (int i=0 ; i<n ; i++)
    {
        tst.push_back(gencode(i));
    }
//    f000ff78    0   34 CacheRangeFlush
    CacheRangeFlush(0, 0, CACHE_SYNC_ALL);
    DwordVector t(n+1);

    t[0]= GetTickCount();
    for (int i=0 ; i<n ; i++)
    {
        callfn(reinterpret_cast<pfn_test>(&tst[i][0]), count);
        //reinterpret_cast<pfn_test>(&tst[i][0])(count);
        t[i+1]= GetTickCount();
    }

    DwordVector tdiff(n);
    for (int i=0 ; i<n ; i++)
        tdiff[i]= t[i+1]-t[i];

    DwordVector tdiff2(n-1);
    for (int i=0 ; i<n-1 ; i++)
        tdiff2[i]= tdiff[i+1]-tdiff[i];

    std::string difflist;
    for (int i=0 ; i<n ; i++)
    {
        if (!difflist.empty())
            difflist+='+';
        difflist+=stringformat("%d", tdiff[i]);
    }

    std::string diff2list;
    for (int i=0 ; i<n-1 ; i++) {
        if (!diff2list.empty())
            diff2list+=' ';
        diff2list+=stringformat("%+d(%5.2f)", tdiff2[i], calc_mhz(tdiff2[i], count));
    }

    debug("took %s : %d msecs\n", difflist.c_str(), t.back()-t.front());
    debug("%s\n", diff2list.c_str());
    debug("avg: %5.2f Mhz\n", calc_mhz(tdiff.back()-tdiff.front(), count)*(n-1));
}
int WINAPI WinMain( HINSTANCE hInstance,
                   HINSTANCE hPrevInstance,
                   LPTSTR    lpCmdLine,
                   int       nCmdShow)
{
    DebugSetLogfile("tstcpu.log");
    StringList args;
    if (!SplitString(ToString(lpCmdLine), args, false))
    {
        error("Error in commandline");
        return false;
    }
    int depth=4;
    DWORD rounds= 0x04000000;
    for (StringList::iterator i= args.begin() ; i!=args.end() ; ++i)
    {
        std::string& arg= *i;
        if (arg[0]=='-') switch(arg[1])
        {
            case 'n': HANDLESTLULOPTION(depth, int); break;
            case 'r': HANDLESTLULOPTION(rounds, int); break;
        }
    }

    KernelMode _km;
    debug("initial\n");
    run_cpu_test(depth, rounds);

#if 0
    for (int mult=14 ; mult<32 ; mult++) {
        set_cpu_speed(0,mult);
        debug("set_cpu_speed(0,%d)\n", mult);
        run_cpu_test(depth, rounds);
        set_cpu_speed(0,15);
        debug("set_cpu_speed(0,15)\n");
        run_cpu_test(depth, rounds);
    }
#endif
    debug("done\n");

    return 0;
}