2018-04-01 06:53:37 +00:00
|
|
|
/*
|
2019-02-24 13:08:51 +00:00
|
|
|
Copyright (C) 2018-2019 Fredrik Öhrström
|
2018-04-01 06:53:37 +00:00
|
|
|
|
|
|
|
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
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
2018-03-05 10:29:25 +00:00
|
|
|
|
|
|
|
|
|
|
|
#include"aes.h"
|
2018-11-25 11:33:14 +00:00
|
|
|
#include"util.h"
|
2018-03-05 10:29:25 +00:00
|
|
|
#include"wmbus.h"
|
|
|
|
|
2020-03-13 09:49:22 +00:00
|
|
|
#include<assert.h>
|
2019-01-04 21:50:31 +00:00
|
|
|
#include<memory.h>
|
|
|
|
|
2020-01-27 08:29:40 +00:00
|
|
|
bool decrypt_ELL_AES_CTR(Telegram *t, vector<uchar> &frame, vector<uchar>::iterator &pos, vector<uchar> &aeskey)
|
2018-03-05 10:29:25 +00:00
|
|
|
{
|
2020-01-27 08:29:40 +00:00
|
|
|
if (aeskey.size() == 0) return true;
|
2018-11-25 11:33:14 +00:00
|
|
|
|
2020-03-13 09:49:22 +00:00
|
|
|
vector<uchar> encrypted_bytes;
|
|
|
|
vector<uchar> decrypted_bytes;
|
|
|
|
encrypted_bytes.insert(encrypted_bytes.end(), pos, frame.end());
|
2020-01-27 08:29:40 +00:00
|
|
|
frame.erase(pos, frame.end());
|
2020-03-13 09:49:22 +00:00
|
|
|
debugPayload("(ELL) decrypting", encrypted_bytes);
|
2018-03-05 10:29:25 +00:00
|
|
|
|
|
|
|
uchar iv[16];
|
|
|
|
int i=0;
|
|
|
|
// M-field
|
2020-01-30 14:45:37 +00:00
|
|
|
iv[i++] = t->dll_mfct_b[0]; iv[i++] = t->dll_mfct_b[1];
|
2018-03-05 10:29:25 +00:00
|
|
|
// A-field
|
2020-01-27 08:29:40 +00:00
|
|
|
for (int j=0; j<6; ++j) { iv[i++] = t->dll_a[j]; }
|
2018-03-05 10:29:25 +00:00
|
|
|
// CC-field
|
2020-01-27 08:29:40 +00:00
|
|
|
iv[i++] = t->ell_cc;
|
2018-03-05 10:29:25 +00:00
|
|
|
// SN-field
|
2020-01-27 08:29:40 +00:00
|
|
|
for (int j=0; j<4; ++j) { iv[i++] = t->ell_sn_b[j]; }
|
2018-03-05 10:29:25 +00:00
|
|
|
// FN
|
|
|
|
iv[i++] = 0; iv[i++] = 0;
|
|
|
|
// BC
|
|
|
|
iv[i++] = 0;
|
|
|
|
|
|
|
|
vector<uchar> ivv(iv, iv+16);
|
|
|
|
string s = bin2hex(ivv);
|
2020-01-27 08:29:40 +00:00
|
|
|
debug("(ELL) IV %s\n", s.c_str());
|
2018-03-05 10:29:25 +00:00
|
|
|
|
2020-03-13 09:49:22 +00:00
|
|
|
int block = 0;
|
|
|
|
for (size_t offset = 0; offset < encrypted_bytes.size(); offset += 16)
|
2020-01-27 08:29:40 +00:00
|
|
|
{
|
2020-03-13 09:49:22 +00:00
|
|
|
size_t block_size = 16;
|
|
|
|
if (offset + block_size > encrypted_bytes.size())
|
|
|
|
{
|
|
|
|
block_size = encrypted_bytes.size() - offset;
|
|
|
|
}
|
2018-03-05 10:29:25 +00:00
|
|
|
|
2020-03-13 09:49:22 +00:00
|
|
|
assert(block_size > 0 && block_size <= 16);
|
2018-03-05 10:29:25 +00:00
|
|
|
|
2020-03-13 09:49:22 +00:00
|
|
|
// Generate the pseudo-random bits from the IV and the key.
|
|
|
|
uchar xordata[16];
|
2018-03-05 10:29:25 +00:00
|
|
|
AES_ECB_encrypt(iv, &aeskey[0], xordata, 16);
|
|
|
|
|
2020-03-13 09:49:22 +00:00
|
|
|
// Xor the data with the pseudo-random bits to decrypt into tmp.
|
|
|
|
uchar tmp[block_size];
|
|
|
|
xorit(xordata, &encrypted_bytes[offset], tmp, block_size);
|
|
|
|
|
|
|
|
debug("(ELL) block %d block_size %d offset %zu\n", block, block_size, offset);
|
|
|
|
block++;
|
2018-03-05 10:29:25 +00:00
|
|
|
|
2020-03-13 09:49:22 +00:00
|
|
|
vector<uchar> tmpv(tmp, tmp+block_size);
|
|
|
|
debugPayload("(ELL) decrypted", tmpv);
|
2018-03-05 10:29:25 +00:00
|
|
|
|
2020-03-13 09:49:22 +00:00
|
|
|
decrypted_bytes.insert(decrypted_bytes.end(), tmpv.begin(), tmpv.end());
|
|
|
|
|
|
|
|
incrementIV(iv, sizeof(iv));
|
2018-03-05 10:29:25 +00:00
|
|
|
}
|
2020-03-13 09:49:22 +00:00
|
|
|
debugPayload("(ELL) decrypted", decrypted_bytes);
|
|
|
|
frame.insert(frame.end(), decrypted_bytes.begin(), decrypted_bytes.end());
|
2020-01-27 08:29:40 +00:00
|
|
|
return true;
|
2018-03-05 10:29:25 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
string frameTypeKamstrupC1(int ft) {
|
|
|
|
if (ft == 0x78) return "long frame";
|
|
|
|
if (ft == 0x79) return "short frame";
|
|
|
|
return "?";
|
|
|
|
}
|
|
|
|
|
2020-01-27 08:29:40 +00:00
|
|
|
bool decrypt_TPL_AES_CBC_IV(Telegram *t, vector<uchar> &frame, vector<uchar>::iterator &pos, vector<uchar> &aeskey)
|
2018-11-23 08:04:31 +00:00
|
|
|
{
|
2020-01-27 08:29:40 +00:00
|
|
|
if (aeskey.size() == 0) return true;
|
|
|
|
|
|
|
|
vector<uchar> buffer;
|
|
|
|
buffer.insert(buffer.end(), pos, frame.end());
|
|
|
|
frame.erase(pos, frame.end());
|
|
|
|
debugPayload("(TPL) decrypting", buffer);
|
2018-11-25 11:33:14 +00:00
|
|
|
|
2020-07-09 12:51:28 +00:00
|
|
|
size_t len = buffer.size();
|
|
|
|
|
|
|
|
if (t->tpl_num_encr_blocks)
|
|
|
|
{
|
|
|
|
len = t->tpl_num_encr_blocks*16;
|
|
|
|
}
|
|
|
|
|
|
|
|
debug("(TPL) num encrypted blocks %d (%d bytes and remaining unencrypted %d bytes)\n",
|
|
|
|
t->tpl_num_encr_blocks, len, buffer.size()-len);
|
|
|
|
|
2018-11-23 08:04:31 +00:00
|
|
|
// The content should be a multiple of 16 since we are using AES CBC mode.
|
2020-07-09 12:51:28 +00:00
|
|
|
if (len % 16 != 0)
|
2018-11-23 08:04:31 +00:00
|
|
|
{
|
2020-01-27 08:29:40 +00:00
|
|
|
warning("(TPL) warning: decryption received non-multiple of 16 bytes! "
|
2018-11-23 08:04:31 +00:00
|
|
|
"Got %zu bytes shrinking message to %zu bytes.\n",
|
2020-07-09 12:51:28 +00:00
|
|
|
len, len - len % 16);
|
|
|
|
len -= len % 16;
|
|
|
|
assert (len % 16 == 0);
|
2018-11-23 08:04:31 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
uchar iv[16];
|
|
|
|
int i=0;
|
|
|
|
// M-field
|
2020-01-30 14:45:37 +00:00
|
|
|
iv[i++] = t->dll_mfct_b[0]; iv[i++] = t->dll_mfct_b[1];
|
2018-11-23 08:04:31 +00:00
|
|
|
// A-field
|
2020-01-27 08:29:40 +00:00
|
|
|
for (int j=0; j<6; ++j) { iv[i++] = t->dll_a[j]; }
|
2018-11-23 08:04:31 +00:00
|
|
|
// ACC
|
2020-01-27 08:29:40 +00:00
|
|
|
for (int j=0; j<8; ++j) { iv[i++] = t->tpl_acc; }
|
2018-11-23 08:04:31 +00:00
|
|
|
|
|
|
|
vector<uchar> ivv(iv, iv+16);
|
|
|
|
string s = bin2hex(ivv);
|
2020-01-27 08:29:40 +00:00
|
|
|
debug("(TPL) IV %s\n", s.c_str());
|
2018-11-23 08:04:31 +00:00
|
|
|
|
2020-01-27 08:29:40 +00:00
|
|
|
uchar buffer_data[buffer.size()];
|
|
|
|
memcpy(buffer_data, &buffer[0], buffer.size());
|
|
|
|
uchar decrypted_data[buffer.size()];
|
2020-07-09 12:51:28 +00:00
|
|
|
AES_CBC_decrypt_buffer(decrypted_data, buffer_data, len, &aeskey[0], iv);
|
2018-11-23 08:04:31 +00:00
|
|
|
|
2020-07-09 12:51:28 +00:00
|
|
|
frame.insert(frame.end(), decrypted_data, decrypted_data+len);
|
|
|
|
debugPayload("(TPL) decrypted ", frame, pos);
|
|
|
|
|
|
|
|
if (len < buffer.size())
|
|
|
|
{
|
|
|
|
frame.insert(frame.end(), buffer_data+len, buffer_data+buffer.size());
|
|
|
|
debugPayload("(TPL) appended ", frame, pos);
|
|
|
|
}
|
2020-01-27 08:29:40 +00:00
|
|
|
return true;
|
2018-11-23 08:04:31 +00:00
|
|
|
}
|
2020-01-27 15:53:18 +00:00
|
|
|
|
|
|
|
bool decrypt_TPL_AES_CBC_NO_IV(Telegram *t, vector<uchar> &frame, vector<uchar>::iterator &pos, vector<uchar> &aeskey)
|
|
|
|
{
|
|
|
|
if (aeskey.size() == 0) return true;
|
|
|
|
|
|
|
|
vector<uchar> buffer;
|
|
|
|
buffer.insert(buffer.end(), pos, frame.end());
|
|
|
|
frame.erase(pos, frame.end());
|
|
|
|
debugPayload("(TPL) decrypting", buffer);
|
|
|
|
|
2020-07-09 12:51:28 +00:00
|
|
|
size_t len = buffer.size();
|
|
|
|
|
|
|
|
if (t->tpl_num_encr_blocks)
|
|
|
|
{
|
|
|
|
len = t->tpl_num_encr_blocks*16;
|
|
|
|
}
|
|
|
|
|
|
|
|
debug("(TPL) num encrypted blocks %d (%d bytes and remaining unencrypted %d bytes)\n",
|
|
|
|
t->tpl_num_encr_blocks, len, buffer.size()-len);
|
|
|
|
|
2020-01-27 15:53:18 +00:00
|
|
|
// The content should be a multiple of 16 since we are using AES CBC mode.
|
2020-07-09 12:51:28 +00:00
|
|
|
if (len % 16 != 0)
|
2020-01-27 15:53:18 +00:00
|
|
|
{
|
|
|
|
warning("(TPL) warning: decryption received non-multiple of 16 bytes! "
|
|
|
|
"Got %zu bytes shrinking message to %zu bytes.\n",
|
2020-07-09 12:51:28 +00:00
|
|
|
len, len - len % 16);
|
|
|
|
len -= len % 16;
|
|
|
|
assert (len % 16 == 0);
|
2020-01-27 15:53:18 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
uchar iv[16];
|
|
|
|
memset(iv, 0, sizeof(iv));
|
|
|
|
|
|
|
|
vector<uchar> ivv(iv, iv+16);
|
|
|
|
string s = bin2hex(ivv);
|
|
|
|
debug("(TPL) IV %s\n", s.c_str());
|
|
|
|
|
|
|
|
uchar buffer_data[buffer.size()];
|
|
|
|
memcpy(buffer_data, &buffer[0], buffer.size());
|
|
|
|
uchar decrypted_data[buffer.size()];
|
|
|
|
AES_CBC_decrypt_buffer(decrypted_data, buffer_data, buffer.size(), &aeskey[0], iv);
|
|
|
|
|
|
|
|
frame.insert(frame.end(), decrypted_data, decrypted_data+buffer.size());
|
2020-07-09 12:51:28 +00:00
|
|
|
debugPayload("(TPL) decrypted ", frame, pos);
|
|
|
|
|
|
|
|
if (len < buffer.size())
|
|
|
|
{
|
|
|
|
frame.insert(frame.end(), buffer_data+len, buffer_data+buffer.size());
|
|
|
|
debugPayload("(TPL) appended ", frame, pos);
|
|
|
|
}
|
|
|
|
|
2020-01-27 15:53:18 +00:00
|
|
|
return true;
|
|
|
|
}
|