ESP 8266; Mbedtls Libary about esp-smartmeter-reader HOT 19 CLOSED

//
// Smartmeter Datenpaket Entschluesselungs Testprogramm
//
#include <stdio.h>
#include <stdint.h>
#include <memory.h>
#include <openssl/aes.h>
#include <openssl/rand.h>
#include <openssl/hmac.h>
#include <openssl/buffer.h>

// Schluessel aus dem Kundenportal der Wiener Netze
uint8_t Key[16] = {0x30, 0xCF, 0x95, ....};

// Beispiel
unsigned char MBusData[] = {
	0x7E,
	0xA0,
	0x67,
	0xCE,
	0xFF,
	0x03,
	0x13,
	0x38,0xBD,
	0xE6,
	0xE7,
	0x00,
	0xDB,
	0x08,
	0x4C,0x47,0x5A,0x??,0x??,0x??,0x??,0x??,
	0x4F,
	0x20,
	0x00,0x20,0xE4,0x10,
	0x27,0xDE,0xCF,0x3D,0x4D,0x8D,0xA8,0x00,0xF3,0x20,0x6C,0x16,0x04,0xA0,0xEC,0x9B,0xB0,0x32,0x56,0x10,0x28,0x48,0x72,0x69,0xAF,0x53,0x05,0xE0,0x1A,0x9E,0x1E,0xE1,0x67,0x73,0xD9,0x7C,0x48,0xC5,0x83,0xF1,0x7F,0x7D,0x3D,0x8C,0xAC,0xDF,0x58,0x35,0x5A,0x8B,0x74,0xE8,0x54,0xFA,0xFB,0x37,0x53,0x9F,0xE5,0x2B,0x68,0x95,0xAE,0xBE,0x23,0x8A,0x71,0x0C,0x80,0xF0,0xC6,0xB5,0x60,0x6B,
	0x5D,0xBB,
	0x7E};

// Strukturen der Datenpakete
#pragma pack(1)
typedef uint32_t  TYP_PacketNumber;
typedef struct
{
    uint16_t Year;
    uint8_t Month;
    uint8_t Day;
    uint8_t DayOfWeek;
    uint8_t Hour;
    uint8_t Minute;
    uint8_t Second;
    uint32_t Unknown;
} TYP_TimeStamp;
typedef uint16_t TYP_Unk1;
typedef uint32_t TYP_32BitVal;
typedef uint8_t TYPID;

enum
{
    TYPID_Unk1 = 0x02,
    TYPID_32BitVal = 0x06,
    TYPID_TimeStamp = 0x0C,
    TYPID_PacketNumber = 0x0F
};

typedef struct
{
    TYPID Type;
    TYP_PacketNumber Data;
} DS_PacketNumber;

typedef struct
{
    TYPID Type;
    TYP_TimeStamp Data;
} DS_TimeStamp;

typedef struct
{
    TYPID Type;
    TYP_Unk1 Data;
} DS_Unk1;

typedef struct
{
    TYPID Type;
    TYP_32BitVal Data;
} DS_32BitVal;

// Variante 3
// https://oesterreichsenergie.at/fileadmin/user_upload/Smart_Meter-Plattform/20200201_Konzept_Kundenschnittstelle_SM.pdf
typedef struct
{
    DS_PacketNumber PacketNumber;
    DS_TimeStamp TimeStamp1;
    DS_Unk1 Unk1;
    DS_TimeStamp TimeStamp2;
    DS_32BitVal pA;    // Wirkenergie +A Wh
    DS_32BitVal nA;   // Wirkenergie -A Wh
    DS_32BitVal pR;   // Blindenergie +R varh
    DS_32BitVal nR;   // Blindenergie -R varh
    DS_32BitVal pP;   // Momentanleistung +P W
    DS_32BitVal nP;   // Momentanleistung -P W
    DS_32BitVal pQ;   // Momentanleistung +Q var
    DS_32BitVal nQ;   // Momentanleistung -Q var
} PAYLOAD_PACKET;

typedef struct {

    uint8_t flag;               // Opening flag (0x7E)
    uint8_t type;               // Frame format type
    uint8_t length;             // Length of packet
    uint8_t client_nr;          // Client number?
    uint8_t dst_address;        // Destination address
    uint8_t src_address;        // Source address
    uint8_t control;            // Control field
    uint16_t hcs;               // Header check sequence, CRC-16/X25 (Byte 2-7)
    uint8_t dst_lsap;           // Destination LSAP
    uint8_t src_lsap;           // Source LSAP
    uint8_t llc_quality;        // LLC quality
    uint8_t llc_sdu;            // LLC service data unit
    uint8_t len_sys_t;          // Length of system title
    uint8_t sys_t[8];           // System title
    uint8_t len_content;        // Length of ciphered content
    uint8_t sc_e;               // SC-E: bit 5=1 = Encryption only
    uint32_t inv_cnt;           // Invocation counter
    PAYLOAD_PACKET data;        // Crypted payload (74 bytes)
    uint16_t fcs;               // Frame check sequence
    uint8_t end;                // End flag (0x7E)
} DATA_FRAME;

#pragma pack()


//
// CRC validierung der Pakete
//
uint16_t HdlcCrcReflect(uint16_t crc, int bitnum)
{

  // reflects the lower 'bitnum' bits of 'crc'

  uint16_t i, j = 1, crcout = 0;

  for (i = (uint16_t)1 << (bitnum - 1); i; i >>= 1)
  {
    if (crc & i)
      crcout |= j;
    j <<= 1;
  }
  return (crcout);
}

#define order 16
#define polynom 0x1021
#define crcinit 0xffff
#define crcxor 0xffff

// http://www.zorc.breitbandkatze.de/crc.html
// http://www.zorc.breitbandkatze.de/crctester.c
uint16_t HdlcCrcBitByBit(unsigned char *p, uint16_t len)
{
  uint16_t crchighbit;
  uint16_t i, j, c, bit, crc;
  uint16_t crcmask;

  crcmask = ((((uint16_t)1 << (order - 1)) - 1) << 1) | 1;
  crchighbit = (uint16_t)1 << (order - 1);

  crc = crcinit;
  for (i = 0; i < order; i++)
  {

    bit = crc & 1;
    if (bit)
      crc ^= polynom;
    crc >>= 1;
    if (bit)
      crc |= crchighbit;
  }

  // bit by bit algorithm with augmented zero bytes.
  // does not use lookup table, suited for polynom orders between 1...32.

  for (i = 0; i < len; i++)
  {

    c = (uint16_t)*p++;
    c = HdlcCrcReflect(c, 8);

    for (j = 0x80; j; j >>= 1)
    {

      bit = crc & crchighbit;
      crc <<= 1;
      if (c & j)
        crc |= 1;
      if (bit)
        crc ^= polynom;
    }
  }

  for (i = 0; i < order; i++)
  {
    bit = crc & crchighbit;
    crc <<= 1;
    if (bit)
      crc ^= polynom;
  }

  crc = HdlcCrcReflect(crc, order);
  crc ^= crcxor;
  crc &= crcmask;

  return (crc);
}

int CheckValues(uint8_t *data, size_t dataSize)
{
  uint16_t crc = HdlcCrcBitByBit(data, dataSize - 2); // -2 because last 2 bytes are the checksum I have to compare it to
  uint8_t hiByte = (crc & 0xff00) >> 8;
  uint8_t loByte = (crc & 0xff);

  return hiByte == data[dataSize - 1] && loByte == data[dataSize - 2];
}




int main(int argc, char **argv)
{
	AES_KEY key;
	unsigned char ecount_buf[AES_BLOCK_SIZE] = {0};
	int num = 0;
	unsigned int i;
	DATA_FRAME *fullData = (DATA_FRAME*)MBusData;
    // Initialisation Vector zusammenbaun:
    uint8_t iv[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02};
    uint8_t plainText[sizeof(fullData->data)]={0};

	// Rahmen-CRC pruefen
	printf ("Rahmen-Pruefsumme des Pakets ist %s\n", (CheckValues((unsigned char*)&fullData->type, sizeof(DATA_FRAME)-2)?"gueltig":"UNGUELTIG!!!"));

    // 8 Bytes von 13 (=system-title)
    memcpy(&iv[0], fullData->sys_t, 8);
    // 4 Bytes von 23 (=nonce). INFO: Nonce wird bei jedem Paket um 1 groesser (=increment counter)
    memcpy(&iv[8], &fullData->inv_cnt, 4);

    // Entschluesseln
	AES_set_encrypt_key(Key, 128, &key);
    AES_ctr128_encrypt((unsigned char*)&fullData->data, plainText, sizeof(fullData->data), &key, iv, ecount_buf, &num);
	//memcpy(plainText, &fullData->data, sizeof(fullData->data));

	printf ("Entschluesseltes Datenpaket: ");
	for (i=0; i<sizeof(plainText); i++) printf ("%02X ", plainText[i]);
	printf("\n\n");

	if (fullData->data.PacketNumber.Type != TYPID_PacketNumber)
		fprintf(stderr, "Entschluesselung fehlgeschlagen? Falscher Paketstart\n");

}