|
|
|
@ -3,6 +3,7 @@
@@ -3,6 +3,7 @@
|
|
|
|
|
*/ |
|
|
|
|
#include "AP_Filesystem.h" |
|
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
|
|
|
#include <AP_Math/AP_Math.h> |
|
|
|
|
#include <stdio.h> |
|
|
|
|
|
|
|
|
|
#if HAVE_FILESYSTEM_SUPPORT && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS |
|
|
|
@ -23,6 +24,10 @@ static bool remount_needed;
@@ -23,6 +24,10 @@ static bool remount_needed;
|
|
|
|
|
#define FATFS_W (S_IWUSR | S_IWGRP | S_IWOTH) /*< FatFs Write perms */ |
|
|
|
|
#define FATFS_X (S_IXUSR | S_IXGRP | S_IXOTH) /*< FatFs Execute perms */ |
|
|
|
|
|
|
|
|
|
// don't write more than 4k at a time to prevent needing too much
|
|
|
|
|
// DMAable memory
|
|
|
|
|
#define MAX_IO_SIZE 4096 |
|
|
|
|
|
|
|
|
|
// use a semaphore to ensure that only one filesystem operation is
|
|
|
|
|
// happening at a time. A recursive semaphore is used to cope with the
|
|
|
|
|
// mkdir() inside sdcard_retry()
|
|
|
|
@ -379,7 +384,6 @@ int AP_Filesystem::close(int fileno)
@@ -379,7 +384,6 @@ int AP_Filesystem::close(int fileno)
|
|
|
|
|
|
|
|
|
|
ssize_t AP_Filesystem::read(int fd, void *buf, size_t count) |
|
|
|
|
{ |
|
|
|
|
UINT size; |
|
|
|
|
UINT bytes = count; |
|
|
|
|
int res; |
|
|
|
|
FIL *fh; |
|
|
|
@ -401,17 +405,31 @@ ssize_t AP_Filesystem::read(int fd, void *buf, size_t count)
@@ -401,17 +405,31 @@ ssize_t AP_Filesystem::read(int fd, void *buf, size_t count)
|
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
res = f_read(fh, (void *) buf, bytes, &size); |
|
|
|
|
if (res != FR_OK) { |
|
|
|
|
errno = fatfs_to_errno((FRESULT)res); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
return (ssize_t)size; |
|
|
|
|
UINT total = 0; |
|
|
|
|
do { |
|
|
|
|
UINT size = 0; |
|
|
|
|
UINT n = MIN(bytes, MAX_IO_SIZE); |
|
|
|
|
res = f_read(fh, (void *)buf, n, &size); |
|
|
|
|
if (res != FR_OK) { |
|
|
|
|
errno = fatfs_to_errno((FRESULT)res); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
if (size > n || size == 0) { |
|
|
|
|
errno = EIO; |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
total += size; |
|
|
|
|
buf = (void *)(((uint8_t *)buf)+size); |
|
|
|
|
bytes -= size; |
|
|
|
|
if (size < n) { |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} while (bytes > 0); |
|
|
|
|
return (ssize_t)total; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
ssize_t AP_Filesystem::write(int fd, const void *buf, size_t count) |
|
|
|
|
{ |
|
|
|
|
UINT size; |
|
|
|
|
UINT bytes = count; |
|
|
|
|
FRESULT res; |
|
|
|
|
FIL *fh; |
|
|
|
@ -428,19 +446,34 @@ ssize_t AP_Filesystem::write(int fd, const void *buf, size_t count)
@@ -428,19 +446,34 @@ ssize_t AP_Filesystem::write(int fd, const void *buf, size_t count)
|
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
res = f_write(fh, buf, bytes, &size); |
|
|
|
|
if (res == FR_DISK_ERR && RETRY_ALLOWED()) { |
|
|
|
|
// one retry on disk error
|
|
|
|
|
hal.scheduler->delay(100); |
|
|
|
|
if (remount_file_system()) { |
|
|
|
|
res = f_write(fh, buf, bytes, &size); |
|
|
|
|
UINT total = 0; |
|
|
|
|
do { |
|
|
|
|
UINT n = MIN(bytes, MAX_IO_SIZE); |
|
|
|
|
UINT size = 0; |
|
|
|
|
res = f_write(fh, buf, n, &size); |
|
|
|
|
if (res == FR_DISK_ERR && RETRY_ALLOWED()) { |
|
|
|
|
// one retry on disk error
|
|
|
|
|
hal.scheduler->delay(100); |
|
|
|
|
if (remount_file_system()) { |
|
|
|
|
res = f_write(fh, buf, n, &size); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
if (res != FR_OK) { |
|
|
|
|
errno = fatfs_to_errno(res); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
return (ssize_t)size; |
|
|
|
|
if (size > n || size == 0) { |
|
|
|
|
errno = EIO; |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
if (res != FR_OK || size > n) { |
|
|
|
|
errno = fatfs_to_errno(res); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
total += size; |
|
|
|
|
buf = (void *)(((uint8_t *)buf)+size); |
|
|
|
|
bytes -= size; |
|
|
|
|
if (size < n) { |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} while (bytes > 0); |
|
|
|
|
return (ssize_t)total; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
int AP_Filesystem::fsync(int fileno) |
|
|
|
|