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
}

Read our documentation on viewing source code .

Loading