DRESULT disk_ioctl (
BYTE pdrv, /* Physical drive number (0..) */
BYTE cmd, /* Control code */
void *buff /* Buffer to send/receive control data */
)
{
if (pdrv < CTR_DISK_COUNT)
{
ctr_fatfs_disk *disk = &ctr_fatfs_disks[pdrv];
switch (cmd)
{
case GET_SECTOR_COUNT:
if (!(disk->status & STA_NOINIT))
{
*(DWORD*)buff = disk->sectors;
return RES_OK;
}
return RES_NOTRDY;
case GET_SECTOR_SIZE:
if (!(disk->status & STA_NOINIT))
{
*(WORD*)buff = ctr_io_sector_size(disk->io);
return RES_OK;
}
return RES_NOTRDY;
case GET_BLOCK_SIZE:
*(DWORD*)buff = 1; //Not sure what the block size for SD is...
case CTRL_TRIM:
case CTRL_SYNC:
return RES_OK;
case CTR_SETUP_DISK:
{
ctr_setup_disk_parameters *params = (ctr_setup_disk_parameters*)buff;
ctr_fatfs_disk_initialize(disk, params->io, params->sector_offset, params->sectors);
}
return RES_OK;
default:
return RES_PARERR;
}
return RES_NOTRDY;
}
return RES_PARERR;
}
int ctr_crypto_interface_write_sector(void *io, const void *buffer, size_t buffer_size, size_t sector)
{
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 number_of_sectors = FLOOR(buffer_size, sector_size);
const size_t block_size = AES_BLOCK_SIZE;
size_t sectors_per_block = block_size / sector_size;
if (!sectors_per_block)
sectors_per_block = 1;
if (number_of_sectors)
{
size_t block_sector_lcm = lcm(sector_size, block_size);
uint8_t window_buffer[4 * block_sector_lcm + sectors_per_block * sector_size];
write_window window =
{
.window = window_buffer,
.window_size = sizeof(window_buffer),
.window_offset = 0,
.buffer = buffer,
.buffer_size = buffer_size,
.buffer_offset = 0,
.current_sector = sector
};
setup_window(io, &window, sector, sector_size, block_size);
res = process_window(io, &window, sector_size, block_size, input, output);
while(!res)
{
res = process_window(io, &window, sector_size, block_size, input, output);
}
}
return res == 1 ? 0 : 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;
}
