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