#include "lip_misc_data_copy.h"
#include <string.h>

void lip_data_copy_memcpy(void *dest, const void *src, size_t size, void *ctx) {
    memcpy(dest, src, size);
}

void lip_data_copy_dest_rw16(void *dest, const void *src, size_t size, void *ctx) {
    intptr_t dst_addr = (intptr_t)dest;
    volatile uint16_t *dst_words = (uint16_t *)(dst_addr & ~1);
    const uint8_t *src_bytes = (const uint8_t *)src;
    /* Если адрес сохранения не выровнен на 2, то для сохранения первого байта
     * 16-битным словом сперва считываем слово из памяти назначения, а затем
     * подменяем в нем один байт и сохраняем обратно */
    if (dst_addr & 1) {
        uint16_t first_wrd = *dst_words;
        const uint8_t b1 = *src_bytes++;
        first_wrd = LIP_WORD16_MAKE(LIP_WORD16_B0(first_wrd),  b1);
        *dst_words++ = first_wrd;
        size--;
    }
    /* Далее пишем 16-битными словами. Если текущий адрес исходных
     * данных выравнен на 16-битное слово, то можем копировать слова напрямую,
     * иначе копируем из источника по байтам, а сохраняем уже 16-битными словами
     * после объединения, т.к. в общем случае невыравненное обращение к массиву
     * 16-битных слов имеет право не работать */

    if (((intptr_t)src_bytes & 1) == 0) {
        const uint16_t *src_wrds = (const uint16_t *)src_bytes;
        for (uint32_t i = 0; i < size/2; ++i) {
            *dst_words++ = *src_wrds++;
        }
        src_bytes = (const uint8_t *)src_wrds;
    } else {
        for (uint32_t i = 0; i < size/2; ++i) {
            const uint8_t b0 = *src_bytes++;
            const uint8_t b1 = *src_bytes++;
            *dst_words++ = LIP_WORD16_MAKE(b0, b1);
        }
    }
    /* Если остался последний байт не входящий в 16-битное слово, сохраняем его
     * аналогичным способом, как первый невыровненный байт */
    if (size & 1) {
        uint16_t last_wrd = *dst_words;
        const uint8_t b0 = *src_bytes;
        last_wrd = LIP_WORD16_MAKE(b0, LIP_WORD16_B1(last_wrd));
        *dst_words = last_wrd;
    }
}

void lip_data_copy_dest_rw32(void *dest, const void *src, size_t size, void *ctx) {
    intptr_t dst_addr = (intptr_t)dest;
    const uint8_t *src_bytes = (const uint8_t *)src;
    volatile uint32_t *dst_words = (uint32_t *)(dst_addr & ~3);
    /* Если адрес сохранения не выровнен на 4, то для сохранения первых 1-3 байт
     * 32-битным словом сперва считываем слово из памяти назначения, а затем
     * подменяем в нем нужное число байт и сохраняем обратно */
    if ((dst_addr & 0x3) != 0) {
        uint32_t first_wrd = *dst_words;
        if ((dst_addr & 0x3) == 3) {
            const uint8_t b3 = *src_bytes++;
            first_wrd = LIP_WORD32_MAKE(LIP_WORD32_B0(first_wrd),
                                        LIP_WORD32_B1(first_wrd),
                                        LIP_WORD32_B2(first_wrd),
                                        b3);
            size--;
        } else if ((dst_addr & 0x3) == 2) {
            const uint8_t b2 = *src_bytes++;
            const uint8_t b3 = *src_bytes++;
            first_wrd = LIP_WORD32_MAKE(LIP_WORD32_B0(first_wrd),
                                        LIP_WORD32_B1(first_wrd),
                                        b2,
                                        b3);
            size -= 2;
        } else if ((dst_addr & 0x3) == 1) {
            const uint8_t b1 = *src_bytes++;
            const uint8_t b2 = *src_bytes++;
            const uint8_t b3 = *src_bytes++;
            first_wrd = LIP_WORD32_MAKE(LIP_WORD32_B0(first_wrd),
                                        b1,
                                        b2,
                                        b3);
            size -= 3;
        }
        *dst_words++ = first_wrd;
    }

    /* Далее пишем 32-битными словами. Если текущий адрес исходных
     * данных выравнен на 32-битное слово, то можем копировать слова напрямую,
     * иначе копируем из источника по байтам, а сохраняем уже 32-битными словами
     * после объединения, т.к. в общем случае невыравненное обращение к массиву
     * 32-битных слов имеет право не работать
     * (даже если память поддерживает невыровненное обращение компилятор может
     * использовать инструкции, которые требуют выравненных данных) */
    const intptr_t src_addr = (intptr_t)src_bytes;
    if ((src_addr & 0x3) == 0) {
        const uint32_t *src_wrds = (const uint32_t *)src_bytes;
        for (uint32_t i = 0; i < size/sizeof(uint32_t); ++i) {
            *dst_words++ = *src_wrds++;
        }
        src_bytes = (const uint8_t *)src_wrds;
    } else {

        for (uint32_t i = 0; i < size/sizeof(uint32_t); ++i) {
            const uint8_t b0 = *src_bytes++;
            const uint8_t b1 = *src_bytes++;
            const uint8_t b2 = *src_bytes++;
            const uint8_t b3 = *src_bytes++;
            *dst_words++ = LIP_WORD32_MAKE(b0, b1, b2, b3);
        }
    }

    /* Если осталось байты, не входящие в 32-битное слово, сохраняем их
     * аналогичным способом, как начальные невыравненные байты */
    uint8_t rem_size = size & 3;
    if (rem_size != 0) {

        uint32_t last_wrd = *dst_words;
        if (rem_size == 3) {
            const uint8_t b0 = *src_bytes++;
            const uint8_t b1 = *src_bytes++;
            const uint8_t b2 = *src_bytes;
            last_wrd = LIP_WORD32_MAKE(b0, b1, b2,
                                       LIP_WORD32_B3(last_wrd));
        } else if (rem_size == 2) {
            const uint8_t b0 = *src_bytes++;
            const uint8_t b1 = *src_bytes;
            last_wrd = LIP_WORD32_MAKE(b0, b1,
                                     LIP_WORD32_B2(last_wrd),
                                     LIP_WORD32_B3(last_wrd)
                                     );
        } else if (rem_size == 1) {
            const uint8_t b0 = *src_bytes;
            last_wrd = LIP_WORD32_MAKE(b0,
                                     LIP_WORD32_B1(last_wrd),
                                     LIP_WORD32_B2(last_wrd),
                                     LIP_WORD32_B3(last_wrd)
                                     );
        }
        *dst_words = last_wrd;
    }
}
