C_UTILS/utils_binary.h at master · z3rone/C_UTILS · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
/*
 * utils_binary.h
 *
 *  Created on: 11 Mar 2020
 *      Author: Falk
 */
#include <sys/types.h>
#include <stdlib.h>

#ifndef UTILS_UTILS_BINARY_H_
#define UTILS_UTILS_BINARY_H_

static inline
unsigned int del_bits(uint val, uint offset, uint len) {
	uint mask = ~0 << offset;      // ...11110000...
	mask &= ~(~0 << (offset+len)); // ...11110000... & ...01111111... --> ...01110000...
	return val & ~mask;
}

static inline
void change_byte_order(uint8_t data[], int len) {
	int last = len-1;
	for(int i = 0; i < (int)(len/2); i++) {
		uint8_t tmp = data[i];
		data[i] = data[last-i];
		data[last-i] = tmp;
	}
}

static inline
unsigned int get_bits(uint8_t* data, int start, int amt) {
	return get_bits_lsbf(data, start, amt);
}

/**
 * @brief Extracts bit sequence from byte array.
 * @param data  Byte array
 * @param start Start of bit section
 * @param amt   Number of bits
 * @return Bit sequence
 */
static inline
unsigned int get_bits_lsbf(uint8_t* data, int start, int amt) {
	div_t begin = div(start, 8);
	int index = begin.quot;
	int offset = begin.rem;

	div_t end = div(amt + offset, 8);
	int bytes = end.quot;

	int val = 0;
	for(int i = 0; i <= bytes; i++) {
		int shift = 8*i - offset;
		if(shift > 0) {
			val |= data[index+i] << shift;
		} else {
			val |= data[index+i] >> -shift;
		}
	}

	val &= ~(~0 << amt); // Cut off additional bits at the left
	return val;
}

/**
 * @brief Extracts bit sequence from byte array.
 * @param data  Byte array
 * @param start Start of bit section
 * @param amt   Number of bits
 * @return Bit sequence
 */
static inline
unsigned int get_bits_msbf(uint8_t* data, int start, int amt) {
	div_t begin = div(start, 8);
	int index = begin.quot;
	int offset = begin.rem;

	div_t end = div(amt + offset, 8);
	int bytes = end.quot;

	int val = 0;
	for(int i = bytes; i <= 0; i--) {
		int shift = 8*i - offset;
		if(shift > 0) {
			val |= data[index+i] << shift;
		} else {
			val |= data[index+i] >> -shift;
		}
	}

	val &= ~(~0 << amt); // Cut off additional bits at the left
	return val;
}

/**
 * @brief Inserts bit sequence from byte array.
 * @param data  Byte array
 * @param val   Bits to insert
 * @param start Start of insert
 * @param amt   Number of bits
 */
static inline
void set_bits_lsbf(uint8_t* data, unsigned int val, int start, int amt) {
	div_t begin = div(start, 8);
	int index = begin.quot;
	int offset = begin.rem;

	div_t end = div(amt + offset, 8);
	int bytes = end.quot;
	int bits = end.rem;

	data[index] &= ~(~0 << offset);
	data[index] |= (val << offset) & 0xFF;

	for(int i = 1; i < bytes; i++) {
		int shift = 8*i - offset;
		if(shift > 0) {
			data[index+i] = val >> shift;
		} else {
			data[index+i] = val << -shift;
		}
	}

	uint8_t lb_mask = ~0 << bits;
	data[index+bytes] &= lb_mask;
	data[index+bytes] |= (val >> (bytes*8 - offset)) & ~lb_mask;
}

/**
 * @brief Inserts bit sequence from byte array.
 * @param data  Byte array
 * @param val   Bits to insert
 * @param start Start of insert
 * @param amt   Number of bits
 */
static inline
void set_bits_msbf(uint8_t* data, unsigned int val, int start, int amt) {
	/* TODO */
}

#endif /* UTILS_UTILS_BINARY_H_ */