Subversion Repositories HelenOS

#include <unistd.h>

#include <align.h>

/** Fill memory block with a constant value. */

void *memset(void *dest, int b, size_t n)

{

    char *pb;

    unsigned long *pw;

    size_t word_size;

    size_t n_words;

    unsigned long pattern;

    size_t i;

    size_t fill;

    /* Fill initial segment. */

    word_size = sizeof(unsigned long);

    fill = word_size - ((uintptr_t) dest & (word_size - 1));

    if (fill > n) fill = n;

    pb = dest;

    i = fill;

    while (i-- != 0)

        *pb++ = b;

    /* Compute remaining size. */

    n -= fill;

    if (n == 0) return dest;

    n_words = n / word_size;

    n = n % word_size;

    pw = (unsigned long *) pb;

    /* Create word-sized pattern for aligned segment. */

    pattern = 0;

    i = word_size;

    while (i-- != 0)

        pattern = (pattern << 8) | (uint8_t) b;

    /* Fill aligned segment. */

    i = n_words;

    while (i-- != 0)

        *pw++ = pattern;

    pb = (char *) pw;

    /* Fill final segment. */

    i = n;

    while (i-- != 0)

        *pb++ = b;

    return dest;

}

struct along {

    unsigned long n;

} __attribute__ ((packed));

static void *unaligned_memcpy(void *dst, const void *src, size_t n)

{

    int i, j;

    struct along *adst = dst;

    const struct along *asrc = src;

    for (i = 0; i < n / sizeof(unsigned long); i++)

        adst[i].n = asrc[i].n;

    for (j = 0; j < n % sizeof(unsigned long); j++)

        ((unsigned char *) (((unsigned long *) dst) + i))[j] =

            ((unsigned char *) (((unsigned long *) src) + i))[j];

    return (char *) dst;

}

/** Copy memory block. */

void *memcpy(void *dst, const void *src, size_t n)

{

    size_t i;

    size_t mod, fill;

    size_t word_size;

    size_t n_words;

    const unsigned long *srcw;

    unsigned long *dstw;

    const uint8_t *srcb;

    uint8_t *dstb;

    word_size = sizeof(unsigned long);

    /*

     * Are source and destination addresses congruent modulo word_size?

     * If not, use unaligned_memcpy().

     */

    if (((uintptr_t) dst & (word_size - 1)) !=

        ((uintptr_t) src & (word_size - 1)))

        return unaligned_memcpy(dst, src, n);

    /*

     * mod is the address modulo word size. fill is the length of the

     * initial buffer segment before the first word boundary.

     * If the buffer is very short, use unaligned_memcpy(), too.

     */

    mod = (uintptr_t) dst & (word_size - 1);

    fill = word_size - mod;

    if (fill > n) fill = n;

    /* Copy the initial segment. */

    srcb = src;

    dstb = dst;

    i = fill;

    while (i-- != 0)

        *dstb++ = *srcb++;

    /* Compute remaining length. */

    n -= fill;

    if (n == 0) return dst;

    /* Pointers to aligned segment. */

    dstw = (unsigned long *) dstb;

    srcw = (const unsigned long *) srcb;

    n_words = n / word_size;    /* Number of whole words to copy. */

    n -= n_words * word_size;   /* Remaining bytes at the end. */

    /* "Fast" copy. */

    i = n_words;

    while (i-- != 0)

        *dstw++ = *srcw++;

    /*

     * Copy the rest.

     */

    srcb = (const uint8_t *) srcw;

    dstb = (uint8_t *) dstw;

    i = n;

    while (i-- != 0)

        *dstb++ = *srcb++;

    return dst;

}

/** Move memory block with possible overlapping. */

void *memmove(void *dst, const void *src, size_t n)

{

    uint8_t *dp, *sp;

    /* Nothing to do? */

    if (src == dst)

        return dst;

    /* Non-overlapping? */

    if (dst >= src + n || src >= dst + n) {

        return memcpy(dst, src, n);

    }

    /* Which direction? */

    if (src > dst) {

        /* Forwards. */

        sp = src;

        dp = dst;

        while (n-- != 0)

            *dp++ = *sp++;

    } else {

        /* Backwards. */

        sp = src + (n - 1);

        dp = dst + (n - 1);

        while (n-- != 0)

            *dp-- = *sp--;

    }

    return dst;

}

/** Compare two memory areas.

 *

 * @param s1        Pointer to the first area to compare.

 * @param s2        Pointer to the second area to compare.

 * @param len       Size of the first area in bytes. Both areas must have

 *          the same length.

 * @return      If len is 0, return zero. If the areas match, return

 *          zero. Otherwise return non-zero.

 */

int bcmp(const char *s1, const char *s2, size_t len)

{

    for (; len && *s1++ == *s2++; len--)

        ;

    return len;

}