📄 rawhdlc_c.htm
字号:
* A smaller state machine could operate on nibbles instead of bytes.
* A state machine for 32-bit architectures could use word offsets
* instead of byte offsets, requiring 5*32 = 160 states; probably
* best to work on nibbles in such a case.
*/</FONT>
<FONT color=#298c52>int</FONT> make_raw_hdlc_data(u_char *src, u_int slen, u_char *dst, u_int dsize)
{
<FONT color=#298c52>register</FONT> u_int i,d_cnt=0;
<FONT color=#298c52>register</FONT> u_char j;
<FONT color=#298c52>register</FONT> u_char val;
<FONT color=#298c52>register</FONT> u_char s_one = 0;
<FONT color=#298c52>register</FONT> u_char out_val = 0;
<FONT color=#298c52>register</FONT> u_char bitcnt = 0;
u_int fcs;
dst[d_cnt++] = HDLC_FLAG_VALUE;
fcs = PPP_INITFCS;
<FONT color=#298c52>for</FONT> (i=0; i<slen; i++) {
val = src[i];
fcs = PPP_FCS (fcs, val);
MAKE_RAW_BYTE;
}
fcs ^= 0xffff;
val = fcs & 0xff;
MAKE_RAW_BYTE;
val = (fcs>>8) & 0xff;
MAKE_RAW_BYTE;
val = HDLC_FLAG_VALUE;
<FONT color=#298c52>for</FONT> (j=0; j<8; j++) {
bitcnt++;
out_val >>= 1;
<FONT color=#298c52>if</FONT> (val & 1)
out_val |= 0x80;
<FONT color=#298c52>else</FONT>
out_val &= 0x7f;
<FONT color=#298c52>if</FONT> (bitcnt==8) {
<FONT color=#298c52>if</FONT> (d_cnt == dsize) <FONT color=#298c52>return</FONT> 0;
dst[d_cnt++] = out_val;
bitcnt = 0;
}
val >>= 1;
}
<FONT color=#298c52>if</FONT> (bitcnt) {
<FONT color=#298c52>while</FONT> (8>bitcnt++) {
out_val >>= 1;
out_val |= 0x80;
}
<FONT color=#298c52>if</FONT> (d_cnt == dsize) <FONT color=#298c52>return</FONT> 0;
dst[d_cnt++] = out_val;
}
<FONT color=#298c52>return</FONT> d_cnt;
}
<FONT color=#298c52>void</FONT> init_hdlc_state(<FONT color=#298c52>struct</FONT> hdlc_state *stateptr, <FONT color=#298c52>int</FONT> mode)
{
stateptr->state = HDLC_ZERO_SEARCH;
stateptr->r_one = 0;
stateptr->r_val = 0;
stateptr->o_bitcnt = 0;
stateptr->i_bitcnt = 0;
stateptr->insane_mode = mode;
}
<FONT color=#0000ff>/* Optimization suggestion: A similar state machine could surely
* be developed for this function as well.
*/</FONT>
<FONT color=#298c52>int</FONT> read_raw_hdlc_data(<FONT color=#298c52>struct</FONT> hdlc_state *saved_state,
u_char *src, u_int slen, u_char *dst, u_int dsize)
{
<FONT color=#298c52>int</FONT> retval=0;
<FONT color=#298c52>register</FONT> u_char val;
<FONT color=#298c52>register</FONT> u_char state = saved_state->state;
<FONT color=#298c52>register</FONT> u_char r_one = saved_state->r_one;
<FONT color=#298c52>register</FONT> u_char r_val = saved_state->r_val;
<FONT color=#298c52>register</FONT> u_int o_bitcnt = saved_state->o_bitcnt;
<FONT color=#298c52>register</FONT> u_int i_bitcnt = saved_state->i_bitcnt;
<FONT color=#298c52>register</FONT> u_int fcs = saved_state->fcs;
<FONT color=#298c52>register</FONT> u_int *isrc = (u_int *) src;
<FONT color=#0000ff>/* Use i_bitcnt (bit offset into source buffer) to reload "val"
* in case we're starting up again partway through a source buffer
*/</FONT>
<FONT color=#298c52>if</FONT> ((i_bitcnt >> 3) < slen) {
<FONT color=#298c52>if</FONT> (saved_state->insane_mode==1) {
val = isrc[(i_bitcnt >> 3)] & 0xff;
} <FONT color=#298c52>else</FONT> <FONT color=#298c52>if</FONT> (saved_state->insane_mode==2) {
val = (isrc[i_bitcnt >> 3] >>8) & 0xff;
} <FONT color=#298c52>else</FONT> {
val = src[i_bitcnt >> 3];
}
val >>= i_bitcnt & 7;
}
<FONT color=#0000ff>/* One bit per loop. Keep going until we've got something to
* report (retval != 0), or we exhaust the source buffer
*/</FONT>
<FONT color=#298c52>while</FONT> ((retval == 0) && ((i_bitcnt >> 3) < slen)) {
<FONT color=#298c52>if</FONT> ((i_bitcnt & 7) == 0) {
<FONT color=#298c52>if</FONT> (saved_state->insane_mode==1) {
val = isrc[(i_bitcnt >> 3)] & 0xff;
} <FONT color=#298c52>else</FONT> <FONT color=#298c52>if</FONT> (saved_state->insane_mode==2) {
val = (isrc[i_bitcnt >> 3] >>8) & 0xff;
} <FONT color=#298c52>else</FONT> {
val = src[i_bitcnt >> 3];
}
<FONT color=#a521f7>#ifdef</FONT> DEBUGME
printf(<FONT color=#ff0000>"Input byte %d: 0x%2x\n"</FONT>, i_bitcnt>>3, val);
<FONT color=#a521f7>#endif</FONT>
<FONT color=#298c52>if</FONT> (val == 0xff) {
state = HDLC_ZERO_SEARCH;
o_bitcnt = 0;
r_one = 0;
i_bitcnt += 8;
<FONT color=#298c52>continue</FONT>;
}
}
<FONT color=#a521f7>#ifdef</FONT> DEBUGME
<FONT color=#0000ff>/* printf("Data bit=%d (%d/%d)\n", val&1, i_bitcnt>>3, i_bitcnt&7);*/</FONT>
<FONT color=#a521f7>#endif</FONT>
<FONT color=#298c52>if</FONT> (state == HDLC_ZERO_SEARCH) {
<FONT color=#298c52>if</FONT> (val & 1) {
r_one++;
} <FONT color=#298c52>else</FONT> {
r_one=0;
state= HDLC_FLAG_SEARCH;
}
} <FONT color=#298c52>else</FONT> <FONT color=#298c52>if</FONT> (state == HDLC_FLAG_SEARCH) {
<FONT color=#298c52>if</FONT> (val & 1) {
r_one++;
<FONT color=#298c52>if</FONT> (r_one>6) {
state=HDLC_ZERO_SEARCH;
}
} <FONT color=#298c52>else</FONT> {
<FONT color=#298c52>if</FONT> (r_one==6) {
o_bitcnt=0;
r_val=0;
state=HDLC_FLAG_FOUND;
}
r_one=0;
}
} <FONT color=#298c52>else</FONT> <FONT color=#298c52>if</FONT> (state == HDLC_FLAG_FOUND) {
<FONT color=#298c52>if</FONT> (val & 1) {
r_one++;
<FONT color=#298c52>if</FONT> (r_one>6) {
state=HDLC_ZERO_SEARCH;
} <FONT color=#298c52>else</FONT> {
r_val >>= 1;
r_val |= 0x80;
o_bitcnt++;
}
} <FONT color=#298c52>else</FONT> {
<FONT color=#298c52>if</FONT> (r_one==6) {
o_bitcnt=0;
r_val=0;
r_one=0;
i_bitcnt++;
val >>= 1;
<FONT color=#298c52>continue</FONT>;
} <FONT color=#298c52>else</FONT> <FONT color=#298c52>if</FONT> (r_one!=5) {
r_val >>= 1;
r_val &= 0x7f;
o_bitcnt++;
}
r_one=0;
}
<FONT color=#298c52>if</FONT> ((state != HDLC_ZERO_SEARCH) &&
!(o_bitcnt & 7)) {
<FONT color=#a521f7>#ifdef</FONT> DEBUGME
printf(<FONT color=#ff0000>"HDLC_FRAME_FOUND at i_bitcnt:%d\n"</FONT>,i_bitcnt);
<FONT color=#a521f7>#endif</FONT>
state=HDLC_FRAME_FOUND;
fcs = PPP_INITFCS;
dst[0] = r_val;
fcs = PPP_FCS (fcs, r_val);
}
} <FONT color=#298c52>else</FONT> <FONT color=#298c52>if</FONT> (state == HDLC_FRAME_FOUND) {
<FONT color=#298c52>if</FONT> (val & 1) {
r_one++;
<FONT color=#298c52>if</FONT> (r_one>6) {
state=HDLC_ZERO_SEARCH;
o_bitcnt=0;
} <FONT color=#298c52>else</FONT> {
r_val >>= 1;
r_val |= 0x80;
o_bitcnt++;
}
} <FONT color=#298c52>else</FONT> {
<FONT color=#298c52>if</FONT> (r_one==6) {
r_val=0;
r_one=0;
o_bitcnt++;
<FONT color=#298c52>if</FONT> (o_bitcnt & 7) {
<FONT color=#0000ff>/* Alignment error */</FONT>
<FONT color=#a521f7>#ifdef</FONT> DEBUGME
printf(<FONT color=#ff0000>"Alignment error\n"</FONT>);
<FONT color=#a521f7>#endif</FONT>
state=HDLC_FLAG_SEARCH;
retval = -1;
} <FONT color=#298c52>else</FONT> <FONT color=#298c52>if</FONT> (fcs==PPP_GOODFCS) {
<FONT color=#0000ff>/* Valid frame */</FONT>
state=HDLC_FLAG_FOUND;
retval = (o_bitcnt>>3)-3;
} <FONT color=#298c52>else</FONT> {
<FONT color=#0000ff>/* CRC error */</FONT>
<FONT color=#a521f7>#ifdef</FONT> DEBUGME
printf(<FONT color=#ff0000>"CRC error; fcs was 0x%x, should have been 0x%x\n"</FONT>, fcs, PPP_GOODFCS);
<FONT color=#a521f7>#endif</FONT>
state=HDLC_FLAG_FOUND;
retval = -1;
}
} <FONT color=#298c52>else</FONT> <FONT color=#298c52>if</FONT> (r_one==5) {
r_one=0;
i_bitcnt++;
val >>= 1;
<FONT color=#298c52>continue</FONT>;
} <FONT color=#298c52>else</FONT> {
r_val >>= 1;
r_val &= 0x7f;
o_bitcnt++;
}
r_one=0;
}
<FONT color=#298c52>if</FONT> ((state == HDLC_FRAME_FOUND) &&
!(o_bitcnt & 7)) {
<FONT color=#298c52>if</FONT> ((o_bitcnt>>3)>=dsize) {
<FONT color=#0000ff>/* Buffer overflow error */</FONT>
<FONT color=#a521f7>#ifdef</FONT> DEBUGME
printf(<FONT color=#ff0000>"Buffer overflow error\n"</FONT>);
<FONT color=#a521f7>#endif</FONT>
r_val=0;
state=HDLC_FLAG_SEARCH;
retval = -1;
} <FONT color=#298c52>else</FONT> {
dst[(o_bitcnt>>3)-1] = r_val;
fcs = PPP_FCS (fcs, r_val);
<FONT color=#a521f7>#ifdef</FONT> DEBUGME
printf(<FONT color=#ff0000>"Output byte %d: 0x%02x; FCS 0x%04x\n"</FONT>, (o_bitcnt>>3)-1, r_val, fcs);
<FONT color=#a521f7>#endif</FONT>
}
}
}
i_bitcnt ++;
val >>= 1;
}
<FONT color=#0000ff>/* We exhausted the source buffer before anything else happened
* (retval==0). Reset i_bitcnt in expectation of a new source
* buffer. Other, we either had an error or a valid frame, so
* reset o_bitcnt in expectation of a new destination buffer.
*/</FONT>
<FONT color=#298c52>if</FONT> (retval == 0) {
i_bitcnt = 0;
} <FONT color=#298c52>else</FONT> {
o_bitcnt = 0;
}
saved_state->state = state;
saved_state->r_one = r_one;
saved_state->r_val = r_val;
saved_state->fcs = fcs;
saved_state->o_bitcnt = o_bitcnt;
saved_state->i_bitcnt = i_bitcnt;
<FONT color=#298c52>return</FONT> (retval);
}
<FONT color=#a521f7>#ifdef</FONT> DEBUGME
<FONT color=#298c52>char</FONT> buffer[1024];
<FONT color=#298c52>char</FONT> obuffer[1024];
main()
{
<FONT color=#298c52>int</FONT> buflen=0;
<FONT color=#298c52>int</FONT> len;
<FONT color=#298c52>struct</FONT> hdlc_state hdlc_state;
<FONT color=#298c52>while</FONT>((buffer[buflen] = getc(stdin)) != EOF && buflen<1024) buflen++;
printf(<FONT color=#ff0000>"buflen = %d\n"</FONT>, buflen);
init_hdlc_state(&hdlc_state, 0);
<FONT color=#298c52>while</FONT> (len = read_raw_hdlc_data(&hdlc_state,buffer,buflen,obuffer,1024)) {
<FONT color=#298c52>if</FONT> (len == -1) printf(<FONT color=#ff0000>"Error @ byte %d/bit %d\n"</FONT>,
hdlc_state.i_bitcnt>>3, hdlc_state.i_bitcnt & 7);
<FONT color=#298c52>else</FONT> {
printf(<FONT color=#ff0000>"Frame received: len %d\n"</FONT>, len);
}
}
printf(<FONT color=#ff0000>"Done\n"</FONT>);
}
<FONT color=#a521f7>#endif</FONT>
</PRE></BODY></HTML>
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -