bitstream.c at 3f88143 ⋅ eerimoq/bitstream

/**
 * The MIT License (MIT)
 *
 * Copyright (c) 2019 Erik Moqvist
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use, copy,
 * modify, merge, publish, distribute, sublicense, and/or sell copies
 * of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <string.h>
#include "bitstream.h"

void bitstream_writer_init(struct bitstream_writer_t *self_p,
                           uint8_t *buf_p)
{
    self_p->buf_p = buf_p;
    self_p->byte_offset = 0;
    self_p->bit_offset = 0;
}

int bitstream_writer_size_in_bits(struct bitstream_writer_t *self_p)
{
    return (8 * self_p->byte_offset + self_p->bit_offset);
}

int bitstream_writer_size_in_bytes(struct bitstream_writer_t *self_p)
{
    return (self_p->byte_offset + (self_p->bit_offset + 7) / 8);
}

void bitstream_writer_write_bit(struct bitstream_writer_t *self_p,
                                int value)
{
    if (self_p->bit_offset == 0) {
        self_p->buf_p[self_p->byte_offset] = (value << 7);
        self_p->bit_offset = 1;
    } else {
        self_p->buf_p[self_p->byte_offset] |= (value << (8 - self_p->bit_offset - 1));

        if (self_p->bit_offset == 7) {
            self_p->bit_offset = 0;
            self_p->byte_offset++;
        } else {
            self_p->bit_offset++;
        }
    }
}

void bitstream_writer_write_bytes(struct bitstream_writer_t *self_p,
                                  const uint8_t *buf_p,
                                  int length)
{
    int i;
    uint8_t *dst_p;

    dst_p = &self_p->buf_p[self_p->byte_offset];

    if (self_p->bit_offset == 0) {
        memcpy(dst_p, buf_p, sizeof(uint8_t) * length);
    } else {
        for (i = 0; i < length; i++) {
            dst_p[i] |= (buf_p[i] >> self_p->bit_offset);
            dst_p[i + 1] = (uint8_t)(buf_p[i] << (8 - self_p->bit_offset));
        }
    }

    self_p->byte_offset += length;
}

void bitstream_writer_write_u8(struct bitstream_writer_t *self_p,
                               uint8_t value)
{
    if (self_p->bit_offset == 0) {
        self_p->buf_p[self_p->byte_offset] = value;
    } else {
        self_p->buf_p[self_p->byte_offset] |= (value >> self_p->bit_offset);
        self_p->buf_p[self_p->byte_offset + 1] =
            (uint8_t)(value << (8 - self_p->bit_offset));
    }

    self_p->byte_offset++;
}

void bitstream_writer_write_u16(struct bitstream_writer_t *self_p,
                                uint16_t value)
{
    if (self_p->bit_offset == 0) {
        self_p->buf_p[self_p->byte_offset] = (value >> 8);
    } else {
        self_p->buf_p[self_p->byte_offset] |= (value >> (8 + self_p->bit_offset));
        self_p->buf_p[self_p->byte_offset + 2] =
            (uint8_t)(value << (8 - self_p->bit_offset));
        value >>= self_p->bit_offset;
    }

    self_p->buf_p[self_p->byte_offset + 1] = (uint8_t)value;
    self_p->byte_offset += 2;
}

void bitstream_writer_write_u32(struct bitstream_writer_t *self_p,
                                uint32_t value)
{
    int i;

    if (self_p->bit_offset == 0) {
        self_p->buf_p[self_p->byte_offset] = (value >> 24);
    } else {
        self_p->buf_p[self_p->byte_offset] |= (value >> (24 + self_p->bit_offset));
        self_p->buf_p[self_p->byte_offset + 4] =
            (uint8_t)(value << (8 - self_p->bit_offset));
        value >>= self_p->bit_offset;
    }

    for (i = 3; i > 0; i--) {
        self_p->buf_p[self_p->byte_offset + i] = value;
        value >>= 8;
    }

    self_p->byte_offset += 4;
}

void bitstream_writer_write_u64(struct bitstream_writer_t *self_p,
                                uint64_t value)
{
    int i;


    if (self_p->bit_offset == 0) {
        self_p->buf_p[self_p->byte_offset] = (value >> 56);
    } else {
        self_p->buf_p[self_p->byte_offset] |= (value >> (56 + self_p->bit_offset));
        self_p->buf_p[self_p->byte_offset + 8] =
            (uint8_t)(value << (8 - self_p->bit_offset));
        value >>= self_p->bit_offset;
    }

    for (i = 7; i > 0; i--) {
        self_p->buf_p[self_p->byte_offset + i] = (uint8_t)value;
        value >>= 8;
    }

    self_p->byte_offset += 8;
}

void bitstream_writer_write_u64_bits(struct bitstream_writer_t *self_p,
                                     uint64_t value,
                                     int number_of_bits)
{
    int i;
    int first_byte_bits;
    int last_byte_bits;
    int full_bytes;

    if (number_of_bits == 0) {
        return;
    }

    /* Align beginning. */
    first_byte_bits = (8 - self_p->bit_offset);

    if (first_byte_bits != 8) {
        if (number_of_bits < first_byte_bits) {
            self_p->buf_p[self_p->byte_offset] |=
                (uint8_t)(value << (first_byte_bits - number_of_bits));
            self_p->bit_offset += number_of_bits;
        } else {
            self_p->buf_p[self_p->byte_offset] |= (value >> (number_of_bits
                                                             - first_byte_bits));
            self_p->byte_offset++;
            self_p->bit_offset = 0;
        }

        number_of_bits -= first_byte_bits;

        if (number_of_bits <= 0) {
            return;
        }
    }

    /* Align end. */
    last_byte_bits = (number_of_bits % 8);
    full_bytes = (number_of_bits / 8);

    if (last_byte_bits != 0) {
        self_p->buf_p[self_p->byte_offset + full_bytes] =
            (uint8_t)(value << (8 - last_byte_bits));
        value >>= last_byte_bits;
        self_p->bit_offset = last_byte_bits;
    }

    /* Copy middle bytes. */
    for (i = full_bytes; i > 0; i--) {
        self_p->buf_p[self_p->byte_offset + i - 1] = (uint8_t)value;
        value >>= 8;
    }

    self_p->byte_offset += full_bytes;
}

void bitstream_writer_write_repeated_bit(struct bitstream_writer_t *self_p,
                                         int value,
                                         int length)
{
    int rest;

    if (value != 0) {
        value = 0xff;
    }

    rest = (length % 8);
    bitstream_writer_write_u64_bits(self_p, value & ((1 << rest) - 1), rest);
    bitstream_writer_write_repeated_u8(self_p, value, length / 8);
}

void bitstream_writer_write_repeated_u8(struct bitstream_writer_t *self_p,
                                        uint8_t value,
                                        int length)
{
    int i;

    for (i = 0; i < length; i++) {
        bitstream_writer_write_u8(self_p, value);
    }
}

void bitstream_writer_insert_bit(struct bitstream_writer_t *self_p,
                                 int value)
{
    struct bitstream_writer_bounds_t bounds;

    bitstream_writer_bounds_save(&bounds,
                                 self_p,
                                 (8 * self_p->byte_offset) + self_p->bit_offset,
                                 1);
    bitstream_writer_write_bit(self_p, value);
    bitstream_writer_bounds_restore(&bounds);
}

void bitstream_writer_insert_bytes(struct bitstream_writer_t *self_p,
                                   const uint8_t *buf_p,
                                   int length)
{
    struct bitstream_writer_bounds_t bounds;

    bitstream_writer_bounds_save(&bounds,
                                 self_p,
                                 (8 * self_p->byte_offset) + self_p->bit_offset,
                                 8 * length);
    bitstream_writer_write_bytes(self_p, buf_p, length);
    bitstream_writer_bounds_restore(&bounds);
}

void bitstream_writer_insert_u8(struct bitstream_writer_t *self_p,
                                uint8_t value)
{
    struct bitstream_writer_bounds_t bounds;

    bitstream_writer_bounds_save(&bounds,
                                 self_p,
                                 (8 * self_p->byte_offset) + self_p->bit_offset,
                                 8);
    bitstream_writer_write_u8(self_p, value);
    bitstream_writer_bounds_restore(&bounds);
}

void bitstream_writer_insert_u16(struct bitstream_writer_t *self_p,
                                 uint16_t value)
{
    struct bitstream_writer_bounds_t bounds;

    bitstream_writer_bounds_save(&bounds,
                                 self_p,
                                 (8 * self_p->byte_offset) + self_p->bit_offset,
                                 16);
    bitstream_writer_write_u16(self_p, value);
    bitstream_writer_bounds_restore(&bounds);
}

void bitstream_writer_insert_u32(struct bitstream_writer_t *self_p,
                                 uint32_t value)
{
    struct bitstream_writer_bounds_t bounds;

    bitstream_writer_bounds_save(&bounds,
                                 self_p,
                                 (8 * self_p->byte_offset) + self_p->bit_offset,
                                 32);
    bitstream_writer_write_u32(self_p, value);
    bitstream_writer_bounds_restore(&bounds);
}

void bitstream_writer_insert_u64(struct bitstream_writer_t *self_p,
                                 uint64_t value)
{
    struct bitstream_writer_bounds_t bounds;

    bitstream_writer_bounds_save(&bounds,
                                 self_p,
                                 (8 * self_p->byte_offset) + self_p->bit_offset,
                                 64);
    bitstream_writer_write_u64(self_p, value);
    bitstream_writer_bounds_restore(&bounds);
}

void bitstream_writer_insert_u64_bits(struct bitstream_writer_t *self_p,
                                      uint64_t value,
                                      int number_of_bits)
{
    struct bitstream_writer_bounds_t bounds;

    bitstream_writer_bounds_save(&bounds,
                                 self_p,
                                 (8 * self_p->byte_offset) + self_p->bit_offset,
                                 number_of_bits);
    bitstream_writer_write_u64_bits(self_p, value, number_of_bits);
    bitstream_writer_bounds_restore(&bounds);
}

void bitstream_writer_seek(struct bitstream_writer_t *self_p,
                           int offset)
{
    offset += ((8 * self_p->byte_offset) + self_p->bit_offset);
    self_p->byte_offset = (offset / 8);
    self_p->bit_offset = (offset % 8);
}

void bitstream_writer_bounds_save(struct bitstream_writer_bounds_t *self_p,
                                  struct bitstream_writer_t *writer_p,
                                  int bit_offset,
                                  int length)
{
    int number_of_bits;

    self_p->writer_p = writer_p;
    number_of_bits = (bit_offset % 8);

    if (number_of_bits == 0) {
        self_p->first_byte_offset = -1;
    } else {
        self_p->first_byte_offset = (bit_offset / 8);
        self_p->first_byte = writer_p->buf_p[self_p->first_byte_offset];
        self_p->first_byte &= (0xff00 >> number_of_bits);
    }

    number_of_bits = ((bit_offset + length) % 8);

    if (number_of_bits == 0) {
        self_p->last_byte_offset = -1;
    } else {
        self_p->last_byte_offset = ((bit_offset + length) / 8);
        self_p->last_byte = writer_p->buf_p[self_p->last_byte_offset];
        self_p->last_byte &= ~(0xff00 >> number_of_bits);
        writer_p->buf_p[self_p->last_byte_offset] = 0;
    }

    if (self_p->first_byte_offset != -1) {
        writer_p->buf_p[self_p->first_byte_offset] = 0;
    }
}

void bitstream_writer_bounds_restore(struct bitstream_writer_bounds_t *self_p)
{
    if (self_p->first_byte_offset != -1) {
        self_p->writer_p->buf_p[self_p->first_byte_offset] |= self_p->first_byte;
    }

    if (self_p->last_byte_offset != -1) {
        self_p->writer_p->buf_p[self_p->last_byte_offset] |= self_p->last_byte;
    }
}

void bitstream_reader_init(struct bitstream_reader_t *self_p,
                           const uint8_t *buf_p)
{
    self_p->buf_p = buf_p;
    self_p->byte_offset = 0;
    self_p->bit_offset = 0;
}

int bitstream_reader_read_bit(struct bitstream_reader_t *self_p)
{
    int value;

    if (self_p->bit_offset == 0) {
        value = (self_p->buf_p[self_p->byte_offset] >> 7);
        self_p->bit_offset = 1;
    } else {
        value = ((self_p->buf_p[self_p->byte_offset] >> (7 - self_p->bit_offset)) & 0x1);

        if (self_p->bit_offset == 7) {
            self_p->bit_offset = 0;
            self_p->byte_offset++;
        } else {
            self_p->bit_offset++;
        }
    }

    return (value);
}

void bitstream_reader_read_bytes(struct bitstream_reader_t *self_p,
                                 uint8_t *buf_p,
                                 int length)
{
    int i;
    const uint8_t *src_p;

    src_p = &self_p->buf_p[self_p->byte_offset];

    if (self_p->bit_offset == 0) {
        memcpy(buf_p, src_p, sizeof(uint8_t) * length);
    } else {
        for (i = 0; i < length; i++) {
            buf_p[i] = (src_p[i] << self_p->bit_offset);
            buf_p[i] |= (src_p[i + 1] >> (8 - self_p->bit_offset));
        }
    }

    self_p->byte_offset += length;
}

uint8_t bitstream_reader_read_u8(struct bitstream_reader_t *self_p)
{
    uint8_t value;

    value = (self_p->buf_p[self_p->byte_offset] << self_p->bit_offset);
    self_p->byte_offset++;

    if (self_p->bit_offset != 0) {
        value |= (self_p->buf_p[self_p->byte_offset] >> (8 - self_p->bit_offset));
    }

    return (value);
}

uint16_t bitstream_reader_read_u16(struct bitstream_reader_t *self_p)
{
    uint16_t value;
    int i;
    int offset;
    const uint8_t *src_p;

    src_p = &self_p->buf_p[self_p->byte_offset];
    offset = (16 + self_p->bit_offset);
    value = 0;

    for (i = 0; i < 2; i++) {
        offset -= 8;
        value |= ((uint16_t)src_p[i] << offset);
    }

    if (offset != 0) {
        value |= (src_p[2] >> (8 - offset));
    }

    self_p->byte_offset += 2;

    return (value);
}

uint32_t bitstream_reader_read_u32(struct bitstream_reader_t *self_p)
{
    uint32_t value;
    int i;
    int offset;
    const uint8_t *src_p;

    src_p = &self_p->buf_p[self_p->byte_offset];
    offset = (32 + self_p->bit_offset);
    value = 0;

    for (i = 0; i < 4; i++) {
        offset -= 8;
        value |= ((uint32_t)src_p[i] << offset);
    }

    if (offset != 0) {
        value |= (src_p[4] >> (8 - offset));
    }

    self_p->byte_offset += 4;

    return (value);
}

uint64_t bitstream_reader_read_u64(struct bitstream_reader_t *self_p)
{
    uint64_t value;
    int i;
    int offset;
    const uint8_t *src_p;

    src_p = &self_p->buf_p[self_p->byte_offset];
    offset = (64 + self_p->bit_offset);
    value = 0;

    for (i = 0; i < 8; i++) {
        offset -= 8;
        value |= ((uint64_t)src_p[i] << offset);
    }

    if (offset != 0) {
        value |= ((uint64_t)src_p[8] >> (8 - offset));
    }

    self_p->byte_offset += 8;

    return (value);
}

uint64_t bitstream_reader_read_u64_bits(struct bitstream_reader_t *self_p,
                                        int number_of_bits)
{
    uint64_t value;
    int i;
    int first_byte_bits;
    int last_byte_bits;
    int full_bytes;

    if (number_of_bits == 0) {
        return (0);
    }

    /* Align beginning. */
    first_byte_bits = (8 - self_p->bit_offset);

    if (first_byte_bits != 8) {
        if (number_of_bits < first_byte_bits) {
            value = (self_p->buf_p[self_p->byte_offset] >> (first_byte_bits
                                                            - number_of_bits));
            value &= ((1 << number_of_bits) - 1);
            self_p->bit_offset += number_of_bits;
        } else {
            value = self_p->buf_p[self_p->byte_offset];
            value &= ((1 << first_byte_bits) - 1);
            self_p->byte_offset++;
            self_p->bit_offset = 0;
        }

        number_of_bits -= first_byte_bits;

        if (number_of_bits <= 0) {
            return (value);
        }
    } else {
        value = 0;
    }

    /* Copy middle bytes. */
    full_bytes = (number_of_bits / 8);

    for (i = 0; i < full_bytes; i++) {
        value <<= 8;
        value |= self_p->buf_p[self_p->byte_offset + i];
    }

    /* Last byte. */
    last_byte_bits = (number_of_bits % 8);

    if (last_byte_bits != 0) {
        value <<= last_byte_bits;
        value |= (self_p->buf_p[self_p->byte_offset + full_bytes]
                  >> (8 - last_byte_bits));
        self_p->bit_offset = last_byte_bits;
    }

    self_p->byte_offset += full_bytes;

    return (value);
}

void bitstream_reader_seek(struct bitstream_reader_t *self_p,
                           int offset)
{
    offset += ((8 * self_p->byte_offset) + self_p->bit_offset);
    self_p->byte_offset = (offset / 8);
    self_p->bit_offset = (offset % 8);
}

int bitstream_reader_tell(struct bitstream_reader_t *self_p)
{
    return ((8 * self_p->byte_offset) + self_p->bit_offset);
}

Read our documentation on viewing source code .

Navigation	Overlay
`t` Navigate files	`h` Toggle hits
`y` Change url to tip of branch	`m` Toggle misses
`b / v` Jump to prev/next hit line	`p` Toggle partial
`z / x` Jump to prev/next missed or partial line	`1..9` Toggle flags
`shift + o` Open current page in GitHub	`a` Toggle all on
`/ or ?` Show keyboard shortcuts dialog	`c` Toggle context lines or commits

1		/**
2		* The MIT License (MIT)
3		*
4		* Copyright (c) 2019 Erik Moqvist
5		*
6		* Permission is hereby granted, free of charge, to any person
7		* obtaining a copy of this software and associated documentation
8		* files (the "Software"), to deal in the Software without
9		* restriction, including without limitation the rights to use, copy,
10		* modify, merge, publish, distribute, sublicense, and/or sell copies
11		* of the Software, and to permit persons to whom the Software is
12		* furnished to do so, subject to the following conditions:
13		*
14		* The above copyright notice and this permission notice shall be
15		* included in all copies or substantial portions of the Software.
16		*
17		* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
18		* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
19		* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
20		* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
21		* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
22		* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
23		* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24		* SOFTWARE.
25		*/
26
27		#include <string.h>
28		#include "bitstream.h"
29
30	1	void bitstream_writer_init(struct bitstream_writer_t *self_p,
31		uint8_t *buf_p)
32		{
33	1	self_p->buf_p = buf_p;
34	1	self_p->byte_offset = 0;
35	1	self_p->bit_offset = 0;
36		}
37
38	1	int bitstream_writer_size_in_bits(struct bitstream_writer_t *self_p)
39		{
40	1	return (8 * self_p->byte_offset + self_p->bit_offset);
41		}
42
43	1	int bitstream_writer_size_in_bytes(struct bitstream_writer_t *self_p)
44		{
45	1	return (self_p->byte_offset + (self_p->bit_offset + 7) / 8);
46		}
47
48	1	void bitstream_writer_write_bit(struct bitstream_writer_t *self_p,
49		int value)
50		{
51	1	if (self_p->bit_offset == 0) {
52	1	self_p->buf_p[self_p->byte_offset] = (value << 7);
53	1	self_p->bit_offset = 1;
54		} else {
55	1	self_p->buf_p[self_p->byte_offset] \|= (value << (8 - self_p->bit_offset - 1));
56
57	1	if (self_p->bit_offset == 7) {
58	1	self_p->bit_offset = 0;
59	1	self_p->byte_offset++;
60		} else {
61	1	self_p->bit_offset++;
62		}
63		}
64		}
65
66	1	void bitstream_writer_write_bytes(struct bitstream_writer_t *self_p,
67		const uint8_t *buf_p,
68		int length)
69		{
70		int i;
71		uint8_t *dst_p;
72
73	1	dst_p = &self_p->buf_p[self_p->byte_offset];
74
75	1	if (self_p->bit_offset == 0) {
76	1	memcpy(dst_p, buf_p, sizeof(uint8_t) * length);
77		} else {
78	1	for (i = 0; i < length; i++) {
79	1	dst_p[i] \|= (buf_p[i] >> self_p->bit_offset);
80	1	dst_p[i + 1] = (uint8_t)(buf_p[i] << (8 - self_p->bit_offset));
81		}
82		}
83
84	1	self_p->byte_offset += length;
85		}
86
87	1	void bitstream_writer_write_u8(struct bitstream_writer_t *self_p,
88		uint8_t value)
89		{
90	1	if (self_p->bit_offset == 0) {
91	1	self_p->buf_p[self_p->byte_offset] = value;
92		} else {
93	1	self_p->buf_p[self_p->byte_offset] \|= (value >> self_p->bit_offset);
94	1	self_p->buf_p[self_p->byte_offset + 1] =
95	1	(uint8_t)(value << (8 - self_p->bit_offset));
96		}
97
98	1	self_p->byte_offset++;
99		}
100
101	1	void bitstream_writer_write_u16(struct bitstream_writer_t *self_p,
102		uint16_t value)
103		{
104	1	if (self_p->bit_offset == 0) {
105	1	self_p->buf_p[self_p->byte_offset] = (value >> 8);
106		} else {
107	1	self_p->buf_p[self_p->byte_offset] \|= (value >> (8 + self_p->bit_offset));
108	1	self_p->buf_p[self_p->byte_offset + 2] =
109	1	(uint8_t)(value << (8 - self_p->bit_offset));
110	1	value >>= self_p->bit_offset;
111		}
112
113	1	self_p->buf_p[self_p->byte_offset + 1] = (uint8_t)value;
114	1	self_p->byte_offset += 2;
115		}
116
117	1	void bitstream_writer_write_u32(struct bitstream_writer_t *self_p,
118		uint32_t value)
119		{
120		int i;
121
122	1	if (self_p->bit_offset == 0) {
123	1	self_p->buf_p[self_p->byte_offset] = (value >> 24);
124		} else {
125	1	self_p->buf_p[self_p->byte_offset] \|= (value >> (24 + self_p->bit_offset));
126	1	self_p->buf_p[self_p->byte_offset + 4] =
127	1	(uint8_t)(value << (8 - self_p->bit_offset));
128	1	value >>= self_p->bit_offset;
129		}
130
131	1	for (i = 3; i > 0; i--) {
132	1	self_p->buf_p[self_p->byte_offset + i] = value;
133	1	value >>= 8;
134		}
135
136	1	self_p->byte_offset += 4;
137		}
138
139	1	void bitstream_writer_write_u64(struct bitstream_writer_t *self_p,
140		uint64_t value)
141		{
142		int i;
143
144
145	1	if (self_p->bit_offset == 0) {
146	1	self_p->buf_p[self_p->byte_offset] = (value >> 56);
147		} else {
148	1	self_p->buf_p[self_p->byte_offset] \|= (value >> (56 + self_p->bit_offset));
149	1	self_p->buf_p[self_p->byte_offset + 8] =
150	1	(uint8_t)(value << (8 - self_p->bit_offset));
151	1	value >>= self_p->bit_offset;
152		}
153
154	1	for (i = 7; i > 0; i--) {
155	1	self_p->buf_p[self_p->byte_offset + i] = (uint8_t)value;
156	1	value >>= 8;
157		}
158
159	1	self_p->byte_offset += 8;
160		}
161
162	1	void bitstream_writer_write_u64_bits(struct bitstream_writer_t *self_p,
163		uint64_t value,
164		int number_of_bits)
165		{
166		int i;
167		int first_byte_bits;
168		int last_byte_bits;
169		int full_bytes;
170
171	1	if (number_of_bits == 0) {
172	1	return;
173		}
174
175		/* Align beginning. */
176	1	first_byte_bits = (8 - self_p->bit_offset);
177
178	1	if (first_byte_bits != 8) {
179	1	if (number_of_bits < first_byte_bits) {
180	1	self_p->buf_p[self_p->byte_offset] \|=
181	1	(uint8_t)(value << (first_byte_bits - number_of_bits));
182	1	self_p->bit_offset += number_of_bits;
183		} else {
184	1	self_p->buf_p[self_p->byte_offset] \|= (value >> (number_of_bits
185	1	- first_byte_bits));
186	1	self_p->byte_offset++;
187	1	self_p->bit_offset = 0;
188		}
189
190	1	number_of_bits -= first_byte_bits;
191
192	1	if (number_of_bits <= 0) {
193	1	return;
194		}
195		}
196
197		/* Align end. */
198	1	last_byte_bits = (number_of_bits % 8);
199	1	full_bytes = (number_of_bits / 8);
200
201	1	if (last_byte_bits != 0) {
202	1	self_p->buf_p[self_p->byte_offset + full_bytes] =
203	1	(uint8_t)(value << (8 - last_byte_bits));
204	1	value >>= last_byte_bits;
205	1	self_p->bit_offset = last_byte_bits;
206		}
207
208		/* Copy middle bytes. */
209	1	for (i = full_bytes; i > 0; i--) {
210	1	self_p->buf_p[self_p->byte_offset + i - 1] = (uint8_t)value;
211	1	value >>= 8;
212		}
213
214	1	self_p->byte_offset += full_bytes;
215		}
216
217	1	void bitstream_writer_write_repeated_bit(struct bitstream_writer_t *self_p,
218		int value,
219		int length)
220		{
221		int rest;
222
223	1	if (value != 0) {
224	1	value = 0xff;
225		}
226
227	1	rest = (length % 8);
228	1	bitstream_writer_write_u64_bits(self_p, value & ((1 << rest) - 1), rest);
229	1	bitstream_writer_write_repeated_u8(self_p, value, length / 8);
230		}
231
232	1	void bitstream_writer_write_repeated_u8(struct bitstream_writer_t *self_p,
233		uint8_t value,
234		int length)
235		{
236		int i;
237
238	1	for (i = 0; i < length; i++) {
239	1	bitstream_writer_write_u8(self_p, value);
240		}
241		}
242
243	1	void bitstream_writer_insert_bit(struct bitstream_writer_t *self_p,
244		int value)
245		{
246		struct bitstream_writer_bounds_t bounds;
247
248	1	bitstream_writer_bounds_save(&bounds,
249		self_p,
250	1	(8 * self_p->byte_offset) + self_p->bit_offset,
251		1);
252	1	bitstream_writer_write_bit(self_p, value);
253	1	bitstream_writer_bounds_restore(&bounds);
254		}
255
256	1	void bitstream_writer_insert_bytes(struct bitstream_writer_t *self_p,
257		const uint8_t *buf_p,
258		int length)
259		{
260		struct bitstream_writer_bounds_t bounds;
261
262	1	bitstream_writer_bounds_save(&bounds,
263		self_p,
264	1	(8 * self_p->byte_offset) + self_p->bit_offset,
265		8 * length);
266	1	bitstream_writer_write_bytes(self_p, buf_p, length);
267	1	bitstream_writer_bounds_restore(&bounds);
268		}
269
270	1	void bitstream_writer_insert_u8(struct bitstream_writer_t *self_p,
271		uint8_t value)
272		{
273		struct bitstream_writer_bounds_t bounds;
274
275	1	bitstream_writer_bounds_save(&bounds,
276		self_p,
277	1	(8 * self_p->byte_offset) + self_p->bit_offset,
278		8);
279	1	bitstream_writer_write_u8(self_p, value);
280	1	bitstream_writer_bounds_restore(&bounds);
281		}
282
283	1	void bitstream_writer_insert_u16(struct bitstream_writer_t *self_p,
284		uint16_t value)
285		{
286		struct bitstream_writer_bounds_t bounds;
287
288	1	bitstream_writer_bounds_save(&bounds,
289		self_p,
290	1	(8 * self_p->byte_offset) + self_p->bit_offset,
291		16);
292	1	bitstream_writer_write_u16(self_p, value);
293	1	bitstream_writer_bounds_restore(&bounds);
294		}
295
296	1	void bitstream_writer_insert_u32(struct bitstream_writer_t *self_p,
297		uint32_t value)
298		{
299		struct bitstream_writer_bounds_t bounds;
300
301	1	bitstream_writer_bounds_save(&bounds,
302		self_p,
303	1	(8 * self_p->byte_offset) + self_p->bit_offset,
304		32);
305	1	bitstream_writer_write_u32(self_p, value);
306	1	bitstream_writer_bounds_restore(&bounds);
307		}
308
309	1	void bitstream_writer_insert_u64(struct bitstream_writer_t *self_p,
310		uint64_t value)
311		{
312		struct bitstream_writer_bounds_t bounds;
313
314	1	bitstream_writer_bounds_save(&bounds,
315		self_p,
316	1	(8 * self_p->byte_offset) + self_p->bit_offset,
317		64);
318	1	bitstream_writer_write_u64(self_p, value);
319	1	bitstream_writer_bounds_restore(&bounds);
320		}
321
322	1	void bitstream_writer_insert_u64_bits(struct bitstream_writer_t *self_p,
323		uint64_t value,
324		int number_of_bits)
325		{
326		struct bitstream_writer_bounds_t bounds;
327
328	1	bitstream_writer_bounds_save(&bounds,
329		self_p,
330	1	(8 * self_p->byte_offset) + self_p->bit_offset,
331		number_of_bits);
332	1	bitstream_writer_write_u64_bits(self_p, value, number_of_bits);
333	1	bitstream_writer_bounds_restore(&bounds);
334		}
335
336	1	void bitstream_writer_seek(struct bitstream_writer_t *self_p,
337		int offset)
338		{
339	1	offset += ((8 * self_p->byte_offset) + self_p->bit_offset);
340	1	self_p->byte_offset = (offset / 8);
341	1	self_p->bit_offset = (offset % 8);
342		}
343
344	1	void bitstream_writer_bounds_save(struct bitstream_writer_bounds_t *self_p,
345		struct bitstream_writer_t *writer_p,
346		int bit_offset,
347		int length)
348		{
349		int number_of_bits;
350
351	1	self_p->writer_p = writer_p;
352	1	number_of_bits = (bit_offset % 8);
353
354	1	if (number_of_bits == 0) {
355	1	self_p->first_byte_offset = -1;
356		} else {
357	1	self_p->first_byte_offset = (bit_offset / 8);
358	1	self_p->first_byte = writer_p->buf_p[self_p->first_byte_offset];
359	1	self_p->first_byte &= (0xff00 >> number_of_bits);
360		}
361
362	1	number_of_bits = ((bit_offset + length) % 8);
363
364	1	if (number_of_bits == 0) {
365	1	self_p->last_byte_offset = -1;
366		} else {
367	1	self_p->last_byte_offset = ((bit_offset + length) / 8);
368	1	self_p->last_byte = writer_p->buf_p[self_p->last_byte_offset];
369	1	self_p->last_byte &= ~(0xff00 >> number_of_bits);
370	1	writer_p->buf_p[self_p->last_byte_offset] = 0;
371		}
372
373	1	if (self_p->first_byte_offset != -1) {
374	1	writer_p->buf_p[self_p->first_byte_offset] = 0;
375		}
376		}
377
378	1	void bitstream_writer_bounds_restore(struct bitstream_writer_bounds_t *self_p)
379		{
380	1	if (self_p->first_byte_offset != -1) {
381	1	self_p->writer_p->buf_p[self_p->first_byte_offset] \|= self_p->first_byte;
382		}
383
384	1	if (self_p->last_byte_offset != -1) {
385	1	self_p->writer_p->buf_p[self_p->last_byte_offset] \|= self_p->last_byte;
386		}
387		}
388
389	1	void bitstream_reader_init(struct bitstream_reader_t *self_p,
390		const uint8_t *buf_p)
391		{
392	1	self_p->buf_p = buf_p;
393	1	self_p->byte_offset = 0;
394	1	self_p->bit_offset = 0;
395		}
396
397	1	int bitstream_reader_read_bit(struct bitstream_reader_t *self_p)
398		{
399		int value;
400
401	1	if (self_p->bit_offset == 0) {
402	1	value = (self_p->buf_p[self_p->byte_offset] >> 7);
403	1	self_p->bit_offset = 1;
404		} else {
405	1	value = ((self_p->buf_p[self_p->byte_offset] >> (7 - self_p->bit_offset)) & 0x1);
406
407	1	if (self_p->bit_offset == 7) {
408	1	self_p->bit_offset = 0;
409	1	self_p->byte_offset++;
410		} else {
411	1	self_p->bit_offset++;
412		}
413		}
414
415	1	return (value);
416		}
417
418	1	void bitstream_reader_read_bytes(struct bitstream_reader_t *self_p,
419		uint8_t *buf_p,
420		int length)
421		{
422		int i;
423		const uint8_t *src_p;
424
425	1	src_p = &self_p->buf_p[self_p->byte_offset];
426
427	1	if (self_p->bit_offset == 0) {
428	1	memcpy(buf_p, src_p, sizeof(uint8_t) * length);
429		} else {
430	1	for (i = 0; i < length; i++) {
431	1	buf_p[i] = (src_p[i] << self_p->bit_offset);
432	1	buf_p[i] \|= (src_p[i + 1] >> (8 - self_p->bit_offset));
433		}
434		}
435
436	1	self_p->byte_offset += length;
437		}
438
439	1	uint8_t bitstream_reader_read_u8(struct bitstream_reader_t *self_p)
440		{
441		uint8_t value;
442
443	1	value = (self_p->buf_p[self_p->byte_offset] << self_p->bit_offset);
444	1	self_p->byte_offset++;
445
446	1	if (self_p->bit_offset != 0) {
447	1	value \|= (self_p->buf_p[self_p->byte_offset] >> (8 - self_p->bit_offset));
448		}
449
450	1	return (value);
451		}
452
453	1	uint16_t bitstream_reader_read_u16(struct bitstream_reader_t *self_p)
454		{
455		uint16_t value;
456		int i;
457		int offset;
458		const uint8_t *src_p;
459
460	1	src_p = &self_p->buf_p[self_p->byte_offset];
461	1	offset = (16 + self_p->bit_offset);
462	1	value = 0;
463
464	1	for (i = 0; i < 2; i++) {
465	1	offset -= 8;
466	1	value \|= ((uint16_t)src_p[i] << offset);
467		}
468
469	1	if (offset != 0) {
470	1	value \|= (src_p[2] >> (8 - offset));
471		}
472
473	1	self_p->byte_offset += 2;
474
475	1	return (value);
476		}
477
478	1	uint32_t bitstream_reader_read_u32(struct bitstream_reader_t *self_p)
479		{
480		uint32_t value;
481		int i;
482		int offset;
483		const uint8_t *src_p;
484
485	1	src_p = &self_p->buf_p[self_p->byte_offset];
486	1	offset = (32 + self_p->bit_offset);
487	1	value = 0;
488
489	1	for (i = 0; i < 4; i++) {
490	1	offset -= 8;
491	1	value \|= ((uint32_t)src_p[i] << offset);
492		}
493
494	1	if (offset != 0) {
495	1	value \|= (src_p[4] >> (8 - offset));
496		}
497
498	1	self_p->byte_offset += 4;
499
500	1	return (value);
501		}
502
503	1	uint64_t bitstream_reader_read_u64(struct bitstream_reader_t *self_p)
504		{
505		uint64_t value;
506		int i;
507		int offset;
508		const uint8_t *src_p;
509
510	1	src_p = &self_p->buf_p[self_p->byte_offset];
511	1	offset = (64 + self_p->bit_offset);
512	1	value = 0;
513
514	1	for (i = 0; i < 8; i++) {
515	1	offset -= 8;
516	1	value \|= ((uint64_t)src_p[i] << offset);
517		}
518
519	1	if (offset != 0) {
520	1	value \|= ((uint64_t)src_p[8] >> (8 - offset));
521		}
522
523	1	self_p->byte_offset += 8;
524
525	1	return (value);
526		}
527
528	1	uint64_t bitstream_reader_read_u64_bits(struct bitstream_reader_t *self_p,
529		int number_of_bits)
530		{
531		uint64_t value;
532		int i;
533		int first_byte_bits;
534		int last_byte_bits;
535		int full_bytes;
536
537	1	if (number_of_bits == 0) {
538	1	return (0);
539		}
540
541		/* Align beginning. */
542	1	first_byte_bits = (8 - self_p->bit_offset);
543
544	1	if (first_byte_bits != 8) {
545	1	if (number_of_bits < first_byte_bits) {
546	1	value = (self_p->buf_p[self_p->byte_offset] >> (first_byte_bits
547	1	- number_of_bits));
548	1	value &= ((1 << number_of_bits) - 1);
549	1	self_p->bit_offset += number_of_bits;
550		} else {
551	1	value = self_p->buf_p[self_p->byte_offset];
552	1	value &= ((1 << first_byte_bits) - 1);
553	1	self_p->byte_offset++;
554	1	self_p->bit_offset = 0;
555		}
556
557	1	number_of_bits -= first_byte_bits;
558
559	1	if (number_of_bits <= 0) {
560	1	return (value);
561		}
562		} else {
563	1	value = 0;
564		}
565
566		/* Copy middle bytes. */
567	1	full_bytes = (number_of_bits / 8);
568
569	1	for (i = 0; i < full_bytes; i++) {
570	1	value <<= 8;
571	1	value \|= self_p->buf_p[self_p->byte_offset + i];
572		}
573
574		/* Last byte. */
575	1	last_byte_bits = (number_of_bits % 8);
576
577	1	if (last_byte_bits != 0) {
578	1	value <<= last_byte_bits;
579	1	value \|= (self_p->buf_p[self_p->byte_offset + full_bytes]
580	1	>> (8 - last_byte_bits));
581	1	self_p->bit_offset = last_byte_bits;
582		}
583
584	1	self_p->byte_offset += full_bytes;
585
586	1	return (value);
587		}
588
589	1	void bitstream_reader_seek(struct bitstream_reader_t *self_p,
590		int offset)
591		{
592	1	offset += ((8 * self_p->byte_offset) + self_p->bit_offset);
593	1	self_p->byte_offset = (offset / 8);
594	1	self_p->bit_offset = (offset % 8);
595		}
596
597	1	int bitstream_reader_tell(struct bitstream_reader_t *self_p)
598		{
599	1	return ((8 * self_p->byte_offset) + self_p->bit_offset);
600		}