TheThingsNetworkGateway/Raw-RFM95/Raw-RFM95.ino

// LORA_ARM_TX made from from AEStest and SPITEST
//
#define JOB_TX      1
#define JOB_MAILBOX 2
#define JOB_TEMP    3
#define JOB_TEST1   4

#define JOB JOB_TEST1

// from SPITest par 23jul18 rfm95 comms
// reset=PA3=8, NSS=PA4=7, MOSI=PA7=4, CLK=PB0=3, MISO=PA6=5, DIO0=PB12=31
#define RESET 8
#define NSS 7
#define MOSI 4
#define CLK 3
#define MISO 5
#define DIO0PIN 31

// RGB led pins acive high
#define RLED 17
#define GLED 18
#define BLED 19

#define ASENSE 12
#define BSENSE 13
#define CSENSE 15
#define NSENSE 14

#define ADRIVE 10
#define BDRIVE 9
#define CDRIVE 11

#define MAPLELED  33
#define DHT22 26


// pins used for mode selction options

// opt0 = enable serial comms for debug
// opt1 = enable fast transmit rate

#define OPTION1 30
#define OPTION2 29
#define OPTION3 28
#define OPTION4 27

#define IRDRIVE 9

// REGISTERS in RFM95
#define REG_FIFO          0x00
#define REG_MODE          0x01
#define REG_FREQ_H        0x06
#define REG_FREQ_M        0x07
#define REG_FREQ_L        0x08
#define REG_PACONFIG      0x09
#define REG_FIFOADDRPTR   0x0D
#define REG_FIFOTXBASEADDR  0X0E
#define REG_FIFORXBASEADDR  0x0F
#define REG_MODEMCONFIG   0x1D
#define REG_MODEMCONFIG2  0x1E
#define REG_SYMBTIMEOUT   0x1F
#define REG_PREAMBLE_MSB  0x20
#define REG_PREAMBLE_LSB  0x21
#define REG_PAYLOADLENGTH 0x22
#define REG_MODEMCONFIG3  0x26
#define REG_NODEADR       0x33
#define REG_SYNC          0x39
#define REG_IMAGECAL      0x3B
#define REG_DIOMAPPING1   0x40

// Mode constants
#define MODE_SLEEP        0x00
#define MODE_STANDBY      0x01
#define MODE_LORA_SLEEP   0x80
#define MODE_LORA_STANDBY 0x81
#define MODE_LORA_TX      0x83


int fb1,fb2,fb3;

// LOCATION WHERE FRAME COUNTER IS STORED in EEPROM
#define count_addr 0
unsigned int MyFrameCounter = 0;
#include <EEPROM.h>

int optSerial=0,optFast=0,optIR=0,optResetCounter=0;

void readFrameCounter()
{
  int l = EEPROM.read(count_addr) & 255;
  int h = EEPROM.read(count_addr+1) & 255;
  MyFrameCounter = l | h<<8;
}

void writeFrameCounter()
{
  unsigned char l = MyFrameCounter & 255;
  unsigned char h = (MyFrameCounter >> 8) & 255;
  EEPROM.write(count_addr,l);
  EEPROM.write(count_addr+1,h);
}

void swait()  // tiny delays
{
  int y=10; // was 100
  while(y--);
}

int xfr(unsigned char iswrite,unsigned char addr,unsigned char data)
{
  int d1;
  digitalWrite(NSS,0);
  swait();
  swait();
  d1 = addr & 127;
  if(iswrite!=0) d1 |= 128;
  swait();
  int m=128;
  for(int bit=0;bit<8;bit++)
  {
    if( d1 & m)  digitalWrite(MOSI,1);
    else        digitalWrite(MOSI,0);
    swait();
    digitalWrite(CLK,1);
    swait();
    digitalWrite(CLK,0);
    swait();
    m = (m >> 1) & 127;    
  }
  m = 128;
  int d2=data,res=0;
  for(int bit=0;bit<8;bit++)
  {
    if((d2 & m)>0) digitalWrite(MOSI,1);
    else        digitalWrite(MOSI,0);
    swait();
    res = res << 1;
    digitalWrite(CLK,1);
    if(digitalRead(MISO)>0) 
      res |= 1;
    swait();
    digitalWrite(CLK,0);
    m >>= 1;
  }
  digitalWrite(NSS,1);
  swait();
  return res;
}

void writer(unsigned char addr, unsigned char data)
{
  xfr(1,addr,data);
}

unsigned char reader(unsigned char addr)
{
  return xfr(0,addr,0);
}

void spiInit() { // using my own SPI code
  pinMode(RESET,OUTPUT);
  pinMode(NSS,OUTPUT);
  pinMode(CLK,OUTPUT);
  pinMode(MOSI,OUTPUT);
  pinMode(MISO,INPUT);
  pinMode(DIO0PIN,INPUT);
  digitalWrite(CLK,0);
  digitalWrite(NSS,1);
  digitalWrite(MOSI,0);
  digitalWrite(RESET,0);
  delay(1000);
  digitalWrite(RESET,1);
}

void optionInit() // one-time decision about operating mode required, set on links
{
  pinMode(OPTION1,INPUT_PULLUP);
  pinMode(OPTION2,INPUT_PULLUP);
  pinMode(OPTION3,INPUT_PULLUP);
  pinMode(OPTION4,INPUT_PULLUP);
  delay(10);
  optSerial = digitalRead(OPTION1)==0;
  optFast = digitalRead(OPTION2)==0;
  optIR = digitalRead(OPTION3)==0;
}


/////////////////////////////////////////////////////////////////////
/*
 * AES TEST EXAMPLE
 Copied from "CLE's" ATtiny85 + RFM95 Temperature Sensor
*/

unsigned char NwkSkey[16] = { 0xBB, 0x80, 0x64, 0x68, 0xF1, 0xE2, 0x3D, 0x06, 0xC0, 0xAF, 0x11, 0x9C, 0xAF, 0x4B, 0x07, 0xE5 };
unsigned char AppSkey[16] = { 0xE9, 0x74, 0x8A, 0xA3, 0xAA, 0x2F, 0x28, 0x4D, 0xD9, 0xFB, 0x2A, 0xEE, 0xDA, 0xD2, 0x9D, 0x7C };
unsigned char DevAddr[4] = { 0x26, 0x00, 0x13, 0x8D };
//unsigned int Frame_Counter_Tx = 0x0000;

// after char  PROGMEM
static const unsigned char PROGMEM S_Table[16][16] = {
  {0x63,0x7C,0x77,0x7B,0xF2,0x6B,0x6F,0xC5,0x30,0x01,0x67,0x2B,0xFE,0xD7,0xAB,0x76},
  {0xCA,0x82,0xC9,0x7D,0xFA,0x59,0x47,0xF0,0xAD,0xD4,0xA2,0xAF,0x9C,0xA4,0x72,0xC0},
  {0xB7,0xFD,0x93,0x26,0x36,0x3F,0xF7,0xCC,0x34,0xA5,0xE5,0xF1,0x71,0xD8,0x31,0x15},
  {0x04,0xC7,0x23,0xC3,0x18,0x96,0x05,0x9A,0x07,0x12,0x80,0xE2,0xEB,0x27,0xB2,0x75},
  {0x09,0x83,0x2C,0x1A,0x1B,0x6E,0x5A,0xA0,0x52,0x3B,0xD6,0xB3,0x29,0xE3,0x2F,0x84},
  {0x53,0xD1,0x00,0xED,0x20,0xFC,0xB1,0x5B,0x6A,0xCB,0xBE,0x39,0x4A,0x4C,0x58,0xCF},
  {0xD0,0xEF,0xAA,0xFB,0x43,0x4D,0x33,0x85,0x45,0xF9,0x02,0x7F,0x50,0x3C,0x9F,0xA8},
  {0x51,0xA3,0x40,0x8F,0x92,0x9D,0x38,0xF5,0xBC,0xB6,0xDA,0x21,0x10,0xFF,0xF3,0xD2},
  {0xCD,0x0C,0x13,0xEC,0x5F,0x97,0x44,0x17,0xC4,0xA7,0x7E,0x3D,0x64,0x5D,0x19,0x73},
  {0x60,0x81,0x4F,0xDC,0x22,0x2A,0x90,0x88,0x46,0xEE,0xB8,0x14,0xDE,0x5E,0x0B,0xDB},
  {0xE0,0x32,0x3A,0x0A,0x49,0x06,0x24,0x5C,0xC2,0xD3,0xAC,0x62,0x91,0x95,0xE4,0x79},
  {0xE7,0xC8,0x37,0x6D,0x8D,0xD5,0x4E,0xA9,0x6C,0x56,0xF4,0xEA,0x65,0x7A,0xAE,0x08},
  {0xBA,0x78,0x25,0x2E,0x1C,0xA6,0xB4,0xC6,0xE8,0xDD,0x74,0x1F,0x4B,0xBD,0x8B,0x8A},
  {0x70,0x3E,0xB5,0x66,0x48,0x03,0xF6,0x0E,0x61,0x35,0x57,0xB9,0x86,0xC1,0x1D,0x9E},
  {0xE1,0xF8,0x98,0x11,0x69,0xD9,0x8E,0x94,0x9B,0x1E,0x87,0xE9,0xCE,0x55,0x28,0xDF},
  {0x8C,0xA1,0x89,0x0D,0xBF,0xE6,0x42,0x68,0x41,0x99,0x2D,0x0F,0xB0,0x54,0xBB,0x16}
};


// definitions becvause soiurce i n reverse def order
void Encrypt_Payload(unsigned char *Data, unsigned char Data_Length, unsigned int Frame_Counter, unsigned char Direction);
void Calculate_MIC(unsigned char *Data, unsigned char *Final_MIC, unsigned char Data_Length, unsigned int Frame_Counter, unsigned char Direction);
void RFM_Send_Package(unsigned char *RFM_Tx_Package, unsigned char Package_Length);
void AES_Encrypt(unsigned char *Data, unsigned char *Key);
void Generate_Keys(unsigned char *K1, unsigned char *K2);
void XOR(unsigned char *New_Data,unsigned char *Old_Data);
void Shift_Left(unsigned char *Data);
void AES_Add_Round_Key(unsigned char *Round_Key, unsigned char (*State)[4]);
unsigned char AES_Sub_Byte(unsigned char Byte);
void AES_Shift_Rows(unsigned char (*State)[4]);
void AES_Mix_Collums(unsigned char (*State)[4]);
void AES_Calculate_Round_Key(unsigned char Round, unsigned char *Round_Key);

int DIO0()
{
    return digitalRead(DIO0PIN); 
}

void memcpy(char *dest,char *src,int len)
{
    while(len--)
    {
        *dest++ = *src++;
    }
}

/*
*****************************************************************************************
* Description : Function contstructs a LoRaWAN package and sends it
*
* Arguments   : *Data pointer to the array of data that will be transmitted
*               Data_Length nuber of bytes to be transmitted
*               Frame_Counter_Up  Frame counter of upstream frames
*****************************************************************************************
*/
void LORA_Send_Data(unsigned char *Data, unsigned char Data_Length, unsigned int Frame_Counter_Tx)
{
  //Define variables
  unsigned char i;

  //Direction of frame is up
  unsigned char Direction = 0x00;

  unsigned char RFM_Data[64];
  unsigned char RFM_Package_Length;

  unsigned char MIC[4];

  //Unconfirmed data up
  unsigned char Mac_Header = 0x40;

  unsigned char Frame_Control = 0x00;
  unsigned char Frame_Port = 0x01;

  //Encrypt the data
  Encrypt_Payload(Data, Data_Length, Frame_Counter_Tx, Direction);
  
  //Build the Radio Package
  RFM_Data[0] = Mac_Header;

  RFM_Data[1] = DevAddr[3];
  RFM_Data[2] = DevAddr[2];
  RFM_Data[3] = DevAddr[1];
  RFM_Data[4] = DevAddr[0];

  RFM_Data[5] = Frame_Control;

  RFM_Data[6] = (Frame_Counter_Tx & 0x00FF);
  RFM_Data[7] = ((Frame_Counter_Tx >> 8) & 0x00FF);

  RFM_Data[8] = Frame_Port;

  //Set Current package length
  RFM_Package_Length = 9;

  //Load Data
  for(i = 0; i < Data_Length; i++)
  {
    RFM_Data[RFM_Package_Length + i] = Data[i];
  }

  //Add data Lenth to package length
  RFM_Package_Length = RFM_Package_Length + Data_Length;
  
  //Calculate MIC
  Calculate_MIC(RFM_Data, MIC, RFM_Package_Length, Frame_Counter_Tx, Direction);

  //Load MIC in package
  for(i = 0; i < 4; i++)
  {
    RFM_Data[i + RFM_Package_Length] = MIC[i];
  }

  //Add MIC length to RFM package length
  RFM_Package_Length = RFM_Package_Length + 4;
 
  //Send Package
  RFM_Send_Package(RFM_Data, RFM_Package_Length);
}

void RFM_Write(char w,char d)
{
    writer(w,d); // calls my SPI code
}


void SetMode(unsigned char d)
{
  writer(REG_MODE,d);
}

unsigned char GetMode()
{
  return reader(REG_MODE);
}

void WriteFIFO(unsigned char d)
{
  writer(REG_FIFO,d);
}

void msg(char *m)
{
  if(optSerial){
    if(Serial) Serial.print(m);
  }
}

void RFM_Init()
{
  digitalWrite(RESET,0);
  delay(1000);
  digitalWrite(RESET,1);
  delay(100);
  //Switch RFM to sleep
  SetMode(MODE_SLEEP);
  delay(10);
  //Set RFM in LoRa mode
  SetMode(MODE_LORA_SLEEP);  // WAS LORA SLEEP
  delay(100);
  //Set RFM in Standby mode wait on mode ready
  SetMode(MODE_LORA_STANDBY);
  delay(100);
  char wk[100];
  sprintf(wk,"Expect lstby, Mode now %02X\n",GetMode());
  msg(wk);
  /*
  while (digitalRead(DIO5) == LOW)
  {
  }
  */
  delay(10);
  //Set carrier frequency
  // 868.100 MHz / 61.035 Hz = 14222987 = 0xD9068B
  RFM_Write(REG_FREQ_H,0xD9);
  RFM_Write(REG_FREQ_M,0x06);
  RFM_Write(REG_FREQ_L,0x8B);

  RFM_Write(REG_PACONFIG,0xff); // ff is max   //PA pin (maximal power)
  RFM_Write(REG_MODEMCONFIG,0x72);  //BW = 125 kHz, Coding rate 4/5, Explicit header mode
  RFM_Write(REG_MODEMCONFIG2,0xB4);  //Spreading factor 7, PayloadCRC On
  RFM_Write(REG_SYMBTIMEOUT,0x25);  //Rx Timeout set to 37 symbols
  RFM_Write(REG_PREAMBLE_MSB,0x00);
  RFM_Write(REG_PREAMBLE_LSB,0x08);  //Preamble length set to 8 symbols 0x0008 + 4 = 12
  RFM_Write(REG_MODEMCONFIG3,0x0C);  //Low datarate optimization off AGC auto on

  
  RFM_Write(0x39,0x34); //Set LoRa sync word
    
  //Set IQ to normal values
  RFM_Write(0x33,0x27);
  RFM_Write(0x3B,0x1D);

  //Set FIFO pointers
  RFM_Write(REG_FIFOTXBASEADDR,0x80);
  RFM_Write(REG_FIFORXBASEADDR,0x00);
  SetMode(MODE_LORA_SLEEP);
}


void setFreq(double freq)
{
  double d = freq*1.0e6/61.035;
  int l = (int)d;
  fb1 = l >> 16 & 255;
  fb2 = l >> 8 & 255;
  fb3 = l & 255; 
}

/*
*****************************************************************************************
* Description : Function for sending a package with the RFM
*
* Arguments   : *RFM_Tx_Package Pointer to arry with data to be send
*               Package_Length  Length of the package to send
*****************************************************************************************
*/

void RFM_Send_Package(unsigned char *RFM_Tx_Package, unsigned char Package_Length)
{
  unsigned char i;
  unsigned char RFM_Tx_Location = 0x00;
  //Set RFM in Standby mode wait on mode ready
  SetMode(MODE_SLEEP);
  SetMode(MODE_LORA_SLEEP);
  SetMode(MODE_LORA_STANDBY);
  /*
  while (digitalRead(DIO5) == LOW)  {  }
  */
  delay(100);
  //Switch DIO0 to TxDone
  //RFM_Write(0x40,0x40);
  //Set carrier frequency 868.100 MHz / 61.035 Hz = 14222987 = 0xD9068B
  //RFM_Write(REG_FREQ_H,0xD9);
  //RFM_Write(REG_FREQ_M,0x06);
  //RFM_Write(REG_FREQ_L,0x8B);

  RFM_Write(REG_FREQ_H,fb1);
  RFM_Write(REG_FREQ_M,fb2);
  RFM_Write(REG_FREQ_L,fb3);

  // RE ORDR NEXT 2 LINES
  RFM_Write(REG_MODEMCONFIG,0x72); //125 kHz 4/5 coding rate explicit header mode
  //RFM_Write(REG_MODEMCONFIG2, 0x74); //SF7 CRC On (LOOKS OK)
  RFM_Write(REG_MODEMCONFIG2,0x70); //SF7 CRC On (LOOKS OK)
  //RFM_Write(REG_MODEMCONFIG3,0x04); //Low datarate optimization off AGC auto on
  RFM_Write(REG_MODEMCONFIG3,0x00); //Low datarate optimization off AGC auto on
  RFM_Write(REG_PACONFIG,0xff); // ff is maxc POWER LEVEL
  //Set IQ to normal values
  // RFM_Write(0x33,0x27); // TAKE OUT 2 LINES
  //RFM_Write(0x3B,0x1D);
  RFM_Write(REG_PREAMBLE_MSB,0x00);
  RFM_Write(REG_PREAMBLE_LSB,13);  //Preamble length set to 12 +4

  RFM_Write(REG_SYNC,0x34); // newer
  RFM_Write(REG_PAYLOADLENGTH,Package_Length);
  RFM_Tx_Location = reader(REG_FIFOTXBASEADDR);  //Get location of Tx part of FiFo
  RFM_Write(REG_FIFOADDRPTR,RFM_Tx_Location);  //Set SPI pointer to start of Tx part in FiFo
  RFM_Write(REG_FIFOADDRPTR,0x80); /// new
  RFM_Write(REG_FIFOTXBASEADDR,0x80); /// new

  //Write Payload to FiFo
  for (i = 0;i < Package_Length; i++)
      WriteFIFO(*RFM_Tx_Package++);
  char wk[100];
  int wt = 0,m=0;
  //sprintf(wk,"Before tx starts, DIO0 is %d, mode is %02X, pkglen %d\n",DIO0(),GetMode(),Package_Length);
  //msg(wk);
  //Wait for TxDone
  //Switch RFM to Tx
  SetMode(MODE_LORA_TX);
  m = GetMode();
  while(GetMode()==MODE_LORA_TX) // was DIO0()==0
  {  wt++; 
  };
  sprintf(wk,"count %d mode %02X  ",wt,m);
  msg(wk);
  //Switch RFM to sleep
  SetMode(MODE_LORA_SLEEP);
}

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];

  //Calculate number of blocks
  Number_of_Blocks = Data_Length / 16;
  Incomplete_Block_Size = Data_Length % 16;
  if(Incomplete_Block_Size != 0)
  {
    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] = Direction;

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

    Block_A[10] = (Frame_Counter & 0x00FF);
    Block_A[11] = ((Frame_Counter >> 8) & 0x00FF);

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

    Block_A[14] = 0x00;

    Block_A[15] = i;

    //Calculate S
    AES_Encrypt(Block_A,AppSkey); //original
    

    //Check for last block
    if(i != Number_of_Blocks)
    {
      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++;
      }
    }
  }
}

void Calculate_MIC(unsigned char *Data, unsigned char *Final_MIC, unsigned char Data_Length, unsigned int Frame_Counter, unsigned char Direction)
{
  unsigned char i;
  unsigned char Block_B[16];
  
  unsigned char Key_K1[16] = {
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
  };
  unsigned char Key_K2[16] = {
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
  };

  //unsigned char Data_Copy[16];

  unsigned char Old_Data[16] = {
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
  };
  unsigned char New_Data[16] = {
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
  };
  
  
  unsigned char Number_of_Blocks = 0x00;
  unsigned char Incomplete_Block_Size = 0x00;
  unsigned char Block_Counter = 0x01;

  //Create Block_B
  Block_B[0] = 0x49;
  Block_B[1] = 0x00;
  Block_B[2] = 0x00;
  Block_B[3] = 0x00;
  Block_B[4] = 0x00;

  Block_B[5] = Direction;

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

  Block_B[10] = (Frame_Counter & 0x00FF);
  Block_B[11] = ((Frame_Counter >> 8) & 0x00FF);

  Block_B[12] = 0x00; //Frame counter upper bytes
  Block_B[13] = 0x00;

  Block_B[14] = 0x00;
  Block_B[15] = Data_Length;

  //Calculate number of Blocks and blocksize of last block
  Number_of_Blocks = Data_Length / 16;
  Incomplete_Block_Size = Data_Length % 16;

  if(Incomplete_Block_Size != 0)
  {
    Number_of_Blocks++;
  }

  Generate_Keys(Key_K1, Key_K2);

  //Preform Calculation on Block B0

  //Preform AES encryption
  AES_Encrypt(Block_B,NwkSkey);

  //Copy Block_B to Old_Data
  for(i = 0; i < 16; i++)
  {
    Old_Data[i] = Block_B[i];
  }

  //Preform full calculating until n-1 messsage blocks
  while(Block_Counter < Number_of_Blocks)
  {
    //Copy data into array
    for(i = 0; i < 16; i++)
    {
      New_Data[i] = *Data;
      Data++;
    }

    //Preform XOR with old data
    XOR(New_Data,Old_Data);

    //Preform AES encryption
    AES_Encrypt(New_Data,NwkSkey);

    //Copy New_Data to Old_Data
    for(i = 0; i < 16; i++)
    {
      Old_Data[i] = New_Data[i];
    }

    //Raise Block counter
    Block_Counter++;
  }

  //Perform calculation on last block
  //Check if Datalength is a multiple of 16
  if(Incomplete_Block_Size == 0)
  {
    //Copy last data into array
    for(i = 0; i < 16; i++)
    {
      New_Data[i] = *Data;
      Data++;
    }

    //Preform XOR with Key 1
    XOR(New_Data,Key_K1);

    //Preform XOR with old data
    XOR(New_Data,Old_Data);
    
    //Preform last AES routine
    // read NwkSkey from PROGMEM
    AES_Encrypt(New_Data,NwkSkey);
  }
  else
  {
    //Copy the remaining data and fill the rest
    for(i =  0; i < 16; i++)
    {
      if(i < Incomplete_Block_Size)
      {
        New_Data[i] = *Data;
        Data++;
      }
      if(i == Incomplete_Block_Size)
      {
        New_Data[i] = 0x80;
      }
      if(i > Incomplete_Block_Size)
      {
        New_Data[i] = 0x00;
      }
    }

    //Preform XOR with Key 2
    XOR(New_Data,Key_K2);

    //Preform XOR with Old data
    XOR(New_Data,Old_Data);

    //Preform last AES routine
    AES_Encrypt(New_Data,NwkSkey);
  }

  Final_MIC[0] = New_Data[0];
  Final_MIC[1] = New_Data[1];
  Final_MIC[2] = New_Data[2];
  Final_MIC[3] = New_Data[3];
}

void Generate_Keys(unsigned char *K1, unsigned char *K2)
{
  unsigned char i;
  unsigned char MSB_Key;

  //Encrypt the zeros in K1 with the NwkSkey
  AES_Encrypt(K1,NwkSkey);

  //Create K1
  //Check if MSB is 1
  if((K1[0] & 0x80) == 0x80)
  {
    MSB_Key = 1;
  }
  else
  {
    MSB_Key = 0;
  }

  //Shift K1 one bit left
  Shift_Left(K1);

  //if MSB was 1
  if(MSB_Key == 1)
  {
    K1[15] = K1[15] ^ 0x87;
  }

  //Copy K1 to K2
  for( i = 0; i < 16; i++)
  {
    K2[i] = K1[i];
  }

  //Check if MSB is 1
  if((K2[0] & 0x80) == 0x80)
  {
    MSB_Key = 1;
  }
  else
  {
    MSB_Key = 0;
  }

  //Shift K2 one bit left
  Shift_Left(K2);

  //Check if MSB was 1
  if(MSB_Key == 1)
  {
    K2[15] = K2[15] ^ 0x87;
  }
}

void Shift_Left(unsigned char *Data)
{
  unsigned char i;
  unsigned char Overflow = 0;
  //unsigned char High_Byte, Low_Byte;

  for(i = 0; i < 16; i++)
  {
    //Check for overflow on next byte except for the last byte
    if(i < 15)
    {
      //Check if upper bit is one
      if((Data[i+1] & 0x80) == 0x80)
      {
        Overflow = 1;
      }
      else
      {
        Overflow = 0;
      }
    }
    else
    {
      Overflow = 0;
    }

    //Shift one left
    Data[i] = (Data[i] << 1) + Overflow;
  }
}

void XOR(unsigned char *New_Data,unsigned char *Old_Data)
{
  unsigned char i;

  for(i = 0; i < 16; i++)
  {
    New_Data[i] = New_Data[i] ^ Old_Data[i];
  }
}

/*
*****************************************************************************************
* Title         : AES_Encrypt
* Description  : 
*****************************************************************************************
*/
void AES_Encrypt(unsigned char *Data, unsigned char *Key)
{
  unsigned char Row, Column, Round = 0;
  unsigned char Round_Key[16];
    unsigned char State[4][4];

  //  Copy input to State arry
  for( Column = 0; Column < 4; Column++ )
  {
    for( Row = 0; Row < 4; Row++ )
    {
      State[Row][Column] = Data[Row + (Column << 2)];
    }
  }

  //  Copy key to round key
  memcpy( &Round_Key[0], &Key[0], 16 );

  //  Add round key
  AES_Add_Round_Key( Round_Key, State );

  //  Preform 9 full rounds with mixed collums
  for( Round = 1 ; Round < 10 ; Round++ )
  {
    //  Perform Byte substitution with S table
    for( Column = 0 ; Column < 4 ; Column++ )
    {
      for( Row = 0 ; Row < 4 ; Row++ )
      {
        State[Row][Column] = AES_Sub_Byte( State[Row][Column] );
      }
    }

    //  Perform Row Shift
    AES_Shift_Rows(State);

    //  Mix Collums
    AES_Mix_Collums(State);

    //  Calculate new round key
    AES_Calculate_Round_Key(Round, Round_Key);

        //  Add the round key to the Round_key
    AES_Add_Round_Key(Round_Key, State);
  }

  //  Perform Byte substitution with S table whitout mix collums
  for( Column = 0 ; Column < 4 ; Column++ )
  {
    for( Row = 0; Row < 4; Row++ )
    {
      State[Row][Column] = AES_Sub_Byte(State[Row][Column]);
    }
  }
    
  //  Shift rows
  AES_Shift_Rows(State);

  //  Calculate new round key
  AES_Calculate_Round_Key( Round, Round_Key );

    //  Add round key
  AES_Add_Round_Key( Round_Key, State );

  //  Copy the State into the data array
  for( Column = 0; Column < 4; Column++ )
  {
    for( Row = 0; Row < 4; Row++ )
    {
      Data[Row + (Column << 2)] = State[Row][Column];
    }
  }
} // AES_Encrypt


/*
*****************************************************************************************
* Title         : AES_Add_Round_Key
* Description : 
*****************************************************************************************
*/
void AES_Add_Round_Key(unsigned char *Round_Key, unsigned char (*State)[4])
{
  unsigned char Row, Collum;

  for(Collum = 0; Collum < 4; Collum++)
  {
    for(Row = 0; Row < 4; Row++)
    {
      State[Row][Collum] ^= Round_Key[Row + (Collum << 2)];
    }
  }
} // AES_Add_Round_Key


/*
*****************************************************************************************
* Title         : AES_Sub_Byte
* Description : 
*****************************************************************************************
*/
unsigned char AES_Sub_Byte(unsigned char Byte)
{
//  unsigned char S_Row,S_Collum;
//  unsigned char S_Byte;
//
//  S_Row    = ((Byte >> 4) & 0x0F);
//  S_Collum = ((Byte >> 0) & 0x0F);
//  S_Byte   = S_Table [S_Row][S_Collum];

  //return S_Table [ ((Byte >> 4) & 0x0F) ] [ ((Byte >> 0) & 0x0F) ]; // original
  return S_Table [((Byte >> 4) & 0x0F)] [((Byte >> 0) & 0x0F)];
} //    AES_Sub_Byte



/*
*****************************************************************************************
* Title         : AES_Shift_Rows
* Description : 
*****************************************************************************************
*/
void AES_Shift_Rows(unsigned char (*State)[4])
{
  unsigned char Buffer;

  //Store firt byte in buffer
  Buffer      = State[1][0];
  //Shift all bytes
  State[1][0] = State[1][1];
  State[1][1] = State[1][2];
  State[1][2] = State[1][3];
  State[1][3] = Buffer;

  Buffer      = State[2][0];
  State[2][0] = State[2][2];
  State[2][2] = Buffer;
  Buffer      = State[2][1];
  State[2][1] = State[2][3];
  State[2][3] = Buffer;

  Buffer      = State[3][3];
  State[3][3] = State[3][2];
  State[3][2] = State[3][1];
  State[3][1] = State[3][0];
  State[3][0] = Buffer;
}   //  AES_Shift_Rows


/*
*****************************************************************************************
* Title         : AES_Mix_Collums
* Description : 
*****************************************************************************************
*/
void AES_Mix_Collums(unsigned char (*State)[4])
{
  unsigned char Row,Collum;
  unsigned char a[4], b[4];
    
    
  for(Collum = 0; Collum < 4; Collum++)
  {
    for(Row = 0; Row < 4; Row++)
    {
      a[Row] =  State[Row][Collum];
      b[Row] = (State[Row][Collum] << 1);

      if((State[Row][Collum] & 0x80) == 0x80)
      {
        b[Row] ^= 0x1B;
      }
    }
        
    State[0][Collum] = b[0] ^ a[1] ^ b[1] ^ a[2] ^ a[3];
    State[1][Collum] = a[0] ^ b[1] ^ a[2] ^ b[2] ^ a[3];
    State[2][Collum] = a[0] ^ a[1] ^ b[2] ^ a[3] ^ b[3];
    State[3][Collum] = a[0] ^ b[0] ^ a[1] ^ a[2] ^ b[3];
  }
}   //  AES_Mix_Collums



/*
*****************************************************************************************
* Title         : AES_Calculate_Round_Key
* Description : 
*****************************************************************************************
*/
void AES_Calculate_Round_Key(unsigned char Round, unsigned char *Round_Key)
{
  unsigned char i, j, b, Rcon;
  unsigned char Temp[4];
 
  //Calculate Rcon
  Rcon = 0x01;
  while(Round != 1)
  {
    b = Rcon & 0x80;
    Rcon = Rcon << 1;
        
    if(b == 0x80)
    {
      Rcon ^= 0x1b;
    }
    Round--;
  }
    
  //  Calculate first Temp
  //  Copy laste byte from previous key and subsitute the byte, but shift the array contents around by 1.
    Temp[0] = AES_Sub_Byte( Round_Key[12 + 1] );
    Temp[1] = AES_Sub_Byte( Round_Key[12 + 2] );
    Temp[2] = AES_Sub_Byte( Round_Key[12 + 3] );
    Temp[3] = AES_Sub_Byte( Round_Key[12 + 0] );

  //  XOR with Rcon
  Temp[0] ^= Rcon;

  //  Calculate new key
  for(i = 0; i < 4; i++)
  {
    for(j = 0; j < 4; j++)
    {
      Round_Key[j + (i << 2)]  ^= Temp[j];
      Temp[j]                   = Round_Key[j + (i << 2)];
    }
  }
}   //  AES_Calculate_Round_Key

float readThermistor() {
  return 25.0f;
}

void DoMessage() {
    uint16_t temp;
    uint8_t Data[32];
    uint8_t Data_Length;
    //temp = (int)(readThermistor() * 100);
    //Data[0] = (temp >> 8) & 0xff;
    //Data[1] = temp & 0xff;
    Data_Length = 5;
    memcpy(Data,"windy!",Data_Length);    
    LORA_Send_Data(Data, Data_Length, MyFrameCounter);
    MyFrameCounter++;
    writeFrameCounter(); // save counter to EEPROM
}

void DoMessage_Temp(int temperature) {
    uint16_t temp;
    uint8_t Data[32];
    uint8_t Data_Length;
    temp = temperature;
    Data[0] = (temp >> 8) & 0xff; // high byte first
    Data[1] = temp & 0xff;
    Data_Length = 2;
    LORA_Send_Data(Data, Data_Length, MyFrameCounter);
    MyFrameCounter++;
    writeFrameCounter(); // save counter to EEPROM
}


double readerd(unsigned int addr)
{
    int n = (int)reader(addr);
    return (double)n;
}

double getFreqMHz()
{
  return  61.035e-6*(readerd(8)+readerd(7)*256.0+readerd(6)*256.0*256.0);
}

// IR section
#define AIN1  6
int diff,previousdiff = 0;

void irTest()
{
  char buf[80];
  int n0A,n0B,n0C,n1A,n1B,n1C;
  digitalWrite(NSENSE,0); // sensors all on
  digitalWrite(ADRIVE,1);
  digitalWrite(BDRIVE,1); // off
  digitalWrite(CDRIVE,1);
  digitalWrite(GLED,1);
  delay(10);
  n0A = analogRead(ASENSE);
  n0B = analogRead(BSENSE);
  n0C = analogRead(CSENSE);
  sprintf(buf,"LED off : %5d %5d %5d\n",n0A,n0B,n0C);
  msg(buf);
  digitalWrite(ADRIVE,0);
  digitalWrite(BDRIVE,0); // on
  digitalWrite(CDRIVE,0);
  delay(10);
  n1A = analogRead(ASENSE);
  n1B = analogRead(BSENSE);
  n1C = analogRead(CSENSE);
  
  diff = abs(n1B-n0B);
  digitalWrite(ADRIVE,1);
  digitalWrite(BDRIVE,1); // off
  digitalWrite(CDRIVE,1);
  sprintf(buf,"LED ON : %5d %5d %5d \n",n1A,n1B,n1C);
  msg(buf);
  if(abs(diff-previousdiff)>30)
    {
      sprintf(buf,"******* BIG CHANGE \n");
      msg(buf);     
    }
  previousdiff = diff;
}

void MapleLED(int n)
{
  digitalWrite(MAPLELED,n);
}

void otest()
{
  char buf[80];
  int o1,o2,o3,o4;
  while(1)
  {
  o1 = digitalRead(OPTION1);
  o2 = digitalRead(OPTION2);
  o3 = digitalRead(OPTION3);
  o4 = digitalRead(OPTION4);
  sprintf(buf," %d %d %d %d\n",o1,o2,o3,o4);
  msg(buf);
  }
}

void t_test()
{
  char mess[80];
  int temp = 143;
  sprintf(mess,"Faked Temperature %5.1f decimal %d\n",(float)temp/10.0,temp);
  msg(mess);
  DoMessage_Temp(temp);  // read DHT22 temperature and send it
}

void t_test2()
{
  char mess[80];
  int i,dat[40],lo,hi,d,n,w0,w1,mask,by[5],b,ptr,bit,thr;
  
  sprintf(mess,"Reading DHT22\n");
  msg(mess);
  pinMode(DHT22,OUTPUT);
  digitalWrite(DHT22,0);
  delay(18);
  digitalWrite(DHT22,1);
  pinMode(DHT22,INPUT);
  lo = hi = 0;
  while(digitalRead(DHT22)==1);
  // expect low for 50us, then high for 50us
  // then 40 lots of 50us low, 26/70us high
  while(digitalRead(DHT22)==0) lo++;
  while(digitalRead(DHT22)==1) hi++;

  for(d=0;d<40;d++)
    {
      n = 0;
      while(digitalRead(DHT22)==0) n++;
      n = 0;
      while(digitalRead(DHT22)==1) n++;
      dat[d]=n;
    }
  delay(100);
  pinMode(DHT22,OUTPUT);
  thr = lo/2;
  ptr = 0; // array walk
  for(b=0;b<5;b++){
      mask = 0x80;
      by[b]=0;  
      for(bit=0;bit<8;bit++)
          {
          if(dat[ptr]>thr) 
            by[b] |= mask;
          mask = mask >> 1;
          ptr ++;
          }
    }
  //sprintf(mess,"bytes %d %d %d %d    %d, sum%d \n",by[0],by[1],by[2],by[3],by[4],(by[0]+by[1]+by[2]+by[3]) % 256);
  //msg(mess);
  sprintf(mess,"RH %d%% Temp %dC\n",by[0],by[2]);
  msg(mess);
}

void test1()
{
  char mess[80];
  int i,v;
#define NN 1000000
  sprintf(mess,"START %d  ",NN);
  msg(mess);
  for(i=0;i<NN;i++)
    v = analogRead(ASENSE);
  msg("DONE\n");  
  
}



void setup() {
  optionInit(); // header pins determine function
  if(optSerial){
    Serial.begin(9600);
    while(!Serial);
  }
//  otest();
  if(optResetCounter) {
    MyFrameCounter = 0;
    writeFrameCounter();  
  }
  spiInit();  
  pinMode(MAPLELED,OUTPUT);
  digitalWrite(MAPLELED,0);
  pinMode(IRDRIVE,OUTPUT);  
  digitalWrite(IRDRIVE,0);
  pinMode(RLED,OUTPUT);
  pinMode(GLED,OUTPUT);
  pinMode(BLED,OUTPUT);
  pinMode(NSENSE,OUTPUT);
  pinMode(ASENSE,INPUT);
  pinMode(BSENSE,INPUT);
  pinMode(CSENSE,INPUT);
  pinMode(ADRIVE,OUTPUT);
  pinMode(BDRIVE,OUTPUT);
  pinMode(CDRIVE,OUTPUT);
  pinMode(DHT22,OUTPUT);
  digitalWrite(RLED,0);
  digitalWrite(GLED,0);
  digitalWrite(BLED,0);
  digitalWrite(ADRIVE,1);
  digitalWrite(BDRIVE,1);
  digitalWrite(CDRIVE,1);
  digitalWrite(NSENSE,1);
  digitalWrite(DHT22,0); 
  msg("LORA_ARM_TX Various mode version - Starting up...\n"); 
  RFM_Init();
  readFrameCounter();
  setFreq(868.1);
}


void loop() {
  char buf[80];
  switch(JOB){
    case JOB_TX:
        {
        sprintf(buf,"TX Freq %6.1f MHz Counter %d\n",getFreqMHz(),MyFrameCounter);        
        msg(buf);
        delay(1000);   
        MapleLED(1);
        DoMessage();
        MapleLED(0);
        msg("Time to next tx : ");
        int cyc=30;
        for(int i=cyc;i>0;i--)
          {
          if(i % 5 == 0 || i<5 )
            {
            sprintf(buf,"%d ",i);    
            msg(buf);
            }
          delay(1000);
          }
        msg("\n");
        }
        break;
      
    case JOB_MAILBOX:
        {
        msg("IR Sensor B only\n");       
        delay(1000);
        irTest();
        }
        break;

    case JOB_TEMP:
        {
        msg("Temperature/Humidity\n");
        delay(1000);
        t_test2();
        delay(3000);
        //delay(60000);
        }
        break;

    case JOB_TEST1:
        {
        msg("TEST1 - see how fast it samples\n");
        test1();
        }
    break;
    default:     
        break;
  }
 
}
// end