98a857a660ed1cbcebdbecb06cf5c8f7738f34ca
[gps-watch.git] / src / common / logger.rs
1 /*
2  * Copyright (c) 2017-2020 Tilman Sauerbeck (tilman at code-monkey de)
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining
5  * a copy of this software and associated documentation files (the
6  * "Software"), to deal in the Software without restriction, including
7  * without limitation the rights to use, copy, modify, merge, publish,
8  * distribute, sublicense, and/or sell copies of the Software, and to
9  * permit persons to whom the Software is furnished to do so, subject to
10  * the following conditions:
11  *
12  * The above copyright notice and this permission notice shall be
13  * included in all copies or substantial portions of the Software.
14  *
15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
18  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
19  * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
20  * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
21  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22  */
23
24 use gps::TimeAndPos;
25 use storage::Storage;
26 use varint;
27 use systick::elapsed_ms;
28 use buffer::Buffer;
29 use yencode::Yencode;
30 use fmt::*;
31 use time::Time;
32
33 pub const MEMORY_SIZE: usize = 2 << 20;
34 const SECTOR_SIZE: usize = 4 << 10;
35
36 const NUM_SECTORS: usize = MEMORY_SIZE / SECTOR_SIZE;
37
38 #[derive(Clone, Copy, PartialEq, Debug)]
39 pub enum Error {
40     NoSuchRecording,
41     StorageError,
42 }
43
44 enum SectorFlag {
45     InUse      = 1 << 0,
46     DataRecord = 1 << 1,
47
48     // Overrides InUse. The idea is to have erased flash sectors
49     // (0xff..ff) be detected as not in use.
50     NotInUse   = 1 << 7,
51 }
52
53 #[repr(C)]
54 #[derive(Clone, Copy)]
55 struct SectorHeader {
56     flags: u16, // Combination of SectorFlag items.
57     recording_id: u16, // Zero is considered invalid.
58     start_time: u32, // UNIX time. Only present if flag DataRecord isn't set.
59 }
60
61 impl SectorHeader {
62     fn new() -> SectorHeader {
63         SectorHeader {
64             flags: 0,
65             recording_id: 0,
66             start_time: 0,
67         }
68     }
69
70     fn is_in_use(&self) -> bool {
71         let mask = (SectorFlag::InUse as u16) | (SectorFlag::NotInUse as u16);
72         let value = SectorFlag::InUse as u16;
73
74         (self.flags & mask) == value
75     }
76
77     fn starts_recording(&self) -> bool {
78         let mask = (SectorFlag::InUse as u16)
79                  | (SectorFlag::DataRecord as u16)
80                  | (SectorFlag::NotInUse as u16);
81         let value = SectorFlag::InUse as u16;
82
83         (self.flags & mask) == value
84     }
85
86     fn belongs_to(&self, recording_id: u16) -> bool {
87         self.is_in_use() && self.recording_id == recording_id
88     }
89 }
90
91 #[derive(Clone, Copy)]
92 struct InFlight {
93     d_time_s: u32,
94     d_lat: i32,
95     d_lon: i32,
96 }
97
98 impl InFlight {
99     fn new() -> InFlight {
100         InFlight {
101             d_time_s: 0,
102             d_lat: 0,
103             d_lon: 0,
104         }
105     }
106 }
107
108 pub struct Logger<'a> {
109     pub storage: &'a mut dyn Storage,
110
111     recording_id: u16, // Zero is considered invalid.
112
113     // The index of the first sector of the currently running recording.
114     // Only written in logger_start_recording.
115     first_sector: u16,
116
117     recording_started: u32,
118
119     // The total distance logged of the currently running recording.
120     pub total_distance_cm: u32,
121
122     split_distance_cm: u32,
123     split_duration_ms: u32,
124     pub pace_s: u32,
125
126     // The number of slots filled in num_flight.
127     num_in_flight: usize,
128
129     // Deltas not yet written out to write_buffer.
130     //
131     // Limiting ourselves to 7 items here means we can use
132     // 0xff as a padding byte.
133     in_flight: [InFlight; 7],
134
135     sector_header: [SectorHeader; NUM_SECTORS],
136
137     sectors_written: u16,
138
139     write_buffer_offset: usize,
140     write_buffer: [u8; SECTOR_SIZE],
141 }
142
143 struct SectorHeaderIter<'a> {
144     sector_header: &'a [SectorHeader; NUM_SECTORS],
145     it_front: usize,
146     it_back: usize,
147     indices: [u16; NUM_SECTORS],
148 }
149
150 fn cmp_sector_header_indices(a: u16, b: u16,
151                              sector_header: &[SectorHeader]) -> i32 {
152     let header_a = &sector_header[a as usize];
153     let header_b = &sector_header[b as usize];
154
155     if header_a.starts_recording() && header_b.starts_recording() {
156         // Latest entries come first.
157         if header_a.start_time > header_b.start_time {
158             -1
159         } else if header_a.start_time < header_b.start_time {
160             1
161         } else {
162             0
163         }
164     } else if header_a.starts_recording() {
165         -1
166     } else if header_b.starts_recording() {
167         1
168     } else if a < b {
169         -1
170     } else if a > b {
171         1
172     } else {
173         0
174     }
175 }
176
177 fn downheap(heap: &mut [u16], mut index: usize, num_elts: usize,
178             sector_header: &[SectorHeader]) {
179     let orig = heap[index];
180
181     loop {
182         let mut worker = index * 2;
183
184         if worker < num_elts &&
185            cmp_sector_header_indices(heap[worker], heap[worker + 1], sector_header) < 0 {
186             worker += 1;
187         }
188
189         if worker > num_elts ||
190            cmp_sector_header_indices(orig, heap[worker], sector_header) >= 0 {
191             break;
192         }
193
194         heap[index] = heap[worker];
195         index = worker;
196     }
197
198     heap[index] = orig;
199 }
200
201 impl<'a> SectorHeaderIter<'a> {
202     fn new(logger: &'a Logger) -> SectorHeaderIter<'a> {
203         let mut iter = SectorHeaderIter {
204             sector_header: &logger.sector_header,
205             it_front: 0,
206             it_back: NUM_SECTORS,
207             indices: [0; NUM_SECTORS]
208         };
209
210         for i in 0..NUM_SECTORS {
211             iter.indices[i] = i as u16;
212         }
213
214         iter.sort(NUM_SECTORS);
215
216         // XXX:
217         // Need to handle those sectors that don't start recordings
218         // but that are still used.
219
220         iter
221     }
222
223     fn sort(&mut self, num_elts_to_sort: usize) {
224         for i in (1..((num_elts_to_sort + 1) / 2) + 1).rev() {
225             downheap(&mut self.indices, i - 1, num_elts_to_sort - 1,
226                      self.sector_header);
227         }
228
229         for i in (1..num_elts_to_sort).rev() {
230             self.indices.swap(0, i);
231
232             downheap(&mut self.indices, 0, i - 1, self.sector_header);
233         }
234     }
235 }
236
237 impl<'a> Iterator for SectorHeaderIter<'a> {
238     type Item = usize;
239
240     fn next(&mut self) -> Option<usize> {
241         if self.it_front == self.it_back {
242             None
243         } else {
244             let next_index = self.indices[self.it_front] as usize;
245
246             self.it_front += 1;
247
248             Some(next_index)
249         }
250     }
251 }
252
253 impl<'a> DoubleEndedIterator for SectorHeaderIter<'a> {
254     fn next_back(&mut self) -> Option<usize> {
255         if self.it_back == self.it_front {
256             None
257         } else {
258             self.it_back -= 1;
259
260             let next_index = self.indices[self.it_back] as usize;
261
262             Some(next_index)
263         }
264     }
265 }
266
267 fn normalize_angle(mut angle: i32) -> i32 {
268     let deg90 = 90 * 60 * 10000;
269     let deg180 = deg90 << 1;
270     let deg360 = deg180 << 1;
271
272     while angle >= deg180 {
273         angle -= deg360;
274     }
275
276     while angle <= -deg180 {
277         angle += deg360;
278     }
279
280     angle
281 }
282
283 fn max<T>(a: T, b: T) -> T
284           where T: PartialOrd {
285     if a > b {
286         a
287     } else {
288         b
289     }
290 }
291
292 impl<'a> Logger<'a> {
293     pub fn new(storage: &'a mut dyn Storage) -> Logger {
294         Logger {
295             storage: storage,
296
297             recording_id: 0,
298             first_sector: 0,
299             recording_started: 0,
300             total_distance_cm: 0,
301             split_distance_cm: 0,
302             split_duration_ms: 0,
303             pace_s: 0,
304             num_in_flight: 0,
305
306             in_flight: [InFlight::new(); 7],
307             sector_header: [SectorHeader::new(); NUM_SECTORS],
308
309             sectors_written: 0,
310
311             write_buffer_offset: 0,
312             write_buffer: [0xff; SECTOR_SIZE],
313         }
314     }
315
316     pub fn init(&mut self) {
317         // Reading the directory entries one by one means
318         // we won't need an as large buffer on the stack.
319         for i in 0..NUM_SECTORS {
320             self.read_sector_header(i);
321         }
322     }
323
324     fn read_sector_header(&mut self, sector_index: usize) {
325         let address = sector_index * SECTOR_SIZE;
326         let mut chunk = [0u8; 8];
327
328         self.storage.read(address, &mut chunk);
329
330         let sector_header_ptr: *mut SectorHeader =
331             &mut self.sector_header[sector_index];
332
333         unsafe {
334             core::ptr::copy(chunk.as_ptr(),
335                             sector_header_ptr as *mut u8,
336                             chunk.len());
337         }
338     }
339
340     fn prepare_write_buffer(&mut self, is_initial: bool) {
341         self.write_buffer = [0xff; SECTOR_SIZE];
342
343         let flags = if is_initial {
344             (SectorFlag::InUse as u16)
345         } else {
346             (SectorFlag::InUse as u16) | (SectorFlag::DataRecord as u16)
347         };
348
349         // Write sector header.
350         self.write_buffer[0..2].copy_from_slice(&flags.to_le_bytes());
351         self.write_buffer[2..4].copy_from_slice(&self.recording_id.to_le_bytes());
352
353         self.write_buffer_offset = 4;
354
355         if is_initial {
356             let start = self.write_buffer_offset;
357             let end = start + 4;
358
359             self.write_buffer[start..end].copy_from_slice(
360                 &self.recording_started.to_le_bytes());
361
362             self.write_buffer_offset += 4;
363         }
364     }
365
366     pub fn start_recording(&mut self, tap: &TimeAndPos) -> u16 {
367         self.find_next_record_slot();
368
369         self.sectors_written = 0;
370         self.recording_started = tap.unix_time;
371         self.total_distance_cm = 0;
372         self.split_distance_cm = 0;
373         self.split_duration_ms = 0;
374         self.pace_s = 0;
375         self.num_in_flight = 0;
376
377         self.prepare_write_buffer(true);
378
379         self.write_packet(0, tap.latitude_deg, tap.longitude_deg);
380
381         self.recording_id
382     }
383
384     pub fn log(&mut self, prev_tap: &TimeAndPos, tap: &TimeAndPos) {
385         let d_time_ms = elapsed_ms(tap.system_time, prev_tap.system_time);
386
387         // We know that our hardware cannot deliver updates more often
388         // than once a second. However when there's a delay in evaluating
389         // the hardware's messages, we will end up with intervals like
390         // 1050ms and 950ms (the latter will "make up" for the slowness
391         // in the former). To avoid logging deltas of 0 seconds, we round
392         // the intervals to full seconds.
393         let d_time_s = (d_time_ms + 500) / 1000;
394
395         let d_lat = tap.latitude_deg - prev_tap.latitude_deg;
396         let d_lon = tap.longitude_deg - prev_tap.longitude_deg;
397
398         if self.write_packet(d_time_s, d_lat, d_lon) {
399             self.flush_in_flight(false);
400         }
401
402         let distance_cm = tap.distance_cm(&prev_tap) as u32;
403
404         self.total_distance_cm += distance_cm;
405         self.split_distance_cm += distance_cm;
406
407         self.split_duration_ms += d_time_ms;
408
409         if self.split_distance_cm >= 100_000 {
410             self.split_distance_cm -= 100_000;
411
412             self.pace_s = self.split_duration_ms / 1000;
413             self.split_duration_ms = 0;
414         }
415     }
416
417     pub fn stop_recording(&mut self, tap: &TimeAndPos) -> u16 {
418         // Mark the end of the points stream.
419         self.write_packet(0xffffffff, 0, 0);
420         self.flush_in_flight(true);
421
422         // Write footer.
423         let duration = (tap.unix_time - self.recording_started) as u16;
424
425         let start = self.write_buffer_offset;
426         let end = start + 2;
427         let dst = &mut self.write_buffer[start..end];
428
429         dst.copy_from_slice(&duration.to_le_bytes());
430
431         let this_sector = self.first_sector + self.sectors_written;
432
433         if self.storage.write(this_sector as usize * SECTOR_SIZE,
434                               &self.write_buffer).is_err() {
435             // XXX
436         }
437
438         self.sectors_written += 1;
439
440         for i in 0..self.sectors_written {
441             self.read_sector_header((self.first_sector + i) as usize);
442         }
443
444         self.sectors_written
445     }
446
447     fn sector_header_iter(&self) -> SectorHeaderIter {
448         SectorHeaderIter::new(self)
449     }
450
451     fn find_next_record_slot(&mut self) {
452         let mut candidate_index = 0;
453         let mut max_recording_id = 0;
454
455         for index in self.sector_header_iter() {
456             candidate_index = index;
457
458             let sector_header = &self.sector_header[index];
459
460             if !sector_header.is_in_use() {
461                 // Due to our sorting we know that there will be no more
462                 // used directory entries following. At this point
463                 // we aren't interested in unused ones, so break the loop.
464                 break;
465             }
466
467             max_recording_id =
468                 max(max_recording_id, sector_header.recording_id);
469         }
470
471         self.first_sector = candidate_index as u16;
472         self.recording_id = max_recording_id.wrapping_add(1);
473     }
474
475     fn write_packet(&mut self, d_time_s: u32, d_lat: i32, d_lon: i32) -> bool {
476         {
477             let in_flight = &mut self.in_flight[self.num_in_flight];
478
479             in_flight.d_time_s = d_time_s;
480             in_flight.d_lat = normalize_angle(d_lat);
481             in_flight.d_lon = normalize_angle(d_lon);
482         }
483
484         self.num_in_flight += 1;
485
486         self.num_in_flight == self.in_flight.len()
487     }
488
489     // Flushes the "in flight" items to the write buffer.
490     //
491     // @param is_final @c true iff this is the final flush in this recording.
492     //
493     // @note May only be called if logger.num_in_flight is greater than zero.
494     fn flush_in_flight(&mut self, is_final: bool) {
495         let mut flags = 0u8;
496
497         // Normally our items will have a time delta of one second.
498         // Mark the ones that differ from that.
499         for i in 0..self.num_in_flight {
500             if self.in_flight[i].d_time_s != 1 {
501                 flags |= 1 << i;
502             }
503         }
504
505         let mut buffer = [0u8; 128];
506         let mut offset = 0;
507
508         buffer[offset] = flags;
509         offset += 1;
510
511         for i in 0..(self.num_in_flight - 1) {
512             let in_flight = &self.in_flight[i];
513
514             // Only write the time delta for the atypical cases.
515             if (flags & (1 << i)) != 0 {
516                 offset +=
517                     varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
518             }
519
520             offset +=
521                 varint::write_s32(&mut buffer[offset..], in_flight.d_lat);
522
523             offset +=
524                 varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
525         }
526
527         let i = self.num_in_flight - 1;
528         let in_flight = &self.in_flight[i];
529
530         // Only write the time delta for the atypical cases.
531         if (flags & (1 << i)) != 0 {
532             offset +=
533                 varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
534         }
535
536         // The final point is an end-of-stream marker and doesn't store
537         // d_lat or d_lon.
538         if !is_final {
539             offset +=
540                 varint::write_s32(&mut buffer[offset..], in_flight.d_lat);
541
542             offset +=
543                 varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
544         }
545
546         self.num_in_flight = 0;
547
548         let num_bytes_written = offset;
549
550         let remaining = self.write_buffer.len() - self.write_buffer_offset;
551
552         if remaining < num_bytes_written {
553             // We may use 0xff as padding bytes, since 0xff isn't a valid
554             // first byte in a points batch. prepare_write_buffer() fills
555             // our buffer with 0xff, so we don't need to do anything here.
556             let this_sector = self.first_sector + self.sectors_written;
557
558             if self.storage.write(this_sector as usize * SECTOR_SIZE,
559                                   &self.write_buffer).is_err() {
560                 // XXX
561             }
562
563             self.sectors_written += 1;
564
565             self.prepare_write_buffer(false);
566         }
567
568         let start = self.write_buffer_offset;
569         let end = start + num_bytes_written;
570         let dst = &mut self.write_buffer[start..end];
571
572         dst.copy_from_slice(&buffer[0..num_bytes_written]);
573
574         self.write_buffer_offset += num_bytes_written;
575     }
576
577     ///
578     /// Write a listing of the stored recordings to @p tx_buf.
579     pub fn list_recordings(&mut self, tx_buf: &mut Buffer) {
580         for index in self.sector_header_iter().rev() {
581             let sector_header = &self.sector_header[index as usize];
582
583             if !sector_header.starts_recording() {
584                 continue;
585             }
586
587             let mut num_data_sectors = 0;
588
589             for d in 1..NUM_SECTORS {
590                 let wrapped_index = ((index + d) & (NUM_SECTORS - 1)) as usize;
591                 let other_sector_header = &self.sector_header[wrapped_index];
592
593                 if other_sector_header.belongs_to(sector_header.recording_id) {
594                     num_data_sectors += 1;
595                 }
596             }
597
598             let mut date_time_s = [b' '; 19];
599
600             if let Some(tm) = Time::from_unix_time(sector_header.start_time) {
601                 tm.fmt_date(&mut date_time_s[0..]);
602                 tm.fmt_time(&mut date_time_s[11..]);
603             }
604
605             let recording_size = (num_data_sectors + 1) * (SECTOR_SIZE >> 10);
606
607             let mut recording_size_s = [b'0'; 9];
608             let recording_size_s_len = fmt_u32_pad(&mut recording_size_s,
609                                                    recording_size as u32,
610                                                    8, b' ');
611
612             let mut recording_id_s = [b'0'; 9];
613             let recording_id_s_len =
614                 fmt_u32_pad(&mut recording_id_s,
615                             sector_header.recording_id as u32,
616                             8, b' ');
617
618             tx_buf.write(&date_time_s);
619             tx_buf.write(&recording_size_s[0..recording_size_s_len]);
620             tx_buf.write(b"K");
621             tx_buf.write(&recording_id_s[0..recording_id_s_len]);
622             tx_buf.write(b"\n");
623
624             tx_buf.flush();
625         }
626     }
627
628     ///
629     /// Check whether or not recording @p recording_id exists.
630     pub fn has_recording(&mut self, recording_id: u16) -> bool {
631         if recording_id == 0 {
632             return false;
633         }
634
635         self.sector_header_iter().find(|&index| {
636             let sector_header = &self.sector_header[index as usize];
637
638             sector_header.recording_id == recording_id &&
639             sector_header.starts_recording()
640         }).is_some()
641     }
642
643     ///
644     /// Retrieve recording @p recording_id and
645     /// write it to @p tx_buf in yencoded form.
646     pub fn get_recording(&mut self, recording_id: u16,
647                          tx_buf: &mut Buffer) -> Result<(), Error> {
648         if recording_id == 0 {
649             return Err(Error::NoSuchRecording);
650         }
651
652         if let Some(found_index) = self.sector_header_iter().find(|&index| {
653             let sector_header = &self.sector_header[index as usize];
654
655             sector_header.recording_id == recording_id &&
656             sector_header.starts_recording()
657         }) {
658             let mut filename = [b' '; 29];
659
660             filename[0..].copy_from_slice(b"gps-watch-recording-XXXXX.bin");
661
662             fmt_u32_pad(&mut filename[20..], recording_id as u32, 5, b'0');
663
664             let mut yenc = Yencode::new(tx_buf);
665
666             yenc.start(&filename);
667
668             let format_version = 1u8;
669             yenc.data(&[format_version]);
670
671             let mut next_sector = found_index as usize;
672
673             for _ in 0..NUM_SECTORS {
674                 let address = next_sector * SECTOR_SIZE;
675                 let mut buf = [0u8; SECTOR_SIZE];
676
677                 self.storage.read(address, &mut buf);
678
679                 // Skip flags and recording ID.
680                 yenc.data(&buf[4..]);
681
682                 next_sector += 1;
683                 next_sector &= NUM_SECTORS - 1;
684
685                 if !self.sector_header[next_sector].belongs_to(recording_id) {
686                     break;
687                 }
688             }
689
690             yenc.finish();
691
692             tx_buf.flush();
693
694             Ok(())
695         } else {
696             Err(Error::NoSuchRecording)
697         }
698     }
699
700     ///
701     /// Remove recording @p recording_id.
702     pub fn remove_recording(&mut self, recording_id: u16) -> Result<(), Error> {
703         if let Some(found_index) = (0..NUM_SECTORS).find(|&index| {
704             let sector_header = &self.sector_header[index as usize];
705
706             sector_header.recording_id == recording_id &&
707             sector_header.starts_recording()
708         }) {
709             let mut next_sector = found_index as usize;
710
711             for _ in 0..NUM_SECTORS {
712                 let address = next_sector * SECTOR_SIZE;
713
714                 if let Err(_) = self.storage.erase(address) {
715                     return Err(Error::StorageError);
716                 }
717
718                 // Mark this sector as eligible for the next recording
719                 // and ensure it won't be picked up by list_recordings().
720                 self.read_sector_header(next_sector);
721
722                 next_sector += 1;
723                 next_sector &= NUM_SECTORS - 1;
724
725                 if !self.sector_header[next_sector].belongs_to(recording_id) {
726                     break;
727                 }
728             }
729
730             Ok(())
731         } else {
732             Err(Error::NoSuchRecording)
733         }
734     }
735 }