Line data Source code
1 : /*
2 : block based logging, for boards with flash logging
3 : */
4 :
5 : #include "AP_Logger_config.h"
6 :
7 : #if HAL_LOGGING_BLOCK_ENABLED
8 :
9 : #include "AP_Logger_Block.h"
10 : #include "AP_Logger.h"
11 : #include <AP_HAL/AP_HAL.h>
12 : #include <stdio.h>
13 : #include <AP_RTC/AP_RTC.h>
14 : #include <GCS_MAVLink/GCS.h>
15 :
16 : const extern AP_HAL::HAL& hal;
17 :
18 : // the last page holds the log format in first 4 bytes. Please change
19 : // this if (and only if!) the low level format changes
20 : #define DF_LOGGING_FORMAT 0x1901201B
21 :
22 5 : AP_Logger_Block::AP_Logger_Block(AP_Logger &front, LoggerMessageWriter_DFLogStart *writer) :
23 : AP_Logger_Backend(front, writer),
24 5 : writebuf(0)
25 : {
26 5 : df_stats_clear();
27 5 : }
28 :
29 : // Init() is called after driver Init(), it is the responsibility of the driver to make sure the
30 : // device is ready to accept commands before Init() is called
31 5 : void AP_Logger_Block::Init(void)
32 : {
33 : // buffer is used for both reads and writes so access must always be within the semaphore
34 5 : buffer = (uint8_t *)hal.util->malloc_type(df_PageSize, AP_HAL::Util::MEM_DMA_SAFE);
35 5 : if (buffer == nullptr) {
36 0 : AP_HAL::panic("Out of DMA memory for logging");
37 : }
38 :
39 : //flash_test();
40 :
41 5 : if (CardInserted()) {
42 : // reserve space for version in last sector
43 5 : df_NumPages -= df_PagePerBlock;
44 :
45 : // determine and limit file backend buffersize
46 5 : uint32_t bufsize = _front._params.file_bufsize;
47 5 : if (bufsize > 64) {
48 5 : bufsize = 64;
49 : }
50 5 : bufsize *= 1024;
51 :
52 : // If we can't allocate the full size, try to reduce it until we can allocate it
53 5 : while (!writebuf.set_size(bufsize) && bufsize >= df_PageSize * df_PagePerBlock) {
54 0 : DEV_PRINTF("AP_Logger_Block: Couldn't set buffer size to=%u\n", (unsigned)bufsize);
55 0 : bufsize >>= 1;
56 : }
57 :
58 5 : if (!writebuf.get_size()) {
59 0 : DEV_PRINTF("Out of memory for logging\n");
60 0 : return;
61 : }
62 :
63 5 : DEV_PRINTF("AP_Logger_Block: buffer size=%u\n", (unsigned)bufsize);
64 5 : _initialised = true;
65 : }
66 :
67 5 : WITH_SEMAPHORE(sem);
68 :
69 5 : if (NeedErase()) {
70 0 : EraseAll();
71 : } else {
72 5 : validate_log_structure();
73 : }
74 5 : }
75 :
76 199845 : uint32_t AP_Logger_Block::bufferspace_available()
77 : {
78 : // because AP_Logger_Block devices are ring buffers, we *always*
79 : // have room...
80 199845 : return df_NumPages * df_PageSize;
81 : }
82 :
83 : // *** LOGGER PUBLIC FUNCTIONS ***
84 22 : void AP_Logger_Block::StartWrite(uint32_t PageAdr)
85 : {
86 22 : df_PageAdr = PageAdr;
87 22 : }
88 :
89 42525 : void AP_Logger_Block::FinishWrite(void)
90 : {
91 : // Write Buffer to flash
92 42525 : BufferToPage(df_PageAdr);
93 42525 : df_PageAdr++;
94 :
95 : // If we reach the end of the memory, start from the beginning
96 42525 : if (df_PageAdr > df_NumPages) {
97 8 : df_PageAdr = 1;
98 : }
99 :
100 : // when starting a new sector, erase it
101 42525 : if ((df_PageAdr-1) % df_PagePerBlock == 0) {
102 : // if we have wrapped over an existing log, force the oldest to be recalculated
103 171 : if (_cached_oldest_log > 0) {
104 59 : uint16_t log_num = StartRead(df_PageAdr);
105 59 : if (log_num != 0xFFFF && log_num >= _cached_oldest_log) {
106 1 : _cached_oldest_log = 0;
107 : }
108 : }
109 : // are we about to erase a sector with our own headers in it?
110 171 : if (df_Write_FilePage > df_NumPages - df_PagePerBlock) {
111 2 : chip_full = true;
112 2 : return;
113 : }
114 169 : SectorErase(get_block(df_PageAdr));
115 : }
116 : }
117 :
118 1076044 : bool AP_Logger_Block::WritesOK() const
119 : {
120 1076044 : if (!CardInserted() || erase_started) {
121 86 : return false;
122 : }
123 1075961 : return true;
124 : }
125 :
126 397926 : bool AP_Logger_Block::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
127 : {
128 : // is_critical is ignored - we're a ring buffer and never run out
129 : // of space. possibly if we do more complicated bandwidth
130 : // limiting we can reserve bandwidth based on is_critical
131 397926 : if (!WritesOK()) {
132 0 : return false;
133 : }
134 :
135 397926 : WITH_SEMAPHORE(write_sem);
136 :
137 397926 : const uint32_t space = writebuf.space();
138 :
139 444743 : if (_writing_startup_messages &&
140 46817 : _startup_messagewriter->fmt_done()) {
141 : // the state machine has called us, and it has finished
142 : // writing format messages out. It can always get back to us
143 : // with more messages later, so let's leave room for other
144 : // things:
145 44300 : const uint32_t now = AP_HAL::millis();
146 44300 : const bool must_dribble = (now - last_messagewrite_message_sent) > 100;
147 88083 : if (!must_dribble &&
148 43783 : space < non_messagewriter_message_reserved_space(writebuf.get_size())) {
149 : // this message isn't dropped, it will be sent again...
150 21413 : return false;
151 : }
152 22887 : last_messagewrite_message_sent = now;
153 : } else {
154 : // we reserve some amount of space for critical messages:
155 353626 : if (!is_critical && space < critical_message_reserved_space(writebuf.get_size())) {
156 144617 : _dropped++;
157 144617 : return false;
158 : }
159 : }
160 :
161 : // if no room for entire message - drop it:
162 231896 : if (space < size) {
163 179 : _dropped++;
164 179 : return false;
165 : }
166 :
167 231717 : writebuf.write((uint8_t*)pBuffer, size);
168 231717 : df_stats_gather(size, writebuf.space());
169 :
170 231717 : return true;
171 397926 : }
172 :
173 : // read from the page address and return the file number at that location
174 77462 : uint16_t AP_Logger_Block::StartRead(uint32_t PageAdr)
175 : {
176 : // copy flash page to buffer
177 77462 : if (erase_started) {
178 4 : df_Read_PageAdr = PageAdr;
179 4 : memset(buffer, 0xff, df_PageSize);
180 : } else {
181 77458 : PageToBuffer(PageAdr);
182 : }
183 77462 : return ReadHeaders();
184 : }
185 :
186 : // read the headers at the current read point returning the file number
187 103888 : uint16_t AP_Logger_Block::ReadHeaders()
188 : {
189 : // We are starting a new page - read FileNumber and FilePage
190 : struct PageHeader ph;
191 103888 : BlockRead(0, &ph, sizeof(ph));
192 103888 : df_FileNumber = ph.FileNumber;
193 103888 : df_FilePage = ph.FilePage;
194 : #if BLOCK_LOG_VALIDATE
195 : if (ph.crc != DF_LOGGING_FORMAT + df_FilePage && df_FileNumber != 0xFFFF) {
196 : printf("ReadHeaders: invalid block read at %d\n", df_Read_PageAdr);
197 : }
198 : #endif
199 103888 : df_Read_BufferIdx = sizeof(ph);
200 : // we are at the start of a file, read the file header
201 103888 : if (df_FilePage == 1) {
202 : struct FileHeader fh;
203 303 : BlockRead(sizeof(ph), &fh, sizeof(fh));
204 303 : df_FileTime = fh.utc_secs;
205 303 : df_Read_BufferIdx += sizeof(fh);
206 : }
207 :
208 103888 : return df_FileNumber;
209 : }
210 :
211 73408 : bool AP_Logger_Block::ReadBlock(void *pBuffer, uint16_t size)
212 : {
213 73408 : if (erase_started) {
214 0 : return false;
215 : }
216 173236 : while (size > 0) {
217 99828 : uint16_t n = df_PageSize - df_Read_BufferIdx;
218 99828 : if (n > size) {
219 73402 : n = size;
220 : }
221 :
222 99828 : if (!BlockRead(df_Read_BufferIdx, pBuffer, n)) {
223 0 : return false;
224 : }
225 99828 : size -= n;
226 99828 : pBuffer = (void *)(n + (uintptr_t)pBuffer);
227 :
228 99828 : df_Read_BufferIdx += n;
229 :
230 99828 : if (df_Read_BufferIdx == df_PageSize) {
231 26426 : uint32_t new_page_addr = df_Read_PageAdr + 1;
232 26426 : if (new_page_addr > df_NumPages) {
233 1 : new_page_addr = 1;
234 : }
235 26426 : if (erase_started) {
236 0 : memset(buffer, 0xff, df_PageSize);
237 0 : df_Read_PageAdr = new_page_addr;
238 : } else {
239 26426 : PageToBuffer(new_page_addr);
240 : }
241 :
242 : // We are starting a new page - read FileNumber and FilePage
243 26426 : ReadHeaders();
244 : }
245 : }
246 73408 : return true;
247 : }
248 :
249 : // initialize the log data for the given file number
250 15 : void AP_Logger_Block::StartLogFile(uint16_t FileNumber)
251 : {
252 15 : df_FileNumber = FileNumber;
253 15 : df_Write_FileNumber = FileNumber;
254 15 : df_FilePage = 1;
255 15 : df_Write_FilePage = 1;
256 15 : }
257 :
258 3630 : uint16_t AP_Logger_Block::GetFileNumber() const
259 : {
260 3630 : return df_FileNumber;
261 : }
262 :
263 7 : void AP_Logger_Block::EraseAll()
264 : {
265 7 : if (hal.util->get_soft_armed()) {
266 : // do not want to do any filesystem operations while we are e.g. flying
267 0 : return;
268 : }
269 :
270 : // push out the message before stopping logging
271 7 : if (!erase_started) {
272 7 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Chip erase started");
273 : }
274 :
275 7 : WITH_SEMAPHORE(sem);
276 :
277 7 : if (erase_started) {
278 : // already erasing
279 0 : return;
280 : }
281 7 : erase_started = true;
282 :
283 : // remember what we were doing
284 7 : new_log_pending = log_write_started;
285 :
286 : // throw away everything
287 7 : log_write_started = false;
288 7 : writebuf.clear();
289 :
290 : // reset the format version and wrapped status so that any incomplete erase will be caught
291 7 : Sector4kErase(get_sector(df_NumPages));
292 :
293 7 : StartErase();
294 7 : }
295 :
296 380 : void AP_Logger_Block::periodic_1Hz()
297 : {
298 380 : AP_Logger_Backend::periodic_1Hz();
299 :
300 385 : if (rate_limiter == nullptr &&
301 5 : (_front._params.blk_ratemax > 0 ||
302 1 : _front._params.disarm_ratemax > 0 ||
303 0 : _front._log_pause)) {
304 : // setup rate limiting if log rate max > 0Hz or log pause of streaming entries is requested
305 5 : rate_limiter = NEW_NOTHROW AP_Logger_RateLimiter(_front, _front._params.blk_ratemax, _front._params.disarm_ratemax);
306 : }
307 :
308 380 : if (!io_thread_alive()) {
309 0 : if (warning_decimation_counter == 0 && _initialised) {
310 : // we don't print this error unless we did initialise. When _initialised is set to true
311 : // we register the IO timer callback
312 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AP_Logger: IO thread died");
313 : }
314 0 : if (warning_decimation_counter++ > 57) {
315 0 : warning_decimation_counter = 0;
316 : }
317 0 : _initialised = false;
318 380 : } else if (chip_full) {
319 19 : if (warning_decimation_counter == 0) {
320 1 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Chip full, logging stopped");
321 : }
322 19 : if (warning_decimation_counter++ > 57) {
323 0 : warning_decimation_counter = 0;
324 : }
325 : }
326 380 : }
327 :
328 : // EraseAll is asynchronous, but we must not start a new
329 : // log in a child thread so this task picks up the hint from the io timer
330 : // keeping locking to a minimum
331 3556 : void AP_Logger_Block::periodic_10Hz(const uint32_t now)
332 : {
333 3556 : if (erase_started || InErase()) {
334 0 : return;
335 : }
336 :
337 : // don't print status messages in io thread, do it here
338 3556 : switch (status_msg) {
339 7 : case StatusMessage::ERASE_COMPLETE:
340 7 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Chip erase complete");
341 7 : status_msg = StatusMessage::NONE;
342 7 : break;
343 0 : case StatusMessage::RECOVERY_COMPLETE:
344 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Log recovery complete");
345 0 : status_msg = StatusMessage::NONE;
346 0 : break;
347 3549 : case StatusMessage::NONE:
348 3549 : break;
349 : }
350 :
351 : // EraseAll should only set this in the main thread
352 3556 : if (new_log_pending) {
353 1 : start_new_log();
354 : }
355 : }
356 :
357 : /*
358 : * we need to erase if the logging format has changed
359 : */
360 5 : bool AP_Logger_Block::NeedErase(void)
361 : {
362 5 : uint32_t version = 0;
363 5 : PageToBuffer(df_NumPages+1); // last page
364 5 : BlockRead(0, &version, sizeof(version));
365 5 : if (version == DF_LOGGING_FORMAT) {
366 : // only leave the read point in a sane place if we are not about to destroy everything
367 5 : StartRead(1);
368 5 : return false;
369 : }
370 0 : return true;
371 : }
372 :
373 : /*
374 : * iterate through all of the logs files looking for ones that are corrupted and correct.
375 : */
376 5 : void AP_Logger_Block::validate_log_structure()
377 : {
378 5 : WITH_SEMAPHORE(sem);
379 5 : bool wrapped = is_wrapped();
380 5 : uint32_t page = 1;
381 5 : uint32_t page_start = 1;
382 :
383 5 : uint16_t file = StartRead(page);
384 5 : uint16_t first_file = file;
385 5 : uint16_t next_file = file;
386 5 : uint16_t last_file = 0;
387 :
388 12 : while (file != 0xFFFF && page <= df_NumPages && (file == next_file || (wrapped && file < next_file))) {
389 7 : uint32_t end_page = find_last_page_of_log(file);
390 7 : if (end_page == 0 || end_page < page) { // this can happen and may be responsible for corruption that we have seen
391 : break;
392 : }
393 7 : page = end_page + 1;
394 7 : file = StartRead(page);
395 7 : next_file++;
396 : // skip over the rest of an erased block
397 7 : if (wrapped && file == 0xFFFF) {
398 0 : file = StartRead((get_block(page) + 1) * df_PagePerBlock + 1);
399 : }
400 7 : if (wrapped && file < next_file) {
401 0 : page_start = page;
402 0 : next_file = file;
403 0 : first_file = file;
404 7 : } else if (last_file < next_file) {
405 7 : last_file = file;
406 : }
407 7 : if (file == next_file) {
408 2 : DEV_PRINTF("Found complete log %d at %X-%X\n", int(file), unsigned(page), unsigned(find_last_page_of_log(file)));
409 : }
410 : }
411 :
412 5 : if (file != 0xFFFF && file != next_file && page <= df_NumPages && page > 0) {
413 0 : DEV_PRINTF("Found corrupt log %d at 0x%04X, erasing", int(file), unsigned(page));
414 0 : df_EraseFrom = page;
415 5 : } else if (next_file != 0xFFFF && page > 0 && next_file > 1) { // chip is empty
416 5 : DEV_PRINTF("Found %d complete logs at 0x%04X-0x%04X", int(next_file - first_file), unsigned(page_start), unsigned(page - 1));
417 : }
418 5 : }
419 :
420 : /**
421 : * get raw data from a log - page is the start page of the log, offset is the offset within the log starting at that page
422 : */
423 73408 : int16_t AP_Logger_Block::get_log_data_raw(uint16_t log_num, uint32_t page, uint32_t offset, uint16_t len, uint8_t *data)
424 : {
425 73408 : WITH_SEMAPHORE(sem);
426 73408 : const uint16_t data_page_size = df_PageSize - sizeof(struct PageHeader);
427 73408 : const uint16_t first_page_size = data_page_size - sizeof(struct FileHeader);
428 :
429 : // offset is the true offset in the file, so we have to calculate the offset accounting for page headers
430 73408 : if (offset >= first_page_size) {
431 73390 : offset -= first_page_size;
432 73390 : page = page + offset / data_page_size + 1;
433 73390 : offset %= data_page_size;
434 :
435 73390 : if (page > df_NumPages) {
436 33422 : page = page % df_NumPages;
437 : }
438 : }
439 :
440 : // Sanity check we haven't been asked for an offset beyond the end of the log
441 73408 : if (StartRead(page) != log_num) {
442 0 : return -1;
443 : }
444 :
445 73408 : df_Read_BufferIdx += offset;
446 :
447 73408 : if (!ReadBlock(data, len)) {
448 0 : return -1;
449 : }
450 :
451 73408 : return (int16_t)len;
452 73408 : }
453 :
454 : /**
455 : get data from a log, accounting for adding FMT headers
456 : */
457 73408 : int16_t AP_Logger_Block::get_log_data(uint16_t list_entry, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data)
458 : {
459 73408 : const uint16_t log_num = log_num_from_list_entry(list_entry);
460 :
461 73408 : if (log_num == 0) {
462 : // that failed - probably no logs
463 0 : return -1;
464 : }
465 :
466 : //printf("get_log_data(%d, %d, %d, %d)\n", log_num, page, offset, len);
467 73408 : WITH_SEMAPHORE(sem);
468 :
469 73408 : uint16_t ret = 0;
470 73408 : if (len > 0) {
471 73408 : const int16_t bytes = get_log_data_raw(log_num, page, offset, len, data);
472 73408 : if (bytes == -1) {
473 0 : return -1;
474 : }
475 73408 : ret += bytes;
476 : }
477 :
478 73408 : return ret;
479 73408 : }
480 :
481 :
482 : // This function determines the number of whole log files in the AP_Logger
483 : // partial logs are rejected as without the headers they are relatively useless
484 49 : uint16_t AP_Logger_Block::get_num_logs(void)
485 : {
486 49 : WITH_SEMAPHORE(sem);
487 : uint32_t lastpage;
488 : uint32_t last;
489 :
490 49 : if (!CardInserted() || find_last_page() == 1) {
491 0 : return 0;
492 : }
493 :
494 49 : uint32_t first = StartRead(1);
495 :
496 49 : if (first == 0xFFFF) {
497 0 : return 0;
498 : }
499 :
500 49 : lastpage = find_last_page();
501 49 : last = StartRead(lastpage);
502 :
503 49 : if (is_wrapped()) {
504 : // if we wrapped then the rest of the block will be filled with 0xFFFF because we always erase
505 : // a block before writing to it, in order to find the first page we therefore have to read after the
506 : // next block boundary
507 8 : first = StartRead((get_block(lastpage) + 1) * df_PagePerBlock + 1);
508 : // unless we happen to land on the first page of the file that is being overwritten we skip to the next file
509 8 : if (df_FilePage > 1) {
510 8 : first++;
511 : }
512 : }
513 :
514 49 : if (last == first) {
515 16 : return 1;
516 : }
517 :
518 33 : return (last - first + 1);
519 49 : }
520 :
521 : // stop logging immediately
522 2 : void AP_Logger_Block::stop_logging(void)
523 : {
524 2 : WITH_SEMAPHORE(sem);
525 :
526 2 : log_write_started = false;
527 :
528 : // nuke writing any previous log
529 2 : writebuf.clear();
530 2 : }
531 :
532 : // stop logging and flush any remaining data
533 3 : void AP_Logger_Block::stop_logging_async(void)
534 : {
535 3 : stop_log_pending = true;
536 3 : }
537 :
538 : // This function starts a new log file in the AP_Logger
539 : // no actual data should be written to the storage here
540 : // that should all be handled by the IO thread
541 101 : void AP_Logger_Block::start_new_log(void)
542 : {
543 101 : if (erase_started) {
544 : // currently erasing
545 86 : return;
546 : }
547 :
548 15 : WITH_SEMAPHORE(sem);
549 :
550 15 : if (logging_started()) {
551 0 : stop_logging();
552 : }
553 :
554 : // no need to schedule this anymore
555 15 : new_log_pending = false;
556 :
557 15 : uint32_t last_page = find_last_page();
558 :
559 15 : StartRead(last_page);
560 :
561 15 : log_write_started = true;
562 15 : uint16_t new_log_num = 1;
563 :
564 15 : if (find_last_log() == 0 || GetFileNumber() == 0xFFFF) {
565 7 : StartLogFile(new_log_num);
566 7 : StartWrite(1);
567 : // Check for log of length 1 page and suppress
568 8 : } else if (df_FilePage <= 1) {
569 0 : new_log_num = GetFileNumber();
570 : // Last log too short, reuse its number
571 : // and overwrite it
572 0 : StartLogFile(new_log_num);
573 0 : StartWrite(last_page);
574 : } else {
575 8 : new_log_num = GetFileNumber()+1;
576 8 : if (last_page == 0xFFFF) {
577 0 : last_page=0;
578 : }
579 8 : StartLogFile(new_log_num);
580 8 : StartWrite(last_page + 1);
581 : }
582 :
583 : // save UTC time in the first 4 bytes so that we can retrieve it later
584 : uint64_t utc_usec;
585 15 : FileHeader hdr {};
586 15 : if (AP::rtc().get_utc_usec(utc_usec)) {
587 10 : hdr.utc_secs = utc_usec / 1000000U;
588 : }
589 15 : writebuf.write((uint8_t*)&hdr, sizeof(FileHeader));
590 :
591 15 : start_new_log_reset_variables();
592 :
593 15 : return;
594 15 : }
595 :
596 : // This function finds the first and last pages of a log file
597 : // The first page may be greater than the last page if the AP_Logger has been filled and partially overwritten.
598 28 : void AP_Logger_Block::get_log_boundaries(uint16_t list_entry, uint32_t & start_page, uint32_t & end_page)
599 : {
600 28 : const uint16_t log_num = log_num_from_list_entry(list_entry);
601 28 : if (log_num == 0) {
602 : // that failed - probably no logs
603 0 : start_page = 0;
604 0 : end_page = 0;
605 0 : return;
606 : }
607 :
608 28 : WITH_SEMAPHORE(sem);
609 28 : uint16_t num = get_num_logs();
610 : uint32_t look;
611 :
612 28 : end_page = find_last_page_of_log(log_num);
613 :
614 28 : if (num == 1 || log_num == 1) {
615 13 : if (!is_wrapped()) {
616 9 : start_page = 1;
617 : } else {
618 4 : StartRead(end_page);
619 4 : start_page = (end_page + df_NumPages - df_FilePage) % df_NumPages + 1;
620 : }
621 : } else {
622 : // looking for the first log which might have a gap in front of it
623 15 : if (list_entry == 1) {
624 0 : StartRead(end_page);
625 0 : if (end_page > df_FilePage) { // log is not wrapped
626 0 : start_page = end_page - df_FilePage + 1;
627 : } else { // log is wrapped
628 0 : start_page = (end_page + df_NumPages - df_FilePage) % df_NumPages + 1;
629 : }
630 : } else {
631 15 : look = log_num-1;
632 : do {
633 15 : start_page = find_last_page_of_log(look) + 1;
634 15 : look--;
635 15 : } while (start_page <= 0 && look >=1);
636 : }
637 : }
638 :
639 28 : if (start_page == df_NumPages + 1 || start_page == 0) {
640 0 : start_page = 1;
641 : }
642 :
643 28 : if (end_page == 0) {
644 0 : end_page = start_page;
645 : }
646 :
647 28 : }
648 :
649 : // return true if logging has wrapped around to the beginning of the chip
650 119 : bool AP_Logger_Block::is_wrapped(void)
651 : {
652 119 : return StartRead(df_NumPages) != 0xFFFF;
653 : }
654 :
655 :
656 : // This function finds the last log number
657 21 : uint16_t AP_Logger_Block::find_last_log(void)
658 : {
659 21 : WITH_SEMAPHORE(sem);
660 21 : uint32_t last_page = find_last_page();
661 21 : return StartRead(last_page);
662 21 : }
663 :
664 : // This function finds the last page of the last file
665 186 : uint32_t AP_Logger_Block::find_last_page(void)
666 : {
667 : uint32_t look;
668 186 : uint32_t bottom = 1;
669 186 : uint32_t top = df_NumPages;
670 : uint64_t look_hash;
671 : uint64_t bottom_hash;
672 : uint64_t top_hash;
673 :
674 186 : WITH_SEMAPHORE(sem);
675 :
676 186 : StartRead(bottom);
677 186 : bottom_hash = ((int64_t)GetFileNumber()<<32) | df_FilePage;
678 :
679 2776 : while (top-bottom > 1) {
680 2590 : look = (top+bottom)/2;
681 2590 : StartRead(look);
682 2590 : look_hash = (int64_t)GetFileNumber()<<32 | df_FilePage;
683 : // erased sector so can discount everything above
684 2590 : if (look_hash >= 0xFFFF00000000) {
685 1387 : look_hash = 0;
686 : }
687 :
688 2590 : if (look_hash < bottom_hash) {
689 : // move down
690 1429 : top = look;
691 : } else {
692 : // move up
693 1161 : bottom = look;
694 1161 : bottom_hash = look_hash;
695 : }
696 : }
697 :
698 186 : StartRead(top);
699 186 : top_hash = ((int64_t)GetFileNumber()<<32) | df_FilePage;
700 186 : if (top_hash >= 0xFFFF00000000) {
701 165 : top_hash = 0;
702 : }
703 186 : if (top_hash > bottom_hash) {
704 0 : return top;
705 : }
706 :
707 186 : return bottom;
708 186 : }
709 :
710 : // This function finds the last page of a particular log file
711 52 : uint32_t AP_Logger_Block::find_last_page_of_log(uint16_t log_number)
712 : {
713 : uint32_t look;
714 : uint32_t bottom;
715 : uint32_t top;
716 : uint64_t look_hash;
717 : uint64_t check_hash;
718 :
719 52 : WITH_SEMAPHORE(sem);
720 :
721 52 : if (is_wrapped()) {
722 4 : bottom = StartRead(1);
723 4 : if (bottom > log_number) {
724 0 : bottom = find_last_page();
725 0 : top = df_NumPages;
726 : } else {
727 4 : bottom = 1;
728 4 : top = find_last_page();
729 : }
730 : } else {
731 48 : bottom = 1;
732 48 : top = find_last_page();
733 : }
734 :
735 52 : check_hash = (int64_t)log_number<<32 | 0xFFFFFFFF;
736 :
737 697 : while (top-bottom > 1) {
738 645 : look = (top+bottom)/2;
739 645 : StartRead(look);
740 645 : look_hash = (int64_t)GetFileNumber()<<32 | df_FilePage;
741 645 : if (look_hash >= 0xFFFF00000000) {
742 0 : look_hash = 0;
743 : }
744 :
745 645 : if (look_hash > check_hash) {
746 : // move down
747 179 : top = look;
748 : } else {
749 : // move up
750 466 : bottom = look;
751 : }
752 : }
753 :
754 52 : if (StartRead(top) == log_number) {
755 24 : return top;
756 : }
757 :
758 28 : if (StartRead(bottom) == log_number) {
759 28 : return bottom;
760 : }
761 :
762 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "No last page of log %d at top=%X or bot=%X", int(log_number), unsigned(top), unsigned(bottom));
763 0 : return 0;
764 52 : }
765 :
766 22 : void AP_Logger_Block::get_log_info(uint16_t list_entry, uint32_t &size, uint32_t &time_utc)
767 : {
768 : uint32_t start, end;
769 :
770 22 : WITH_SEMAPHORE(sem);
771 :
772 22 : get_log_boundaries(list_entry, start, end);
773 22 : if (end >= start) {
774 19 : size = (end + 1 - start) * (uint32_t)(df_PageSize - sizeof(PageHeader));
775 : } else {
776 3 : size = (df_NumPages + end + 1 - start) * (uint32_t)(df_PageSize - sizeof(PageHeader));
777 : }
778 :
779 22 : size -= sizeof(FileHeader);
780 :
781 : //printf("LOG %d(%d), %d-%d, size %d\n", log_num_from_list_entry(list_entry), list_entry, start, end, size);
782 :
783 22 : StartRead(start);
784 :
785 : // the log we are currently writing
786 22 : if (df_FileTime == 0 && df_FileNumber == df_Write_FileNumber) {
787 : uint64_t utc_usec;
788 4 : if (AP::rtc().get_utc_usec(utc_usec)) {
789 4 : df_FileTime = utc_usec / 1000000U;
790 : }
791 : }
792 22 : time_utc = df_FileTime;
793 22 : }
794 :
795 : // read size bytes of data from the buffer
796 204024 : bool AP_Logger_Block::BlockRead(uint16_t IntPageAdr, void *pBuffer, uint16_t size)
797 : {
798 204024 : memcpy(pBuffer, &buffer[IntPageAdr], size);
799 204024 : return true;
800 : }
801 :
802 3685 : bool AP_Logger_Block::logging_failed() const
803 : {
804 3685 : if (!_initialised) {
805 0 : return true;
806 : }
807 3685 : if (!io_thread_alive()) {
808 0 : return true;
809 : }
810 3685 : if (chip_full) {
811 217 : return true;
812 : }
813 :
814 3468 : return false;
815 : }
816 :
817 : // detect whether the IO thread is running, since this is considered a catastrophic failure for the logging system
818 : // better be really, really sure
819 4065 : bool AP_Logger_Block::io_thread_alive() const
820 : {
821 : // if the io thread hasn't had a heartbeat in 3s it is dead
822 4065 : return (AP_HAL::millis() - io_timer_heartbeat) < 3000U || !hal.scheduler->is_system_initialized();
823 : }
824 :
825 : /*
826 : IO timer running on IO thread
827 : The IO timer runs every 1ms or at 1Khz. The standard flash chip can write roughly 130Kb/s
828 : so there is little point in trying to write more than 130 bytes - or 1 page (256 bytes).
829 : The W25Q128FV datasheet gives tpp as typically 0.7ms yielding an absolute maximum rate of
830 : 365Kb/s or just over a page per cycle.
831 : */
832 136996 : void AP_Logger_Block::io_timer(void)
833 : {
834 136996 : uint32_t tnow = AP_HAL::millis();
835 136996 : io_timer_heartbeat = tnow;
836 :
837 : // don't write anything for the first 2s to give the dataflash chip a chance to be ready
838 136996 : if (!_initialised || tnow < 2000) {
839 1498 : return;
840 : }
841 :
842 135498 : if (erase_started) {
843 7 : WITH_SEMAPHORE(sem);
844 :
845 7 : if (InErase()) {
846 0 : return;
847 : }
848 : // write the logging format in the last page
849 7 : StartWrite(df_NumPages+1);
850 7 : uint32_t version = DF_LOGGING_FORMAT;
851 7 : memset(buffer, 0, df_PageSize);
852 7 : memcpy(buffer, &version, sizeof(version));
853 7 : FinishWrite();
854 7 : erase_started = false;
855 7 : chip_full = false;
856 7 : status_msg = StatusMessage::ERASE_COMPLETE;
857 7 : return;
858 7 : }
859 :
860 135491 : if (df_EraseFrom > 0) {
861 0 : WITH_SEMAPHORE(sem);
862 :
863 0 : const uint32_t sectors = df_NumPages / df_PagePerSector;
864 0 : const uint32_t block_size = df_PagePerBlock * df_PageSize;
865 0 : const uint32_t sectors_in_block = block_size / (df_PagePerSector * df_PageSize);
866 0 : uint32_t next_sector = get_sector(df_EraseFrom);
867 0 : const uint32_t aligned_sector = sectors - (((df_NumPages - df_EraseFrom + 1) / df_PagePerSector) / sectors_in_block) * sectors_in_block;
868 0 : while (next_sector < aligned_sector) {
869 0 : Sector4kErase(next_sector);
870 0 : io_timer_heartbeat = AP_HAL::millis();
871 0 : next_sector++;
872 : }
873 0 : while (next_sector < sectors) {
874 0 : SectorErase(next_sector / sectors_in_block);
875 0 : io_timer_heartbeat = AP_HAL::millis();
876 0 : next_sector += sectors_in_block;
877 : }
878 0 : status_msg = StatusMessage::RECOVERY_COMPLETE;
879 0 : df_EraseFrom = 0;
880 0 : }
881 :
882 135491 : if (!CardInserted() || new_log_pending || chip_full) {
883 8871 : return;
884 : }
885 :
886 : // we have been asked to stop logging, flush everything
887 126620 : if (stop_log_pending) {
888 6 : WITH_SEMAPHORE(sem);
889 :
890 6 : log_write_started = false;
891 :
892 : // complete writing any previous log, a page at a time to avoid holding the lock for too long
893 6 : if (writebuf.available()) {
894 3 : write_log_page();
895 : } else {
896 3 : writebuf.clear();
897 3 : stop_log_pending = false;
898 : }
899 :
900 : // write at most one page
901 126620 : } else if (writebuf.available() >= df_PageSize - sizeof(struct PageHeader)) {
902 42515 : WITH_SEMAPHORE(sem);
903 :
904 42515 : write_log_page();
905 42515 : }
906 : }
907 :
908 : // write out a page of log data
909 42518 : void AP_Logger_Block::write_log_page()
910 : {
911 : struct PageHeader ph;
912 42518 : ph.FileNumber = df_Write_FileNumber;
913 42518 : ph.FilePage = df_Write_FilePage;
914 : #if BLOCK_LOG_VALIDATE
915 : ph.crc = DF_LOGGING_FORMAT + df_Write_FilePage;
916 : #endif
917 42518 : memcpy(buffer, &ph, sizeof(ph));
918 42518 : const uint32_t pagesize = df_PageSize - sizeof(ph);
919 42518 : uint32_t nbytes = writebuf.read(&buffer[sizeof(ph)], pagesize);
920 42518 : if (nbytes < pagesize) {
921 3 : memset(&buffer[sizeof(ph) + nbytes], 0, pagesize - nbytes);
922 : }
923 42518 : FinishWrite();
924 42518 : df_Write_FilePage++;
925 42518 : }
926 :
927 0 : void AP_Logger_Block::flash_test()
928 : {
929 0 : uint32_t pages_to_check = 128;
930 0 : for (uint32_t i=1; i<=pages_to_check; i++) {
931 0 : if ((i-1) % df_PagePerBlock == 0) {
932 0 : printf("Block erase %u\n", get_block(i));
933 0 : SectorErase(get_block(i));
934 : }
935 0 : memset(buffer, uint8_t(i), df_PageSize);
936 0 : if (i<5) {
937 0 : printf("Flash fill 0x%x\n", uint8_t(i));
938 0 : } else if (i==5) {
939 0 : printf("Flash fill pages 5-%u\n", pages_to_check);
940 : }
941 0 : BufferToPage(i);
942 : }
943 0 : for (uint32_t i=1; i<=pages_to_check; i++) {
944 0 : if (i<5) {
945 0 : printf("Flash check 0x%x\n", uint8_t(i));
946 0 : } else if (i==5) {
947 0 : printf("Flash check pages 5-%u\n", pages_to_check);
948 : }
949 0 : PageToBuffer(i);
950 0 : uint32_t bad_bytes = 0;
951 0 : uint32_t first_bad_byte = 0;
952 0 : for (uint32_t j=0; j<df_PageSize; j++) {
953 0 : if (buffer[j] != uint8_t(i)) {
954 0 : bad_bytes++;
955 0 : if (bad_bytes == 1) {
956 0 : first_bad_byte = j;
957 : }
958 : }
959 : }
960 0 : if (bad_bytes > 0) {
961 0 : printf("Test failed: page %u, %u of %u bad bytes, first=0x%x\n",
962 0 : i, bad_bytes, df_PageSize, buffer[first_bad_byte]);
963 : }
964 : }
965 :
966 : // speed test
967 0 : pages_to_check = 4096; // 1mB / 8mB
968 0 : for (uint32_t i=1; i<=pages_to_check; i++) {
969 0 : if ((i-1) % df_PagePerBlock == 0) {
970 0 : SectorErase(get_block(i));
971 : }
972 : }
973 0 : uint32_t now_ms = AP_HAL::millis();
974 0 : for (uint32_t i=1; i<=pages_to_check; i++) {
975 0 : BufferToPage(i);
976 : }
977 0 : printf("Flash speed test: %ukB/s\n", unsigned((pages_to_check * df_PageSize * 1000) / (1024 * (AP_HAL::millis() - now_ms))));
978 0 : }
979 :
980 : #endif // HAL_LOGGING_BLOCK_ENABLED
|
