diff --git a/libraries/wutdevoptab/devoptab_fs_read.c b/libraries/wutdevoptab/devoptab_fs_read.c index fb3b2559..8047ff14 100644 --- a/libraries/wutdevoptab/devoptab_fs_read.c +++ b/libraries/wutdevoptab/devoptab_fs_read.c @@ -1,62 +1,64 @@ #include "devoptab_fs.h" +#include + ssize_t __wut_fs_read(struct _reent *r, void *fd, char *ptr, size_t len) { - FSStatus status = 0; - FSCmdBlock cmd; - size_t bytesRead = 0; - size_t partSize[3]; - - __wut_fs_file_t *file = (__wut_fs_file_t *) fd; - __attribute__((aligned(0x40))) uint8_t alignedBuffer[0x40]; - if (!fd || !ptr) { r->_errno = EINVAL; return -1; } - FSInitCmdBlock(&cmd); - // Check that the file was opened with read access + __wut_fs_file_t *file = (__wut_fs_file_t *) fd; if ((file->flags & O_ACCMODE) == O_WRONLY) { r->_errno = EBADF; return -1; } - partSize[0] = (0x40 - ((uintptr_t)ptr & 0x3f)) & 0x3f; - if (partSize[0] > len) partSize[0] = len; - partSize[1] = len - partSize[0]; - partSize[2] = partSize[1] & 0x3f; - partSize[1] -= partSize[2]; + // cache-aligned, cache-line-sized + __attribute__((aligned(0x40))) uint8_t alignedBuffer[0x40]; - if (partSize[0] + partSize[2] <= 0x40 && partSize[1] == 0) { - partSize[0] += partSize[2]; - partSize[2] = 0; - } + FSCmdBlock cmd; + FSInitCmdBlock(&cmd); - uint8_t *tmp; + size_t bytesRead = 0; + while (bytesRead < len) { + // only use input buffer if cache-aligned and read size is a multiple of cache line size + // otherwise read into alignedBuffer + uint8_t *tmp = (uint8_t*) ptr; + size_t size = len - bytesRead; - for (int part=0; part<3; part++) { - - if (!partSize[0]) continue; - - if (part == 1) { - tmp = (uint8_t *)ptr; + if ((uintptr_t) ptr & 0x3F) { + // read partial cache-line front-end + tmp = alignedBuffer; + size = MIN(size, 0x40 - ((uintptr_t) ptr & 0x3F)); + } else if (size < 0x40) { + // read partial cache-line back-end + tmp = alignedBuffer; } else { - tmp = alignedBuffer; + // read whole cache lines + size &= ~0x3F; } - status = FSReadFile(__wut_devoptab_fs_client, &cmd, tmp, 1, partSize[part], file->fd, 0, FS_ERROR_FLAG_ALL); + FSStatus status = FSReadFile(__wut_devoptab_fs_client, &cmd, tmp, 1, size, + file->fd, 0, FS_ERROR_FLAG_ALL); + if (status < 0) { + if (bytesRead != 0) + return bytesRead; // error after partial read + r->_errno = __wut_fs_translate_error(status); return -1; } - - if (part != 1) memcpy(ptr, alignedBuffer, status); - + + if (tmp == alignedBuffer) + memcpy(ptr, alignedBuffer, status); + bytesRead += status; ptr += status; - if (status != partSize[part]) return bytesRead; + if (status != size) + return bytesRead; // partial read } return bytesRead;