/
HDLC.py
191 lines (135 loc) · 4.64 KB
/
HDLC.py
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
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
#!/usr/bin/python
import sys
from CRC import CRC16CCITT
from io import StringIO
class HDLC:
SEARCH = 0
HUNT = 1
FRAME = 2
FLAG = 0x7E
ABORT = 0x7F
IDLE = 0xFF
BOGONS = [0xFF00, 0xFE00, 0x7F00]
FRAME_ERRORS = [0xFF00, 0xFE00, 0xFC00, 0x7F00, 0x7E00]
def __init__(self, passall = False, callback = None):
self.passall = passall
self.callback = callback
self.reset()
self.action = [self.search, self.hunt, self.do_frame]
def reset(self):
self.state = HDLC.SEARCH
self.bits = 0
self.ones = 0
self.buffer = 0
self.frame = StringIO()
self.ready = False
self.pll = False
def __call__(self, bit, pll):
result = None
if not pll:
if self.frame.tell() > 14 and self.passall:
result = (self.crc(), self.frame.getvalue())
# print "lost lock"
self.reset()
else:
self.pll = pll
result = self.process(bit)
if result is not None:
crc = result[0]
self.reset()
if crc != 0xf0b8 and not self.passall:
result = None
else:
self.start_hunt()
return result
def framing(self): return self.state == HDLC.FRAME
def _add_bit(self, bit):
self.buffer >>= 1
self.buffer |= (0x8000 * int(bit))
self.bits += 1
# print hex(self.buffer), self.bits, bit
def _have_flag(self):
return (self.buffer & 0xFF00) == 0x7E00
def _have_bogon(self):
return (self.buffer & 0xFF00) in HDLC.BOGONS
def _have_frame_error(self):
return (self.buffer & 0xFF00) in HDLC.FRAME_ERRORS
def start_search(self):
# print "search"
self.state = HDLC.SEARCH
def search(self, bit):
self._add_bit(bit)
if self._have_flag():
self.start_hunt()
return None
def start_hunt(self):
# print "hunt"
self.state = HDLC.HUNT
self.bits = 0
self.buffer = 0
def hunt(self, bit):
self._add_bit(bit)
if self.bits == 8:
if self._have_flag(): self.start_hunt()
elif self._have_bogon(): self.start_search()
elif not self._have_frame_error(): self.start_frame()
else: self.start_search()
return None
def start_frame(self):
# sys.stdout.write("\rframe")
# sys.stdout.flush()
self.state = HDLC.FRAME
self.frame = StringIO()
self.ones = 0
self.buffer &= 0xFF00
def getchar(self):
return chr(self.buffer & 0xFF)
def consume_byte(self):
self.buffer &= 0xFF00
self.bits -= 8
def consume_bit(self):
tmp = (self.buffer & 0x7F) << 1
self.buffer &= 0xFF00
self.buffer |= tmp
self.bits -= 1
def do_frame(self, bit):
result = None
self._add_bit(bit)
if self.ones < 5:
if (self.buffer & 0x80) == 0x80:
self.ones += 1
else:
self.ones = 0
if self.bits == 16:
self.frame.write(self.getchar())
self.consume_byte()
if self._have_flag():
if self.frame.tell() > 14:
# result = self.check_frame()
result = (self.crc(), self.frame.getvalue()[:-2])
self.start_frame()
else: # too many ones
if (self.buffer & 0x80) == 0:
self.consume_bit()
self.ones = 0
else:
# print "frame error"
if self.passall and self.frame.tell() > 14:
result = (self.crc(), self.frame.getvalue())
# Framing error
if ((self.buffer >> (16 - self.bits)) & 0xFF) == 0x7E:
# Cannot call start_hunt(); we need to preserve buffer
self.bits -= 8
self.state = HDLC.HUNT
self.frame = StringIO()
else:
self.start_search()
return result
def process(self, bit):
return self.action[self.state](bit)
def check_frame(self):
if self.crc() == 0xF0B8: return self.frame.getvalue()
return None
def crc(self):
crc = CRC16CCITT()
return crc.compute(self.frame.getvalue())