SBRL-Archive
/
TheThingsNetworkGateway
Archived
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity
This repository has been archived on 2019-08-15. You can view files and clone it, but cannot push or open issues or pull requests.
TheThingsNetworkGateway/LoRaARMTXDemo/LoRaARMTXDemo.ino

1030 lines
26 KiB
C++
Raw Blame History

// LORA_ARM_TX_DEMO made from from AEStest and SPITEST
// runtime/build options
// if optSerial is 1, assume we want messages up the USB serial interface to the Arduino tool "Serial Monitor"
// the application hangs waiting for you to open the serialo monitor. If not interested in the mesages, set to 0.
// if optResetCounter is 1, initialise the EEPROM frame counter to 0. This is only necessary once, the first time you run the code,
// because we don't know if the EEPROM has been used before or not.
int optSerial=1; // <<-- change to 0 if you don't use the serial monitor
int optResetCounter=1; // <<-- change to 0 after your first run of this program
// port definitions for R%FM95 interface
// reset=PA3=8, NSS=PA4=7, MOSI=PA7=4, CLK=PB0=3, MISO=PA6=5, DIO0=PB12=31
// I/O definitions = channel numbers on the STM32F103
#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 MAPLELED 33
// 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; // lora frequency bytes
// LOCATION WHERE FRAME COUNTER IS STORED in EEPROM
#define count_addr 0
unsigned int MyFrameCounter = 0;
#include <EEPROM.h>
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);
}
/////////////////////////////////////////////////////////////////////
/*
* AES TEST EXAMPLE
Code copied from "CLE's" ATtiny85 + RFM95 Temperature Sensor
*/
// fakedev3 (ABP)
unsigned char NwkSkey[16] ={ 0x47, 0x74, 0x34, 0x24, 0x08, 0x81, 0x9E, 0xB5, 0xF6, 0x3F, 0x5A, 0x9C, 0x00, 0x1F, 0x16, 0x3B };
unsigned char DevAddr[4] = { 0x26, 0x01, 0x15, 0xCD };
unsigned char AppSkey[16] ={ 0x8A, 0x84, 0x0D, 0x95, 0xA5, 0x4A, 0x95, 0x25, 0x00, 0xEF, 0x2F, 0x5D, 0x6F, 0x64, 0x2A, 0xCC };
// original set now defunc
///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
// HIGH LEVEL MESSAGE SENDING CODE
void DoMessage_Text() {
uint8_t Data[32],Data_Length;
Data_Length = 11;
memcpy(Data,"Hello World",Data_Length);
LORA_Send_Data(Data, Data_Length, MyFrameCounter);
MyFrameCounter++;
writeFrameCounter(); // save counter to EEPROM
}
int Temperature = 123; // 12.30
void DoMessage_Temp() {
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);
Temperature += 1; // add 0.1C
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);
}
void MapleLED(int n) {
digitalWrite(MAPLELED,n);
}
void setup() {
if(optSerial) {
Serial.begin(9600);
while(!Serial);
}
if(optResetCounter) {
MyFrameCounter = 0; // its important to do this once, only. TTN expect to see the number changing UPWARDS only.
writeFrameCounter();
}
spiInit();
pinMode(MAPLELED,OUTPUT);
digitalWrite(MAPLELED,0);
msg("LORA_ARM_TX_DEMO Various mode version - Starting up...\n");
RFM_Init();
readFrameCounter();
setFreq(868.1);
}
void loop() {
char buf[80];
sprintf(buf,"TX Freq %6.1f MHz Counter %d\n",getFreqMHz(),MyFrameCounter);
msg(buf);
delay(1000);
MapleLED(1); // indicate we're about to send
// enable one of the following 2 lines as you wish
//DoMessage_Text();// payload is header with fixd text Hello World (all of which is encrypted)
DoMessage_Temp(); // alternbative message, header with 2 byte temperature, again, encrypted
MapleLED(0); // its all done
msg("Countdown 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");
}
// end
Reference in New Issue View Git Blame Copy Permalink