C语言比特拷贝 bitcopy

协议栈最近又做起来了，碰到一个按比特拷贝的需求。
类似C语言memcpy函数，只不过memcpy是按字节拷贝的：

void *memcpy(void *to, const void *from, size_t n)

比特拷贝的函数应该是这样：

int bitcpy(void *to, unsigned int tOfs, int tCnt, const void * from, unsigned int fOfs, int fCnt)

函数说明：from地址处，从fOfs位偏移开始，连续拷贝fCnt个比特，到to地址的tOfs偏移处，且最多拷贝tCnt个比特。返回实际拷贝的比特数。

话不多讲，下面是C语言参考实现，拿走不谢：

int ubitcopy(void * to, unsigned int tOfs, int tCnt, const void * from, unsigned int fOfs, int fCnt) {
	#define OFS_LESS(type, data, offset)	((data) & ~((type)(-1) << offset))
	#define OFS_MORE(type, data, offset)	((data) &  ((type)(-1) << offset))
	HALF_WORD *_to = (HALF_WORD *)to;
	const HALF_WORD * _from = (const HALF_WORD *)from;
	int bCnt = (fCnt < tCnt) ? fCnt : tCnt;
	int BitsOfHalfWord = sizeof(HALF_WORD) * 8;
	int NbrOfHalfWord = bCnt / BitsOfHalfWord;
	int NbrOfBits = bCnt % BitsOfHalfWord;
	HALF_WORD rMask, wMask;
	HALF_WORD fprev = 0, tprev = OFS_LESS(HALF_WORD, *to, tOfs);
	WORD temp;
	
	/*
		Sample for half word copy
				|<-----WORD----->|
				01234567|01234567|012
		from:	###-----|--=----#|###
		to:		#####---|----=---|-##
		bCnt(8) fOfs(3) tOfs(5)
		rMask(11111000)
		wMask(11100000)
	*/
	// using half word data to copy bit
	while (NbrOfHalfWord--) {
		temp = ((*_from >> fOfs) << tOfs) | tprev;
		fprev = *_from++ >> (BitsOfHalfWord - tOfs + fOfs);  // cache bits shifted out
		*_to++ = temp;
		tprev = fprev | (OFS_LESS(HALF_WORD, *from, fOfs) << (tOfs - fOfs));
	}
	/*
		Sample for word copy
				|<-----WORD----->|
				01234567|01234567|012
		from:	###*****|***----#|###
		to:		#####***|*****---|-##
		bCnt(4) fOfs(3) tOfs(5)
		rMask(01111000)
		wMask(11100000)
	*/
	// using word data to copy bit
	if (NbrOfBits) {
		WORD *_dto = (WORD *) _to;
		WORD *_dfrom = (WORD *) _from;
		wMask = ~(((WORD)-1) << (tOfs + NbrOfBits));
		rMask = (((WORD)-1) << fOfs) ^ (((WORD)-1) << (fOfs + NbrOfBits));
		temp = (((*_dfrom & rMask) >> fOfs) << tOfs) | tprev;
		*_dto &= ~wMask;
		*_dto |= temp;
	}
	return bCnt;
}