Commit | Line | Data |
---|---|---|
1da177e4 LT |
1 | /* |
2 | * Driver for ST5481 USB ISDN modem | |
3 | * | |
4 | * Author Frode Isaksen | |
5 | * Copyright 2001 by Frode Isaksen <fisaksen@bewan.com> | |
6 | * 2001 by Kai Germaschewski <kai.germaschewski@gmx.de> | |
7 | * | |
8 | * This software may be used and distributed according to the terms | |
9 | * of the GNU General Public License, incorporated herein by reference. | |
10 | * | |
11 | */ | |
12 | ||
13 | #include <linux/crc-ccitt.h> | |
14 | #include "st5481_hdlc.h" | |
15 | ||
16 | ||
17 | enum { | |
18 | HDLC_FAST_IDLE,HDLC_GET_FLAG_B0,HDLC_GETFLAG_B1A6,HDLC_GETFLAG_B7, | |
19 | HDLC_GET_DATA,HDLC_FAST_FLAG | |
20 | }; | |
21 | ||
22 | enum { | |
23 | HDLC_SEND_DATA,HDLC_SEND_CRC1,HDLC_SEND_FAST_FLAG, | |
24 | HDLC_SEND_FIRST_FLAG,HDLC_SEND_CRC2,HDLC_SEND_CLOSING_FLAG, | |
25 | HDLC_SEND_IDLE1,HDLC_SEND_FAST_IDLE,HDLC_SENDFLAG_B0, | |
26 | HDLC_SENDFLAG_B1A6,HDLC_SENDFLAG_B7,STOPPED | |
27 | }; | |
28 | ||
29 | void | |
30 | hdlc_rcv_init(struct hdlc_vars *hdlc, int do_adapt56) | |
31 | { | |
32 | hdlc->bit_shift = 0; | |
33 | hdlc->hdlc_bits1 = 0; | |
34 | hdlc->data_bits = 0; | |
35 | hdlc->ffbit_shift = 0; | |
36 | hdlc->data_received = 0; | |
37 | hdlc->state = HDLC_GET_DATA; | |
38 | hdlc->do_adapt56 = do_adapt56; | |
39 | hdlc->dchannel = 0; | |
40 | hdlc->crc = 0; | |
41 | hdlc->cbin = 0; | |
42 | hdlc->shift_reg = 0; | |
43 | hdlc->ffvalue = 0; | |
44 | hdlc->dstpos = 0; | |
45 | } | |
46 | ||
47 | void | |
48 | hdlc_out_init(struct hdlc_vars *hdlc, int is_d_channel, int do_adapt56) | |
49 | { | |
50 | hdlc->bit_shift = 0; | |
51 | hdlc->hdlc_bits1 = 0; | |
52 | hdlc->data_bits = 0; | |
53 | hdlc->ffbit_shift = 0; | |
54 | hdlc->data_received = 0; | |
55 | hdlc->do_closing = 0; | |
56 | hdlc->ffvalue = 0; | |
57 | if (is_d_channel) { | |
58 | hdlc->dchannel = 1; | |
59 | hdlc->state = HDLC_SEND_FIRST_FLAG; | |
60 | } else { | |
61 | hdlc->dchannel = 0; | |
62 | hdlc->state = HDLC_SEND_FAST_FLAG; | |
63 | hdlc->ffvalue = 0x7e; | |
64 | } | |
65 | hdlc->cbin = 0x7e; | |
66 | hdlc->bit_shift = 0; | |
67 | if(do_adapt56){ | |
68 | hdlc->do_adapt56 = 1; | |
69 | hdlc->data_bits = 0; | |
70 | hdlc->state = HDLC_SENDFLAG_B0; | |
71 | } else { | |
72 | hdlc->do_adapt56 = 0; | |
73 | hdlc->data_bits = 8; | |
74 | } | |
75 | hdlc->shift_reg = 0; | |
76 | } | |
77 | ||
78 | /* | |
79 | hdlc_decode - decodes HDLC frames from a transparent bit stream. | |
80 | ||
81 | The source buffer is scanned for valid HDLC frames looking for | |
82 | flags (01111110) to indicate the start of a frame. If the start of | |
83 | the frame is found, the bit stuffing is removed (0 after 5 1's). | |
84 | When a new flag is found, the complete frame has been received | |
85 | and the CRC is checked. | |
86 | If a valid frame is found, the function returns the frame length | |
87 | excluding the CRC with the bit HDLC_END_OF_FRAME set. | |
88 | If the beginning of a valid frame is found, the function returns | |
89 | the length. | |
90 | If a framing error is found (too many 1s and not a flag) the function | |
91 | returns the length with the bit HDLC_FRAMING_ERROR set. | |
92 | If a CRC error is found the function returns the length with the | |
93 | bit HDLC_CRC_ERROR set. | |
94 | If the frame length exceeds the destination buffer size, the function | |
95 | returns the length with the bit HDLC_LENGTH_ERROR set. | |
96 | ||
97 | src - source buffer | |
98 | slen - source buffer length | |
99 | count - number of bytes removed (decoded) from the source buffer | |
100 | dst _ destination buffer | |
101 | dsize - destination buffer size | |
102 | returns - number of decoded bytes in the destination buffer and status | |
103 | flag. | |
104 | */ | |
105 | int hdlc_decode(struct hdlc_vars *hdlc, const unsigned char *src, | |
106 | int slen, int *count, unsigned char *dst, int dsize) | |
107 | { | |
108 | int status=0; | |
109 | ||
110 | static const unsigned char fast_flag[]={ | |
111 | 0x00,0x00,0x00,0x20,0x30,0x38,0x3c,0x3e,0x3f | |
112 | }; | |
113 | ||
114 | static const unsigned char fast_flag_value[]={ | |
115 | 0x00,0x7e,0xfc,0xf9,0xf3,0xe7,0xcf,0x9f,0x3f | |
116 | }; | |
117 | ||
118 | static const unsigned char fast_abort[]={ | |
119 | 0x00,0x00,0x80,0xc0,0xe0,0xf0,0xf8,0xfc,0xfe,0xff | |
120 | }; | |
121 | ||
122 | *count = slen; | |
123 | ||
124 | while(slen > 0){ | |
125 | if(hdlc->bit_shift==0){ | |
126 | hdlc->cbin = *src++; | |
127 | slen--; | |
128 | hdlc->bit_shift = 8; | |
129 | if(hdlc->do_adapt56){ | |
130 | hdlc->bit_shift --; | |
131 | } | |
132 | } | |
133 | ||
134 | switch(hdlc->state){ | |
135 | case STOPPED: | |
136 | return 0; | |
137 | case HDLC_FAST_IDLE: | |
138 | if(hdlc->cbin == 0xff){ | |
139 | hdlc->bit_shift = 0; | |
140 | break; | |
141 | } | |
142 | hdlc->state = HDLC_GET_FLAG_B0; | |
143 | hdlc->hdlc_bits1 = 0; | |
144 | hdlc->bit_shift = 8; | |
145 | break; | |
146 | case HDLC_GET_FLAG_B0: | |
147 | if(!(hdlc->cbin & 0x80)) { | |
148 | hdlc->state = HDLC_GETFLAG_B1A6; | |
149 | hdlc->hdlc_bits1 = 0; | |
150 | } else { | |
151 | if(!hdlc->do_adapt56){ | |
152 | if(++hdlc->hdlc_bits1 >=8 ) if(hdlc->bit_shift==1) | |
153 | hdlc->state = HDLC_FAST_IDLE; | |
154 | } | |
155 | } | |
156 | hdlc->cbin<<=1; | |
157 | hdlc->bit_shift --; | |
158 | break; | |
159 | case HDLC_GETFLAG_B1A6: | |
160 | if(hdlc->cbin & 0x80){ | |
161 | hdlc->hdlc_bits1++; | |
162 | if(hdlc->hdlc_bits1==6){ | |
163 | hdlc->state = HDLC_GETFLAG_B7; | |
164 | } | |
165 | } else { | |
166 | hdlc->hdlc_bits1 = 0; | |
167 | } | |
168 | hdlc->cbin<<=1; | |
169 | hdlc->bit_shift --; | |
170 | break; | |
171 | case HDLC_GETFLAG_B7: | |
172 | if(hdlc->cbin & 0x80) { | |
173 | hdlc->state = HDLC_GET_FLAG_B0; | |
174 | } else { | |
175 | hdlc->state = HDLC_GET_DATA; | |
176 | hdlc->crc = 0xffff; | |
177 | hdlc->shift_reg = 0; | |
178 | hdlc->hdlc_bits1 = 0; | |
179 | hdlc->data_bits = 0; | |
180 | hdlc->data_received = 0; | |
181 | } | |
182 | hdlc->cbin<<=1; | |
183 | hdlc->bit_shift --; | |
184 | break; | |
185 | case HDLC_GET_DATA: | |
186 | if(hdlc->cbin & 0x80){ | |
187 | hdlc->hdlc_bits1++; | |
188 | switch(hdlc->hdlc_bits1){ | |
189 | case 6: | |
190 | break; | |
191 | case 7: | |
192 | if(hdlc->data_received) { | |
193 | // bad frame | |
194 | status = -HDLC_FRAMING_ERROR; | |
195 | } | |
196 | if(!hdlc->do_adapt56){ | |
197 | if(hdlc->cbin==fast_abort[hdlc->bit_shift+1]){ | |
198 | hdlc->state = HDLC_FAST_IDLE; | |
199 | hdlc->bit_shift=1; | |
200 | break; | |
201 | } | |
202 | } else { | |
203 | hdlc->state = HDLC_GET_FLAG_B0; | |
204 | } | |
205 | break; | |
206 | default: | |
207 | hdlc->shift_reg>>=1; | |
208 | hdlc->shift_reg |= 0x80; | |
209 | hdlc->data_bits++; | |
210 | break; | |
211 | } | |
212 | } else { | |
213 | switch(hdlc->hdlc_bits1){ | |
214 | case 5: | |
215 | break; | |
216 | case 6: | |
217 | if(hdlc->data_received){ | |
218 | if (hdlc->dstpos < 2) { | |
219 | status = -HDLC_FRAMING_ERROR; | |
220 | } else if (hdlc->crc != 0xf0b8){ | |
221 | // crc error | |
222 | status = -HDLC_CRC_ERROR; | |
223 | } else { | |
224 | // remove CRC | |
225 | hdlc->dstpos -= 2; | |
226 | // good frame | |
227 | status = hdlc->dstpos; | |
228 | } | |
229 | } | |
230 | hdlc->crc = 0xffff; | |
231 | hdlc->shift_reg = 0; | |
232 | hdlc->data_bits = 0; | |
233 | if(!hdlc->do_adapt56){ | |
234 | if(hdlc->cbin==fast_flag[hdlc->bit_shift]){ | |
235 | hdlc->ffvalue = fast_flag_value[hdlc->bit_shift]; | |
236 | hdlc->state = HDLC_FAST_FLAG; | |
237 | hdlc->ffbit_shift = hdlc->bit_shift; | |
238 | hdlc->bit_shift = 1; | |
239 | } else { | |
240 | hdlc->state = HDLC_GET_DATA; | |
241 | hdlc->data_received = 0; | |
242 | } | |
243 | } else { | |
244 | hdlc->state = HDLC_GET_DATA; | |
245 | hdlc->data_received = 0; | |
246 | } | |
247 | break; | |
248 | default: | |
249 | hdlc->shift_reg>>=1; | |
250 | hdlc->data_bits++; | |
251 | break; | |
252 | } | |
253 | hdlc->hdlc_bits1 = 0; | |
254 | } | |
255 | if (status) { | |
256 | hdlc->dstpos = 0; | |
257 | *count -= slen; | |
258 | hdlc->cbin <<= 1; | |
259 | hdlc->bit_shift--; | |
260 | return status; | |
261 | } | |
262 | if(hdlc->data_bits==8){ | |
263 | hdlc->data_bits = 0; | |
264 | hdlc->data_received = 1; | |
265 | hdlc->crc = crc_ccitt_byte(hdlc->crc, hdlc->shift_reg); | |
266 | ||
267 | // good byte received | |
268 | if (dsize--) { | |
269 | dst[hdlc->dstpos++] = hdlc->shift_reg; | |
270 | } else { | |
271 | // frame too long | |
272 | status = -HDLC_LENGTH_ERROR; | |
273 | hdlc->dstpos = 0; | |
274 | } | |
275 | } | |
276 | hdlc->cbin <<= 1; | |
277 | hdlc->bit_shift--; | |
278 | break; | |
279 | case HDLC_FAST_FLAG: | |
280 | if(hdlc->cbin==hdlc->ffvalue){ | |
281 | hdlc->bit_shift = 0; | |
282 | break; | |
283 | } else { | |
284 | if(hdlc->cbin == 0xff){ | |
285 | hdlc->state = HDLC_FAST_IDLE; | |
286 | hdlc->bit_shift=0; | |
287 | } else if(hdlc->ffbit_shift==8){ | |
288 | hdlc->state = HDLC_GETFLAG_B7; | |
289 | break; | |
290 | } else { | |
291 | hdlc->shift_reg = fast_abort[hdlc->ffbit_shift-1]; | |
292 | hdlc->hdlc_bits1 = hdlc->ffbit_shift-2; | |
293 | if(hdlc->hdlc_bits1<0)hdlc->hdlc_bits1 = 0; | |
294 | hdlc->data_bits = hdlc->ffbit_shift-1; | |
295 | hdlc->state = HDLC_GET_DATA; | |
296 | hdlc->data_received = 0; | |
297 | } | |
298 | } | |
299 | break; | |
300 | default: | |
301 | break; | |
302 | } | |
303 | } | |
304 | *count -= slen; | |
305 | return 0; | |
306 | } | |
307 | ||
308 | /* | |
309 | hdlc_encode - encodes HDLC frames to a transparent bit stream. | |
310 | ||
311 | The bit stream starts with a beginning flag (01111110). After | |
312 | that each byte is added to the bit stream with bit stuffing added | |
313 | (0 after 5 1's). | |
314 | When the last byte has been removed from the source buffer, the | |
315 | CRC (2 bytes is added) and the frame terminates with the ending flag. | |
316 | For the dchannel, the idle character (all 1's) is also added at the end. | |
317 | If this function is called with empty source buffer (slen=0), flags or | |
318 | idle character will be generated. | |
319 | ||
320 | src - source buffer | |
321 | slen - source buffer length | |
322 | count - number of bytes removed (encoded) from source buffer | |
323 | dst _ destination buffer | |
324 | dsize - destination buffer size | |
325 | returns - number of encoded bytes in the destination buffer | |
326 | */ | |
327 | int hdlc_encode(struct hdlc_vars *hdlc, const unsigned char *src, | |
328 | unsigned short slen, int *count, | |
329 | unsigned char *dst, int dsize) | |
330 | { | |
331 | static const unsigned char xfast_flag_value[] = { | |
332 | 0x7e,0x3f,0x9f,0xcf,0xe7,0xf3,0xf9,0xfc,0x7e | |
333 | }; | |
334 | ||
335 | int len = 0; | |
336 | ||
337 | *count = slen; | |
338 | ||
339 | while (dsize > 0) { | |
340 | if(hdlc->bit_shift==0){ | |
341 | if(slen && !hdlc->do_closing){ | |
342 | hdlc->shift_reg = *src++; | |
343 | slen--; | |
344 | if (slen == 0) | |
345 | hdlc->do_closing = 1; /* closing sequence, CRC + flag(s) */ | |
346 | hdlc->bit_shift = 8; | |
347 | } else { | |
348 | if(hdlc->state == HDLC_SEND_DATA){ | |
349 | if(hdlc->data_received){ | |
350 | hdlc->state = HDLC_SEND_CRC1; | |
351 | hdlc->crc ^= 0xffff; | |
352 | hdlc->bit_shift = 8; | |
353 | hdlc->shift_reg = hdlc->crc & 0xff; | |
354 | } else if(!hdlc->do_adapt56){ | |
355 | hdlc->state = HDLC_SEND_FAST_FLAG; | |
356 | } else { | |
357 | hdlc->state = HDLC_SENDFLAG_B0; | |
358 | } | |
359 | } | |
360 | ||
361 | } | |
362 | } | |
363 | ||
364 | switch(hdlc->state){ | |
365 | case STOPPED: | |
366 | while (dsize--) | |
367 | *dst++ = 0xff; | |
368 | ||
369 | return dsize; | |
370 | case HDLC_SEND_FAST_FLAG: | |
371 | hdlc->do_closing = 0; | |
372 | if(slen == 0){ | |
373 | *dst++ = hdlc->ffvalue; | |
374 | len++; | |
375 | dsize--; | |
376 | break; | |
377 | } | |
378 | if(hdlc->bit_shift==8){ | |
379 | hdlc->cbin = hdlc->ffvalue>>(8-hdlc->data_bits); | |
380 | hdlc->state = HDLC_SEND_DATA; | |
381 | hdlc->crc = 0xffff; | |
382 | hdlc->hdlc_bits1 = 0; | |
383 | hdlc->data_received = 1; | |
384 | } | |
385 | break; | |
386 | case HDLC_SENDFLAG_B0: | |
387 | hdlc->do_closing = 0; | |
388 | hdlc->cbin <<= 1; | |
389 | hdlc->data_bits++; | |
390 | hdlc->hdlc_bits1 = 0; | |
391 | hdlc->state = HDLC_SENDFLAG_B1A6; | |
392 | break; | |
393 | case HDLC_SENDFLAG_B1A6: | |
394 | hdlc->cbin <<= 1; | |
395 | hdlc->data_bits++; | |
396 | hdlc->cbin++; | |
397 | if(++hdlc->hdlc_bits1 == 6) | |
398 | hdlc->state = HDLC_SENDFLAG_B7; | |
399 | break; | |
400 | case HDLC_SENDFLAG_B7: | |
401 | hdlc->cbin <<= 1; | |
402 | hdlc->data_bits++; | |
403 | if(slen == 0){ | |
404 | hdlc->state = HDLC_SENDFLAG_B0; | |
405 | break; | |
406 | } | |
407 | if(hdlc->bit_shift==8){ | |
408 | hdlc->state = HDLC_SEND_DATA; | |
409 | hdlc->crc = 0xffff; | |
410 | hdlc->hdlc_bits1 = 0; | |
411 | hdlc->data_received = 1; | |
412 | } | |
413 | break; | |
414 | case HDLC_SEND_FIRST_FLAG: | |
415 | hdlc->data_received = 1; | |
416 | if(hdlc->data_bits==8){ | |
417 | hdlc->state = HDLC_SEND_DATA; | |
418 | hdlc->crc = 0xffff; | |
419 | hdlc->hdlc_bits1 = 0; | |
420 | break; | |
421 | } | |
422 | hdlc->cbin <<= 1; | |
423 | hdlc->data_bits++; | |
424 | if(hdlc->shift_reg & 0x01) | |
425 | hdlc->cbin++; | |
426 | hdlc->shift_reg >>= 1; | |
427 | hdlc->bit_shift--; | |
428 | if(hdlc->bit_shift==0){ | |
429 | hdlc->state = HDLC_SEND_DATA; | |
430 | hdlc->crc = 0xffff; | |
431 | hdlc->hdlc_bits1 = 0; | |
432 | } | |
433 | break; | |
434 | case HDLC_SEND_DATA: | |
435 | hdlc->cbin <<= 1; | |
436 | hdlc->data_bits++; | |
437 | if(hdlc->hdlc_bits1 == 5){ | |
438 | hdlc->hdlc_bits1 = 0; | |
439 | break; | |
440 | } | |
441 | if(hdlc->bit_shift==8){ | |
442 | hdlc->crc = crc_ccitt_byte(hdlc->crc, hdlc->shift_reg); | |
443 | } | |
444 | if(hdlc->shift_reg & 0x01){ | |
445 | hdlc->hdlc_bits1++; | |
446 | hdlc->cbin++; | |
447 | hdlc->shift_reg >>= 1; | |
448 | hdlc->bit_shift--; | |
449 | } else { | |
450 | hdlc->hdlc_bits1 = 0; | |
451 | hdlc->shift_reg >>= 1; | |
452 | hdlc->bit_shift--; | |
453 | } | |
454 | break; | |
455 | case HDLC_SEND_CRC1: | |
456 | hdlc->cbin <<= 1; | |
457 | hdlc->data_bits++; | |
458 | if(hdlc->hdlc_bits1 == 5){ | |
459 | hdlc->hdlc_bits1 = 0; | |
460 | break; | |
461 | } | |
462 | if(hdlc->shift_reg & 0x01){ | |
463 | hdlc->hdlc_bits1++; | |
464 | hdlc->cbin++; | |
465 | hdlc->shift_reg >>= 1; | |
466 | hdlc->bit_shift--; | |
467 | } else { | |
468 | hdlc->hdlc_bits1 = 0; | |
469 | hdlc->shift_reg >>= 1; | |
470 | hdlc->bit_shift--; | |
471 | } | |
472 | if(hdlc->bit_shift==0){ | |
473 | hdlc->shift_reg = (hdlc->crc >> 8); | |
474 | hdlc->state = HDLC_SEND_CRC2; | |
475 | hdlc->bit_shift = 8; | |
476 | } | |
477 | break; | |
478 | case HDLC_SEND_CRC2: | |
479 | hdlc->cbin <<= 1; | |
480 | hdlc->data_bits++; | |
481 | if(hdlc->hdlc_bits1 == 5){ | |
482 | hdlc->hdlc_bits1 = 0; | |
483 | break; | |
484 | } | |
485 | if(hdlc->shift_reg & 0x01){ | |
486 | hdlc->hdlc_bits1++; | |
487 | hdlc->cbin++; | |
488 | hdlc->shift_reg >>= 1; | |
489 | hdlc->bit_shift--; | |
490 | } else { | |
491 | hdlc->hdlc_bits1 = 0; | |
492 | hdlc->shift_reg >>= 1; | |
493 | hdlc->bit_shift--; | |
494 | } | |
495 | if(hdlc->bit_shift==0){ | |
496 | hdlc->shift_reg = 0x7e; | |
497 | hdlc->state = HDLC_SEND_CLOSING_FLAG; | |
498 | hdlc->bit_shift = 8; | |
499 | } | |
500 | break; | |
501 | case HDLC_SEND_CLOSING_FLAG: | |
502 | hdlc->cbin <<= 1; | |
503 | hdlc->data_bits++; | |
504 | if(hdlc->hdlc_bits1 == 5){ | |
505 | hdlc->hdlc_bits1 = 0; | |
506 | break; | |
507 | } | |
508 | if(hdlc->shift_reg & 0x01){ | |
509 | hdlc->cbin++; | |
510 | } | |
511 | hdlc->shift_reg >>= 1; | |
512 | hdlc->bit_shift--; | |
513 | if(hdlc->bit_shift==0){ | |
514 | hdlc->ffvalue = xfast_flag_value[hdlc->data_bits]; | |
515 | if(hdlc->dchannel){ | |
516 | hdlc->ffvalue = 0x7e; | |
517 | hdlc->state = HDLC_SEND_IDLE1; | |
518 | hdlc->bit_shift = 8-hdlc->data_bits; | |
519 | if(hdlc->bit_shift==0) | |
520 | hdlc->state = HDLC_SEND_FAST_IDLE; | |
521 | } else { | |
522 | if(!hdlc->do_adapt56){ | |
523 | hdlc->state = HDLC_SEND_FAST_FLAG; | |
524 | hdlc->data_received = 0; | |
525 | } else { | |
526 | hdlc->state = HDLC_SENDFLAG_B0; | |
527 | hdlc->data_received = 0; | |
528 | } | |
529 | // Finished with this frame, send flags | |
530 | if (dsize > 1) dsize = 1; | |
531 | } | |
532 | } | |
533 | break; | |
534 | case HDLC_SEND_IDLE1: | |
535 | hdlc->do_closing = 0; | |
536 | hdlc->cbin <<= 1; | |
537 | hdlc->cbin++; | |
538 | hdlc->data_bits++; | |
539 | hdlc->bit_shift--; | |
540 | if(hdlc->bit_shift==0){ | |
541 | hdlc->state = HDLC_SEND_FAST_IDLE; | |
542 | hdlc->bit_shift = 0; | |
543 | } | |
544 | break; | |
545 | case HDLC_SEND_FAST_IDLE: | |
546 | hdlc->do_closing = 0; | |
547 | hdlc->cbin = 0xff; | |
548 | hdlc->data_bits = 8; | |
549 | if(hdlc->bit_shift == 8){ | |
550 | hdlc->cbin = 0x7e; | |
551 | hdlc->state = HDLC_SEND_FIRST_FLAG; | |
552 | } else { | |
553 | *dst++ = hdlc->cbin; | |
554 | hdlc->bit_shift = hdlc->data_bits = 0; | |
555 | len++; | |
556 | dsize = 0; | |
557 | } | |
558 | break; | |
559 | default: | |
560 | break; | |
561 | } | |
562 | if(hdlc->do_adapt56){ | |
563 | if(hdlc->data_bits==7){ | |
564 | hdlc->cbin <<= 1; | |
565 | hdlc->cbin++; | |
566 | hdlc->data_bits++; | |
567 | } | |
568 | } | |
569 | if(hdlc->data_bits==8){ | |
570 | *dst++ = hdlc->cbin; | |
571 | hdlc->data_bits = 0; | |
572 | len++; | |
573 | dsize--; | |
574 | } | |
575 | } | |
576 | *count -= slen; | |
577 | ||
578 | return len; | |
579 | } | |
580 |