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
  32 pub const MEMORY_SIZE: usize = 2 << 20;
  33 const SECTOR_SIZE: usize = 4 << 10;
  34
  35 const NUM_SECTORS: usize = MEMORY_SIZE / SECTOR_SIZE;
  36
  37 #[derive(Clone, Copy, PartialEq, Debug)]
  38 pub enum Error {
  39     NoSuchRecording,
  40 }
  41
  42 enum SectorFlag {
  43     InUse      = 1 << 0,
  44     DataRecord = 1 << 1,
  45
  46     // Overrides InUse. The idea is to have erased flash sectors
  47     // (0xff..ff) be detected as not in use.
  48     NotInUse   = 1 << 7,
  49 }
  50
  51 #[repr(C)]
  52 #[derive(Clone, Copy)]
  53 struct SectorHeader {
  54     flags: u16, // Combination of SectorFlag items.
  55     recording_id: u16, // Zero is considered invalid.
  56     start_time: u32, // UNIX time. Only present if flag DataRecord isn't set.
  57 }
  58
  59 impl SectorHeader {
  60     fn new() -> SectorHeader {
  61         SectorHeader {
  62             flags: 0,
  63             recording_id: 0,
  64             start_time: 0,
  65         }
  66     }
  67
  68     fn is_in_use(&self) -> bool {
  69         let mask = (SectorFlag::InUse as u16) | (SectorFlag::NotInUse as u16);
  70         let value = SectorFlag::InUse as u16;
  71
  72         (self.flags & mask) == value
  73     }
  74
  75     fn starts_recording(&self) -> bool {
  76         let mask = (SectorFlag::InUse as u16)
  77                  | (SectorFlag::DataRecord as u16)
  78                  | (SectorFlag::NotInUse as u16);
  79         let value = SectorFlag::InUse as u16;
  80
  81         (self.flags & mask) == value
  82     }
  83
  84     fn belongs_to(&self, recording_id: u16) -> bool {
  85         self.is_in_use() && self.recording_id == recording_id
  86     }
  87 }
  88
  89 #[derive(Clone, Copy)]
  90 struct InFlight {
  91     d_time_s: u32,
  92     d_lat: i32,
  93     d_lon: i32,
  94 }
  95
  96 impl InFlight {
  97     fn new() -> InFlight {
  98         InFlight {
  99             d_time_s: 0,
 100             d_lat: 0,
 101             d_lon: 0,
 102         }
 103     }
 104 }
 105
 106 pub struct Logger<'a> {
 107     pub storage: &'a mut dyn Storage,
 108
 109     recording_id: u16, // Zero is considered invalid.
 110
 111     // The index of the first sector of the currently running recording.
 112     // Only written in logger_start_recording.
 113     first_sector: u16,
 114
 115     recording_started: u32,
 116
 117     // The number of slots filled in num_flight.
 118     num_in_flight: usize,
 119
 120     // Deltas not yet written out to write_buffer.
 121     //
 122     // Limiting ourselves to 7 items here means we can use
 123     // 0xff as a padding byte.
 124     in_flight: [InFlight; 7],
 125
 126     sector_header: [SectorHeader; NUM_SECTORS],
 127
 128     sectors_written: u16,
 129
 130     write_buffer_offset: usize,
 131     write_buffer: [u8; SECTOR_SIZE],
 132 }
 133
 134 struct SectorHeaderIter<'a> {
 135     sector_header: &'a [SectorHeader; NUM_SECTORS],
 136     it_front: usize,
 137     it_back: usize,
 138     indices: [u16; NUM_SECTORS],
 139 }
 140
 141 fn cmp_sector_header_indices(a: u16, b: u16,
 142                              sector_header: &[SectorHeader]) -> i32 {
 143     let header_a = &sector_header[a as usize];
 144     let header_b = &sector_header[b as usize];
 145
 146     // Latest entries come first.
 147     if header_a.start_time > header_b.start_time {
 148         -1
 149     } else if header_a.start_time < header_b.start_time {
 150         1
 151     } else {
 152         0
 153     }
 154 }
 155
 156 fn downheap(heap: &mut [u16], mut index: usize, num_elts: usize,
 157             sector_header: &[SectorHeader]) {
 158     let orig = heap[index];
 159
 160     loop {
 161         let mut worker = index * 2;
 162
 163         if worker < num_elts &&
 164            cmp_sector_header_indices(heap[worker], heap[worker + 1], sector_header) < 0 {
 165             worker += 1;
 166         }
 167
 168         if worker > num_elts ||
 169            cmp_sector_header_indices(orig, heap[worker], sector_header) >= 0 {
 170             break;
 171         }
 172
 173         heap[index] = heap[worker];
 174         index = worker;
 175     }
 176
 177     heap[index] = orig;
 178 }
 179
 180 impl<'a> SectorHeaderIter<'a> {
 181     fn new(logger: &'a Logger) -> SectorHeaderIter<'a> {
 182         let mut iter = SectorHeaderIter {
 183             sector_header: &logger.sector_header,
 184             it_front: 0,
 185             it_back: NUM_SECTORS,
 186             indices: [0; NUM_SECTORS]
 187         };
 188
 189         let mut num_used = 0;
 190
 191         // Put the indices of the used directory entries at the beginning
 192         // of the array. Ignore the unused ones since we are not going
 193         // to sort them anyway.
 194         for i in 0..NUM_SECTORS {
 195             let sector_header = &iter.sector_header[i];
 196
 197             if sector_header.starts_recording() {
 198                 iter.indices[num_used] = i as u16;
 199                 num_used += 1;
 200             }
 201         }
 202
 203         let num_elts_to_sort = num_used;
 204
 205         if num_elts_to_sort != 0 {
 206             // Sort the used directory entries.
 207             for i in (1..((num_elts_to_sort + 1) / 2) + 1).rev() {
 208                 downheap(&mut iter.indices, i - 1, num_elts_to_sort - 1,
 209                          iter.sector_header);
 210             }
 211
 212             for i in (1..num_elts_to_sort).rev() {
 213                 let t = iter.indices[0];
 214                 iter.indices[0] = iter.indices[i];
 215                 iter.indices[i] = t;
 216
 217                 downheap(&mut iter.indices, 0, i - 1, iter.sector_header);
 218             }
 219         }
 220
 221         // Now put the indices of the unused directory entries in the array.
 222         if num_used == 0 {
 223             for i in 0..NUM_SECTORS {
 224                 iter.indices[i] = i as u16;
 225             }
 226         } else {
 227             let latest_used = iter.indices[0] as usize;
 228             let mut offset_unused = num_used;
 229
 230             // First put the entries that come after the latest one in use...
 231             for i in (latest_used + 1)..NUM_SECTORS {
 232                 let sector_header = &iter.sector_header[i];
 233
 234                 if !sector_header.is_in_use() {
 235                     iter.indices[offset_unused] = i as u16;
 236                     offset_unused += 1;
 237                 }
 238             }
 239
 240             // ... then wrap around if necessary.
 241             for i in 0..latest_used {
 242                 let sector_header = &iter.sector_header[i];
 243
 244                 if !sector_header.is_in_use() {
 245                     iter.indices[offset_unused] = i as u16;
 246                     offset_unused += 1;
 247                 }
 248             }
 249         }
 250
 251         // XXX:
 252         // Need to handle those sectors that don't start recordings
 253         // but that are still used.
 254
 255         iter
 256     }
 257 }
 258
 259 impl<'a> Iterator for SectorHeaderIter<'a> {
 260     type Item = usize;
 261
 262     fn next(&mut self) -> Option<usize> {
 263         if self.it_front == self.it_back {
 264             None
 265         } else {
 266             let next_index = self.indices[self.it_front] as usize;
 267
 268             self.it_front += 1;
 269
 270             Some(next_index)
 271         }
 272     }
 273 }
 274
 275 impl<'a> DoubleEndedIterator for SectorHeaderIter<'a> {
 276     fn next_back(&mut self) -> Option<usize> {
 277         if self.it_back == self.it_front {
 278             None
 279         } else {
 280             self.it_back -= 1;
 281
 282             let next_index = self.indices[self.it_back] as usize;
 283
 284             Some(next_index)
 285         }
 286     }
 287 }
 288
 289 fn normalize_angle(mut angle: i32) -> i32 {
 290     let deg90 = 90 * 60 * 10000;
 291     let deg180 = deg90 << 1;
 292     let deg360 = deg180 << 1;
 293
 294     while angle >= deg180 {
 295         angle -= deg360;
 296     }
 297
 298     while angle <= -deg180 {
 299         angle += deg360;
 300     }
 301
 302     angle
 303 }
 304
 305 fn max<T>(a: T, b: T) -> T
 306           where T: PartialOrd {
 307     if a > b {
 308         a
 309     } else {
 310         b
 311     }
 312 }
 313
 314 impl<'a> Logger<'a> {
 315     pub fn new(storage: &'a mut dyn Storage) -> Logger {
 316         Logger {
 317             storage: storage,
 318
 319             recording_id: 0,
 320             first_sector: 0,
 321             recording_started: 0,
 322             num_in_flight: 0,
 323
 324             in_flight: [InFlight::new(); 7],
 325             sector_header: [SectorHeader::new(); NUM_SECTORS],
 326
 327             sectors_written: 0,
 328
 329             write_buffer_offset: 0,
 330             write_buffer: [0xff; SECTOR_SIZE],
 331         }
 332     }
 333
 334     pub fn init(&mut self) {
 335         // Reading the directory entries one by one means
 336         // we won't need an as large buffer on the stack.
 337         for i in 0..NUM_SECTORS {
 338             self.read_sector_header(i);
 339         }
 340     }
 341
 342     fn read_sector_header(&mut self, sector_index: usize) {
 343         let address = sector_index * SECTOR_SIZE;
 344         let mut chunk = [0u8; 4];
 345
 346         self.storage.read(address, &mut chunk);
 347
 348         let sector_header_ptr: *mut SectorHeader =
 349             &mut self.sector_header[sector_index];
 350
 351         unsafe {
 352             core::ptr::copy(chunk.as_ptr(),
 353                             sector_header_ptr as *mut u8,
 354                             chunk.len());
 355         }
 356     }
 357
 358     fn prepare_write_buffer(&mut self, is_initial: bool) {
 359         self.write_buffer = [0xff; SECTOR_SIZE];
 360
 361         let flags = if is_initial {
 362             (SectorFlag::InUse as u16)
 363         } else {
 364             (SectorFlag::InUse as u16) | (SectorFlag::DataRecord as u16)
 365         };
 366
 367         // Write sector header.
 368         self.write_buffer[0..2].copy_from_slice(&flags.to_le_bytes());
 369         self.write_buffer[2..4].copy_from_slice(&self.recording_id.to_le_bytes());
 370
 371         self.write_buffer_offset = 4;
 372
 373         if is_initial {
 374             let start = self.write_buffer_offset;
 375             let end = start + 4;
 376
 377             self.write_buffer[start..end].copy_from_slice(
 378                 &self.recording_started.to_le_bytes());
 379
 380             self.write_buffer_offset += 4;
 381         }
 382     }
 383
 384     pub fn start_recording(&mut self, tap: &TimeAndPos) -> u16 {
 385         self.find_next_record_slot();
 386
 387         self.sectors_written = 0;
 388         self.recording_started = tap.unix_time;
 389         self.num_in_flight = 0;
 390
 391         self.prepare_write_buffer(true);
 392
 393         self.write_packet(0, tap.latitude, tap.longitude);
 394
 395         self.recording_id
 396     }
 397
 398     pub fn log(&mut self, prev_tap: &TimeAndPos, tap: &TimeAndPos) {
 399         let d_time_ms = elapsed_ms(tap.system_time, prev_tap.system_time);
 400
 401         // We know that our hardware cannot deliver updates more often
 402         // than once a second. However when there's a delay in evaluating
 403         // the hardware's messages, we will end up with intervals like
 404         // 1050ms and 950ms (the latter will "make up" for the slowness
 405         // in the former). To avoid logging deltas of 0 seconds, we round
 406         // the intervals to full seconds.
 407         let d_time_s = (d_time_ms + 500) / 1000;
 408
 409         let d_lat = tap.latitude - prev_tap.latitude;
 410         let d_lon = tap.longitude - prev_tap.longitude;
 411
 412         if self.write_packet(d_time_s, d_lat, d_lon) {
 413             self.flush_in_flight(false);
 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     /// Check whether or not recording @p recording_id exists.
 579     pub fn has_recording(&mut self, recording_id: u16) -> bool {
 580         if recording_id == 0 {
 581             return false;
 582         }
 583
 584         self.sector_header_iter().find(|&index| {
 585             let sector_header = &self.sector_header[index as usize];
 586
 587             sector_header.recording_id == recording_id &&
 588             sector_header.starts_recording()
 589         }).is_some()
 590     }
 591
 592     ///
 593     /// Retrieve recording @p recording_id and
 594     /// write it to @p tx_buf in yencoded form.
 595     pub fn get_recording(&mut self, recording_id: u16,
 596                          tx_buf: &mut Buffer) -> Result<(), Error> {
 597         if recording_id == 0 {
 598             return Err(Error::NoSuchRecording);
 599         }
 600
 601         if let Some(found_index) = self.sector_header_iter().find(|&index| {
 602             let sector_header = &self.sector_header[index as usize];
 603
 604             sector_header.recording_id == recording_id &&
 605             sector_header.starts_recording()
 606         }) {
 607             let mut filename = [b' '; 29];
 608
 609             filename[0..].copy_from_slice(b"gps-watch-recording-XXXXX.bin");
 610
 611             fmt_u32_pad(&mut filename[20..], recording_id as u32, 5, b'0');
 612
 613             let mut yenc = Yencode::new(tx_buf);
 614
 615             yenc.start(&filename);
 616
 617             let format_version = 1u8;
 618             yenc.data(&[format_version]);
 619
 620             let mut next_sector = found_index as usize;
 621
 622             for _ in 0..NUM_SECTORS {
 623                 let address = next_sector * SECTOR_SIZE;
 624                 let mut buf = [0u8; SECTOR_SIZE];
 625
 626                 self.storage.read(address, &mut buf);
 627
 628                 // Skip flags and recording ID.
 629                 yenc.data(&buf[4..]);
 630
 631                 next_sector += 1;
 632                 next_sector &= NUM_SECTORS - 1;
 633
 634                 if !self.sector_header[next_sector].belongs_to(recording_id) {
 635                     break;
 636                 }
 637             }
 638
 639             yenc.finish();
 640
 641             tx_buf.flush();
 642
 643             Ok(())
 644         } else {
 645             Err(Error::NoSuchRecording)
 646         }
 647     }
 648 }