zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

deadline param encryption added

master
yaozb 5 years ago
parent
c7f96ecb56
commit
259872f93c
8 changed files with 407 additions and 10 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      ArduCopter/AP_Arming.cpp
  2. 328
      ArduCopter/Des.cpp
  3. 21
      ArduCopter/Des.h
  4. 47
      ArduCopter/Parameters.cpp
  5. 11
      ArduCopter/Parameters.h
  6. 2
      ArduCopter/system.cpp
  7. 4
      libraries/GCS_MAVLink/GCS_Common.cpp
  8. 2
      modules/mavlink

2
ArduCopter/AP_Arming.cpp
Unescape View File

@ -467,7 +467,7 @@ bool AP_Arming_Copter::mandatory_deadline_checks(bool display_failure)
// sprintf(temp,"time is up at %d-%d-%d ", deadline / 10000, (deadline % 10000) / 100, deadline % 100); // sprintf(temp,"time is up at %d-%d-%d ", deadline / 10000, (deadline % 10000) / 100, deadline % 100);
// reason = "Trial time has expired!"; // reason = "Trial time has expired!";
// gcs().send_text(MAV_SEVERITY_INFO, "无法解锁，您的无人机授权已于%d-%d-%d到期", deadline / 10000, (deadline % 10000) / 100, deadline % 100); gcs().send_text(MAV_SEVERITY_INFO, "无法解锁，您的无人机授权已于%d-%d-%d到期", deadline / 10000, (deadline % 10000) / 100, deadline % 100);
check_failed(display_failure, "Trial time has expired at %d-%d-%d !", deadline / 10000, (deadline % 10000) / 100, deadline % 100); check_failed(display_failure, "Trial time has expired at %d-%d-%d !", deadline / 10000, (deadline % 10000) / 100, deadline % 100);
return false; return false;
}else{ }else{

328
ArduCopter/Des.cpp
Unescape View File

@ -0,0 +1,328 @@
#include "Des.h"
const uint8_t initial_tr[64] =
{
57, 49, 41, 33, 25, 17, 9, 1,
59, 51, 43, 35, 27, 19, 11, 3,
61, 53, 45, 37, 29, 21, 13, 5,
63, 55, 47, 39, 31, 23, 15, 7,
56, 48, 40, 32, 24, 16, 8, 0,
58, 50, 42, 34, 26, 18, 10, 2,
60, 52, 44, 36, 28, 20, 12, 4,
62, 54, 46, 38, 30, 22, 14, 6};
const uint8_t final_tr[64] =
{
39, 7, 47, 15, 55, 23, 63, 31,
38, 6, 46, 14, 54, 22, 62, 30,
37, 5, 45, 13, 53, 21, 61, 29,
36, 4, 44, 12, 52, 20, 60, 28,
35, 3, 43, 11, 51, 19, 59, 27,
34, 2, 42, 10, 50, 18, 58, 26,
33, 1, 41, 9, 49, 17, 57, 25,
32, 0, 40, 8, 48, 16, 56, 24};
const uint8_t swap[64] =
{
33, 34, 35, 36, 37, 38, 39, 40,
41, 42, 43, 44, 45, 46, 47, 48,
49, 50, 51, 52, 53, 54, 55, 56,
57, 58, 59, 60, 61, 62, 63, 64,
1, 2, 3, 4, 5, 6, 7, 8,
9, 10, 11, 12, 13, 14, 15, 16,
17, 18, 19, 20, 21, 22, 23, 24,
25, 26, 27, 28, 29, 30, 31, 32};
const uint8_t key_tr1[56] =
{
56, 48, 40, 32, 24, 16, 8,
0, 57, 49, 41, 33, 25, 17,
9, 1, 58, 50, 42, 34, 26,
18, 10, 2, 59, 51, 43, 35,
62, 54, 46, 38, 30, 22, 14,
6, 61, 53, 45, 37, 29, 21,
13, 5, 60, 52, 44, 36, 28,
20, 12, 4, 27, 19, 11, 3};
const uint8_t key_tr2[64] =
{
0, 0, 13, 4, 16, 10, 23, 0,
0, 0, 2, 9, 27, 14, 5, 20,
0, 0, 22, 7, 18, 11, 3, 25,
0, 0, 15, 1, 6, 26, 19, 12,
0, 0, 40, 54, 51, 30, 36, 46,
0, 0, 29, 47, 39, 50, 44, 32,
0, 0, 43, 52, 48, 38, 55, 33,
0, 0, 45, 31, 41, 49, 35, 28};
const uint8_t etr[64] =
{
0, 0, 31, 4, 0, 1, 2, 3,
0, 0, 3, 8, 4, 5, 6, 7,
0, 0, 7, 12, 8, 9, 10, 11,
0, 0, 11, 16, 12, 13, 14, 15,
0, 0, 15, 20, 16, 17, 18, 19,
0, 0, 19, 24, 20, 21, 22, 23,
0, 0, 23, 28, 24, 25, 26, 27,
0, 0, 27, 0, 28, 29, 30, 31};
const uint8_t ptr[32] =
{
31, 14, 39, 44, 60, 23, 55, 36,
4, 30, 46, 53, 12, 37, 62, 21,
5, 15, 47, 29, 63, 54, 6, 20,
38, 28, 61, 13, 45, 22, 7, 52};
const uint8_t s[8][64] =
{
{14, 4, 13, 1, 2, 15, 11, 8, 3, 10, 6, 12, 5, 9, 0, 7,
0, 15, 7, 4, 14, 2, 13, 1, 10, 6, 12, 11, 9, 5, 3, 8,
4, 1, 14, 8, 13, 6, 2, 11, 15, 12, 9, 7, 3, 10, 5, 0,
15, 12, 8, 2, 4, 9, 1, 7, 5, 11, 3, 14, 10, 0, 6, 13},
{15, 1, 8, 14, 6, 11, 3, 4, 9, 7, 2, 13, 12, 0, 5, 10,
3, 13, 4, 7, 15, 2, 8, 14, 12, 0, 1, 10, 6, 9, 11, 5,
0, 14, 7, 11, 10, 4, 13, 1, 5, 8, 12, 6, 9, 3, 2, 15,
13, 8, 10, 1, 3, 15, 4, 2, 11, 6, 7, 12, 0, 5, 14, 9},
{10, 0, 9, 14, 6, 3, 15, 5, 1, 13, 12, 7, 11, 4, 2, 8,
13, 7, 0, 9, 3, 4, 6, 10, 2, 8, 5, 14, 12, 11, 15, 1,
13, 6, 4, 9, 8, 15, 3, 0, 11, 1, 2, 12, 5, 10, 14, 7,
1, 10, 13, 0, 6, 9, 8, 7, 4, 15, 14, 3, 11, 5, 2, 12},
{7, 13, 14, 3, 0, 6, 9, 10, 1, 2, 8, 5, 11, 12, 4, 15,
13, 8, 11, 5, 6, 15, 0, 3, 4, 7, 2, 12, 1, 10, 14, 9,
10, 6, 9, 0, 12, 11, 7, 13, 15, 1, 3, 14, 5, 2, 8, 4,
3, 15, 0, 6, 10, 1, 13, 8, 9, 4, 5, 11, 12, 7, 2, 14},
{2, 12, 4, 1, 7, 10, 11, 6, 8, 5, 3, 15, 13, 0, 14, 9,
14, 11, 2, 12, 4, 7, 13, 1, 5, 0, 15, 10, 3, 9, 8, 6,
4, 2, 1, 11, 10, 13, 7, 8, 15, 9, 12, 5, 6, 3, 0, 14,
11, 8, 12, 7, 1, 14, 2, 13, 6, 15, 0, 9, 10, 4, 5, 3},
{12, 1, 10, 15, 9, 2, 6, 8, 0, 13, 3, 4, 14, 7, 5, 11,
10, 15, 4, 2, 7, 12, 9, 5, 6, 1, 13, 14, 0, 11, 3, 8,
9, 14, 15, 5, 2, 8, 12, 3, 7, 0, 4, 10, 1, 13, 11, 6,
4, 3, 2, 12, 9, 5, 15, 10, 11, 14, 1, 7, 6, 0, 8, 13},
{4, 11, 2, 14, 15, 0, 8, 13, 3, 12, 9, 7, 5, 10, 6, 1,
13, 0, 11, 7, 4, 9, 1, 10, 14, 3, 5, 12, 2, 15, 8, 6,
1, 4, 11, 13, 12, 3, 7, 14, 10, 15, 6, 8, 0, 5, 9, 2,
6, 11, 13, 8, 1, 4, 10, 7, 9, 5, 0, 15, 14, 2, 3, 12},
{13, 2, 8, 4, 6, 15, 11, 1, 10, 9, 3, 14, 5, 0, 12, 7,
1, 15, 13, 8, 10, 3, 7, 4, 12, 5, 6, 11, 0, 14, 9, 2,
7, 11, 4, 1, 9, 12, 14, 2, 0, 6, 10, 13, 15, 3, 5, 8,
2, 1, 14, 7, 4, 10, 8, 13, 15, 12, 9, 0, 3, 5, 6, 11}};
const uint8_t rots[16] =
{
1, 1, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2, 2, 1};
const uint8_t bit_msk[8] =
{
0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01};
/************************************************************************/
/* */
/* Module title: des */
/* Module type: des mainrutine */
/* */
/* Author: YXH */
/* Date: 2012-07-13 */
/* */
/* Last changed by: YXH */
/* Date: 2012-07-13 */
/* */
/* Functional Description: Encipher and decipher 64 bits string */
/* according to a 64 bits key string */
/* The string format is shown below */
/* */
/* input parameter 1: pointer to 64 bits input string */
/* 2: pointer to 64 bits key string */
/* 3: boolean if false indicating enciphering */
/* if true dechiphering 1解密 */
/* 4: pointer to a 64 bits output string */
/************************************************************************/
/* */
/* msb lsb */
/* bit bit */
/* +-- -- -- -- -- -- -- --+ */
/* addr !1st 8th! */
/* +-- -- -- -- -- -- -- --+ */
/* addr+1 !9th 16th! */
/* +-- -- -- -- -- -- -- --+ */
/* : : */
/* : : */
/* +-- -- -- -- -- -- -- --+ */
/* addr+7 !57th 64th! */
/* +-- -- -- -- -- -- -- --+ */
/* */
/************************************************************************/
void Des::des(uint8_t* plain_strng, uint8_t* key, uint8_t d, uint8_t* ciph_strng)
{
uint8_t a_str[8], b_str[8], x_str[8];
uint8_t i, j, * pkey, temp;
for (i = 0; i < 8; ++i)
{
if (key[i] != main_key[i])
{
compute_subkeys(key);
i = 7;
}
}
transpose(plain_strng, a_str, initial_tr, 64);
for (i = 1; i < 17; ++i)
{
for (j = 0; j < 8; ++j) { b_str[j] = a_str[j]; }
if (!d) /*enchipher*/
pkey = &sub_keys[i - 1][0];
else /*dechipher*/
pkey = &sub_keys[16 - i][0];
for (j = 0; j < 4; ++j) { a_str[j] = b_str[j + 4]; }
f(pkey, a_str, x_str);
for (j = 0; j < 4; ++j) { a_str[j + 4] = b_str[j] ^ x_str[j]; }
}
temp = a_str[0]; a_str[0] = a_str[4]; a_str[4] = temp;
temp = a_str[1]; a_str[1] = a_str[5]; a_str[5] = temp;
temp = a_str[2]; a_str[2] = a_str[6]; a_str[6] = temp;
temp = a_str[3]; a_str[3] = a_str[7]; a_str[7] = temp;
transpose(a_str, ciph_strng, final_tr, 64);
}
/************************************************************************/
/* */
/* Module title: transpose */
/* Module type: des subrutine */
/* */
/* Author: YXH */
/* Date: 2012-07-13 */
/* */
/* Last changed by: YXH */
/* Date: 2012-07-13 */
/* */
/* Functional Description: Permute n bits in a string, according */
/* to a table describing the new order. */
/* 0 < n <= 64 */
/* */
/* input parameter 1: pointer to first byte in input string */
/* 2: pointer to first byte in output string */
/* 3: pointer to table describing new order */
/* 4: number of bits to be permuted */
/************************************************************************/
void Des:: transpose(uint8_t* idata, uint8_t* odata, const uint8_t* tbl, uint8_t n)
{
const uint8_t* tab_adr;
int i, bi_idx;
tab_adr = &bit_msk[0];
i = 0;
do
{
odata[i++] = 0;
} while (i < 8);
i = 0;
do
{
bi_idx = *tbl++;
if (idata[bi_idx >> 3] & tab_adr[bi_idx & 7])
{
odata[i >> 3] |= tab_adr[i & 7];
}
} while (++i < n);
}
/************************************************************************/
/* */
/* Module title: rotate_l */
/* Module type: des subrutine */
/* */
/* Author: YXH */
/* Date: 2012-07-13 */
/* */
/* Last changed by: YXH */
/* Date: 2012-07-13 */
/* */
/* Functional Description: rotate 2 concatenated strings of 28 */
/* bits one position to the left. */
/* */
/* input parameter 1: pointer to first byte in key string */
/* */
/************************************************************************/
void Des::rotate_l(uint8_t* key)
{
uint8_t str_x[8];
uint8_t i;
for (i = 0; i < 8; ++i) str_x[i] = key[i];
for (i = 0; i < 7; ++i)
{
key[i] = (key[i] << 1);
if ((i < 6) && ((str_x[i + 1] & 128) == 128))
key[i] |= 1;
}
if (str_x[0] & 0x80)
key[3] |= 0x10;
else
key[3] &= ~0x10;
if (str_x[3] & 0x08)
key[6] |= 0x01;
else
key[6] &= ~0x01;
}
/************************************************************************/
/* */
/* Module title: compute_subkeys */
/* Module type: des subrutine */
/* */
/* Author: YXH */
/* Date: 2012-07-13 */
/* */
/* Last changed by: YXH */
/* Date: 2012-07-13 */
/* */
/* Functional Description: Computes the 16 sub keys for use in the */
/* DES algorithm */
/* */
/* input parameter 1: pointer to first byte in key string */
/* output : fills the array sub_keys[16][8] with */
/* sub keys and stores the input key in */
/* main_key[8] */
/************************************************************************/
void Des::compute_subkeys(uint8_t* key)
{
uint8_t i, j, ikey[8], okey[8];
for (i = 0; i < 8; ++i)
{
main_key[i] = key[i];
}
transpose(key, ikey, key_tr1, 56);
for (i = 0; i < 16; ++i)
{
for (j = 0; j < rots[i]; ++j) { rotate_l(ikey); }
transpose(ikey, okey, key_tr2, 64);
for (j = 0; j < 8; ++j)
{
sub_keys[i][j] = okey[j];
}
}
}
/************************************************************************/
/* */
/* Module title: f */
/* Module type: des subrutine */
/* */
/* Author: YXH */
/* Date: 2012-07-13 */
/* */
/* Last changed by: YXH */
/* Date: 2012-07-13 */
/* */
/* Functional Description: The chipher function */
/* */
/* input parameter 1: pointer to first byte in key string */
/* 2: pointer to a 32 bit input string */
/* 3: pointer to a 32 bit output string */
/************************************************************************/
void Des::f(uint8_t* skey, uint8_t* a_str, uint8_t* x_str)
{
uint8_t e_str[8], y_str[8], z_str[8];
uint8_t k;
transpose(a_str, e_str, etr, 64);
for (k = 0; k < 8; ++k)
{
y_str[k] = (e_str[k] ^ skey[k]) & 63;
z_str[k] = s[k][y_str[k]];
}
transpose(z_str, x_str, ptr, 32);
}

21
ArduCopter/Des.h
Unescape View File

@ -0,0 +1,21 @@
#pragma once
#include <stdint.h>
class Des
{
private:
/* data */
public:
uint8_t deskey[8] = {90,114,90,107,85,97,86,103};//{ "ZrZkUaVg" };
uint8_t DES_Encrypt_key[8];
uint8_t DES_Decrypt_key[8];
uint8_t sub_keys[16][8]; //sub_keys[16][8]
uint8_t main_key[8];
void des(uint8_t*, uint8_t*, uint8_t, uint8_t*);
void FLASH_Read_KEYS(uint8_t key_index);
void transpose(uint8_t*, uint8_t*, const uint8_t*, uint8_t);
void rotate_l(uint8_t*);
void compute_subkeys(uint8_t*);
void f(uint8_t*, uint8_t*, uint8_t*);
};

47
ArduCopter/Parameters.cpp
Unescape View File

@ -1,5 +1,5 @@
#include "Copter.h" #include "Copter.h"
#include "Des.h"
/* /*
This program is free software: you can redistribute it and/or modify This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
@ -76,7 +76,10 @@ const AP_Param::Info Copter::var_info[] = {
// @User: Advanced // @User: Advanced
GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255),
GSCALAR(sysid_dead_line, "SYSID_DEADLINE", 401221), GSCALAR(sysid_dl1, "SYSID_DL1", 0x8175),
GSCALAR(sysid_dl2, "SYSID_DL2", 0x6fda),
GSCALAR(sysid_dl3, "SYSID_DL3", 0xf38f),
GSCALAR(sysid_dl4, "SYSID_DL4", 0xbf48),
// @Param: PILOT_THR_FILT // @Param: PILOT_THR_FILT
// @DisplayName: Throttle filter cutoff // @DisplayName: Throttle filter cutoff
@ -1638,6 +1641,42 @@ const char* Copter::get_sysid_board_id(void)
snprintf(buf, sizeof(buf), "Version: zr-v4.0 ,Board ID: ZRZK.%s.%d",name,(int)g.sysid_board_id); snprintf(buf, sizeof(buf), "Version: zr-v4.0 ,Board ID: ZRZK.%s.%d",name,(int)g.sysid_board_id);
return buf; return buf;
} }
void Copter::get_deadline_params(int32_t &deadline ){
deadline = 20000000+ g.sysid_dead_line; void Copter::get_deadline_params(int32_t &deadline)
{
uint8_t tempbuf[8];
tempbuf[1] = g.sysid_dl1 & 0xff;
tempbuf[0] = (g.sysid_dl1 & 0xff00) >> 8;
// gcs().send_text(MAV_SEVERITY_INFO, "tempbuf0: %d ,tempbuf1:%d",tempbuf[0] ,tempbuf[1]);
tempbuf[3] = g.sysid_dl2 & 0xff;
tempbuf[2] = (g.sysid_dl2 & 0xff00) >> 8;
//gcs().send_text(MAV_SEVERITY_INFO, "tempbuf2: %d ,tempbuf3:%d",tempbuf[2] ,tempbuf[3]);
tempbuf[5] = g.sysid_dl3 & 0xff;
tempbuf[4] = (g.sysid_dl3 & 0xff00) >> 8;
//gcs().send_text(MAV_SEVERITY_INFO, "tempbuf4: %d ,tempbuf5:%d",tempbuf[4] ,tempbuf[5]);
tempbuf[7] = g.sysid_dl4 & 0xff;
tempbuf[6] = (g.sysid_dl4 & 0xff00) >> 8;
//gcs().send_text(MAV_SEVERITY_INFO, "tempbuf6: %d ,tempbuf7:%d",tempbuf[6] ,tempbuf[7]);
uint8_t out_data_temp[8];
Des des;
des.des(&tempbuf[0], &des.deskey[0], 1, &out_data_temp[0]);
//gcs().send_text(MAV_SEVERITY_INFO, "out_data_temp6: %d ,out_data_temp7:%d",out_data_temp[6] ,out_data_temp[7]);
if (out_data_temp[6] == 122 && out_data_temp[7] == 114) //'zr'
{
deadline = 20000000 + ((out_data_temp[0] - 0x30) * 10 + (out_data_temp[1] - 0x30)) * 10000 +
((out_data_temp[2] - 0x30) * 10 + (out_data_temp[3] - 0x30)) * 100 +
((out_data_temp[4] - 0x30) * 10 + (out_data_temp[5] - 0x30));
// gcs().send_text(MAV_SEVERITY_INFO, "deadline:%ld",deadline);
}
else
{
deadline = 20200101;
gcs().send_text(MAV_SEVERITY_INFO, "D_Error: The registration code is wrong!");
}
} }

11
ArduCopter/Parameters.h
Unescape View File

@ -376,7 +376,11 @@ public:
k_param_logger = 253, // 253 - Logging Group k_param_logger = 253, // 253 - Logging Group
// 254,255: reserved // 254,255: reserved
k_param_sysid_dead_line = 300, k_param_sysid_dl1= 300,
k_param_sysid_dl2,
k_param_sysid_dl3,
k_param_sysid_dl4,
// the k_param_* space is 9-bits in size // the k_param_* space is 9-bits in size
// 511: reserved // 511: reserved
}; };
@ -393,7 +397,10 @@ public:
AP_Int16 sysid_my_gcs; AP_Int16 sysid_my_gcs;
AP_Int8 telem_delay; AP_Int8 telem_delay;
AP_Int32 sysid_dead_line; AP_Int32 sysid_dl1;
AP_Int32 sysid_dl2;
AP_Int32 sysid_dl3;
AP_Int32 sysid_dl4;
AP_Float throttle_filt; AP_Float throttle_filt;
AP_Int16 throttle_behavior; AP_Int16 throttle_behavior;

2
ArduCopter/system.cpp
Unescape View File

@ -338,8 +338,10 @@ bool Copter::deadline_ok()
int32_t deadline =0; int32_t deadline =0;
copter.get_deadline_params(deadline); copter.get_deadline_params(deadline);
gcs().send_text(MAV_SEVERITY_INFO,"deadline in ok %ld:",deadline);
int32_t gps_date =0; int32_t gps_date =0;
AP::gps().get_gps_headline_value(gps_date); AP::gps().get_gps_headline_value(gps_date);
gcs().send_text(MAV_SEVERITY_INFO,"gps_date in ok %ld:",gps_date);
if ((deadline - gps_date) > 0) if ((deadline - gps_date) > 0)
{ {
return true; return true;

4
libraries/GCS_MAVLink/GCS_Common.cpp
Unescape View File

@ -3280,10 +3280,10 @@ void GCS_MAVLINK::send_banner()
send_text(MAV_SEVERITY_INFO, "%s", sysid); send_text(MAV_SEVERITY_INFO, "%s", sysid);
} }
#else #else
send_text(MAV_SEVERITY_INFO, gcs_get_sysid_id()); send_text(MAV_SEVERITY_INFO, "%s", gcs_get_sysid_id());
char sysid[40]; char sysid[40];
if (hal.util->get_system_id(sysid)) { if (hal.util->get_system_id(sysid)) {
send_text(MAV_SEVERITY_INFO, sysid); send_text(MAV_SEVERITY_INFO, "%s", sysid);
} }
#endif #endif
} }

2
modules/mavlink

@ -1 +1 @@
Subproject commit ff8ada062c96f04dc830e54b657aca4430408433 Subproject commit 5fc173d4edeaec5e2f2d5b05a03bfb89a60ccc6c
Write Preview
Loading…
Cancel
Save