keeloq.cpp

#include "keeloq.h"
#include <Arduino.h>
#include <vector>

const int PIN = D3;//7;//19;
const int Te = 450;// should be 400, but the fuck do I know :)

uint8_t mkey[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};  // Manufacturer key -> See README.md
uint32_t serial = 0xBADBEEF;                                        // Serial number (28-bit)
int vlow = 1;                                                       // Battery low status (0 or 1)
uint16_t disc = serial & 0x0FFF;                                    // Discrimination bits (12-bit)

#define KEELOQ_NROUNDS  528
#define NLF_LOOKUP_CONSTANT 0x3a5c742e

uint8_t nlf(uint8_t d){
    return (((uint32_t)(NLF_LOOKUP_CONSTANT) >> d) & 0x1);
}

void keeloqEncrypt(uint8_t *key, uint32_t *data, const uint16_t nrounds){
    uint32_t x;
    uint16_t loop;
    uint8_t o, nlfInput,k,ki;

    for (loop = 0; loop < nrounds; loop++){
        nlfInput = (((*data >> 31) & 0x1) << 4) | (((*data >> 26) & 0x1) << 3) | (((*data >> 20) & 0x1) << 2) | (((*data >> 9) & 0x1) << 1) | ((*data >> 1) & 0x1);

        o = nlf(nlfInput);
        ki=loop % 64;
        k=key[ki/8]>>(ki%8);
        x =k ^ (*data >> 16) ^ *data ^ o;
        *data = (*data >> 1) | (x << 31);
    }
}

void keeloqDecrypt(uint8_t *key, uint32_t *data,const uint16_t nrounds){
    uint32_t x;
    uint16_t loop;
    uint8_t o, nlfInput,k,ki;

    for (loop = 0; loop < nrounds; loop++){
        nlfInput = (((*data >> 30) & 0x1) << 4) | (((*data >> 25) & 0x1) << 3) | (((*data >> 19) & 0x1) << 2) | (((*data >> 8) & 0x1) << 1) | (*data & 0x1);

        o = nlf(nlfInput);
        ki=(uint16_t)(15 - loop) % 64;
        k=key[ki/8]>>(ki%8);
        x = k ^ (*data >> 31) ^ (*data >> 15) ^ o;
        *data = (*data << 1) | (x&1);
    }
}

void keeloqGenNormalKey(uint8_t *key, uint8_t *mf_key, uint32_t ser){
    uint32_t temp;

    ser &= 0x0fffffff;

    temp = ser;
    temp |= 0x20000000;
    keeloqDecrypt(mf_key, &temp, KEELOQ_NROUNDS);
    memccpy(&key[0],&temp,1,4);

    temp = ser;
    temp |= 0x60000000;
    keeloqDecrypt(mf_key,&temp,KEELOQ_NROUNDS);
    memccpy(&key[4],&temp,1,4);
}

void keeloqGenSecureKey(uint8_t *key, uint8_t *mf_key, uint32_t seed, uint32_t ser){
    keeloqDecrypt(mf_key, &seed, KEELOQ_NROUNDS);
    memccpy(&key[0],&seed,1,4);

    ser &= 0x0fffffff;
    keeloqDecrypt(mf_key, &ser, KEELOQ_NROUNDS);
    memccpy(&key[4],&ser,1,4);
}

std::pair<int, int> calcCrc2(const std::vector<int>& bits_0_to_64) {
    int crc0 = 0;
    int crc1 = 0;
    for (int din : bits_0_to_64) {
        int new_crc1 = crc0 ^ din;
        int new_crc0 = new_crc1 ^ crc1;
        crc0 = new_crc0 & 1;
        crc1 = new_crc1 & 1;
    }
    return {crc0, crc1};
}

void sendSignal(const uint8_t func, const uint16_t syncCounter, const uint8_t repeats) {
    uint8_t dkey[8];
    keeloqGenNormalKey(dkey, mkey, serial);

    uint32_t data = ((uint32_t)func << 28) | (disc << 16) | syncCounter;

    keeloqEncrypt(dkey, &data, KEELOQ_NROUNDS);

    // Build bits vector (LSB first = bit 0 is first transmitted)
    std::vector<int> bits(67);

    // Hopping code bits 0–31 (LSB first)
    for (int i = 0; i < 32; i++) {
        bits[i] = (data >> i) & 1;
    }

    // Fixed part: serial 28 bits (bits 32–59)
    for (int i = 0; i < 28; i++) {
        bits[32 + i] = (serial >> i) & 1;
    }

    // Buttons 4 bits (bits 60–63)
    for (int i = 0; i < 4; i++) {
        bits[60 + i] = (func >> i) & 1;
    }

    bits[64] = vlow;  // VLOW

    // CRC over 0–64
    std::vector<int> bits_0_to_64(bits.begin(), bits.begin() + 65);
    auto crc = calcCrc2(bits_0_to_64);
    bits[65] = crc.first;
    bits[66] = crc.second;

    pinMode(PIN, OUTPUT);
    digitalWrite(PIN, LOW);   // Idle low
    for(uint8_t i=0;i<repeats;++i){
        // Preamble
        for (int i = 0; i < 9; i++) {
            digitalWrite(PIN, HIGH);
            delayMicroseconds(Te);
            digitalWrite(PIN, LOW);
            delayMicroseconds(Te*2);
        }

        // Sync
        digitalWrite(PIN, HIGH);
        delayMicroseconds(10 * Te);
        // Header
        digitalWrite(PIN, LOW);
        delayMicroseconds(10 * Te);

        // Data
        for (int i = 0; i < 67; i++) {
            int b = bits[i];

            digitalWrite(PIN, HIGH);

            if (b == 1) {
                delayMicroseconds(Te);
                digitalWrite(PIN, LOW);
                delayMicroseconds(2*Te);
            } else {
                delayMicroseconds(2*Te);
                digitalWrite(PIN, LOW);
                delayMicroseconds(Te);
            }
        }

        // Guard time
        delayMicroseconds(18 * Te);
    }
}