LoRaFIIT/Encryption.cpp at master · ladislavz/LoRaFIIT · GitHub

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
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
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
#include "Encryption.h"

/**
 * Encrypts data payload using XXTEA algorithm
 * @param payload
 * @param size
 * @param keyByte
 */
void Encryption::encrypt(uint8_t *payload, uint8_t size, uint8_t *keyByte) {
  uint32_t *v = (uint32_t *) payload;
  int n = size/ sizeof(uint32_t);
  uint32_t *key = (uint32_t *) keyByte;

  uint32_t y, z, sum;
  unsigned p, rounds, e;
  rounds = 6 + 52/n;
  sum = 0;
  z = v[n-1];

  do {
    sum += DELTA;
    e = (sum >> 2) & 3;

    for (p=0; p<n-1; p++) {
        y = v[p+1];
        z = v[p] += MX;
    }

    y = v[0];
    z = v[n-1] += MX;
  } while (--rounds);
}

/**
 * Decrypts data payload using XXTEA algorithm
 * @param payload
 * @param size
 * @param keyByte
 */
void Encryption::decrypt(uint8_t *payload, uint8_t size, uint8_t *keyByte) {
  uint32_t *v = (uint32_t *) payload;
  int n = size/ sizeof(uint32_t);
  uint32_t *key = (uint32_t *) keyByte;

  uint32_t y, z, sum;
  unsigned p, rounds, e;
  rounds = 6 + 52/n;
  sum = rounds*DELTA;
  y = v[0];

  do {
    e = (sum >> 2) & 3;
    for (p=n-1; p>0; p--) {
        z = v[p-1];
        y = v[p] -= MX;
    }
    z = v[n-1];
    y = v[0] -= MX;
    sum -= DELTA;
  } while (--rounds);
}

/**
 * Generates a Message Integrity Code (MIC)
 * @param payload
 * @param size
 * @return
 */
uint32_t Encryption::generateMIC(uint8_t *payload, uint8_t size) {
  // One-at-a-Time hash
  uint32_t hash = 0;

  for (int i = 0; i < size; i++)
  {
    hash += payload[i];
    hash += (hash << 10);
    hash ^= (hash >> 6);
  }

  hash += (hash << 3);
  hash ^= (hash >> 11);
  hash += (hash << 15);
  return hash;
}

/**
 * Checking LoRa Message Integrity Code
 * @param payload
 * @param size
 * @param originalMIC
 * @return
 */
bool Encryption::checkMIC(uint8_t *payload, uint8_t size, uint32_t originalMIC) {
  return generateMIC(payload, size) == originalMIC;
}