rawhdlc.c

内核linux2.4.20,可跟rtlinux3.2打补丁 组成实时linux系统,编译内核

		else
			out_val &= 0x7f;
		if (bitcnt==8) {
			if (d_cnt == dsize) return 0;
			dst[d_cnt++] = out_val;
			bitcnt = 0;
		}
		val >>= 1;
	}
	if (bitcnt) {
		while (8>bitcnt++) {
			out_val >>= 1;
			out_val |= 0x80;
		}
		if (d_cnt == dsize) return 0;
		dst[d_cnt++] = out_val;
	}

	return d_cnt;
}

void init_hdlc_state(struct hdlc_state *stateptr, int 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;
}

/* Optimization suggestion: A similar state machine could surely
 * be developed for this function as well.
 */

int read_raw_hdlc_data(struct hdlc_state *saved_state,
                       u_char *src, u_int slen, u_char *dst, u_int dsize)
{
	int retval=0;
	register u_char val;
	register u_char state = saved_state->state;
	register u_char r_one = saved_state->r_one;
	register u_char r_val = saved_state->r_val;
	register u_int o_bitcnt = saved_state->o_bitcnt;
	register u_int i_bitcnt = saved_state->i_bitcnt;
	register u_int fcs    = saved_state->fcs;
	register u_int *isrc = (u_int *) src;
        
	/* Use i_bitcnt (bit offset into source buffer) to reload "val"
	 * in case we're starting up again partway through a source buffer
	 */

	if ((i_bitcnt >> 3) < slen) {
		if (saved_state->insane_mode==1) {
			val = isrc[(i_bitcnt >> 3)] & 0xff;
		} else if (saved_state->insane_mode==2) {
			val = (isrc[i_bitcnt >> 3] >>8) & 0xff;
		} else {
			val = src[i_bitcnt >> 3];
		}
		val >>= i_bitcnt & 7;
	}

	/* One bit per loop.  Keep going until we've got something to
	 * report (retval != 0), or we exhaust the source buffer
	 */

	while ((retval == 0) && ((i_bitcnt >> 3) < slen)) {
		if ((i_bitcnt & 7) == 0) {
			if (saved_state->insane_mode==1) {
				val = isrc[(i_bitcnt >> 3)] & 0xff;
			} else if (saved_state->insane_mode==2) {
				val = (isrc[i_bitcnt >> 3] >>8) & 0xff;
			} else {
				val = src[i_bitcnt >> 3];
			}
#ifdef DEBUGME
			printf("Input byte %d: 0x%2x\n", i_bitcnt>>3, val);
#endif
			if (val == 0xff) {
				state = HDLC_ZERO_SEARCH;
				o_bitcnt = 0;
				r_one = 0;
				i_bitcnt += 8;
				continue;
			}
		}

#ifdef DEBUGME
		/* printf("Data bit=%d (%d/%d)\n", val&1, i_bitcnt>>3, i_bitcnt&7);*/
#endif

		if (state == HDLC_ZERO_SEARCH) {
			if (val & 1) {
				r_one++;
			} else {
				r_one=0;
				state= HDLC_FLAG_SEARCH;
			}
		} else if (state == HDLC_FLAG_SEARCH) { 
			if (val & 1) {
				r_one++;
				if (r_one>6) {
					state=HDLC_ZERO_SEARCH;
				}
			} else {
				if (r_one==6) {
					o_bitcnt=0;
					r_val=0;
					state=HDLC_FLAG_FOUND;
				}
				r_one=0;
			}
		} else if (state ==  HDLC_FLAG_FOUND) {
			if (val & 1) {
				r_one++;
				if (r_one>6) {
					state=HDLC_ZERO_SEARCH;
				} else {
					r_val >>= 1;
					r_val |= 0x80;
					o_bitcnt++;
				}
			} else {
				if (r_one==6) {
					o_bitcnt=0;
					r_val=0;
					r_one=0;
					i_bitcnt++;
					val >>= 1;
					continue;
				} else if (r_one!=5) {
					r_val >>= 1;
					r_val &= 0x7f;
					o_bitcnt++;
				}
				r_one=0;	
			}
			if ((state != HDLC_ZERO_SEARCH) &&
				!(o_bitcnt & 7)) {
#ifdef DEBUGME
				printf("HDLC_FRAME_FOUND at i_bitcnt:%d\n",i_bitcnt);
#endif
				state=HDLC_FRAME_FOUND;
				fcs = PPP_INITFCS;
				dst[0] = r_val;
				fcs = PPP_FCS (fcs, r_val);
			}
		} else if (state ==  HDLC_FRAME_FOUND) {
			if (val & 1) {
				r_one++;
				if (r_one>6) {
					state=HDLC_ZERO_SEARCH;
					o_bitcnt=0;
				} else {
					r_val >>= 1;
					r_val |= 0x80;
					o_bitcnt++;
				}
			} else {
				if (r_one==6) {
					r_val=0; 
					r_one=0;
					o_bitcnt++;
					if (o_bitcnt & 7) {
						/* Alignment error */
#ifdef DEBUGME
						printf("Alignment error\n");
#endif
						state=HDLC_FLAG_SEARCH;
						retval = -1;
					} else if (fcs==PPP_GOODFCS) {
						/* Valid frame */
						state=HDLC_FLAG_FOUND;
						retval = (o_bitcnt>>3)-3;
					} else {
						/* CRC error */
#ifdef DEBUGME
						printf("CRC error; fcs was 0x%x, should have been 0x%x\n", fcs, PPP_GOODFCS);
#endif
						state=HDLC_FLAG_FOUND;
						retval = -1;
					}
				} else if (r_one==5) {
					r_one=0;
					i_bitcnt++;
					val >>= 1;
					continue;
				} else {
					r_val >>= 1;
					r_val &= 0x7f;
					o_bitcnt++;
				}
				r_one=0;	
			}
			if ((state == HDLC_FRAME_FOUND) &&
				!(o_bitcnt & 7)) {
				if ((o_bitcnt>>3)>=dsize) {
					/* Buffer overflow error */
#ifdef DEBUGME
					printf("Buffer overflow error\n");
#endif
					r_val=0; 
					state=HDLC_FLAG_SEARCH;
					retval = -1;
				} else {
					dst[(o_bitcnt>>3)-1] = r_val;
					fcs = PPP_FCS (fcs, r_val);
#ifdef DEBUGME
					printf("Output byte %d: 0x%02x; FCS 0x%04x\n", (o_bitcnt>>3)-1, r_val, fcs);
#endif
				}
			}
		}
		i_bitcnt ++;
		val >>= 1;
	}

	/* 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.
	 */

	if (retval == 0) {
		i_bitcnt = 0;
	} else {
		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;

	return (retval);
}



#ifdef DEBUGME

char buffer[1024];
char obuffer[1024];

main()
{
  int buflen=0;
  int len;
  struct hdlc_state hdlc_state;

  while((buffer[buflen] = getc(stdin)) != EOF && buflen<1024) buflen++;

  printf("buflen = %d\n", buflen);

  init_hdlc_state(&hdlc_state, 0);

  while (len = read_raw_hdlc_data(&hdlc_state,buffer,buflen,obuffer,1024)) {
    if (len == -1) printf("Error @ byte %d/bit %d\n",
			  hdlc_state.i_bitcnt>>3, hdlc_state.i_bitcnt & 7);
    else {
      printf("Frame received: len %d\n", len);
    }
  }

  printf("Done\n");
}

#endif