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