Tool_Box/hfparse.c at master · BitCrane/Tool_Box · 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
/* hfparse.c */

#include <stdlib.h>
#include <stdint.h>
#include <string.h>

#include "swab.h"
#include "crc.h"
#include "hf_protocol.h"
#include "hfparse.h"


struct hfparseS {
    enum {lookingForHeaderPS, readingDataPS} state;
    unsigned int index;
    unsigned int count;
    hfparseOptsT opts;
    hfPacketT p;
    int lastHeaderGood;
    hfparseStatsT stats;
};


hfparseT *hfparseCreate(hfparseOptsT *opts) {
    hfparseT *obj;

    obj = malloc(sizeof(hfparseT));
    if (obj) {
        memset(obj, 0, sizeof(*obj));
        memcpy(&obj->opts, opts, sizeof(obj->opts));
    }

    return obj;
}

void hfparseRun(hfparseT *obj, unsigned char *d, unsigned int length) {
    unsigned int u;

    while (length) {
        switch (obj->state) {
        case lookingForHeaderPS:
            if (obj->index == 8) {
                if (obj->lastHeaderGood) {
                    obj->stats.syncLoss++;
                    obj->lastHeaderGood = 0;
                }
                obj->stats.bytesDiscarded++;
                memmove(&obj->p.h.b[0], &obj->p.h.b[1], 7);
                obj->index--;
            }
            obj->p.h.b[obj->index++] = *d++;
            if (obj->index == 8 &&
                obj->p.h.h.preamble == HF_PREAMBLE &&
                obj->p.h.h.crc8 ==
                crc8Accumulate(CRC8_INITIAL, &obj->p.h.b[1], 8 - 2)) {
                obj->lastHeaderGood = 1;
                obj->count = obj->p.h.h.data_length;
                obj->index = 0;
                if (obj->count) {
                    obj->state = readingDataPS;
                    obj->count *= 4;
                    if (obj->opts.includeDataCRC)
                        obj->count += 4;
                } else if (obj->opts.packet)
                    obj->opts.packet(obj->opts.user, &obj->p);
            }
            length--;
            break;
        case readingDataPS:
            u = length;
            if (u > obj->count)
                u = obj->count;
            memcpy(&obj->p.d.b[obj->index], d, u);
            obj->index += u;
            d += u;
            obj->count -= u;
            length -= u;
            if (obj->count == 0) {
                if (!obj->opts.includeDataCRC ||
                    LEToNativeUGawble(obj->p.d.g[obj->p.h.h.data_length]) ==
                    crc32Accumulate(CRC32_INITIAL, &obj->p.d.b[0],
                                    obj->index - 4)) {
                    if (obj->opts.packet)
                        obj->opts.packet(obj->opts.user, &obj->p);
                } else
                    obj->stats.dataCRCErrors++;
                obj->index = 0;
                obj->state = lookingForHeaderPS;
            }
            break;
        }
    }
}

void hfparseStats(hfparseT *obj, hfparseStatsT *stats) {

    memcpy(stats, &obj->stats, sizeof(*stats));
}

void hfparseDestroy(hfparseT *obj) {

    if (obj)
        free(obj);
}