SCP/ScpCrypto.cpp at development · houseos/SCP · 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
/*
secure-control-protocol
This is the source file for the ScpCrypto class.

SPDX-License-Identifier: GPL-3.0-or-later

Copyright (C) 2018 - 2020 Benjamin Schilling
*/

#include "ScpCrypto.h"

ScpCrypto::ScpCrypto()
{
  currentNvcn = 1;
}

String ScpCrypto::decodeAndDecrypt(String payload, int payloadLength,
                                   String key, String nonce, String mac)
{
  ScpDecode decoder();
  // decode payload
  rbase64.decode(payload);
  char *decodedPayload = rbase64.result();
  char correctPayload[payloadLength + 1];
  memset(correctPayload, 0, payloadLength + 1);
  memcpy(correctPayload, decodedPayload, payloadLength);
  for (int i = 0; i < payloadLength; i++)
  {
    scpDebug.println(scpDebug.crypto, "ScpCrypto::decodeAndDecrypt: Element Payload " + String(i, DEC) + ": " + String(correctPayload[i], HEX));
  }

  // decode mac
  rbase64.decode(mac);
  char *decodedMac = rbase64.result();
  char correctMac[MAC_LENGTH + 1];
  memset(correctMac, 0, MAC_LENGTH + 1);
  memcpy(correctMac, decodedMac, MAC_LENGTH);
  for (int i = 0; i < MAC_LENGTH; i++)
  {
    scpDebug.println(scpDebug.crypto, "ScpCrypto::decodeAndDecrypt: Element Mac " + String(i, DEC) + ": " + String(correctMac[i], HEX));
  }

  // key
  unsigned char correctKey[KEY_LENGTH + 1];
  memset(correctKey, 0, KEY_LENGTH + 1);
  key.getBytes(correctKey, KEY_LENGTH + 1);

  // decode nonce
  rbase64.decode(nonce);
  char *decodedNonce = rbase64.result();
  char correctNonce[NONCE_LENGTH + 1];
  memset(correctNonce, 0, NONCE_LENGTH + 1);
  memcpy(correctNonce, decodedNonce, NONCE_LENGTH);

  bool decryptionSucceeded = ChaCha20Poly1305::decrypt(correctPayload, payloadLength, correctKey, nullptr, sizeof 0, correctNonce, correctMac);

  if (decryptionSucceeded)
  {
    return String(correctPayload);
  }
  else
  {
    return "";
  }
}

String ScpCrypto::generateHMAC(String payload, uint8_t *key, size_t keyLength)
{
  return SHA512::hmacCT(payload.begin(), key, keyLength, HMAC_LENGTH);
}

String ScpCrypto::getNVCN()
{
  currentNvcn++;
  return String(currentNvcn);
}

bool ScpCrypto::checkNVCN(String nvcn)
{

  scpDebug.println(scpDebug.crypto, "ScpCrypto::checkNVCN: Expected NVCN: " + String(currentNvcn) + "Received NVCN: " + String(nvcn));
  if (currentNvcn == nvcn.toInt())
  {
    return true;
  }
  return false;
}