[deliverable/linux.git] / arch / arm / mach-mvebu / coherency_ll.S

/*
 * Coherency fabric: low level functions
 *
 * Copyright (C) 2012 Marvell
 *
 * Gregory CLEMENT <gregory.clement@free-electrons.com>
 *
 * This file is licensed under the terms of the GNU General Public
 * License version 2.  This program is licensed "as is" without any
 * warranty of any kind, whether express or implied.
 *
 * This file implements the assembly function to add a CPU to the
 * coherency fabric. This function is called by each of the secondary
 * CPUs during their early boot in an SMP kernel, this why this
 * function have to callable from assembly. It can also be called by a
 * primary CPU from C code during its boot.
 */

#include <linux/linkage.h>
#define ARMADA_XP_CFB_CTL_REG_OFFSET 0x0
#define ARMADA_XP_CFB_CFG_REG_OFFSET 0x4

#include <asm/assembler.h>
#include <asm/cp15.h>

	.text

ENTRY(ll_set_cpu_coherent)
	mrc	p15, 0, r1, c1, c0, 0
	tst	r1, #CR_M @ Check MMU bit enabled
	bne	1f

	/* use physical address of the coherency register*/
	adr	r0, 3f
	ldr	r3, [r0]
	ldr	r0, [r0, r3]
	b	2f
1:
	/* use virtual address of the coherency register*/
	ldr	r0, =coherency_base
	ldr	r0, [r0]
2:
	/* Create bit by cpu index */
	mrc	15, 0, r1, cr0, cr0, 5
	and	r1, r1, #15
	mov	r2, #(1 << 24)
	lsl	r1, r2, r1
ARM_BE8(rev	r1, r1)

	/* Add CPU to SMP group - Atomic */
	add	r3, r0, #ARMADA_XP_CFB_CTL_REG_OFFSET
1:
	ldrex	r2, [r3]
	orr	r2, r2, r1
	strex 	r0, r2, [r3]
	cmp	r0, #0
	bne 1b

	/* Enable coherency on CPU - Atomic */
	add	r3, r3, #ARMADA_XP_CFB_CFG_REG_OFFSET
1:
	ldrex	r2, [r3]
	orr	r2, r2, r1
	strex	r0, r2, [r3]
	cmp	r0, #0
	bne 1b

	dsb

	mov	r0, #0
	mov	pc, lr
ENDPROC(ll_set_cpu_coherent)

	.align 2
3:
	.long	coherency_phys_base - .
Commit	Line	Data
009f1315 GC	1	/*
	2	* Coherency fabric: low level functions
	3	*
	4	* Copyright (C) 2012 Marvell
	5	*
	6	* Gregory CLEMENT <gregory.clement@free-electrons.com>
	7	*
	8	* This file is licensed under the terms of the GNU General Public
	9	* License version 2. This program is licensed "as is" without any
	10	* warranty of any kind, whether express or implied.
	11	*
	12	* This file implements the assembly function to add a CPU to the
	13	* coherency fabric. This function is called by each of the secondary
	14	* CPUs during their early boot in an SMP kernel, this why this
	15	* function have to callable from assembly. It can also be called by a
	16	* primary CPU from C code during its boot.
	17	*/
	18
	19	#include <linux/linkage.h>
	20	#define ARMADA_XP_CFB_CTL_REG_OFFSET 0x0
	21	#define ARMADA_XP_CFB_CFG_REG_OFFSET 0x4
	22
bca028e7	23	#include <asm/assembler.h>
ccd6a131	24	#include <asm/cp15.h>
bca028e7	25
009f1315	26	.text
b41375f7	27
009f1315	28	ENTRY(ll_set_cpu_coherent)
ccd6a131 GC	29	mrc p15, 0, r1, c1, c0, 0
	30	tst r1, #CR_M @ Check MMU bit enabled
	31	bne 1f
	32
	33	/* use physical address of the coherency register*/
	34	adr r0, 3f
	35	ldr r3, [r0]
	36	ldr r0, [r0, r3]
	37	b 2f
	38	1:
	39	/* use virtual address of the coherency register*/
	40	ldr r0, =coherency_base
	41	ldr r0, [r0]
	42	2:
009f1315	43	/* Create bit by cpu index */
b41375f7 GC	44	mrc 15, 0, r1, cr0, cr0, 5
	45	and r1, r1, #15
	46	mov r2, #(1 << 24)
	47	lsl r1, r2, r1
bca028e7	48	ARM_BE8(rev r1, r1)
009f1315 GC	49
	50	/* Add CPU to SMP group - Atomic */
	51	add r3, r0, #ARMADA_XP_CFB_CTL_REG_OFFSET
b60b61d4 NH	52	1:
b60b61d4 NH	53	ldrex r2, [r3]
009f1315	54	orr r2, r2, r1
b60b61d4 NH	55	strex r0, r2, [r3]
	56	cmp r0, #0
	57	bne 1b
009f1315 GC	58
009f1315 GC	59	/* Enable coherency on CPU - Atomic */
b60b61d4 NH	60	add r3, r3, #ARMADA_XP_CFB_CFG_REG_OFFSET
	61	1:
	62	ldrex r2, [r3]
009f1315	63	orr r2, r2, r1
b60b61d4 NH	64	strex r0, r2, [r3]
	65	cmp r0, #0
	66	bne 1b
009f1315 GC	67
	68	dsb
	69
	70	mov r0, #0
	71	mov pc, lr
	72	ENDPROC(ll_set_cpu_coherent)
ccd6a131 GC	73
	74	.align 2
	75	3:
	76	.long coherency_phys_base - .