/*
<:copyright-BRCM:2019:DUAL/GPL:standard 

   Copyright (c) 2019 Broadcom 
   All Rights Reserved

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License, version 2, as published by
the Free Software Foundation (the "GPL").

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.


A copy of the GPL is available at http://www.broadcom.com/licenses/GPLv2.php, or by
writing to the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
Boston, MA 02111-1307, USA.

:>
*/

#include <asm_macros.S>
#include <platform_def.h>
#include <cpu_macros.S>

	.globl	a9_mmu_set_ttbr
	.globl	a9_mmu_invalidate_tlb
	.globl	a9_mmu_set_scn
	.globl	a9_mmu_enable
	.globl	a9_l1cache_enable_d
	.globl	a9_l1cache_enable_i
	.globl	a9_gic_secure_init
	.globl	a9_l1cache_inval_d

func a9_mmu_set_ttbr
	push	{r0-r12,r14}
	// Set the access control to client so AP is checked with tlb entry	
	ldr	r1, =0x55555555
	mcr	p15, 0, r1, c3, c0, 0 // Write Domain Access Control Register

	// Set Translation Table base address. r0 must be preserved
	mcr	p15, 0, r0, c2, c0, 0 // Write Translation Table Base Register 0
	isb
	dsb
	pop	{r0-r12,r14}
	mov	pc,lr
endfunc a9_mmu_set_ttbr

func a9_mmu_invalidate_tlb
	push	{r0-r12,r14}
	mov	r0,#0
	mcr	p15,0,r0,c8,c7,0		/* Invalidate TLB */
	isb
	dsb
	pop	{r0-r12,r14}
	mov	pc,lr
endfunc a9_mmu_invalidate_tlb
	


/*
	map_set_scn - sets number (SCN_RNG) of L1 table entries. Operates on 1 MB sections
	r0 - L1 table paddr
	r1 - virtual address start
	r2 - address range in Megabytes 
	r3 - Sections' attributes
	r4 - mapped physical start address
	Table max size 16K (4K*4) 
*/

#define	WORD_SIZE	4  /* bytes */
#define	WORD_BITLEN	32 /*bitlen or shift*/
#define	WORD_ALGN	2  /* word alignment shift*/
#define	SCN_BITLEN	20 /*bitlen or shift */

#define L1TBL_PA	r0 /* */
#define SCN_VA		r1
#define SCN_RNG		r2
#define SCN_ATTR	r3
#define SCN_PA		r4
#define SCN_ENTRY	r5
#define L1TBL_LEN	r6
/* NN (not nested)  - meaning there are no calls to subroutine. LR is not preserved 
	registers r0-r6 are not preserved and must not be relied upon
*/
func a9_mmu_set_scn
	push	{r0-r12,r14}
	mov	SCN_ENTRY,#0
	/* adjust virt/phys addresses for the loop increment */
	lsr	SCN_VA, SCN_VA, #(SCN_BITLEN)
	lsr	SCN_PA, SCN_PA, #(SCN_BITLEN)
	/* clear 12 msb of the section attribute */
	bfc	SCN_ATTR, #SCN_BITLEN, #(WORD_BITLEN-SCN_BITLEN)
	/* set max  range not exceeding 0xfff */
	add	SCN_RNG, SCN_VA, SCN_RNG
	cmp	SCN_RNG, L1TBL_LEN 
	blt	1f
	mov	SCN_RNG, L1TBL_LEN
1:
	orr	SCN_ENTRY, SCN_ATTR, SCN_PA, lsl #(SCN_BITLEN)
	str	SCN_ENTRY, [L1TBL_PA, SCN_VA, lsl #(WORD_ALGN)]
	add	SCN_PA, SCN_PA, #1
	add	SCN_VA, SCN_VA, #1
	cmp	SCN_VA, SCN_RNG
	bne	1b
	pop	{r0-r12,r14}
	mov	pc,lr
endfunc a9_mmu_set_scn

/*
 * CR1 bits (CP#15 CR1)
 */
#define CR_M	(1 << 0)	/* MMU enable				*/
#define CR_C	(1 << 2)	/* Dcache enable			*/
#define CR_Z	(1 << 11)	/* Program Flow Prediction		*/
#define CR_I	(1 << 12)	/* Icache enable			*/


func a9_mmu_enable
	push	{r0-r12,r14}
	mrc	p15, 0, r0, c1, c0, 0     // Read CP15 System Control register
	orr	r0, r0, #CR_M             // Set M bit 0 to enable MMU
	mcr	p15, 0, r0, c1, c0, 0     // Write CP15 System Control register
	isb
	pop	{r0-r12,r14}
	mov	pc, lr
endfunc a9_mmu_enable


func a9_l1cache_enable_i
	push	{r0-r12,r14}
	mrc	p15, 0, r0, c1, c0, 0 // Read Control Register configuration data
	orr	r0, r0, #CR_I         // Enable I Cache
	orr	r0, r0, #CR_Z         // Enable Prediction
	mcr	p15, 0, r0, c1, c0, 0 // Write Control Register configuration data
	isb
	pop	{r0-r12,r14}
	mov	pc, lr
endfunc a9_l1cache_enable_i


func a9_l1cache_enable_d
	push	{r0-r12,r14}
	mrc	p15, 0, r0, c1, c0, 0 // Read Control Register configuration data
	orr	r0, r0, #CR_C         // Enable D Cache
	mcr	p15, 0, r0, c1, c0, 0 // Write Control Register configuration data
	isb
	pop	{r0-r12,r14}
	mov	pc, lr
endfunc a9_l1cache_enable_d

func a9_gic_secure_init
	push	{r0-r12,r14}
	/* setup the GIC for non secure world. group 0 secure state, group 1 non secure state */
	/* enable interrupt for both groups */
	ldr	r0, =GICD_BASE
	ldr	r1, =0x3
	str	r1, [r0, #GICD_CTLR_OFFSET]
	
	/* assign all the interrupt to group 1 */
	ldr	r2, [r0, #GICD_TYPER_OFFSET]
	and	r2, r2, #0x1f
	add	r2, r2, #1
	ldr	r1, =0xffffffff
	add	r0, r0, #GICD_IGROUPR0_OFFSET	
setgroup:
	str	r1, [r0]
	add	r0, #4
	subs	r2, r2, #1
	bne	setgroup

	/* enable both group interrupt in the cpu interface */
	ldr	r0, =GICC_BASE
	ldr	r1, [r0, #GICC_CTLR_OFFSET]
	orr	r1, #0x3
	str	r1, [r0]

	/* set cpu priority mask view for nonsecure */
	mov	r1, #0x80
	str	r1, [r0, #GICC_PMR_OFFSET]
	pop	{r0-r12,r14}
	mov	pc, lr
endfunc a9_gic_secure_init


func a9_l1cache_inval_d
	push	{r0-r12,r14}
	mrc	p15, 1, r0, c0, c0, 1		@ read clidr
	ands	r3, r0, #0x7000000		@ extract loc from clidr
	mov	r3, r3, lsr #23			@ left align loc bit field
	beq	invfinished			@ if loc is 0, then no need to clean
	mov	r10, #0				@ start clean at cache level 0
invloop1:
	add	r2, r10, r10, lsr #1		@ work out 3x current cache level
	mov	r1, r0, lsr r2			@ extract cache type bits from clidr
	and	r1, r1, #7			@ mask of the bits for current cache only
	cmp	r1, #2				@ see what cache we have at this level
	blt	invskip				@ skip if no cache, or just i-cache
	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
	isb
						@ but we compile with armv5
	mrc	p15, 1, r1, c0, c0, 0		@ read the new csidr
	and	r2, r1, #7			@ extract the length of the cache lines
	add	r2, r2, #4			@ add 4 (line length offset)
	ldr	r4, =0x3ff
	ands	r4, r4, r1, lsr #3		@ find maximum number on the way size
	clz	r5, r4				@ find bit position of way size increment
	ldr	r7, =0x7fff
	ands	r7, r7, r1, lsr #13		@ extract max number of the index size
invloop2:
	mov	r9, r4				@ create working copy of max way size
invloop3:
	orr	r6, r10, r9, lsl r5		@ factor way and cache number into r6
	orr	r6, r6, r7, lsl r2		@ factor index number into r6
	mcr	p15, 0, r6, c7, c6, 2		@ invalidate by set/way
	subs	r9, r9, #1			@ decrement the way
	bge	invloop3
	subs	r7, r7, #1			@ decrement the index
	bge	invloop2
invskip:
	add	r10, r10, #2			@ increment cache number
	cmp	r3, r10
	bgt	invloop1
invfinished:
	mov	r10, #0				@ swith back to cache level 0
	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
	isb
	pop	{r0-r12,r14}
	mov	pc, lr
endfunc a9_l1cache_inval_d