Skip to content

Commit

Permalink
Various improvements taken over from D9
Browse files Browse the repository at this point in the history
  • Loading branch information
@d0k3
d0k3 committed Aug 23, 2016
1 parent 7c060e0 commit d15fa62
Show file tree
Hide file tree
Showing 7 changed files with 194 additions and 110 deletions.
11 changes: 7 additions & 4 deletions source/common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,14 +23,17 @@
#define min(a,b) \
(((a) < (b)) ? (a) : (b))
#define getbe16(d) \
(((d)[0]<<8) | (d)[1])
((((u8*)d)[0]<<8) | ((u8*)d)[1])
#define getbe32(d) \
((((u32) getbe16(d))<<16) | ((u32) getbe16(d+2)))
#define getbe64(d) \
((((u64) getbe32(d))<<32) | ((u64) getbe32(d+4)))
#define getle16(d) (*((u16*) (d)))
#define getle32(d) (*((u32*) (d)))
#define getle64(d) (*((u64*) (d)))
#define getle16(d) \
((((u8*)d)[1]<<8) | ((u8*)d)[0])
#define getle32(d) \
((((u32) getle16(d+2))<<16) | ((u32) getle16(d)))
#define getle64(d) \
((((u64) getle32(d+4))<<32) | ((u64) getle32(d)))
#define align(v,a) \
(((v) % (a)) ? ((v) + (a) - ((v) % (a))) : (v))

Expand Down
157 changes: 102 additions & 55 deletions source/decryptor/aes.c
View file
Original file line number Diff line number Diff line change
@@ -1,61 +1,66 @@
/* original version by megazig */
#include "aes.h"

void setup_aeskeyX(u8 keyslot, void* keyx)
//FIXME some things make assumptions about alignemnts!

void setup_aeskeyX(uint8_t keyslot, void* keyx)
{
u32 * _keyx = (u32*)keyx;
uint32_t * _keyx = (uint32_t*)keyx;
*REG_AESKEYCNT = (*REG_AESKEYCNT >> 6 << 6) | keyslot | 0x80;
if (keyslot > 3) {
*REG_AESCNT = (*REG_AESCNT | (AES_CNT_INPUT_ENDIAN | AES_CNT_INPUT_ORDER));
*REG_AESKEYXFIFO = _keyx[0];
*REG_AESKEYXFIFO = _keyx[1];
*REG_AESKEYXFIFO = _keyx[2];
*REG_AESKEYXFIFO = _keyx[3];
} else {
vu32* reg_aeskeyx = REG_AESKEY0123 + (((0x30*keyslot) + 0x10)/4);
uint32_t old_aescnt = *REG_AESCNT;
volatile uint32_t* reg_aeskeyx = REG_AESKEY0123 + (((0x30u * keyslot) + 0x10u)/4u);
*REG_AESCNT = (*REG_AESCNT & ~(AES_CNT_INPUT_ENDIAN | AES_CNT_INPUT_ORDER));
for (u32 i = 0; i < 4; i++)
for (uint32_t i = 0; i < 4u; i++)
reg_aeskeyx[i] = _keyx[i];
*REG_AESCNT = old_aescnt;
}
}

void setup_aeskeyY(u8 keyslot, void* keyy)
void setup_aeskeyY(uint8_t keyslot, void* keyy)
{
u32 * _keyy = (u32*)keyy;
uint32_t * _keyy = (uint32_t*)keyy;
*REG_AESKEYCNT = (*REG_AESKEYCNT >> 6 << 6) | keyslot | 0x80;
if (keyslot > 3) {
*REG_AESCNT = (*REG_AESCNT | (AES_CNT_INPUT_ENDIAN | AES_CNT_INPUT_ORDER));
*REG_AESKEYYFIFO = _keyy[0];
*REG_AESKEYYFIFO = _keyy[1];
*REG_AESKEYYFIFO = _keyy[2];
*REG_AESKEYYFIFO = _keyy[3];
} else {
vu32* reg_aeskeyy = REG_AESKEY0123 + (((0x30*keyslot) + 0x20)/4);
uint32_t old_aescnt = *REG_AESCNT;
volatile uint32_t* reg_aeskeyy = REG_AESKEY0123 + (((0x30u * keyslot) + 0x20u)/4u);
*REG_AESCNT = (*REG_AESCNT & ~(AES_CNT_INPUT_ENDIAN | AES_CNT_INPUT_ORDER));
for (u32 i = 0; i < 4; i++)
for (uint32_t i = 0; i < 4u; i++)
reg_aeskeyy[i] = _keyy[i];
*REG_AESCNT = old_aescnt;
}
}

void setup_aeskey(u8 keyslot, void* key)
void setup_aeskey(uint8_t keyslot, void* key)
{
u32 * _key = (u32*)key;
uint32_t * _key = (uint32_t*)key;
*REG_AESKEYCNT = (*REG_AESKEYCNT >> 6 << 6) | keyslot | 0x80;
if (keyslot > 3) {
*REG_AESCNT = (*REG_AESCNT | (AES_CNT_INPUT_ENDIAN | AES_CNT_INPUT_ORDER));
*REG_AESKEYFIFO = _key[0];
*REG_AESKEYFIFO = _key[1];
*REG_AESKEYFIFO = _key[2];
*REG_AESKEYFIFO = _key[3];
} else {
vu32* reg_aeskey = REG_AESKEY0123 + ((0x30*keyslot)/4);
uint32_t old_aescnt = *REG_AESCNT;
volatile uint32_t* reg_aeskey = REG_AESKEY0123 + ((0x30u * keyslot)/4u);
*REG_AESCNT = (*REG_AESCNT & ~(AES_CNT_INPUT_ENDIAN | AES_CNT_INPUT_ORDER));
for (u32 i = 0; i < 4; i++)
for (uint32_t i = 0; i < 4u; i++)
reg_aeskey[i] = _key[i];
*REG_AESCNT = old_aescnt;
}
}

void use_aeskey(u32 keyno)
void use_aeskey(uint32_t keyno)
{
if (keyno > 0x3F)
return;
Expand All @@ -65,23 +70,23 @@ void use_aeskey(u32 keyno)

void set_ctr(void* iv)
{
u32 * _iv = (u32*)iv;
uint32_t * _iv = (uint32_t*)iv;
*REG_AESCNT = (*REG_AESCNT) | AES_CNT_INPUT_ENDIAN | AES_CNT_INPUT_ORDER;
*(REG_AESCTR + 0) = _iv[3];
*(REG_AESCTR + 1) = _iv[2];
*(REG_AESCTR + 2) = _iv[1];
*(REG_AESCTR + 3) = _iv[0];
}

void add_ctr(void* ctr, u32 carry)
void add_ctr(void* ctr, uint32_t carry)
{
u32 counter[4];
u8 *outctr = (u8 *) ctr;
u32 sum;
uint32_t counter[4];
uint8_t *outctr = (uint8_t *) ctr;
uint32_t sum;
int32_t i;

for(i=0; i<4; i++) {
counter[i] = (outctr[i*4+0]<<24) | (outctr[i*4+1]<<16) | (outctr[i*4+2]<<8) | (outctr[i*4+3]<<0);
for(i = 0; i < 4; i++) {
counter[i] = ((uint32_t)outctr[i*4+0]<<24) | ((uint32_t)outctr[i*4+1]<<16) | ((uint32_t)outctr[i*4+2]<<8) | ((uint32_t)outctr[i*4+3]<<0);
}

for(i=3; i>=0; i--)
Expand All @@ -105,10 +110,29 @@ void add_ctr(void* ctr, u32 carry)
}
}

void aes_decrypt(void* inbuf, void* outbuf, size_t size, u32 mode)
void ctr_decrypt(void *inbuf, void *outbuf, size_t size, uint32_t mode, uint8_t *ctr)
{
u32 in = (u32)inbuf;
u32 out = (u32)outbuf;
size_t blocks_left = size;
size_t blocks;
uint8_t *in = inbuf;
uint8_t *out = outbuf;

while (blocks_left)
{
set_ctr(ctr);
blocks = (blocks_left >= 0xFFFF) ? 0xFFFF : blocks_left;
aes_decrypt(in, out, blocks, mode);
add_ctr(ctr, blocks);
in += blocks * AES_BLOCK_SIZE;
out += blocks * AES_BLOCK_SIZE;
blocks_left -= blocks;
}
}

void aes_decrypt(void* inbuf, void* outbuf, size_t size, uint32_t mode)
{
uint8_t *in = inbuf;
uint8_t *out = outbuf;
size_t block_count = size;
size_t blocks;
while (block_count != 0)
Expand All @@ -130,29 +154,33 @@ void aes_decrypt(void* inbuf, void* outbuf, size_t size, u32 mode)
void aes_cmac(void* inbuf, void* outbuf, size_t size)
{
// only works for full blocks
u32 zeroes[4] = { 0 };
u32 xorpad[4] = { 0 };
u32 mode = AES_CBC_ENCRYPT_MODE | AES_CNT_INPUT_ORDER | AES_CNT_OUTPUT_ORDER |
uint32_t zeroes[4] __attribute__((aligned(32))) = { 0 };
uint32_t xorpad[4] __attribute__((aligned(32))) = { 0 };
uint32_t mode = AES_CBC_ENCRYPT_MODE | AES_CNT_INPUT_ORDER | AES_CNT_OUTPUT_ORDER |
AES_CNT_INPUT_ENDIAN | AES_CNT_OUTPUT_ENDIAN;
u32* out = (u32*) outbuf;
u32* in = (u32*) inbuf;
uint32_t* out = (uint32_t*) outbuf;
uint32_t* in = (uint32_t*) inbuf;

// create xorpad for last block
set_ctr(zeroes);
aes_decrypt(xorpad, xorpad, 1, mode);
for (u32 i = 0; i < 4; i++) {
if (i && (xorpad[i] >> 31))
xorpad[i-i] |= 1;
xorpad[i] <<= 1;
}
char* xorpadb = (void*) xorpad;
char finalxor = (xorpadb[0] & 0x80) ? 0x87 : 0x00;
for (uint32_t i = 0; i < 15; i++) {
xorpadb[i] <<= 1;
xorpadb[i] |= xorpadb[i+1] >> 7;
}
xorpadb[15] <<= 1;
xorpadb[15] ^= finalxor;

// process blocks
memset(outbuf, 0, 16);
for (uint32_t i = 0; i < 4; i++)
out[i] = 0;
while (size-- > 0) {
for (u32 i = 0; i < 4; i++)
for (uint32_t i = 0; i < 4; i++)
out[i] ^= *(in++);
if (!size) { // last block
for (u32 i = 0; i < 4; i++)
for (uint32_t i = 0; i < 4; i++)
out[i] ^= xorpad[i];
}
set_ctr(zeroes);
Expand All @@ -162,60 +190,79 @@ void aes_cmac(void* inbuf, void* outbuf, size_t size)

void aes_fifos(void* inbuf, void* outbuf, size_t blocks)
{
u32 in = (u32)inbuf;
if (!in) return;
if (!inbuf || !outbuf) return;

uint8_t *in = inbuf;
uint8_t *out = outbuf;

u32 out = (u32)outbuf;
size_t curblock = 0;
while (curblock != blocks)
{
while (aescnt_checkwrite());

u32 ii = 0;
for (ii = in; ii != in + AES_BLOCK_SIZE; ii += 4)
size_t blocks_to_read = blocks - curblock > 4 ? 4 : blocks - curblock;

for (size_t wblocks = 0; wblocks < blocks_to_read; ++wblocks)
for (uint8_t *ii = in + AES_BLOCK_SIZE * wblocks; ii != in + (AES_BLOCK_SIZE * (wblocks + 1)); ii += 4)
{
set_aeswrfifo( *(u32*)(ii) );
uint32_t data = ii[0];
data |= (uint32_t)(ii[1]) << 8;
data |= (uint32_t)(ii[2]) << 16;
data |= (uint32_t)(ii[3]) << 24;
set_aeswrfifo(data);
}

if (out)
{
while (aescnt_checkread()) ;
for (ii = out; ii != out + AES_BLOCK_SIZE; ii += 4)
for (size_t rblocks = 0; rblocks < blocks_to_read; ++rblocks)
{
*(u32*)ii = read_aesrdfifo();
while (aescnt_checkread()) ;
for (uint8_t *ii = out + AES_BLOCK_SIZE * rblocks; ii != out + (AES_BLOCK_SIZE * (rblocks + 1)); ii += 4)
{
uint32_t data = read_aesrdfifo();
ii[0] = data;
ii[1] = data >> 8;
ii[2] = data >> 16;
ii[3] = data >> 24;
}
}
}
curblock++;

in += blocks_to_read * AES_BLOCK_SIZE;
out += blocks_to_read * AES_BLOCK_SIZE;
curblock += blocks_to_read;
}
}

void set_aeswrfifo(u32 value)
void set_aeswrfifo(uint32_t value)
{
*REG_AESWRFIFO = value;
}

u32 read_aesrdfifo(void)
uint32_t read_aesrdfifo(void)
{
return *REG_AESRDFIFO;
}

u32 aes_getwritecount()
uint32_t aes_getwritecount()
{
return *REG_AESCNT & 0x1F;
}

u32 aes_getreadcount()
uint32_t aes_getreadcount()
{
return (*REG_AESCNT >> 5) & 0x1F;
}

u32 aescnt_checkwrite()
uint32_t aescnt_checkwrite()
{
size_t ret = aes_getwritecount();
return (ret > 0xF);
}

u32 aescnt_checkread()
uint32_t aescnt_checkread()
{
size_t ret = aes_getreadcount();
return (ret <= 3);
}

Loading

0 comments on commit d15fa62

Please sign in to comment.