key1.c

/*
Implementation of NDS Key1 Encryption
*/

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

#ifdef __AVR__
#include <avr/pgmspace.h>
#endif

static uint32_t keycode[3];
static uint32_t* keybuf = NULL;
static uint8_t keypgm = 0;

#ifdef __AVR__
static uint32_t get_value(uint32_t* res) {
    if (keypgm) {
        return pgm_read_dword(res);
    } else {
        return *res;
    }
}
#else
#define get_value(res) (*(res))
#endif

static uint32_t bswap_32bit(uint32_t value) {
	uint32_t res = 0;
    
    res |= (value & 0xFF) << 24;
    res |= ((value >> 8) & 0xFF) << 16;
    res |= ((value >> 16) & 0xFF) << 8;
    res |= ((value >> 24) & 0xFF);
    
    return res;
}

static uint32_t read_u32(const uint8_t* data) {
    uint32_t res = 0;
    
    res |= (uint32_t)data[0];
    res |= (uint32_t)data[1] << 8;
    res |= (uint32_t)data[2] << 16;
    res |= (uint32_t)data[3] << 24;
    
    return res;
}

static void export_u32(uint32_t value, uint8_t* data) {
    data[0] = value;
    data[1] = value >> 8;
    data[2] = value >> 16;
    data[3] = value >> 24;
}

static void encrypt_64bit(uint32_t* b0, uint32_t* b1) {
    uint32_t y = *b0;
    uint32_t x = *b1;
    
    for (int i = 0; i < 0x10; i++) {
        uint32_t z = get_value(&keybuf[i]) ^ x;
        x = get_value(&keybuf[0x12 + ((z >> 24) & 0xFF)]);
        x += get_value(&keybuf[0x112 + ((z >> 16) & 0xFF)]);
        x ^= get_value(&keybuf[0x212 + ((z >> 8) & 0xFF)]);
        x += get_value(&keybuf[0x312 + (z & 0xFF)]);
        x ^= y;
        y = z;
    }
    
    *b0 = x ^ get_value(&keybuf[0x10]);
    *b1 = y ^ get_value(&keybuf[0x11]);
}

static void decrypt_64bit(uint32_t* b0, uint32_t* b1) {
    uint32_t y = *b0;
    uint32_t x = *b1;
    
    for (int i = 0x11; i > 0x01; i--) {
        uint32_t z = get_value(&keybuf[i]) ^ x;
        x = get_value(&keybuf[0x12 + ((z >> 24) & 0xFF)]);
        x += get_value(&keybuf[0x112 + ((z >> 16) & 0xFF)]);
        x ^= get_value(&keybuf[0x212 + ((z >> 8) & 0xFF)]);
        x += get_value(&keybuf[0x312 + (z & 0xFF)]);
        x ^= y;
        y = z;
    }
    
    *b0 = x ^ get_value(&keybuf[0x01]);
    *b1 = y ^ get_value(&keybuf[0x00]);
}

static void apply_keycode(int modulo) {
    encrypt_64bit(&keycode[1], &keycode[2]);
    encrypt_64bit(&keycode[0], &keycode[1]);
    
    uint32_t scratch[2] = {0, 0};
    for (int i = 0; i < 0x12; i++) {
        keybuf[i] ^= bswap_32bit(keycode[i % (modulo / 4)]);
    }
    
    for (int i = 0; i < 0x412; i += 2) {
        encrypt_64bit(&scratch[0], &scratch[1]);
        keybuf[i] = scratch[1];
        keybuf[i+1] = scratch[0];
    }
}

static void init_keycode(uint32_t idcode, int level, int modulo, const uint8_t* key) {
    memset(keybuf, 0, sizeof(int) * 0x412);
    
    for (int i = 0; i < 0x412; i++) {
        keybuf[i] = read_u32(&key[i*4]);
    }
    
    keycode[0] = idcode;
    keycode[1] = idcode / 2;
    keycode[2] = (idcode * 2);
    
    if (level >= 1) {
        apply_keycode(modulo);
    }
    if (level >= 2) {
        apply_keycode(modulo);
    }
    if (level >= 3) {
        keycode[1] *= 2;
        keycode[2] /= 2;
        
        apply_keycode(modulo);
    }
}

void key1_init(uint32_t idcode, int level, int modulo, const uint8_t* key) {
    if (keypgm == 0 && keybuf != NULL) {
        free(keybuf);
    }
    keypgm = 0;
    keybuf = malloc(sizeof(int) * 0x412);
    init_keycode(idcode, level, modulo, key);
}

#ifdef __AVR__
void key1_init_pgm(const uint32_t* keybufpgm) {
    if (keypgm == 0 && keybuf != NULL) {
        free(keybuf);
    }
    keypgm = 1;
    keybuf = keybufpgm;
}
#endif

void key1_encrypt_cmd(uint8_t cmd[8]) {
    uint32_t b0 = bswap_32bit(read_u32(&cmd[4]));
    uint32_t b1 = bswap_32bit(read_u32(&cmd[0]));
    encrypt_64bit(&b0, &b1);
    export_u32(bswap_32bit(b0), &cmd[4]);
    export_u32(bswap_32bit(b1), &cmd[0]);
}

void key1_decrypt_cmd(uint8_t cmd[8]) {
    uint32_t b0 = bswap_32bit(read_u32(&cmd[4]));
    uint32_t b1 = bswap_32bit(read_u32(&cmd[0]));
    decrypt_64bit(&b0, &b1);
    export_u32(bswap_32bit(b0), &cmd[4]);
    export_u32(bswap_32bit(b1), &cmd[0]);
}

void key1_encrypt_data(uint8_t data[8]) {
    uint32_t b0 = read_u32(&data[0]);
    uint32_t b1 = read_u32(&data[4]);
    encrypt_64bit(&b0, &b1);
    export_u32(b0, &data[0]);
    export_u32(b1, &data[4]);
}

void key1_decrypt_data(uint8_t data[8]) {
    uint32_t b0 = read_u32(&data[0]);
    uint32_t b1 = read_u32(&data[4]);
    decrypt_64bit(&b0, &b1);
    export_u32(b0, &data[0]);
    export_u32(b1, &data[4]);
}