src/main/common/crc.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 <stdint.h>
  22
  23 #include "platform.h"
  24
  25 #include "streambuf.h"
  26
  27
  28 uint16_t crc16_ccitt(uint16_t crc, unsigned char a)
  29 {
  30     crc ^= (uint16_t)a << 8;
  31     for (int ii = 0; ii < 8; ++ii) {
  32         if (crc & 0x8000) {
  33             crc = (crc << 1) ^ 0x1021;
  34         } else {
  35             crc = crc << 1;
  36         }
  37     }
  38     return crc;
  39 }
  40
  41 uint16_t crc16_ccitt_update(uint16_t crc, const void *data, uint32_t length)
  42 {
  43     const uint8_t *p = (const uint8_t *)data;
  44     const uint8_t *pend = p + length;
  45
  46     for (; p != pend; p++) {
  47         crc = crc16_ccitt(crc, *p);
  48     }
  49     return crc;
  50 }
  51
  52 void crc16_ccitt_sbuf_append(sbuf_t *dst, uint8_t *start)
  53 {
  54     uint16_t crc = 0;
  55     const uint8_t * const end = sbufPtr(dst);
  56     for (const uint8_t *ptr = start; ptr < end; ++ptr) {
  57         crc = crc16_ccitt(crc, *ptr);
  58     }
  59     sbufWriteU16(dst, crc);
  60 }
  61
  62 uint8_t crc8_calc(uint8_t crc, unsigned char a, uint8_t poly)
  63 {
  64     crc ^= a;
  65     for (int ii = 0; ii < 8; ++ii) {
  66         if (crc & 0x80) {
  67             crc = (crc << 1) ^ poly;
  68         } else {
  69             crc = crc << 1;
  70         }
  71     }
  72     return crc;
  73 }
  74
  75 uint8_t crc8_update(uint8_t crc, const void *data, uint32_t length, uint8_t poly)
  76 {
  77     const uint8_t *p = (const uint8_t *)data;
  78     const uint8_t *pend = p + length;
  79
  80     for (; p != pend; p++) {
  81         crc = crc8_calc(crc, *p, poly);
  82     }
  83     return crc;
  84 }
  85
  86 void crc8_sbuf_append(sbuf_t *dst, uint8_t *start, uint8_t poly)
  87 {
  88     uint8_t crc = 0;
  89     const uint8_t * const end = dst->ptr;
  90     for (const uint8_t *ptr = start; ptr < end; ++ptr) {
  91         crc = crc8_calc(crc, *ptr, poly);
  92     }
  93     sbufWriteU8(dst, crc);
  94 }
  95
  96 uint8_t crc8_xor_update(uint8_t crc, const void *data, uint32_t length)
  97 {
  98     const uint8_t *p = (const uint8_t *)data;
  99     const uint8_t *pend = p + length;
 100
 101     for (; p != pend; p++) {
 102         crc ^= *p;
 103     }
 104     return crc;
 105 }
 106
 107 void crc8_xor_sbuf_append(sbuf_t *dst, uint8_t *start)
 108 {
 109     uint8_t crc = 0;
 110     const uint8_t *end = dst->ptr;
 111     for (uint8_t *ptr = start; ptr < end; ++ptr) {
 112         crc ^= *ptr;
 113     }
 114     sbufWriteU8(dst, crc);
 115 }
 116