Line data Source code
1 : #include "Storage.h"
2 :
3 : #include <assert.h>
4 : #include <errno.h>
5 : #include <fcntl.h>
6 : #include <stdio.h>
7 : #include <sys/stat.h>
8 : #include <sys/types.h>
9 : #include <unistd.h>
10 :
11 : #include <AP_HAL/AP_HAL.h>
12 :
13 : using namespace Linux;
14 :
15 : /*
16 : This stores 'eeprom' data on the SD card, with a 4k size, and a
17 : in-memory buffer. This keeps the latency down.
18 : */
19 :
20 : // name the storage file after the sketch so you can use the same board
21 : // card for ArduCopter and ArduPlane
22 : #define STORAGE_FILE AP_BUILD_TARGET_NAME ".stg"
23 :
24 : extern const AP_HAL::HAL& hal;
25 :
26 2 : static inline int is_dir(const char *path)
27 : {
28 : struct stat st;
29 :
30 2 : if (stat(path, &st) < 0) {
31 0 : return -errno;
32 : }
33 :
34 2 : return S_ISDIR(st.st_mode);
35 : }
36 :
37 2 : static int mkdir_p(const char *path, int len, mode_t mode)
38 : {
39 : char *start, *end;
40 :
41 2 : start = strndupa(path, len);
42 2 : end = start + len;
43 :
44 : /*
45 : * scan backwards, replacing '/' with '\0' while the component doesn't
46 : * exist
47 : */
48 : for (;;) {
49 2 : int r = is_dir(start);
50 2 : if (r > 0) {
51 2 : end += strlen(end);
52 :
53 2 : if (end == start + len) {
54 2 : return 0;
55 : }
56 :
57 : /* end != start, since it would be caught on the first
58 : * iteration */
59 0 : *end = '/';
60 0 : break;
61 0 : } else if (r == 0) {
62 0 : return -ENOTDIR;
63 : }
64 :
65 0 : if (end == start) {
66 0 : break;
67 : }
68 :
69 0 : *end = '\0';
70 :
71 : /* Find the next component, backwards, discarding extra '/'*/
72 0 : while (end > start && *end != '/') {
73 0 : end--;
74 : }
75 :
76 0 : while (end > start && *(end - 1) == '/') {
77 0 : end--;
78 : }
79 0 : }
80 :
81 0 : while (end < start + len) {
82 0 : if (mkdir(start, mode) < 0 && errno != EEXIST) {
83 0 : return -errno;
84 : }
85 :
86 0 : end += strlen(end);
87 0 : *end = '/';
88 : }
89 :
90 0 : return 0;
91 : }
92 :
93 2 : int Storage::_storage_create(const char *dpath)
94 : {
95 2 : int dfd = -1;
96 :
97 2 : mkdir_p(dpath, strlen(dpath), 0777);
98 2 : dfd = open(dpath, O_RDONLY|O_CLOEXEC);
99 2 : if (dfd == -1) {
100 0 : fprintf(stderr, "Failed to open storage directory: %s (%m)\n", dpath);
101 0 : return -1;
102 : }
103 :
104 2 : int fd = openat(dfd, STORAGE_FILE, O_RDWR|O_CREAT|O_CLOEXEC, 0666);
105 :
106 2 : if (fd == -1) {
107 0 : fprintf(stderr, "Failed to create storage file %s/%s\n", dpath,
108 : STORAGE_FILE);
109 0 : goto fail;
110 : }
111 :
112 : // take up all needed space
113 2 : if (ftruncate(fd, sizeof(_buffer)) == -1) {
114 0 : fprintf(stderr, "Failed to set file size to %u kB (%m)\n",
115 : unsigned(sizeof(_buffer) / 1024));
116 0 : close(fd);
117 0 : goto fail;
118 : }
119 :
120 : // ensure the directory is updated with the new size
121 2 : fsync(fd);
122 2 : fsync(dfd);
123 :
124 2 : close(dfd);
125 :
126 2 : return fd;
127 :
128 0 : fail:
129 0 : close(dfd);
130 0 : return -1;
131 : }
132 :
133 61 : void Storage::init()
134 : {
135 : const char *dpath;
136 :
137 61 : if (_initialised) {
138 59 : return;
139 : }
140 :
141 2 : _dirty_mask = 0;
142 :
143 2 : dpath = hal.util->get_custom_storage_directory();
144 2 : if (!dpath) {
145 2 : dpath = HAL_BOARD_STORAGE_DIRECTORY;
146 : }
147 :
148 2 : int fd = _storage_create(dpath);
149 2 : if (fd == -1) {
150 0 : AP_HAL::panic("Cannot create storage %s (%m)", dpath);
151 : }
152 :
153 2 : ssize_t ret = read(fd, _buffer, sizeof(_buffer));
154 :
155 2 : if (ret != sizeof(_buffer)) {
156 0 : close(fd);
157 0 : AP_HAL::panic("Failed to read %s (%m)", dpath);
158 : }
159 :
160 2 : _fd = fd;
161 2 : _initialised = true;
162 : }
163 :
164 : /*
165 : mark some lines as dirty. Note that there is no attempt to avoid
166 : the race condition between this code and the _timer_tick() code
167 : below, which both update _dirty_mask. If we lose the race then the
168 : result is that a line is written more than once, but it won't result
169 : in a line not being written.
170 : */
171 17 : void Storage::_mark_dirty(uint16_t loc, uint16_t length)
172 : {
173 17 : if (length == 0) {
174 0 : return;
175 : }
176 17 : uint16_t end = loc + length - 1;
177 17 : for (uint8_t line=loc>>LINUX_STORAGE_LINE_SHIFT;
178 34 : line <= end>>LINUX_STORAGE_LINE_SHIFT;
179 : line++) {
180 17 : _dirty_mask |= 1U << line;
181 : }
182 : }
183 :
184 44 : void Storage::read_block(void *dst, uint16_t loc, size_t n)
185 : {
186 44 : if (loc >= sizeof(_buffer)-(n-1)) {
187 0 : return;
188 : }
189 44 : init();
190 44 : memcpy(dst, &_buffer[loc], n);
191 : }
192 :
193 22 : void Storage::write_block(uint16_t loc, const void *src, size_t n)
194 : {
195 22 : if (loc >= sizeof(_buffer)-(n-1)) {
196 0 : return;
197 : }
198 22 : if (memcmp(src, &_buffer[loc], n) != 0) {
199 17 : init();
200 17 : memcpy(&_buffer[loc], src, n);
201 17 : _mark_dirty(loc, n);
202 : }
203 : }
204 :
205 0 : void Storage::_timer_tick(void)
206 : {
207 0 : if (!_initialised || _dirty_mask == 0 || _fd == -1) {
208 0 : return;
209 : }
210 :
211 : // write out the first dirty set of lines. We don't write more
212 : // than one to keep the latency of this call to a minimum
213 : uint8_t i, n;
214 0 : for (i=0; i<LINUX_STORAGE_NUM_LINES; i++) {
215 0 : if (_dirty_mask & (1<<i)) {
216 0 : break;
217 : }
218 : }
219 0 : if (i == LINUX_STORAGE_NUM_LINES) {
220 : // this shouldn't be possible
221 0 : return;
222 : }
223 0 : uint32_t write_mask = (1U<<i);
224 : // see how many lines to write
225 0 : for (n=1; (i+n) < LINUX_STORAGE_NUM_LINES &&
226 : n < (LINUX_STORAGE_MAX_WRITE>>LINUX_STORAGE_LINE_SHIFT); n++) {
227 0 : if (!(_dirty_mask & (1<<(n+i)))) {
228 0 : break;
229 : }
230 : // mark that line clean
231 0 : write_mask |= (1<<(n+i));
232 : }
233 :
234 : /*
235 : write the lines. This also updates _dirty_mask. Note that
236 : because this is a SCHED_FIFO thread it will not be preempted
237 : by the main task except during blocking calls. This means we
238 : don't need a semaphore around the _dirty_mask updates.
239 : */
240 0 : if (lseek(_fd, i<<LINUX_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<LINUX_STORAGE_LINE_SHIFT)) {
241 0 : _dirty_mask &= ~write_mask;
242 0 : if (write(_fd, &_buffer[i<<LINUX_STORAGE_LINE_SHIFT], n<<LINUX_STORAGE_LINE_SHIFT) != n<<LINUX_STORAGE_LINE_SHIFT) {
243 : // write error - likely EINTR
244 0 : _dirty_mask |= write_mask;
245 0 : close(_fd);
246 0 : _fd = -1;
247 : }
248 0 : if (_dirty_mask == 0) {
249 0 : if (fsync(_fd) != 0) {
250 0 : close(_fd);
251 0 : _fd = -1;
252 : }
253 : }
254 : }
255 : }
256 :
257 : /*
258 : get storage size and ptr
259 : */
260 0 : bool Storage::get_storage_ptr(void *&ptr, size_t &size)
261 : {
262 0 : if (!_initialised) {
263 0 : return false;
264 : }
265 0 : ptr = _buffer;
266 0 : size = sizeof(_buffer);
267 0 : return true;
268 : }
|
