encrypt.ino

#include "AES-128_V10.h"

unsigned char NwkSkey[16] = {
  0x00, 0xE2, 0xC1, 0x11, 0xCF, 0x0B, 0xFB, 0x8D,
  0x1D, 0x08, 0x98, 0xF0, 0xCA, 0xA8, 0x77, 0x00
};

unsigned char AppSkey[16] = {
  0x00, 0x9B, 0x51, 0x67, 0x09, 0x92, 0xC2, 0x40,
  0xAC, 0x1C, 0xF4, 0xE2, 0x38, 0x9C, 0x8D, 0x00
};

unsigned char DevAddr[4] = {
  0x01, 0x00, 0x00, 0x07
};

unsigned char FCount[2] = {
  0x00, 0x05
};
 
unsigned char Dir = 0x01;


unsigned char FRMPayload[2] = {
  0x14, 0xA5
};

void setup() {
  //Initialize the UART
  Serial.begin(9600); 
}

void loop() {
  
  Encrypt_Payload(FRMPayload, sizeof(FRMPayload), FCount, Dir);
  
  //Serial.println("Data Desencriptado");
  
  for(int k = 0; k < 2; k++)
      {
        //Serial.write(FRMPayload[k]);
        char datos = FRMPayload[k];
        Serial.write(datos);
      }
  delay(3000);
}

void Encrypt_Payload(unsigned char *Data, unsigned char Data_Length, unsigned int Frame_Counter, unsigned char Direction)
{
  unsigned char i = 0x00;
  unsigned char j;
  unsigned char Number_of_Blocks = 0x00;
  unsigned char Incomplete_Block_Size = 0x00;

  unsigned char Block_A[16];

  Serial.println("\nData encriptado");
  Serial.print(Data[0],HEX);
  Serial.println(Data[1],HEX);
     
  //Serial.println(Data_Length);

  //Calculate number of blocks
  Number_of_Blocks = Data_Length / 16;
  Incomplete_Block_Size = Data_Length % 16;
  if(Incomplete_Block_Size != 0)
  {
    Number_of_Blocks++;
  }
  //Serial.println(Number_of_Blocks);
  for(i = 1; i <= Number_of_Blocks; i++)
  {
    Block_A[0] = 0x01;
    Block_A[1] = 0x00;
    Block_A[2] = 0x00;
    Block_A[3] = 0x00;
    Block_A[4] = 0x00;

    Block_A[5] = Dir;

    Block_A[6] = DevAddr[3];
    Block_A[7] = DevAddr[2];
    Block_A[8] = DevAddr[1];
    Block_A[9] = DevAddr[0];

    Block_A[10] = FCount[1];
    Block_A[11] = FCount[0];

    Block_A[12] = 0x00; //Frame counter upper Bytes
    Block_A[13] = 0x00;

    Block_A[14] = 0x00;

    Block_A[15] = i;
/*
    Serial.println("Block: " );
  for(j = 0; j < 16; j++)
      {
        Serial.print(Block_A[j],HEX);
      }
*/
    //Calculate S
    AES_Encrypt(Block_A,AppSkey);

    //Check for last block
    if(i != (Number_of_Blocks-1))
    {
      for(j = 0; j < 16; j++)
      {
        *Data = *Data ^ Block_A[j];
        Data++;
      }
    }
    else
    {
      if(Incomplete_Block_Size == 0)
      {
        Incomplete_Block_Size = 16;
      }
      for(j = 0; j < Incomplete_Block_Size; j++)
      {
        *Data = *Data ^ Block_A[j];
        Data++;
      }
    }
  }  
}