src/main/pg/pg.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stddef.h>
  22 #include <string.h>
  23 #include <stdint.h>
  24
  25 #include "platform.h"
  26
  27 #include "common/maths.h"
  28
  29 #include "pg.h"
  30
  31 const pgRegistry_t* pgFind(pgn_t pgn)
  32 {
  33     PG_FOREACH(reg) {
  34         if (pgN(reg) == pgn) {
  35             return reg;
  36         }
  37     }
  38     return NULL;
  39 }
  40
  41 static uint8_t *pgOffset(const pgRegistry_t* reg)
  42 {
  43     return reg->address;
  44 }
  45
  46 void pgResetInstance(const pgRegistry_t *reg, uint8_t *base)
  47 {
  48     const uint16_t regSize = pgSize(reg);
  49
  50     memset(base, 0, regSize);
  51     if (reg->reset.ptr >= (void*)__pg_resetdata_start && reg->reset.ptr < (void*)__pg_resetdata_end) {
  52         // pointer points to resetdata section, to it is data template
  53         memcpy(base, reg->reset.ptr, regSize);
  54     } else if (reg->reset.fn) {
  55         // reset function, call it
  56         reg->reset.fn(base);
  57     }
  58 }
  59
  60 void pgReset(const pgRegistry_t* reg)
  61 {
  62     pgResetInstance(reg, pgOffset(reg));
  63 }
  64
  65 bool pgResetCopy(void *copy, pgn_t pgn)
  66 {
  67     const pgRegistry_t *reg = pgFind(pgn);
  68     if (reg) {
  69         pgResetInstance(reg, copy);
  70         return true;
  71     }
  72     return false;
  73 }
  74
  75 bool pgLoad(const pgRegistry_t* reg, const void *from, int size, int version)
  76 {
  77     pgResetInstance(reg, pgOffset(reg));
  78     // restore only matching version, keep defaults otherwise
  79     if (version == pgVersion(reg)) {
  80         const int take = MIN(size, pgSize(reg));
  81         memcpy(pgOffset(reg), from, take);
  82
  83         return true;
  84     }
  85
  86     return false;
  87 }
  88
  89 int pgStore(const pgRegistry_t* reg, void *to, int size)
  90 {
  91     const int take = MIN(size, pgSize(reg));
  92     memcpy(to, pgOffset(reg), take);
  93     return take;
  94 }
  95
  96 void pgResetAll(void)
  97 {
  98     PG_FOREACH(reg) {
  99         pgReset(reg);
 100     }
 101 }