// SPDX-License-Identifier: GPL-2.0-only
#include <string.h>
#include "img_mac80211_types.h"
#include "img_utils.h"
#include "img_osal.h"

#ifdef EXTMEM_UNALIGNED_ACCESS_NOT_PERMITED
void memcpy_unaligned_max32(void *dest, const void *src, unsigned int len)
{
	unsigned int dest_addr;
	unsigned int src_addr;
	unsigned int dest_aligned_addr;
	unsigned int src_aligned_addr;

	int dest_first_part_len = 0;
	int src_first_part_len = 0;
	int dest_second_part_len = 0;
	int src_second_part_len = 0;
	int dest_skip_len = 0;
	int src_skip_len = 0;
	unsigned int dest_mask;
	unsigned int src_mask;
	unsigned int dest_aligned_val;
	unsigned int src_aligned_val;
	unsigned int src_val;
	int i;

	if (len > 4) {
		printk("ERROR: len should be lesthan 4, rcvd len %d\n", len);
	}

	if(len == 0) return;
	/* reading from src buffer */
	dest_addr = (unsigned int)dest;
	src_addr = (unsigned int)src;
	dest_aligned_addr = dest_addr & 0xFFFFFFFC;
	src_aligned_addr = src_addr & 0xFFFFFFFC;
	dest_skip_len = dest_addr & 0x00000003;
	src_skip_len = src_addr & 0x00000003;

	if (((src_addr + len -1) & 0xFFFFFFFC) <= src_addr) {
		/* whole lenght is in same aligned buffer */
		src_first_part_len = len;

	} else {
		src_first_part_len = ((src_addr + len) & 0xFFFFFFFC) - src_addr;
	}

	/* read and hold first part of the source  */
	src_aligned_val = (*(unsigned int *)src_aligned_addr);
	src_val = src_aligned_val >> (src_skip_len * 8);
	src_val = src_val & (0xFFFFFFFF >> ((4 - src_first_part_len) * 8));

	if (len > src_first_part_len) {
		/* read and hold second part of the source  */
		src_second_part_len = len - src_first_part_len;
		src_aligned_addr += 4;
		src_aligned_val = (*(unsigned int *)src_aligned_addr);
		src_aligned_val &= (0xFFFFFFFF >> ((4 - src_second_part_len) * 8));
		src_val |= (src_aligned_val << (src_first_part_len * 8));
	}


	/* writing to dest buffer*/
	dest_aligned_val = *((unsigned int *)dest_aligned_addr);

	if (((dest_addr + len -1) & 0xFFFFFFFC) <= dest_addr) {
		/* whole lenght is in same aligned buffer */
		dest_first_part_len = len;
	} else {
		dest_first_part_len = ((dest_addr + len) & 0xFFFFFFFC) - dest_addr;
	}


	/* write first part */
	dest_mask = 0xFFFFFFFF << (dest_skip_len * 8);
	dest_mask &= 0xFFFFFFFF >> ((4 - dest_skip_len - dest_first_part_len) * 8);
	dest_aligned_val &= ~dest_mask;
	dest_aligned_val |= (src_val << (dest_skip_len * 8)) & dest_mask;
	*((unsigned int *)dest_aligned_addr) = dest_aligned_val;

	if (dest_first_part_len < len) {
		/* write second part */
		src_val >>= (dest_first_part_len * 8);
		dest_aligned_addr += 4;
		dest_second_part_len = len - dest_first_part_len;
		dest_aligned_val = *((unsigned int *)dest_aligned_addr);
		dest_aligned_val &= (0xFFFFFFFF << (dest_second_part_len * 8));
		dest_aligned_val |= src_val;
		*((unsigned int *)dest_aligned_addr) = dest_aligned_val;
	}
	return;
}

void memcpy_unaligned(void *dest, const void *src, unsigned int len)
{
	int i;
	int req_len;

	if (len == 0) {
		return;
	}

	for (i = 0; i <= ((len - 1)/4); i++) {
		req_len = (i < (len/4))?(4):(len%4);
		memcpy_unaligned_max32(&(((unsigned int *)dest)[i]), &(((unsigned int *)src)[i]), req_len);
	}
	return;
}
#else /* EXTMEM_UNALIGNED_ACCESS_NOT_PERMITED  */
void memcpy_unaligned(void *dest, const void *src, unsigned int len)
{
	memcpy(dest, src, len);
}
#endif /* EXTMEM_UNALIGNED_ACCESS_NOT_PERMITED */

/* count return number of address pushed */
int img_write_lifo(struct img_lifo *img_lifo, void *elements, int *count)
{
	int num_copy_elements = *count;
	int elements_index = 0;

	DEFINE_SPIN_LOCK_IRQ(oldIPL);
	spin_lock_irqsave(&oldIPL, 0);

	/* if requested amount of space is not preset then get available space */
	if ((img_lifo->count + *count)  > img_lifo->max_elements) {
		/* no enough space */
		IMG_DBG_INCREMENT(img_lifo->lifo_over_flow, (*count - img_lifo->count));
		IMG_DBG_INCREMENT(img_lifo->unexpected_path, 1);

		num_copy_elements = img_lifo->max_elements - img_lifo->count;
	}

	memcpy(&img_lifo->lifo[img_lifo->count * img_lifo->element_size], elements + (img_lifo->element_size * (*count - num_copy_elements)), img_lifo->element_size * num_copy_elements);

	img_lifo->count += num_copy_elements;
	*count = num_copy_elements;

	spin_unlock_irqrestore(&oldIPL, 0);
	return 0;
}

/* count return number of address poped from queue */
int img_read_lifo(struct img_lifo *img_lifo, void *output_ptr, int *count)
{
	int elements_index = 0;
	int copy_len;
	int num_copy_elements = *count;

	DEFINE_SPIN_LOCK_IRQ(oldIPL);
	spin_lock_irqsave(&oldIPL, 0);

	if (img_lifo->count < *count) {
		/* no enough buffers */
		IMG_DBG_INCREMENT(img_lifo->lifo_under_flow, (*count - img_lifo->count));
		IMG_DBG_INCREMENT(img_lifo->unexpected_path, 1);

		num_copy_elements = img_lifo->count;
	}

	memcpy(output_ptr, &img_lifo->lifo[img_lifo->element_size * (img_lifo->count - num_copy_elements)], img_lifo->element_size * num_copy_elements);

	img_lifo->count -= num_copy_elements;
	*count = num_copy_elements;

	spin_unlock_irqrestore(&oldIPL, 0);
	return 0;
}