#include <stdio.h>
#include <stdint.h>
#include <speck.h>


void Words64ToBytes(u64 words[],u8 bytes[],int numwords)
{
    int i,j=0;
    for(i=0;i<numwords;i++){
        bytes[j]=(u8)words[i];
        bytes[j+1]=(u8)(words[i]>>8);
        bytes[j+2]=(u8)(words[i]>>16);
        bytes[j+3]=(u8)(words[i]>>24);
        bytes[j+4]=(u8)(words[i]>>32);
        bytes[j+5]=(u8)(words[i]>>40);
        bytes[j+6]=(u8)(words[i]>>48);
        bytes[j+7]=(u8)(words[i]>>56);
        j+=8;
    }
}

void BytesToWords64(u8 bytes[],u64 words[],int numbytes)
{
    int i,j=0;
    for(i=0;i<numbytes/8;i++){
        words[i]=(u64)bytes[j] | ((u64)bytes[j+1]<<8) | ((u64)bytes[j+2]<<16) |
        ((u64)bytes[j+3]<<24) | ((u64)bytes[j+4]<<32) | ((u64)bytes[j+5]<<40) |
        ((u64)bytes[j+6]<<48) | ((u64)bytes[j+7]<<56); j+=8; }
}


void Speck128256KeySchedule(u64 K[],u64 rk[])
{
    u64 i,D=K[3],C=K[2],B=K[1],A=K[0];
    for(i=0;i<33;){
        rk[i]=A; ER64(B,A,i++);
        rk[i]=A; ER64(C,A,i++);
        rk[i]=A; ER64(D,A,i++);
    }
    rk[i]=A;
}
void Speck128256Encrypt(u64 Pt[],u64 Ct[],u64 rk[])
{
    u64 i;
    Ct[0]=Pt[0]; Ct[1]=Pt[1];
    for(i=0;i<34;) ER64(Ct[1],Ct[0],rk[i++]);
}

void Speck128256Decrypt(u64 Pt[],u64 Ct[],u64 rk[])
{
    int i;
    Pt[0]=Ct[0]; Pt[1]=Ct[1];
    for(i=33;i>=0;) DR64(Pt[1],Pt[0],rk[i--]);
}



void EncryptBlock(u8 pt[], u8 k[], u8 ct[]) {

    u64 Pt[2] = {0};
    u64 K[4] = {0};
    u64 rk[34] = {0};
    u64 Ct[2] = {0};

    BytesToWords64(pt,Pt,16);
    BytesToWords64(k,K,32);
    Speck128256KeySchedule(K,rk);
    Speck128256Encrypt(Pt,Ct,rk);
    Words64ToBytes(Ct,ct,2);
}