luckfox-pico-sdk/sysdrv/source/kernel/arch/arm/mach-rockchip/rkpm_helpers.c

// SPDX-License-Identifier: (GPL-2.0+ OR MIT)
/*
 * Copyright (c) 2023 Rockchip Electronics Co., Ltd.
 */

#include <asm/arch_timer.h>
#include <linux/init.h>
#include <linux/io.h>
#include <linux/kernel.h>
#include <linux/slab.h>

#include "rkpm_helpers.h"

/* REG region */
#define RGN_LEN(_rgn)		(((_rgn)->end - (_rgn)->start) / (_rgn)->stride + 1)

static u32 *region_mem;
static u32 region_mem_size;
static int region_mem_idx;

static int alloc_region_mem(u32 *buf, int max_len,
			    struct reg_region *rgns, u32 rgn_num)
{
	int i;
	int total_len = 0, len = 0;
	struct reg_region *r = rgns;

	if (!buf || !rgns) {
		pr_err("%s invalid parameter\n", __func__);
		return 0;
	}

	for (i = 0; i < rgn_num; i++, r++) {
		if (total_len < max_len)
			r->buf = &buf[total_len];

		len = RGN_LEN(r);
		total_len += len;
	}

	if (len >= max_len) {
		pr_err("%s The buffer remain length:%d is too small for region:0x%x, at least %d\n",
		       __func__, max_len, rgns[0].start, total_len);
		return -ENOMEM;
	}

	return total_len;
}

/**
 * Alloc memory to reg_region->buf from region_mem.
 * @rgns - struct reg_region array.
 * @rgn_num - struct reg_region array length.
 */
void rkpm_alloc_region_mem(struct reg_region *rgns, u32 rgn_num)
{
	int max_len = 0, len;

	max_len = region_mem_size / sizeof(u32) -
		  region_mem_idx;

	len = alloc_region_mem(region_mem + region_mem_idx, max_len,
			       rgns, rgn_num);

	region_mem_idx += len;
}

void rkpm_region_mem_init(u32 size)
{
	if (!size) {
		pr_err("%s invalid param\n", __func__);
		return;
	}

	region_mem = kmalloc(size, GFP_KERNEL);
	if (!region_mem) {
		pr_err("%s malloc region memory (0x%x) err\n", __func__, size);
		return;
	}

	region_mem_size = size;
}

/**
 * Save (reg_region->start ~ reg_region->end) to reg_region->buf.
 * @rgns - struct reg_region array.
 * @rgn_num - struct reg_region array length.
 */
void rkpm_reg_rgn_save(struct reg_region *rgns, u32 rgn_num)
{
	struct reg_region *r;
	u8 *addr;
	u8 *start, *end;
	int i, j;

	for (i = 0; i < rgn_num; i++) {
		r = &rgns[i];
		start = (char *)(*r->base) + r->start;
		end = (char *)(*r->base) + r->end;
		for (j = 0, addr = start; addr <= end; addr += r->stride, j++)
			r->buf[j] = readl_relaxed(addr);
	}
}

/**
 * Restore reg_region->buf to (reg_region->start ~ reg_region->end).
 * @rgns - struct reg_region array.
 * @rgn_num - struct reg_region array length.
 */
void rkpm_reg_rgn_restore(struct reg_region *rgns, u32 rgn_num)
{
	struct reg_region *r;
	u8 *addr;
	u8 *start, *end;
	int i, j;

	for (i = 0; i < rgn_num; i++) {
		r = &rgns[i];
		start = (char *)(*r->base) + r->start;
		end = (char *)(*r->base) + r->end;
		for (j = 0, addr = start; addr <= end; addr += r->stride, j++)
			writel_relaxed(r->buf[j] | r->wmsk, addr);
	}
}

void rkpm_reg_rgn_restore_reverse(struct reg_region *rgns, u32 rgn_num)
{
	struct reg_region *r;
	u8 *addr;
	u8 *start, *end;
	int i, j;

	for (i = rgn_num - 1; i >= 0; i--) {
		r = &rgns[i];
		start = (char *)(*r->base) + r->start;
		end = (char *)(*r->base) + r->end;
		j = RGN_LEN(r) - 1;
		for (addr = end; addr >= start; addr -= r->stride, j--)
			writel_relaxed(r->buf[j] | r->wmsk, addr);
	}
}

/**
 * Dump reg regions
 * @rgns - struct reg_region array.
 * @rgn_num - struct reg_region array length.
 */
void rkpm_dump_reg_rgns(struct reg_region *rgns, u32 rgn_num)
{
	struct reg_region *r;
	int i;

	for (i = 0; i < rgn_num; i++) {
		r = &rgns[i];
		rkpm_regs_dump(*r->base, r->start, r->end, r->stride);
	}
}

#pragma weak rkpm_printch
void rkpm_printch(int c)
{
}

void rkpm_printstr(const char *s)
{
	while (*s) {
		rkpm_printch(*s);
		s++;
	}
}

void rkpm_printhex(u32 hex)
{
	u8 i = 8;
	u8 c;

	rkpm_printch('0');
	rkpm_printch('x');
	while (i--) {
		c = (hex & 0xf0000000) >> 28;
		rkpm_printch(c < 0xa ? c + '0' : c - 0xa + 'a');
		hex <<= 4;
	}
}

void rkpm_printdec(int dec)
{
	int i, tmp = dec;

	if (dec < 0) {
		rkpm_printch('-');
		tmp = -dec;
		dec = -dec;
	}

	for (i = 1; tmp / 10; tmp /= 10, i *= 10)
		;

	for (; i >= 1; i /= 10) {
		rkpm_printch('0' + (char)(dec / i));
		dec %= i;
	}
}

void rkpm_regs_dump(void __iomem *base,
		    u32 start_offset,
		    u32 end_offset,
		    u32 stride)
{
	u32 i;

	for (i = start_offset; i <= end_offset; i += stride) {
		if ((i - start_offset) % 16 == 0) {
			rkpm_printch('\n');
			rkpm_printhex((u32)base + i);
			rkpm_printch(':');
			rkpm_printch(' ');
			rkpm_printch(' ');
			rkpm_printch(' ');
			rkpm_printch(' ');
		}
		rkpm_printhex(readl_relaxed(base + i));
		rkpm_printch(' ');
		rkpm_printch(' ');
		rkpm_printch(' ');
		rkpm_printch(' ');
	}
	rkpm_printch('\n');
}

void rkpm_raw_udelay(int us)
{
	u64 cur_cnt = __arch_counter_get_cntpct();
	u64 del = us * 24;

	while (__arch_counter_get_cntpct() - cur_cnt < del)
		;
}