attnode/v3_firmware
2
0
Fork 0
Code Issues Pull requests Releases Activity
v3_firmware/LoRaWAN.cpp
Stefan Brand 8c200e3309 Initial Checkin
2020-11-26 16:56:36 +01:00

686 lines
15 KiB
C++
Executable file
Raw Blame History

/*
LoRaWAN.cpp - Library for LoRaWAN protocol, uses RFM95W module
Created by Leo Korbee, March 31, 2018.
Released into the public domain.
@license Attribution-NonCommercial-ShareAlike 4.0 International (CC BY-NC-SA 4.0)
Thanks to all the folks who contributed on the base of this code.
(Gerben den Hartog, et al - Ideetron.nl)
*/
#include "Arduino.h"
#include "LoRaWAN.h"
// constructor
LoRaWAN::LoRaWAN(RFM95 &rfm95)
{
_rfm95 = &rfm95;
}
void LoRaWAN::setKeys(unsigned char NwkSkey[], unsigned char AppSkey[], unsigned char DevAddr[])
{
_NwkSkey = NwkSkey;
_AppSkey = AppSkey;
_DevAddr = DevAddr;
}
/*
*****************************************************************************************
* 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 LoRaWAN::Send_Data(unsigned char *Data, unsigned char Data_Length, unsigned int Frame_Counter_Tx, lora_dr_t datarate,unsigned char Frame_Port)
{
//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];
/*
@leo:
https://hackmd.io/s/S1kg6Ymo-
7…5 bits 4…2 bits 1…0 bits
MType RFU Major
MType Description
000 (0x00) Join Request
001 (0x20) Join Accept
010 (0x40) Unconfirmed Data Up
011 (0x60) Unconfirmed Data Down
100 (0x80) Confirmed Data Up
101 (0xA0) Confirmed Data Down
110 (0xC0) RFU
111 (0xE0) Proprietary
*/
// Unconfirmed data up
unsigned char Mac_Header = 0x40;
// Confirmed data up
// unsigned char Mac_Header = 0x80;
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;
//Set Lora Datarate
_rfm95->RFM_Set_Datarate(datarate);
//Send Package
_rfm95->RFM_Send_Package(RFM_Data, RFM_Package_Length);
}
/*
Encryption stuff after this line
*/
void LoRaWAN::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 LoRaWAN::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 LoRaWAN::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 LoRaWAN::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 LoRaWAN::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 LoRaWAN::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 LoRaWAN::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 LoRaWAN::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 pgm_read_byte(&(S_Table [((Byte >> 4) & 0x0F)] [((Byte >> 0) & 0x0F)]));
} // AES_Sub_Byte
/*
*****************************************************************************************
* Title : AES_Shift_Rows
* Description :
*****************************************************************************************
*/
void LoRaWAN::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 LoRaWAN::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 LoRaWAN::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
Reference in a new issue View git blame Copy permalink