//helper function. Retrieves an AES block, regardless of whether it is aligned or not with the underlying sector geometry
static inline int get_misaligned_block(ctr_crypto_interface *io, size_t sector, size_t sector_size, size_t block_size, uint8_t *buffer, void (*block_function)(void *io, void *buffer, uint64_t block, size_t block_count))
{
size_t sectors_to_copy_prior = get_chunks_to_complete_relative_chunk_backwards(sector, sector_size, block_size);
size_t sectors_to_copy_after = get_chunks_to_complete_relative_chunk(sector, sector_size, block_size);
if (!sectors_to_copy_after)
sectors_to_copy_after = block_size;
size_t sector_count = sectors_to_copy_prior + sectors_to_copy_after;
size_t buf_size = sector_count * sector_size;
uint8_t buf[buf_size];
int res = ctr_io_read_sector(io->lower_io, buf, buf_size, sector - sectors_to_copy_prior, sector_count);
if (res)
return res;
size_t current_block = get_prev_relative_chunk(sector, sector_size, block_size);
uint64_t block_pos = get_chunk_position(current_block, block_size);
uint64_t block_start_offset = block_pos - get_chunk_position(sector - sectors_to_copy_prior, sector_size);
uint8_t *pos = buf + block_start_offset;
block_function(io, pos, current_block, 1);
memcpy(buffer, pos, block_size);
return res;
}
//process the current window, and advance to the next one.
//Returns a negative number on a failure, a zero when there are still more windows to go, and a 1 when done.
static inline int process_window(ctr_crypto_interface *io, write_window *window, size_t sector_size, size_t block_size, void (*input_function)(void *io, void *buffer, uint64_t block, size_t block_count), void (*output_function)(void *io, void *buffer, uint64_t block, size_t block_count))
{
//FIXME what if we try to read more than there is disk? Return zeros?
size_t sectors_to_read = (window->window_size - window->window_offset) / sector_size;
int res = ctr_io_read_sector(io->lower_io, window->window + window->window_offset, window->window_size - window->window_offset, window->current_sector, sectors_to_read);
if (res)
return -1;
size_t block_start_offset = window->block_offset;
uint8_t *pos = window->window + block_start_offset;
size_t amount_to_copy = window->window_size - window->window_offset;
if (amount_to_copy > window->buffer_size - window->buffer_offset)
amount_to_copy = window->buffer_size - window->buffer_offset;
//In the case that blocks are aligned to sectors, this is not necessary... FIXME
//only process parts that won't be overwritten completely
// from block_start_offset to window->window_offset
size_t blocks_to_process = CEIL(window->window_offset - block_start_offset, block_size);
if (!blocks_to_process) blocks_to_process = 1;
output_function(io, pos, window->block, blocks_to_process);
//now copy data from buffer
memcpy(window->window + window->window_offset, window->buffer + window->buffer_offset, amount_to_copy);
window->buffer_offset += amount_to_copy;
blocks_to_process = CEIL(amount_to_copy + window->window_offset - block_start_offset, block_size);
input_function(io, pos, window->block, blocks_to_process);
//We need to write out the full blocks processed actually, not just the sectors
size_t sectors_processed = (amount_to_copy + window->window_offset) / sector_size;
size_t sectors_to_copy = CEIL(blocks_to_process * block_size, sector_size);
res = ctr_io_write_sector(io->lower_io, window->window, sectors_to_copy * sector_size, window->sector);
if (res)
return -2;
//Copy sector that contain part of next block
update_window(window, sectors_processed);
return window->buffer_offset >= window->buffer_size;
}
DRESULT disk_read (
BYTE pdrv, /* Physical drive number to identify the drive */
BYTE *buff, /* Data buffer to store read data */
DWORD sector, /* Sector address in LBA */
UINT count /* Number of sectors to read */
)
{
DRESULT res = RES_PARERR;
if (pdrv < CTR_DISK_COUNT)
{
ctr_fatfs_disk *disk = &ctr_fatfs_disks[pdrv];
if (!(disk->status & STA_NOINIT))
{
int result = ctr_io_read_sector(disk->io, buff, count * 0x200, sector + disk->sector_offset, count);
res = result ? RES_ERROR : RES_OK;
}
}
return res;
}
//Sets up the window
static inline int setup_window(ctr_crypto_interface *io, write_window *window, size_t sector, size_t sector_size, size_t block_size)
{
//window_size is a multiple of the sector size by definition
//What if sectors_to_copy > window_size?
//That can never happen, since
// window_size == lcm(sector_size, block_size) * 4 + sector_size * sectors_per_block
//so window size should be large enough for at least 4 blocks
size_t sectors_to_copy_prior = get_chunks_to_complete_relative_chunk_backwards(sector, sector_size, block_size);
window->sector = sector - sectors_to_copy_prior;
window->current_sector = sector;
window->sector_size = sector_size;
window->block = get_prev_relative_chunk(window->current_sector, sector_size, block_size);
window->block_offset = get_chunk_position(window->block, block_size) - get_chunk_position(window->sector, sector_size);
window->block_size = block_size;
window->window_offset = sectors_to_copy_prior * sector_size;
int res = ctr_io_read_sector(io->lower_io, window->window, window->window_size, window->sector, sectors_to_copy_prior);
return res;
}
int ctr_crypto_interface_read_sector(void *io, void *buffer, size_t buffer_size, size_t sector, size_t count)
{
int res = 0;
ctr_crypto_interface *crypto_io = io;
const size_t sector_size = ctr_io_sector_size(crypto_io->lower_io);
const size_t block_size = AES_BLOCK_SIZE;
size_t result_count = count < buffer_size/sector_size ? count : buffer_size/sector_size;
void (*block_function)(void *io, void *buffer, uint64_t block, size_t block_count);
block_function = output;
if (result_count)
{
size_t result_size = result_count * sector_size;
uint8_t *current_processed = buffer;
size_t current_block = get_prev_relative_chunk(sector, sector_size, block_size);
res = ctr_io_read_sector(crypto_io->lower_io, buffer, buffer_size, sector, result_count);
if (res)
return res;
//Part 1, deal with misaligned first block
size_t sectors_to_copy_prior = get_chunks_to_complete_relative_chunk_backwards(sector, sector_size, block_size);
if (sectors_to_copy_prior)
{
uint8_t buf[block_size];
res = get_misaligned_block(crypto_io, sector, sector_size, block_size, buf, block_function);
if (res)
return res;
//We now have the full block-- we now need to figure out which part of it to copy
uint64_t sector_pos = get_chunk_position(sector, sector_size);
uint64_t block_pos = get_chunk_position(current_block, block_size);
size_t amount_to_copy = block_size - (sector_pos - block_pos);
amount_to_copy = amount_to_copy < buffer_size ? amount_to_copy : buffer_size;
memcpy(buffer, buf + (sector_pos - block_pos), amount_to_copy);
current_processed += amount_to_copy;
current_block++;
}
//Part 2, Deal with all intermediate blocks
size_t bytes_left = result_size - (size_t)(current_processed - (uint8_t*)buffer);
size_t blocks = FLOOR(bytes_left, block_size);
if (blocks)
{
block_function(io, current_processed, current_block, blocks);
current_block += blocks;
current_processed += blocks * block_size;
bytes_left -= blocks * block_size;
}
//Part 3, deal with the final block
//FIXME what if we need to deal with a block that actually continues past the end of the disk? pad with zero?
if (bytes_left)
{
uint8_t buf[block_size];
//size_t current_sector = get_next_relative_block
size_t block_sector = get_prev_relative_chunk(current_block, block_size, sector_size);
res = get_misaligned_block(crypto_io, block_sector, sector_size, block_size, buf, block_function);
if (res)
return res;
//We now have the full block-- we now need to figure out which part of it to copy
uint64_t sector_pos = get_chunk_position(block_sector, sector_size);
uint64_t block_pos = get_chunk_position(current_block, block_size);
size_t amount_to_copy = block_size - (sector_pos - block_pos);
amount_to_copy = amount_to_copy < bytes_left ? amount_to_copy : bytes_left;
memcpy(current_processed, buf + (sector_pos - block_pos), amount_to_copy);
}
}
return res;
}