2 * Copyright (c) 2017-2020 Tilman Sauerbeck (tilman at code-monkey de)
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:
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
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.
27 use systick::elapsed_ms;
33 pub const MEMORY_SIZE: usize = 2 << 20;
34 const SECTOR_SIZE: usize = 4 << 10;
36 const NUM_SECTORS: usize = MEMORY_SIZE / SECTOR_SIZE;
38 #[derive(Clone, Copy, PartialEq, Debug)]
48 // Overrides InUse. The idea is to have erased flash sectors
49 // (0xff..ff) be detected as not in use.
54 #[derive(Clone, Copy)]
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.
62 fn new() -> SectorHeader {
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;
74 (self.flags & mask) == value
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;
83 (self.flags & mask) == value
86 fn belongs_to(&self, recording_id: u16) -> bool {
87 self.is_in_use() && self.recording_id == recording_id
91 #[derive(Clone, Copy)]
99 fn new() -> InFlight {
108 pub struct Logger<'a> {
109 pub storage: &'a mut dyn Storage,
111 recording_id: u16, // Zero is considered invalid.
113 // The index of the first sector of the currently running recording.
114 // Only written in logger_start_recording.
117 recording_started: u32,
119 // The total distance logged of the currently running recording.
120 pub total_distance_cm: u32,
122 // The number of slots filled in num_flight.
123 num_in_flight: usize,
125 // Deltas not yet written out to write_buffer.
127 // Limiting ourselves to 7 items here means we can use
128 // 0xff as a padding byte.
129 in_flight: [InFlight; 7],
131 sector_header: [SectorHeader; NUM_SECTORS],
133 sectors_written: u16,
135 write_buffer_offset: usize,
136 write_buffer: [u8; SECTOR_SIZE],
139 struct SectorHeaderIter<'a> {
140 sector_header: &'a [SectorHeader; NUM_SECTORS],
143 indices: [u16; NUM_SECTORS],
146 fn cmp_sector_header_indices(a: u16, b: u16,
147 sector_header: &[SectorHeader]) -> i32 {
148 let header_a = §or_header[a as usize];
149 let header_b = §or_header[b as usize];
151 if header_a.starts_recording() && header_b.starts_recording() {
152 // Latest entries come first.
153 if header_a.start_time > header_b.start_time {
155 } else if header_a.start_time < header_b.start_time {
160 } else if header_a.starts_recording() {
162 } else if header_b.starts_recording() {
173 fn downheap(heap: &mut [u16], mut index: usize, num_elts: usize,
174 sector_header: &[SectorHeader]) {
175 let orig = heap[index];
178 let mut worker = index * 2;
180 if worker < num_elts &&
181 cmp_sector_header_indices(heap[worker], heap[worker + 1], sector_header) < 0 {
185 if worker > num_elts ||
186 cmp_sector_header_indices(orig, heap[worker], sector_header) >= 0 {
190 heap[index] = heap[worker];
197 impl<'a> SectorHeaderIter<'a> {
198 fn new(logger: &'a Logger) -> SectorHeaderIter<'a> {
199 let mut iter = SectorHeaderIter {
200 sector_header: &logger.sector_header,
202 it_back: NUM_SECTORS,
203 indices: [0; NUM_SECTORS]
206 for i in 0..NUM_SECTORS {
207 iter.indices[i] = i as u16;
210 iter.sort(NUM_SECTORS);
213 // Need to handle those sectors that don't start recordings
214 // but that are still used.
219 fn sort(&mut self, num_elts_to_sort: usize) {
220 for i in (1..((num_elts_to_sort + 1) / 2) + 1).rev() {
221 downheap(&mut self.indices, i - 1, num_elts_to_sort - 1,
225 for i in (1..num_elts_to_sort).rev() {
226 self.indices.swap(0, i);
228 downheap(&mut self.indices, 0, i - 1, self.sector_header);
233 impl<'a> Iterator for SectorHeaderIter<'a> {
236 fn next(&mut self) -> Option<usize> {
237 if self.it_front == self.it_back {
240 let next_index = self.indices[self.it_front] as usize;
249 impl<'a> DoubleEndedIterator for SectorHeaderIter<'a> {
250 fn next_back(&mut self) -> Option<usize> {
251 if self.it_back == self.it_front {
256 let next_index = self.indices[self.it_back] as usize;
263 fn normalize_angle(mut angle: i32) -> i32 {
264 let deg90 = 90 * 60 * 10000;
265 let deg180 = deg90 << 1;
266 let deg360 = deg180 << 1;
268 while angle >= deg180 {
272 while angle <= -deg180 {
279 fn max<T>(a: T, b: T) -> T
280 where T: PartialOrd {
288 impl<'a> Logger<'a> {
289 pub fn new(storage: &'a mut dyn Storage) -> Logger {
295 recording_started: 0,
296 total_distance_cm: 0,
299 in_flight: [InFlight::new(); 7],
300 sector_header: [SectorHeader::new(); NUM_SECTORS],
304 write_buffer_offset: 0,
305 write_buffer: [0xff; SECTOR_SIZE],
309 pub fn init(&mut self) {
310 // Reading the directory entries one by one means
311 // we won't need an as large buffer on the stack.
312 for i in 0..NUM_SECTORS {
313 self.read_sector_header(i);
317 fn read_sector_header(&mut self, sector_index: usize) {
318 let address = sector_index * SECTOR_SIZE;
319 let mut chunk = [0u8; 8];
321 self.storage.read(address, &mut chunk);
323 let sector_header_ptr: *mut SectorHeader =
324 &mut self.sector_header[sector_index];
327 core::ptr::copy(chunk.as_ptr(),
328 sector_header_ptr as *mut u8,
333 fn prepare_write_buffer(&mut self, is_initial: bool) {
334 self.write_buffer = [0xff; SECTOR_SIZE];
336 let flags = if is_initial {
337 (SectorFlag::InUse as u16)
339 (SectorFlag::InUse as u16) | (SectorFlag::DataRecord as u16)
342 // Write sector header.
343 self.write_buffer[0..2].copy_from_slice(&flags.to_le_bytes());
344 self.write_buffer[2..4].copy_from_slice(&self.recording_id.to_le_bytes());
346 self.write_buffer_offset = 4;
349 let start = self.write_buffer_offset;
352 self.write_buffer[start..end].copy_from_slice(
353 &self.recording_started.to_le_bytes());
355 self.write_buffer_offset += 4;
359 pub fn start_recording(&mut self, tap: &TimeAndPos) -> u16 {
360 self.find_next_record_slot();
362 self.sectors_written = 0;
363 self.recording_started = tap.unix_time;
364 self.total_distance_cm = 0;
365 self.num_in_flight = 0;
367 self.prepare_write_buffer(true);
369 self.write_packet(0, tap.latitude_deg, tap.longitude_deg);
374 pub fn log(&mut self, prev_tap: &TimeAndPos, tap: &TimeAndPos) {
375 self.total_distance_cm += tap.distance_cm(&prev_tap) as u32;
377 let d_time_ms = elapsed_ms(tap.system_time, prev_tap.system_time);
379 // We know that our hardware cannot deliver updates more often
380 // than once a second. However when there's a delay in evaluating
381 // the hardware's messages, we will end up with intervals like
382 // 1050ms and 950ms (the latter will "make up" for the slowness
383 // in the former). To avoid logging deltas of 0 seconds, we round
384 // the intervals to full seconds.
385 let d_time_s = (d_time_ms + 500) / 1000;
387 let d_lat = tap.latitude_deg - prev_tap.latitude_deg;
388 let d_lon = tap.longitude_deg - prev_tap.longitude_deg;
390 if self.write_packet(d_time_s, d_lat, d_lon) {
391 self.flush_in_flight(false);
395 pub fn stop_recording(&mut self, tap: &TimeAndPos) -> u16 {
396 // Mark the end of the points stream.
397 self.write_packet(0xffffffff, 0, 0);
398 self.flush_in_flight(true);
401 let duration = (tap.unix_time - self.recording_started) as u16;
403 let start = self.write_buffer_offset;
405 let dst = &mut self.write_buffer[start..end];
407 dst.copy_from_slice(&duration.to_le_bytes());
409 let this_sector = self.first_sector + self.sectors_written;
411 if self.storage.write(this_sector as usize * SECTOR_SIZE,
412 &self.write_buffer).is_err() {
416 self.sectors_written += 1;
418 for i in 0..self.sectors_written {
419 self.read_sector_header((self.first_sector + i) as usize);
425 fn sector_header_iter(&self) -> SectorHeaderIter {
426 SectorHeaderIter::new(self)
429 fn find_next_record_slot(&mut self) {
430 let mut candidate_index = 0;
431 let mut max_recording_id = 0;
433 for index in self.sector_header_iter() {
434 candidate_index = index;
436 let sector_header = &self.sector_header[index];
438 if !sector_header.is_in_use() {
439 // Due to our sorting we know that there will be no more
440 // used directory entries following. At this point
441 // we aren't interested in unused ones, so break the loop.
446 max(max_recording_id, sector_header.recording_id);
449 self.first_sector = candidate_index as u16;
450 self.recording_id = max_recording_id.wrapping_add(1);
453 fn write_packet(&mut self, d_time_s: u32, d_lat: i32, d_lon: i32) -> bool {
455 let in_flight = &mut self.in_flight[self.num_in_flight];
457 in_flight.d_time_s = d_time_s;
458 in_flight.d_lat = normalize_angle(d_lat);
459 in_flight.d_lon = normalize_angle(d_lon);
462 self.num_in_flight += 1;
464 self.num_in_flight == self.in_flight.len()
467 // Flushes the "in flight" items to the write buffer.
469 // @param is_final @c true iff this is the final flush in this recording.
471 // @note May only be called if logger.num_in_flight is greater than zero.
472 fn flush_in_flight(&mut self, is_final: bool) {
475 // Normally our items will have a time delta of one second.
476 // Mark the ones that differ from that.
477 for i in 0..self.num_in_flight {
478 if self.in_flight[i].d_time_s != 1 {
483 let mut buffer = [0u8; 128];
486 buffer[offset] = flags;
489 for i in 0..(self.num_in_flight - 1) {
490 let in_flight = &self.in_flight[i];
492 // Only write the time delta for the atypical cases.
493 if (flags & (1 << i)) != 0 {
495 varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
499 varint::write_s32(&mut buffer[offset..], in_flight.d_lat);
502 varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
505 let i = self.num_in_flight - 1;
506 let in_flight = &self.in_flight[i];
508 // Only write the time delta for the atypical cases.
509 if (flags & (1 << i)) != 0 {
511 varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
514 // The final point is an end-of-stream marker and doesn't store
518 varint::write_s32(&mut buffer[offset..], in_flight.d_lat);
521 varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
524 self.num_in_flight = 0;
526 let num_bytes_written = offset;
528 let remaining = self.write_buffer.len() - self.write_buffer_offset;
530 if remaining < num_bytes_written {
531 // We may use 0xff as padding bytes, since 0xff isn't a valid
532 // first byte in a points batch. prepare_write_buffer() fills
533 // our buffer with 0xff, so we don't need to do anything here.
534 let this_sector = self.first_sector + self.sectors_written;
536 if self.storage.write(this_sector as usize * SECTOR_SIZE,
537 &self.write_buffer).is_err() {
541 self.sectors_written += 1;
543 self.prepare_write_buffer(false);
546 let start = self.write_buffer_offset;
547 let end = start + num_bytes_written;
548 let dst = &mut self.write_buffer[start..end];
550 dst.copy_from_slice(&buffer[0..num_bytes_written]);
552 self.write_buffer_offset += num_bytes_written;
556 /// Write a listing of the stored recordings to @p tx_buf.
557 pub fn list_recordings(&mut self, tx_buf: &mut Buffer) {
558 for index in self.sector_header_iter().rev() {
559 let sector_header = &self.sector_header[index as usize];
561 if !sector_header.starts_recording() {
565 let mut num_data_sectors = 0;
567 for d in 1..NUM_SECTORS {
568 let wrapped_index = ((index + d) & (NUM_SECTORS - 1)) as usize;
569 let other_sector_header = &self.sector_header[wrapped_index];
571 if other_sector_header.belongs_to(sector_header.recording_id) {
572 num_data_sectors += 1;
576 let mut date_time_s = [b' '; 19];
578 if let Some(tm) = Time::from_unix_time(sector_header.start_time) {
579 tm.fmt_date(&mut date_time_s[0..]);
580 tm.fmt_time(&mut date_time_s[11..]);
583 let recording_size = (num_data_sectors + 1) * (SECTOR_SIZE >> 10);
585 let mut recording_size_s = [b'0'; 9];
586 let recording_size_s_len = fmt_u32_pad(&mut recording_size_s,
587 recording_size as u32,
590 let mut recording_id_s = [b'0'; 9];
591 let recording_id_s_len =
592 fmt_u32_pad(&mut recording_id_s,
593 sector_header.recording_id as u32,
596 tx_buf.write(&date_time_s);
597 tx_buf.write(&recording_size_s[0..recording_size_s_len]);
599 tx_buf.write(&recording_id_s[0..recording_id_s_len]);
607 /// Check whether or not recording @p recording_id exists.
608 pub fn has_recording(&mut self, recording_id: u16) -> bool {
609 if recording_id == 0 {
613 self.sector_header_iter().find(|&index| {
614 let sector_header = &self.sector_header[index as usize];
616 sector_header.recording_id == recording_id &&
617 sector_header.starts_recording()
622 /// Retrieve recording @p recording_id and
623 /// write it to @p tx_buf in yencoded form.
624 pub fn get_recording(&mut self, recording_id: u16,
625 tx_buf: &mut Buffer) -> Result<(), Error> {
626 if recording_id == 0 {
627 return Err(Error::NoSuchRecording);
630 if let Some(found_index) = self.sector_header_iter().find(|&index| {
631 let sector_header = &self.sector_header[index as usize];
633 sector_header.recording_id == recording_id &&
634 sector_header.starts_recording()
636 let mut filename = [b' '; 29];
638 filename[0..].copy_from_slice(b"gps-watch-recording-XXXXX.bin");
640 fmt_u32_pad(&mut filename[20..], recording_id as u32, 5, b'0');
642 let mut yenc = Yencode::new(tx_buf);
644 yenc.start(&filename);
646 let format_version = 1u8;
647 yenc.data(&[format_version]);
649 let mut next_sector = found_index as usize;
651 for _ in 0..NUM_SECTORS {
652 let address = next_sector * SECTOR_SIZE;
653 let mut buf = [0u8; SECTOR_SIZE];
655 self.storage.read(address, &mut buf);
657 // Skip flags and recording ID.
658 yenc.data(&buf[4..]);
661 next_sector &= NUM_SECTORS - 1;
663 if !self.sector_header[next_sector].belongs_to(recording_id) {
674 Err(Error::NoSuchRecording)
679 /// Remove recording @p recording_id.
680 pub fn remove_recording(&mut self, recording_id: u16) -> Result<(), Error> {
681 if let Some(found_index) = (0..NUM_SECTORS).find(|&index| {
682 let sector_header = &self.sector_header[index as usize];
684 sector_header.recording_id == recording_id &&
685 sector_header.starts_recording()
687 let mut next_sector = found_index as usize;
689 for _ in 0..NUM_SECTORS {
690 let address = next_sector * SECTOR_SIZE;
692 if let Err(_) = self.storage.erase(address) {
693 return Err(Error::StorageError);
696 // Mark this sector as eligible for the next recording
697 // and ensure it won't be picked up by list_recordings().
698 self.read_sector_header(next_sector);
701 next_sector &= NUM_SECTORS - 1;
703 if !self.sector_header[next_sector].belongs_to(recording_id) {
710 Err(Error::NoSuchRecording)