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     // The number of slots filled in num_flight.
 123     num_in_flight: usize,
 124
 125     // Deltas not yet written out to write_buffer.
 126     //
 127     // Limiting ourselves to 7 items here means we can use
 128     // 0xff as a padding byte.
 129     in_flight: [InFlight; 7],
 130
 131     sector_header: [SectorHeader; NUM_SECTORS],
 132
 133     sectors_written: u16,
 134
 135     write_buffer_offset: usize,
 136     write_buffer: [u8; SECTOR_SIZE],
 137 }
 138
 139 struct SectorHeaderIter<'a> {
 140     sector_header: &'a [SectorHeader; NUM_SECTORS],
 141     it_front: usize,
 142     it_back: usize,
 143     indices: [u16; NUM_SECTORS],
 144 }
 145
 146 fn cmp_sector_header_indices(a: u16, b: u16,
 147                              sector_header: &[SectorHeader]) -> i32 {
 148     let header_a = &sector_header[a as usize];
 149     let header_b = &sector_header[b as usize];
 150
 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 {
 154             -1
 155         } else if header_a.start_time < header_b.start_time {
 156             1
 157         } else {
 158             0
 159         }
 160     } else if header_a.starts_recording() {
 161         -1
 162     } else if header_b.starts_recording() {
 163         1
 164     } else if a < b {
 165         -1
 166     } else if a > b {
 167         1
 168     } else {
 169         0
 170     }
 171 }
 172
 173 fn downheap(heap: &mut [u16], mut index: usize, num_elts: usize,
 174             sector_header: &[SectorHeader]) {
 175     let orig = heap[index];
 176
 177     loop {
 178         let mut worker = index * 2;
 179
 180         if worker < num_elts &&
 181            cmp_sector_header_indices(heap[worker], heap[worker + 1], sector_header) < 0 {
 182             worker += 1;
 183         }
 184
 185         if worker > num_elts ||
 186            cmp_sector_header_indices(orig, heap[worker], sector_header) >= 0 {
 187             break;
 188         }
 189
 190         heap[index] = heap[worker];
 191         index = worker;
 192     }
 193
 194     heap[index] = orig;
 195 }
 196
 197 impl<'a> SectorHeaderIter<'a> {
 198     fn new(logger: &'a Logger) -> SectorHeaderIter<'a> {
 199         let mut iter = SectorHeaderIter {
 200             sector_header: &logger.sector_header,
 201             it_front: 0,
 202             it_back: NUM_SECTORS,
 203             indices: [0; NUM_SECTORS]
 204         };
 205
 206         for i in 0..NUM_SECTORS {
 207             iter.indices[i] = i as u16;
 208         }
 209
 210         iter.sort(NUM_SECTORS);
 211
 212         // XXX:
 213         // Need to handle those sectors that don't start recordings
 214         // but that are still used.
 215
 216         iter
 217     }
 218
 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,
 222                      self.sector_header);
 223         }
 224
 225         for i in (1..num_elts_to_sort).rev() {
 226             self.indices.swap(0, i);
 227
 228             downheap(&mut self.indices, 0, i - 1, self.sector_header);
 229         }
 230     }
 231 }
 232
 233 impl<'a> Iterator for SectorHeaderIter<'a> {
 234     type Item = usize;
 235
 236     fn next(&mut self) -> Option<usize> {
 237         if self.it_front == self.it_back {
 238             None
 239         } else {
 240             let next_index = self.indices[self.it_front] as usize;
 241
 242             self.it_front += 1;
 243
 244             Some(next_index)
 245         }
 246     }
 247 }
 248
 249 impl<'a> DoubleEndedIterator for SectorHeaderIter<'a> {
 250     fn next_back(&mut self) -> Option<usize> {
 251         if self.it_back == self.it_front {
 252             None
 253         } else {
 254             self.it_back -= 1;
 255
 256             let next_index = self.indices[self.it_back] as usize;
 257
 258             Some(next_index)
 259         }
 260     }
 261 }
 262
 263 fn normalize_angle(mut angle: i32) -> i32 {
 264     let deg90 = 90 * 60 * 10000;
 265     let deg180 = deg90 << 1;
 266     let deg360 = deg180 << 1;
 267
 268     while angle >= deg180 {
 269         angle -= deg360;
 270     }
 271
 272     while angle <= -deg180 {
 273         angle += deg360;
 274     }
 275
 276     angle
 277 }
 278
 279 fn max<T>(a: T, b: T) -> T
 280           where T: PartialOrd {
 281     if a > b {
 282         a
 283     } else {
 284         b
 285     }
 286 }
 287
 288 impl<'a> Logger<'a> {
 289     pub fn new(storage: &'a mut dyn Storage) -> Logger {
 290         Logger {
 291             storage: storage,
 292
 293             recording_id: 0,
 294             first_sector: 0,
 295             recording_started: 0,
 296             total_distance_cm: 0,
 297             num_in_flight: 0,
 298
 299             in_flight: [InFlight::new(); 7],
 300             sector_header: [SectorHeader::new(); NUM_SECTORS],
 301
 302             sectors_written: 0,
 303
 304             write_buffer_offset: 0,
 305             write_buffer: [0xff; SECTOR_SIZE],
 306         }
 307     }
 308
 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);
 314         }
 315     }
 316
 317     fn read_sector_header(&mut self, sector_index: usize) {
 318         let address = sector_index * SECTOR_SIZE;
 319         let mut chunk = [0u8; 8];
 320
 321         self.storage.read(address, &mut chunk);
 322
 323         let sector_header_ptr: *mut SectorHeader =
 324             &mut self.sector_header[sector_index];
 325
 326         unsafe {
 327             core::ptr::copy(chunk.as_ptr(),
 328                             sector_header_ptr as *mut u8,
 329                             chunk.len());
 330         }
 331     }
 332
 333     fn prepare_write_buffer(&mut self, is_initial: bool) {
 334         self.write_buffer = [0xff; SECTOR_SIZE];
 335
 336         let flags = if is_initial {
 337             (SectorFlag::InUse as u16)
 338         } else {
 339             (SectorFlag::InUse as u16) | (SectorFlag::DataRecord as u16)
 340         };
 341
 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());
 345
 346         self.write_buffer_offset = 4;
 347
 348         if is_initial {
 349             let start = self.write_buffer_offset;
 350             let end = start + 4;
 351
 352             self.write_buffer[start..end].copy_from_slice(
 353                 &self.recording_started.to_le_bytes());
 354
 355             self.write_buffer_offset += 4;
 356         }
 357     }
 358
 359     pub fn start_recording(&mut self, tap: &TimeAndPos) -> u16 {
 360         self.find_next_record_slot();
 361
 362         self.sectors_written = 0;
 363         self.recording_started = tap.unix_time;
 364         self.total_distance_cm = 0;
 365         self.num_in_flight = 0;
 366
 367         self.prepare_write_buffer(true);
 368
 369         self.write_packet(0, tap.latitude_deg, tap.longitude_deg);
 370
 371         self.recording_id
 372     }
 373
 374     pub fn log(&mut self, prev_tap: &TimeAndPos, tap: &TimeAndPos) {
 375         self.total_distance_cm += tap.distance_cm(&prev_tap) as u32;
 376
 377         let d_time_ms = elapsed_ms(tap.system_time, prev_tap.system_time);
 378
 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;
 386
 387         let d_lat = tap.latitude_deg - prev_tap.latitude_deg;
 388         let d_lon = tap.longitude_deg - prev_tap.longitude_deg;
 389
 390         if self.write_packet(d_time_s, d_lat, d_lon) {
 391             self.flush_in_flight(false);
 392         }
 393     }
 394
 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);
 399
 400         // Write footer.
 401         let duration = (tap.unix_time - self.recording_started) as u16;
 402
 403         let start = self.write_buffer_offset;
 404         let end = start + 2;
 405         let dst = &mut self.write_buffer[start..end];
 406
 407         dst.copy_from_slice(&duration.to_le_bytes());
 408
 409         let this_sector = self.first_sector + self.sectors_written;
 410
 411         if self.storage.write(this_sector as usize * SECTOR_SIZE,
 412                               &self.write_buffer).is_err() {
 413             // XXX
 414         }
 415
 416         self.sectors_written += 1;
 417
 418         for i in 0..self.sectors_written {
 419             self.read_sector_header((self.first_sector + i) as usize);
 420         }
 421
 422         self.sectors_written
 423     }
 424
 425     fn sector_header_iter(&self) -> SectorHeaderIter {
 426         SectorHeaderIter::new(self)
 427     }
 428
 429     fn find_next_record_slot(&mut self) {
 430         let mut candidate_index = 0;
 431         let mut max_recording_id = 0;
 432
 433         for index in self.sector_header_iter() {
 434             candidate_index = index;
 435
 436             let sector_header = &self.sector_header[index];
 437
 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.
 442                 break;
 443             }
 444
 445             max_recording_id =
 446                 max(max_recording_id, sector_header.recording_id);
 447         }
 448
 449         self.first_sector = candidate_index as u16;
 450         self.recording_id = max_recording_id.wrapping_add(1);
 451     }
 452
 453     fn write_packet(&mut self, d_time_s: u32, d_lat: i32, d_lon: i32) -> bool {
 454         {
 455             let in_flight = &mut self.in_flight[self.num_in_flight];
 456
 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);
 460         }
 461
 462         self.num_in_flight += 1;
 463
 464         self.num_in_flight == self.in_flight.len()
 465     }
 466
 467     // Flushes the "in flight" items to the write buffer.
 468     //
 469     // @param is_final @c true iff this is the final flush in this recording.
 470     //
 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) {
 473         let mut flags = 0u8;
 474
 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 {
 479                 flags |= 1 << i;
 480             }
 481         }
 482
 483         let mut buffer = [0u8; 128];
 484         let mut offset = 0;
 485
 486         buffer[offset] = flags;
 487         offset += 1;
 488
 489         for i in 0..(self.num_in_flight - 1) {
 490             let in_flight = &self.in_flight[i];
 491
 492             // Only write the time delta for the atypical cases.
 493             if (flags & (1 << i)) != 0 {
 494                 offset +=
 495                     varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
 496             }
 497
 498             offset +=
 499                 varint::write_s32(&mut buffer[offset..], in_flight.d_lat);
 500
 501             offset +=
 502                 varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
 503         }
 504
 505         let i = self.num_in_flight - 1;
 506         let in_flight = &self.in_flight[i];
 507
 508         // Only write the time delta for the atypical cases.
 509         if (flags & (1 << i)) != 0 {
 510             offset +=
 511                 varint::write_u32(&mut buffer[offset..], in_flight.d_time_s);
 512         }
 513
 514         // The final point is an end-of-stream marker and doesn't store
 515         // d_lat or d_lon.
 516         if !is_final {
 517             offset +=
 518                 varint::write_s32(&mut buffer[offset..], in_flight.d_lat);
 519
 520             offset +=
 521                 varint::write_s32(&mut buffer[offset..], in_flight.d_lon);
 522         }
 523
 524         self.num_in_flight = 0;
 525
 526         let num_bytes_written = offset;
 527
 528         let remaining = self.write_buffer.len() - self.write_buffer_offset;
 529
 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;
 535
 536             if self.storage.write(this_sector as usize * SECTOR_SIZE,
 537                                   &self.write_buffer).is_err() {
 538                 // XXX
 539             }
 540
 541             self.sectors_written += 1;
 542
 543             self.prepare_write_buffer(false);
 544         }
 545
 546         let start = self.write_buffer_offset;
 547         let end = start + num_bytes_written;
 548         let dst = &mut self.write_buffer[start..end];
 549
 550         dst.copy_from_slice(&buffer[0..num_bytes_written]);
 551
 552         self.write_buffer_offset += num_bytes_written;
 553     }
 554
 555     ///
 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];
 560
 561             if !sector_header.starts_recording() {
 562                 continue;
 563             }
 564
 565             let mut num_data_sectors = 0;
 566
 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];
 570
 571                 if other_sector_header.belongs_to(sector_header.recording_id) {
 572                     num_data_sectors += 1;
 573                 }
 574             }
 575
 576             let mut date_time_s = [b' '; 19];
 577
 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..]);
 581             }
 582
 583             let recording_size = (num_data_sectors + 1) * (SECTOR_SIZE >> 10);
 584
 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,
 588                                                    8, b' ');
 589
 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,
 594                             8, b' ');
 595
 596             tx_buf.write(&date_time_s);
 597             tx_buf.write(&recording_size_s[0..recording_size_s_len]);
 598             tx_buf.write(b"K");
 599             tx_buf.write(&recording_id_s[0..recording_id_s_len]);
 600             tx_buf.write(b"\n");
 601
 602             tx_buf.flush();
 603         }
 604     }
 605
 606     ///
 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 {
 610             return false;
 611         }
 612
 613         self.sector_header_iter().find(|&index| {
 614             let sector_header = &self.sector_header[index as usize];
 615
 616             sector_header.recording_id == recording_id &&
 617             sector_header.starts_recording()
 618         }).is_some()
 619     }
 620
 621     ///
 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);
 628         }
 629
 630         if let Some(found_index) = self.sector_header_iter().find(|&index| {
 631             let sector_header = &self.sector_header[index as usize];
 632
 633             sector_header.recording_id == recording_id &&
 634             sector_header.starts_recording()
 635         }) {
 636             let mut filename = [b' '; 29];
 637
 638             filename[0..].copy_from_slice(b"gps-watch-recording-XXXXX.bin");
 639
 640             fmt_u32_pad(&mut filename[20..], recording_id as u32, 5, b'0');
 641
 642             let mut yenc = Yencode::new(tx_buf);
 643
 644             yenc.start(&filename);
 645
 646             let format_version = 1u8;
 647             yenc.data(&[format_version]);
 648
 649             let mut next_sector = found_index as usize;
 650
 651             for _ in 0..NUM_SECTORS {
 652                 let address = next_sector * SECTOR_SIZE;
 653                 let mut buf = [0u8; SECTOR_SIZE];
 654
 655                 self.storage.read(address, &mut buf);
 656
 657                 // Skip flags and recording ID.
 658                 yenc.data(&buf[4..]);
 659
 660                 next_sector += 1;
 661                 next_sector &= NUM_SECTORS - 1;
 662
 663                 if !self.sector_header[next_sector].belongs_to(recording_id) {
 664                     break;
 665                 }
 666             }
 667
 668             yenc.finish();
 669
 670             tx_buf.flush();
 671
 672             Ok(())
 673         } else {
 674             Err(Error::NoSuchRecording)
 675         }
 676     }
 677
 678     ///
 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];
 683
 684             sector_header.recording_id == recording_id &&
 685             sector_header.starts_recording()
 686         }) {
 687             let mut next_sector = found_index as usize;
 688
 689             for _ in 0..NUM_SECTORS {
 690                 let address = next_sector * SECTOR_SIZE;
 691
 692                 if let Err(_) = self.storage.erase(address) {
 693                     return Err(Error::StorageError);
 694                 }
 695
 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);
 699
 700                 next_sector += 1;
 701                 next_sector &= NUM_SECTORS - 1;
 702
 703                 if !self.sector_header[next_sector].belongs_to(recording_id) {
 704                     break;
 705                 }
 706             }
 707
 708             Ok(())
 709         } else {
 710             Err(Error::NoSuchRecording)
 711         }
 712     }
 713 }