mirror of https://github.com/rusefi/rusefi-1.git
114 lines
2.8 KiB
C
114 lines
2.8 KiB
C
/**
|
|
* @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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#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++;
|
|
}
|
|
}
|