QES Key Recovery

#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>

typedef u_int8_t uchar;

// inp could point the same memory as outp
template <typename T>
void qes_encrypt_block(T* inp, T *outp, T *matrix)
{
    T buf[3];

    buf[0] = matrix[0] * inp[0] + matrix[1] * inp[1] + matrix[2] * inp[2];
    buf[1] = matrix[3] * inp[0] + matrix[4] * inp[1] + matrix[5] * inp[2];
    buf[2] = matrix[6] * inp[0] + matrix[7] * inp[1] + matrix[8] * inp[2];

    outp[0] = buf[0];
    outp[1] = buf[1];
    outp[2] = buf[2];
}

template <typename T>
class vec3
{
    T vec[3];

public:
    T& operator[](int i)
    {
        return vec[i];
    }
};

template <typename T, int N, int M>
class matrix
{
    T matrix[N*M];

public:
    T& at(int i, int j)
    {
        return matrix[i+j*N];
    }

    T* raw()
    {
        return &matrix[0];
    }
};

template <typename T>
class matrix3x3
    : public matrix<T,3,3>
{
public:
    uchar det()
    {
        return    this->at(0,0) * ( this->at(1,1)*this->at(2,2) - this->at(1,2)*this->at(2,1) )
            - this->at(0,1) * ( this->at(1,0)*this->at(2,2) - this->at(2,0)*this->at(1,2) )
            + this->at(0,2) * ( this->at(1,0)*this->at(2,1) - this->at(1,1)*this->at(2,0) );
    }

    void setColumn(int id, vec3<T>& vec)
    {
        for (int i=0; i<3; i++)
            this->at(i,id) = vec[i];
    }


    void setRow(int id, vec3<T>& vec)
    {
        for (int i=0; i<3; i++)
            this->at(id,i) = vec[i];
    }
};


template <typename T, unsigned TMAX>
class analyzer
{
public:
    vec3<T> m[3]; // three plaintext messages
    vec3<T> c[3]; // three encrypted messages

    matrix3x3<T> E; // resulting master matrix, we are looking for

    bool evaluate()
    {
        matrix3x3<T> EQM;

        for (int i=0; i<3; i++)
            for (int j=0; j<3; j++)
                EQM.at(i,j) = m[i][j];

        T det = EQM.det();

        if (det == 0)
        {
            return false;
        }


        for (int jidx = 0; jidx < 3; jidx ++)
        {
            for (int idx = 0; idx < 3; idx ++ )
            {
                matrix3x3<T> e0EQM = EQM; // default C-CTOR is ok
                e0EQM.at(0, idx) = c[0][jidx];
                e0EQM.at(1, idx) = c[1][jidx];
                e0EQM.at(2, idx) = c[2][jidx];

                T det0 = e0EQM.det();

                int first = 1;
                for (int i=0; i<10; i++)
                {
                    if ( ((det0+TMAX*i) % det) == 0)
                    {
                        T result = (det0+TMAX*i) / det;

                    //  if (!first)
                    //      printf(" or ");

                    //  printf("%d", result);

                        if (first)
                            E.at(idx,jidx) = result;

                        first = 0;
                    }
                }
                //printf("\n");
            }
        }
    }
};



int main()
{
    uchar input[9] = {0, 9, 2, 3, 4, 5, 6, 7, 8 };
    uchar output[9];

    uchar key[9] = {10,5,7,1,4,2,9,7,8};

    qes_encrypt_block(&input[0], &output[0], &key[0]);
    qes_encrypt_block(&input[3], &output[3], &key[0]);
    qes_encrypt_block(&input[6], &output[6], &key[0]);

    printf("Encrypted: ");

    for (int i=0; i<9; i++)
        printf("%d ", output[i]);
    printf("\n");

    printf("key: ");
    for (int i=0; i<9; i++)
        printf("%d ", key[i]);
    printf("\n");


    analyzer<uchar,256> a;

    // preparing analyzer - feeding in the data
    for (int i=0; i<3; i++)
    {
        for (int j=0; j<3; j++)
        {
            a.m[i][j] = input[j+3*i];
            a.c[i][j] = output[j+3*i];
        }
    }

    if (a.evaluate())
    {
        printf("recovered key: ");
        for (int i=0; i<9; i++)
            printf("%d ", a.E.raw()[i]);
        printf("\n");
    }
    else
    {
        printf("Select another dataset, determenant is zero\n");
    }
}