src/main/common/uvarint.c

   1 /*
   2  * This file is part of Cleanflight, Betaflight and INAV.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  *
  24  * @author Alberto Garcia Hierro <alberto@garciahierro.com>
  25  */
  26
  27 #include "platform.h"
  28
  29 #include "common/uvarint.h"
  30
  31 int uvarintEncode(uint32_t val, uint8_t *ptr, size_t size)
  32 {
  33     unsigned ii = 0;
  34     while (val > 0x80) {
  35         if (ii >= size) {
  36             return -1;
  37         }
  38         ptr[ii] = (val & 0xFF) | 0x80;
  39         val >>= 7;
  40         ii++;
  41     }
  42     if (ii >= size) {
  43         return -1;
  44     }
  45     ptr[ii] = val & 0xFF;
  46     return ii + 1;
  47 }
  48
  49 int uvarintDecode(uint32_t *val, const uint8_t *ptr, size_t size)
  50 {
  51     unsigned s = 0;
  52     *val = 0;
  53     for (size_t ii = 0; ii < size; ii++) {
  54         uint8_t b = ptr[ii];
  55         if (b < 0x80) {
  56             if (ii > 5 || (ii == 5 && b > 1)) {
  57                 // uint32_t overflow
  58                 return -2;
  59             }
  60             *val |= ((uint32_t)b) << s;
  61             return ii + 1;
  62         }
  63         *val |= ((uint32_t)(b & 0x7f)) << s;
  64         s += 7;
  65     }
  66     // no value could be decoded and we have no data left
  67     return -1;
  68 }