bit-stuffing in C

bit-stuffing在HDLC中有应用，如果不使用HDLC芯片解析，则需要自己来处理。
下面是bit-stuffing的简单实现。

int bit_stuffing(
    unsigned char *psrc, /* source buffer */
    unsigned short lsrc, /* source buffer length in bytes */
    unsigned char *pdest, /* destination buffer */
    unsigned short ldest) /* destination buffer length in bytes */
{
    unsigned int bytepos = 0;
    unsigned int bitpos = 0;
    
    unsigned int i = 0, j = 0;
    unsigned int count_one = 0;
    unsigned char mask[8] = {0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01};
    
    for (i = 0; i < lsrc; i++)
    {
        for (j = 0; j < 8; j++)
        {
            if (psrc[i] & mask[j])
            {
                count_one++;
                /* copy current '1' bit */
                pdest[bytepos] |= mask[bitpos];
                if (count_one == 5)
                {
                    /* move position */
                    bitpos++;
                    if (bitpos == 8)
                    {
                        bytepos++;
                        bitpos = 0;
                    }         
                    
                    /* insert a '0' bit */
                    pdest[bytepos] &= (~mask[bitpos]);           
                    count_one = 0;
                }
            }
            else
            {
                count_one = 0;
            }
            
            /* move position */
            bitpos++;
            if (bitpos == 8)
            {
                bytepos++;
                bitpos = 0;
            }
        }
    }
    
    return(0);
}


int bit_destuffing(
    unsigned char *psrc, /* source buffer */
    unsigned short lsrc, /* source buffer length in bytes */
    unsigned char *pdest, /* destination buffer */
    unsigned short ldest) /* destination buffer length in bytes */
{
    unsigned int bytepos = 0;
    unsigned int bitpos = 0;
    
    unsigned int i = 0, j = 0;
    unsigned int count_one = 0;
    unsigned char mask[8] = {0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01};
    
    for (i = 0; i < lsrc; i++)
    {
        for (j = 0; j < 8; j++)
        {
            if (psrc[i] & mask[j])
            {
                count_one++;
                /* copy current '1' bit */
                pdest[bytepos] |= mask[bitpos];
                
                /* move position */
                bitpos++;
                if (bitpos == 8)
                {
                    bytepos++;
                    bitpos = 0;
                }

            }
            else
            {
                if (count_one == 5) /* remove the '0' bit */
                {   
                }
                else
                {
                    /* move position */
                    bitpos++;
                    if (bitpos == 8)
                    {
                        bytepos++;
                        bitpos = 0;
                    }
                }
                count_one = 0;
            }
        }
    }
    
    return(0);
}