key2.c

/*
Implementation of NDS Key2 Encryption
*/

#include <stdio.h>
#include <stdint.h>
#include <string.h>

static uint8_t x[5];
static uint8_t y[5];
static uint8_t seeds[8] = {0xE8, 0x4D, 0x5A, 0xB1, 0x17, 0x8F, 0x99, 0xD5};

void key2_init(uint8_t mm, uint8_t mn, uint8_t nn, uint8_t seed) {
    // prepare seed0
    uint8_t seed0[5];
    seed0[4] = (mm >> 1);
    seed0[3] = (mm << 7) | (mn >> 1);
    seed0[2] = (mn << 7) | (nn >> 1);
    seed0[1] = (nn << 7) | 0x60;
    seed0[0] = seeds[seed];
    
    // prepare seed1 (pre-reversed)
    uint8_t seed1[5] = {0x9d, 0xf0, 0xec, 0x6c, 0x50};
    
    // reverse seed0 into x
    memset(x, 0, sizeof(x));
    
    for (uint8_t bit = 0; bit < 39; bit++) {
        if (seed0[bit >> 3] & (1 << (bit & 7))) {
            x[(38 - bit) >> 3] |= (1 << ((38 - bit) & 7));
        }
    }
    
    // copy seed1 into y
    memcpy(y, seed1, sizeof(seed1));
}

void key2_init_cmd(uint8_t cmd[8], uint8_t seed) {
    // init after receiving the command
    uint8_t mm = (cmd[2] << 4) | (cmd[3] >> 4);
    uint8_t mn = (cmd[3] << 4) | (cmd[4] >> 4);
    uint8_t nn = (cmd[4] << 4) | (cmd[5] >> 4);
    key2_init(mm, mn, nn, seed);
}

uint8_t key2_apply(uint8_t data) {
    uint8_t tmp = ((x[0] >> 5) | (x[1] << 3)) ^ ((x[2] >> 1) | (x[3] << 7)) ^ ((x[2] >> 2) | (x[3] << 6)) ^ ((x[3] >> 7) | (x[4] << 1));
        
    // roll values to the left
    x[4] = x[3] & 0x7F; // discard last bit
    x[3] = x[2];
    x[2] = x[1];
    x[1] = x[0];
    x[0] = tmp;    
    
    // same goes for seed 1
    tmp = ((y[0] >> 5) | (y[1] << 3)) ^ ((y[2] >> 7) | (y[3] << 1)) ^ ((y[2] >> 2) | (y[3] << 6)) ^ ((y[3] >> 7) | (y[4] << 1));
    y[4] = y[3] & 0x7F; // discard last bit
    y[3] = y[2];
    y[2] = y[1];
    y[1] = y[0];
    y[0] = tmp;
    
    data = (data ^ x[0] ^ y[0]) & 0xFF;
    return data;
}