From e7991eacf8cf2a24e325b413cb23f66a0a17fae4 Mon Sep 17 00:00:00 2001 From: dron0gus Date: Fri, 15 May 2020 22:55:06 +0300 Subject: [PATCH] Some bit, unaligned and different endian access helpers (#1436) * util: add some common macros * util: add unaligned access helpers --- firmware/util/common.h | 35 ++++++++++++ firmware/util/unaligned.c | 113 ++++++++++++++++++++++++++++++++++++++ firmware/util/unaligned.h | 105 +++++++++++++++++++++++++++++++++++ firmware/util/util.mk | 3 +- 4 files changed, 255 insertions(+), 1 deletion(-) create mode 100644 firmware/util/common.h create mode 100644 firmware/util/unaligned.c create mode 100644 firmware/util/unaligned.h diff --git a/firmware/util/common.h b/firmware/util/common.h new file mode 100644 index 0000000000..0ae45ad87d --- /dev/null +++ b/firmware/util/common.h @@ -0,0 +1,35 @@ +/** + * @file common.h + * @brief Common macros definitions + * + * @date May, 2019 + * @author Andrey Gusakov, (c) 2019 + * + * This file is part of rusEfi - see http://rusefi.com + * + * rusEfi is free software; you can redistribute it and/or modify it under the terms of + * the GNU General Public License as published by the Free Software Foundation; either + * version 3 of the License, or (at your option) any later version. + * + * rusEfi is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without + * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with this program. + * If not, see . + */ + +#ifndef COMMON_H_INCLUDED +#define COMMON_H_INCLUDED + +#define MIN(a,b) (((a)<(b))?(a):(b)) +#define MAX(a,b) (((a)>(b))?(a):(b)) + +#define CLAMP(x, low, high) ({\ + __typeof__(x) __x = (x); \ + __typeof__(low) __low = (low);\ + __typeof__(high) __high = (high);\ + (__x > __high) ? __high : ((__x < __low) ? __low : __x);\ +}) + +#endif /* COMMON_H_INCLUDED */ diff --git a/firmware/util/unaligned.c b/firmware/util/unaligned.c new file mode 100644 index 0000000000..977abdcaf1 --- /dev/null +++ b/firmware/util/unaligned.c @@ -0,0 +1,113 @@ +/** + * @file unaligned.c + * @brief unaligned data access helpers + * + * @date May, 2019 + * @author Andrey Gusakov, (c) 2019 + * + * This file is part of rusEfi - see http://rusefi.com + * + * rusEfi is free software; you can redistribute it and/or modify it under the terms of + * the GNU General Public License as published by the Free Software Foundation; either + * version 3 of the License, or (at your option) any later version. + * + * rusEfi is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without + * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with this program. + * If not, see . + */ + +#include "unaligned.h" +#include "common.h" + +/* generate mask */ +static inline uint32_t bit_mask(unsigned int from, unsigned int to) +{ + uint32_t mask = 0; + uint32_t shift; + + if (to < from) + return 0; + + shift = to - from + 1U; + + if ((shift > 0U) && (shift <= 32U) && (from <= 31U)) { + if (shift < 32U) { + mask = (uint32_t)((1UL << shift) - 1UL); + mask = mask << from; + } else { + mask = 0xFFFFFFFFUL; + } + } + + return mask; +} + +/* get upto 32 bits from char array p, from bit position pos, lenght len */ +uint32_t bits_get(uint8_t *p, unsigned int pos, int len) +{ + int i; + unsigned int offset = 0; + uint32_t res = 0; + + i = (int)pos / 8; + + while (len > 0) { + uint32_t tmp; + uint32_t mask; + int nbits = 8 - ((int)pos % 8); + + /* get */ + tmp = (uint32_t)p[i]; + /* shift */ + tmp = tmp >> (8U - (uint32_t)nbits); + /* mask */ + mask = bit_mask(0, MIN((uint32_t)len - 1U, (uint32_t)nbits - 1U)); + tmp = tmp & mask; + res = res | ((tmp) << offset); + + /* adjust for the next iteration */ + offset += (unsigned int)nbits; + len -= nbits; + pos += (unsigned int)nbits; + i++; + } + return res; +} + +/* set upto 32 bits in char array p, from bit position pos, lenght len */ +void bits_set(uint8_t *p, unsigned int pos, int len, uint32_t val) +{ + int i; + unsigned int offset = 0; + + i = (int)pos / 8; + + while (len > 0) { + uint32_t tmp; + uint32_t mask; + + /* get number of bits to shift to get to the target range */ + int shift = (int)pos % 8; + /* get next byte */ + tmp = (val >> offset) & 0xffU; + /* shift temporary value to the start of the target range */ + tmp = tmp << (uint8_t)shift; + /* calculate mask */ + mask = bit_mask((uint32_t)shift, MIN(8U - 1U, (unsigned int)shift + (unsigned int)len - 1U)); + /* clean all bits outside of the target range */ + tmp &= mask; + /* pre-clean all target bits */ + p[i] = p[i] & ~((uint8_t)mask); + /* finally set active bits */ + p[i] |= (uint8_t)tmp; + + /* adjust for the next iteration */ + offset += ((uint32_t)8U - (uint32_t)shift); + len -= (8 - shift); + pos += ((unsigned int)8U - (unsigned int)shift); + i++; + } +} diff --git a/firmware/util/unaligned.h b/firmware/util/unaligned.h new file mode 100644 index 0000000000..2b1a11217f --- /dev/null +++ b/firmware/util/unaligned.h @@ -0,0 +1,105 @@ +/** + * @file unaligned.h + * @brief unaligned data access helpers header file + * + * @date May, 2019 + * @author Andrey Gusakov, (c) 2019 + * + * This file is part of rusEfi - see http://rusefi.com + * + * rusEfi is free software; you can redistribute it and/or modify it under the terms of + * the GNU General Public License as published by the Free Software Foundation; either + * version 3 of the License, or (at your option) any later version. + * + * rusEfi is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without + * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with this program. + * If not, see . + */ + +#ifndef UNALIGNED_H_INCLUDED +#define UNALIGNED_H_INCLUDED + +#include + +/* bit operations */ +uint32_t bits_get(uint8_t *p, unsigned int pos, int len); +void bits_set(uint8_t *p, unsigned int pos, int len, uint32_t val); +#define bit_set(p, pos) do {(p)[(pos) / 8] |= (1 << ((pos) % 8));} while(0) +#define bit_clr(p, pos) do {(p)[(pos) / 8] &= ~(1 << ((pos) % 8));} while(0) +#define bit_get(p, pos) (!!((p)[(pos) / 8] & (1 << ((pos) % 8)))) + +/* unaligned access */ +static inline void put_be8(uint8_t *p, uint8_t v) +{ + p[0] = v; +} + +static inline void put_le8(uint8_t *p, uint8_t v) +{ + p[0] = v; +} + +static inline void put_be16(uint8_t *p, uint16_t v) +{ + p[0] = (uint8_t)(v >> 8); + p[1] = (uint8_t)(v >> 0); +} + +static inline void put_le16(uint8_t *p, uint16_t v) +{ + p[0] = (uint8_t)(v >> 0); + p[1] = (uint8_t)(v >> 8); +} + +static inline void put_be32(uint8_t *p, uint32_t v) +{ + p[0] = (uint8_t)(v >> 24); + p[1] = (uint8_t)(v >> 16); + p[2] = (uint8_t)(v >> 8); + p[3] = (uint8_t)(v >> 0); +} + +static inline void put_le32(uint8_t *p, uint32_t v) +{ + p[0] = (uint8_t)(v >> 0); + p[1] = (uint8_t)(v >> 8); + p[2] = (uint8_t)(v >> 16); + p[3] = (uint8_t)(v >> 24); +} + +static inline uint8_t get_be8(uint8_t *p) +{ + return p[0]; +} + +static inline uint8_t get_le8(uint8_t *p) +{ + return p[0]; +} + +static inline uint16_t get_be16(uint8_t *p) +{ + return ((uint16_t)p[0] << 8) | p[1]; +} + +static inline uint16_t get_le16(uint8_t *p) +{ + return ((uint16_t)p[1] << 8) | p[0]; +} + +static inline uint32_t get_be32(uint8_t *p) +{ + return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) | + ((uint32_t)p[2] << 8) | p[3]; +} + +static inline uint32_t get_le32(uint8_t *p) +{ + return ((uint32_t)p[3] << 24) | ((uint32_t)p[2] << 16) | + ((uint32_t)p[1] << 8) | p[0]; +} + +#endif /* UNALIGNED_H_INCLUDED */ diff --git a/firmware/util/util.mk b/firmware/util/util.mk index d70103429d..9e75e589d7 100644 --- a/firmware/util/util.mk +++ b/firmware/util/util.mk @@ -4,7 +4,8 @@ UTILSRC = \ $(UTIL_DIR)/containers/data_buffer.c \ $(UTIL_DIR)/math/crc.c \ $(UTIL_DIR)/os_util.c \ - $(UTIL_DIR)/histogram.c + $(UTIL_DIR)/histogram.c \ + $(UTIL_DIR)/unaligned.c UTILSRC_CPP = \ $(UTIL_DIR)/containers/cyclic_buffer.cpp \