Микроконтроллеры

/******************************************************************************
 * DESCRIPTION:
 * 
 * 0:0aaaaaaa 1:0bbbbbbb 2:0ccccccc 3:0ddddddd 4:0eeeeeee 5:0fffffff 6:0ggggggg  7:0hhhhhhh
 * 0:aaaaaaab 1:bbbbbbcc 2:cccccddd 3:ddddeeee 4:eeefffff 5:ffgggggg 6:ghhhhhhh
 *****************************************************************************/
u16_t Pack7bData(u8_t InBuf[],u16_t _sz) {
  
 REG_t u32_t lw1         ;
 REG_t u32_t lw2         ;
 REG_t u8_t  *src = InBuf;
 REG_t u8_t  *dst = InBuf;
       u16_t rsz  = _sz  ;
       u16_t tsz  = 0    ;

  while(1) {
	 //  0aaaaaaa 0bbbbbbb 0ccccccc 0ddddddd
	 //  aaaaaaab bbbbbbcc cccccddd ddddxxxx
	 //  ******* first 4 bytes *********
	 lw1  =  ((u32_t)*src++ << 25); 
	 lw1 |=  ((u32_t)*src++ << 18);
	 lw1 |=  ((u32_t)*src++ << 11);
	 lw1 |=  ((u32_t)*src++ << 4 );

	 //  ******* second 4 bytes *********
     //  0eeeeeee 0fffffff 0ggggggg 0hhhhhhh
	 //  eeefffff ffgggggg ghhhhhhh xxxxxxxx
	 lw1 |=  *src >> 3;
	 lw2  =  ((u32_t)*src++ << 29); 
	 lw2 |=  ((u32_t)*src++ << 22);
	 lw2 |=  ((u32_t)*src++ << 15);
	 lw2 |=  ((u32_t)*src++ << 8 );
	 
	 if (rsz < 8) {
	   break;
	 }

	 ST_U32PI(dst,lw1)         ;
	 *dst++ = (u8_t)(lw2 >> 24);
     *dst++ = (u8_t)(lw2 >> 16);
     *dst++ = (u8_t)(lw2 >> 8 );

	 rsz -= 8;
	 tsz += 7;
  }

  // shift pad in lw1 & lw2
  // --------------- lw1 --------------- --------------- lw2 ---------------
  // 0:       1:       2:       3:       4:       5:       6:        7:
  // 00000001 11111122 22222333 33334444 44455555 55666666 67777777  xxxxxxxx
  tsz += rsz;
  while (rsz) {
     *dst++ = (u8_t)(lw1 >> 24);
	 lw1  <<= 8;
	 lw1   |= (u8_t)(lw2 >> 24);
	 lw2  <<= 8;
	 rsz--;
  }
	 
  return tsz;

}