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
|
/*
* dectmon DLC message reassembly
*
* Copyright (c) 2010 Patrick McHardy <kaber@trash.net>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*/
#include <assert.h>
#include <stdio.h>
#include <dect/libdect.h>
#include <dectmon.h>
#include <mac.h>
#include <dlc.h>
#define dlc_print(fmt, args...) \
do { \
if (dumpopts & DECTMON_DUMP_DLC) \
printf(fmt, ## args); \
} while (0)
#if 1
#define lc_debug(lc, fmt, args...) dlc_print(fmt, ## args)
#else
#define lc_debug(lc, fmt, args...)
#endif
static void dect_fa_parse_len(struct dect_fa_len *len,
const struct dect_msg_buf *mb)
{
uint8_t l;
l = mb->data[DECT_FA_LI_OFF];
len->len = (l & DECT_FA_LI_LENGTH_MASK) >> DECT_FA_LI_LENGTH_SHIFT;
len->more = (l & DECT_FA_LI_M_FLAG);
}
static struct dect_lc *dect_lc_init(struct dect_mac_con *mc)
{
struct dect_lc *lc;
lc = calloc(1, sizeof(*lc));
if ((mc->pmid & 0xf0000) != 0xe0000)
lc->lsig = mc->pmid;
return lc;
}
static bool dect_fa_frame_csum_verify(const struct dect_lc *lc,
struct dect_msg_buf *mb)
{
uint8_t *data = mb->data;
unsigned int i;
uint8_t c0 = 0, c1 = 0;
uint16_t t;
data[mb->len - 2] ^= lc->lsig >> 8;
data[mb->len - 1] ^= lc->lsig & 0xff;
for (i = 0; i < mb->len; i++) {
t = c0 + data[i];
c0 = (t & 0xffU) + ((t >> 8) & 0x1U);
t = c1 + c0;
c1 = (t & 0xffU) + ((t >> 8) & 0x1U);
}
lc_debug(lc, "csum verify: lsig %.4x c0: %.2x c1: %.2x\n",
lc->lsig, c0, c1);
return c0 == (uint8_t)~0 && c1 == (uint8_t)~0;
}
static const uint8_t channel_sdu_size[] = {
[DECT_MC_C_S] = DECT_C_S_SDU_SIZE,
[DECT_MC_C_F] = DECT_C_F_SDU_SIZE,
};
#define roundup(x, y) ((((x) + ((y) - 1)) / (y)) * (y))
static struct dect_msg_buf *dect_lc_reassemble(struct dect_lc *lc,
enum dect_data_channels chan,
struct dect_msg_buf *mb)
{
struct dect_fa_len fl;
uint8_t sdu_len, len;
sdu_len = channel_sdu_size[chan];
if (lc->rx_buf == NULL) {
dect_fa_parse_len(&fl, mb);
len = fl.len;
len += DECT_FA_HDR_SIZE + DECT_FA_CSUM_SIZE;
lc->rx_len = roundup(len, sdu_len);
lc->rx_buf = dect_mbuf_alloc(dh);
if (lc->rx_buf == NULL)
goto err;
}
memcpy(dect_mbuf_put(lc->rx_buf, sdu_len), mb->data, sdu_len);
mb = NULL;
if (lc->rx_buf->len >= lc->rx_len) {
assert(lc->rx_buf->len == lc->rx_len);
mb = lc->rx_buf;
lc->rx_buf = NULL;
if (!dect_fa_frame_csum_verify(lc, mb))
goto err;
/* Trim checksum and filling */
dect_fa_parse_len(&fl, mb);
mb->len = fl.len + DECT_FA_HDR_SIZE;
lc_debug(lc, "reassembled SDU len %u\n", mb->len);
}
return mb;
err:
lc_debug(lc, "reassembly failed\n");
return NULL;
}
void dect_mac_co_data_ind(struct dect_mac_con *mc, enum dect_data_channels chan,
struct dect_msg_buf *mb)
{
struct dect_lc *lc;
//printf("MAC_CO_DATA-ind\n");
if (mc->lc == NULL) {
lc = dect_lc_init(mc);
if (lc == NULL)
return;
mc->lc = lc;
}
mb = dect_lc_reassemble(mc->lc, chan, mb);
if (mb != NULL && mb->len > DECT_FA_HDR_SIZE) {
dect_mbuf_pull(mb, DECT_FA_HDR_SIZE);
dect_dl_data_ind(&mc->tbc->dl, mb);
}
}
|
