BitMemCpy() - bit based memory copy, with imperfec

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <time.h>
#include <strings.h>

#if !defined(TRUE)
#define TRUE    1
#endif

#if !defined(FALSE)
#define FALSE    0
#endif

/*
 * copy the 'i'th bit of 'pSrc' to 'pDst', 'pSrc's first valid
 * bit is the 'startBit' bit
 *
 * NOTE: pDst and pSrc are treat as bit-array
 */
static inline void BitCpy(unsigned char *pDst, const unsigned char *pSrc,
                         unsigned int i, unsigned int startBit)
{
    unsigned char byte;
    int dstByte;
    int dstBit;
    int srcByte;
    int srcBit;

    /*
     * calc position
     */
    dstByte = i / 8;
    dstBit = i % 8;
    srcByte = (i + startBit) / 8;
    srcBit = (i + startBit) % 8;

    /*
     * copy bit
     */
    byte = (pSrc[srcByte] & (1 << srcBit)) & 0xFF;
    if (byte) {
        pDst[dstByte] |= byte;
    } else {
        byte = ~(1 << srcBit);
        pDst[dstByte] &= byte;
    }
}

/*
 * bit based memory copy
 *
 * @Params:
 *         dst            destination memory
 *         src            source memory
 *         byteLen        bits to be copied = byteLen * 8
 *         startBit    starting bit in the first byte, [0,7]
 * @Returns:
 *         destination memory contain the copied bits
 */
void* BitMemCpy(void * dst, const void * src,
                unsigned int byteLen, unsigned int startBit)
{
    unsigned char *pDst;
    unsigned char *pSrc;
    int i;

    /*
     * defensive precaution
     */
    if (!dst || !src || (startBit > 7)) {
        return (NULL);
    }

    pDst = (unsigned char *)dst;
    pSrc = (unsigned char *)src;

    if (dst < src) {
        for (i = 0; i < byteLen * sizeof(unsigned char) * 8; i++) {
            BitCpy(pDst, pSrc, i, startBit);
         }
    } else {
        for (i = byteLen * sizeof(unsigned char) * 8 - 1; i >= 0; i--) {
            BitCpy(pDst, pSrc, i, startBit);
        }
    }

    return (pDst);
}

/*
 * check whether copied bits is the same as the original bits
 */
static inline void AssertSame(void * dst, const void * src,
                             unsigned int byteLen, unsigned int startBit)
{
    unsigned char *pDst;
    unsigned char *pSrc;
    unsigned char dbyte;
    unsigned char sbyte;
    int i;
    int dstByte;
    int dstBit;
    int srcByte;
    int srcBit;

    pDst = (unsigned char *)dst;
    pSrc = (unsigned char *)src;

    for (i = 0; i < byteLen * sizeof(unsigned char) * 8; i++) {
        /*
         * calc position
         */
        dstByte = i / 8;
        dstBit = i % 8;
        srcByte = (i + startBit) / 8;
        srcBit = (i + startBit) % 8;

        /*
         * compare bit
         */
        dbyte = ((pDst[dstByte] & (1 << dstBit)) & 0xFF) >> dstBit;
        sbyte = ((pSrc[srcByte] & (1 << srcBit)) & 0xFF) >> srcBit;
        if (dbyte ^ sbyte) {
            fprintf(stderr, "error\n");
        } else {
            /**/
        }
    }
}

int main(void)
{
    unsigned char *p;
    int i;

    typedef struct _TestSet {
        void * dst;
        void * src;
        int     byteLen;
        int     startBit;
    } TestSet;

    TestSet testSet[10];

    /*
     * generate test case
     */
    testSet[0].dst = malloc(sizeof(char) * 2);
    testSet[0].src = malloc(sizeof(char) * 2);
    testSet[0].byteLen = 1;
    testSet[0].startBit = 4;
    p = testSet[0].src;
    p[0] = 0x0F;
    p[1] = 0xC0;

    testSet[1].dst = malloc(sizeof(char) * 2);
    testSet[1].src = malloc(sizeof(char) * 2);
    testSet[1].byteLen = 1;
    testSet[1].startBit = 0;
    p = testSet[1].src;
    p[0] = 0x0F;
    p[1] = 0xC0;

    testSet[2].dst = malloc(sizeof(char) * 2);
    testSet[2].src = malloc(sizeof(char) * 2);
    testSet[2].byteLen = 1;
    testSet[2].startBit = 7;
    p = testSet[2].src;
    p[0] = 0x0F;
    p[1] = 0xC0;

    testSet[3].dst = malloc(sizeof(char) * 2);
    testSet[3].src = malloc(sizeof(char) * 2);
    testSet[3].byteLen = 1;
    testSet[3].startBit = 8;
    p = testSet[3].src;
    p[0] = 0x0F;
    p[1] = 0xC0;

    for (i = 0; i < 4; i++) {
        if ((p = BitMemCpy(testSet[i].dst, testSet[i].src,
                         testSet[i].byteLen, testSet[i].startBit))) {
            AssertSame(testSet[i].dst, testSet[i].src,
                     testSet[i].byteLen, testSet[i].startBit);
        } else {
            printf("invalid parameters\n");
        }
    }

    printf("all test cases passed\n");

    return (0);
}