#include "ht2crack2utils.h"

// writes a value into a buffer as a series of bytes
void writebuf(unsigned char *buf, uint64_t val, unsigned int len)
{
    int i;
    char c;

    for (i=len-1; i>=0; i--)
    {
        c = val & 0xff;
        buf[i] = c;
        val = val >> 8;
    }

}


/* simple hexdump for testing purposes */
void shexdump(unsigned char *data, int data_len)
{
    int i;

    if (!data || (data_len <= 0)) {
        printf("shexdump: invalid parameters\n");
        return;
    }
    
    printf("Hexdump from %p:\n", data);

    for (i=0; i<data_len; i++) {
        if ((i % HEX_PER_ROW) == 0) {
            printf("\n0x%04x: ", i);
        }
        printf("%02x ", data[i]);
    }
    printf("\n\n");
}



void printbin(unsigned char *c)
{
    int i, j;
    unsigned char x;

    if (!c) {
        printf("printbin: invalid params\n");
        return;
    }

    for (i=0; i<6; i++) {
        x = c[i];
        for (j=0; j<8; j++) {
            printf("%d", (x & 0x80) >> 7);
            x = x << 1;
        }
    }
    printf("\n");
}


void printbin2(uint64_t val, unsigned int size)
{
    int i;
    uint64_t mask = 1;

    mask = mask << (size - 1);

    for (i=0; i<size; i++) {
        if (val & mask) {
            printf("1");
        } else {
            printf("0");
        }
        val = val << 1;
    }
}


void printstate(Hitag_State *hstate)
{
    printf("shiftreg =\t");
    printbin2(hstate->shiftreg, 48);
    printf("\n");
}




// convert hex char to binary
unsigned char hex2bin(unsigned char c)
{
    if ((c >= '0') && (c <= '9')) {
        return (c - '0');
    } else if ((c >= 'a') && (c <= 'f')) {
        return (c - 'a' + 10);
    } else if ((c >= 'A') && (c <= 'F')) {
        return (c - 'A' + 10);
    } else {
        return 0;
    }
}

// return a single bit from a value
int bitn(uint64_t x, int bit)
{
    uint64_t bitmask = 1;

    bitmask = bitmask << bit;

    if (x & bitmask) {
        return 1;
    } else {
        return 0;
    }
}


// the sub-function R that rollback depends upon
int fnR(uint64_t x)
{
    // renumbered bits because my state is 0-47, not 1-48
    return (bitn(x, 1) ^ bitn(x, 2) ^ bitn(x, 5) ^ bitn(x, 6) ^ bitn(x, 7) ^
        bitn(x, 15) ^ bitn(x, 21) ^ bitn(x, 22) ^ bitn(x, 25) ^ bitn(x, 29) ^ bitn(x, 40) ^
        bitn(x, 41) ^ bitn(x, 42) ^ bitn(x, 45) ^ bitn(x, 46) ^ bitn(x, 47));
}

// the rollback function that lets us go backwards in time
void rollback(Hitag_State *hstate, unsigned int steps)
{
    int i;

    for (i=0; i<steps; i++) {
        hstate->shiftreg = ((hstate->shiftreg << 1) & 0xffffffffffff) | fnR(hstate->shiftreg);
    }

}


// the three filter sub-functions that feed fnf
int fa(unsigned int i)
{
    return bitn(0x2C79, i);
}

int fb(unsigned int i)
{
    return bitn(0x6671, i);
}

int fc(unsigned int i)
{
    return bitn(0x7907287B, i);
}

// the filter function that generates a bit of output from the prng state
int fnf(uint64_t s)
{
    unsigned int x1, x2, x3, x4, x5, x6;

    x1 = (bitn(s,  2) << 0) | (bitn(s,  3) << 1) | (bitn(s,  5) << 2) | (bitn(s,  6) << 3);
    x2 = (bitn(s,  8) << 0) | (bitn(s, 12) << 1) | (bitn(s, 14) << 2) | (bitn(s, 15) << 3);
    x3 = (bitn(s, 17) << 0) | (bitn(s, 21) << 1) | (bitn(s, 23) << 2) | (bitn(s, 26) << 3);
    x4 = (bitn(s, 28) << 0) | (bitn(s, 29) << 1) | (bitn(s, 31) << 2) | (bitn(s, 33) << 3);
    x5 = (bitn(s, 34) << 0) | (bitn(s, 43) << 1) | (bitn(s, 44) << 2) | (bitn(s, 46) << 3);

    x6 = (fa(x1) << 0) | (fb(x2) << 1) | (fb(x3) << 2) | (fb(x4) << 3) | (fa(x5) << 4);

    return fc(x6);
}

// builds the lfsr for the prng (quick calcs for hitag2_nstep())
void buildlfsr(Hitag_State *hstate)
{
    uint64_t state = hstate->shiftreg;
    uint64_t temp;

    temp = state ^ (state >> 1);
    hstate->lfsr = state ^ (state >>  6) ^ (state >> 16)
              ^ (state >> 26) ^ (state >> 30) ^ (state >> 41)
              ^ (temp >>  2) ^ (temp >>  7) ^ (temp >> 22)
              ^ (temp >> 42) ^ (temp >> 46);
}



