do_sgi.S

__start:
	mov	x19, #(1 << 20)

	mrs	x0, id_aa64pfr0_el1
	ubfx	x0, x0, #24, #4
	and	x0, x0, #0xf
	cbz	x0, do_v2

	mrs	x0, s3_0_c12_c12_5	// ICC_SRE_EL1
	and	x0, x0, #1		// SRE bit
	cbnz	x0, do_v3

do_v2:
	mov	x0, #0x3fff0000		// Dist
	mov	x1, #0x3ffd0000		// CPU
	mov	w2, #1
	str	w2, [x0]		// Enable Group0
	ldr	w2, =0xa0a0a0a0
	str	w2, [x0, 0x400]		// A0 priority for SGI0-3
	mov	w2, #0x0f
	str	w2, [x0, #0x100]	// Enable SGI0-3
	mov	w2, #0xf0
	str	w2, [x1, #4]		// PMR
	mov	w2, #1
	str	w2, [x1]		// Enable CPU interface

1:
	mov	w2, #(2 << 24)		// Interrupt self with SGI0
	str	w2, [x0, #0xf00]

2:	ldr	w2, [x1, #0x0c]		// GICC_IAR
	cmp	w2, #0x3ff
	b.ne	3f

	wfi
	b	2b

3:	str	w2, [x1, #0x10]		// EOI

	sub	x19, x19, #1
	cbnz	x19, 1b

die:
	mov	x0, #0x84000000
	add	x0, x0, #8
	hvc	#0
	b	.


do_v3:
	mov	x0, #0x3fff0000		// Dist
	mov	x1, #0x3fbf0000		// Redist 0
	mov	x2, #0x10000
	add	x1, x1, x2		// SGI page
	mov	w2, #2
	str	w2, [x0]		// Enable Group1
	ldr	w2, =0xa0a0a0a0
	str	w2, [x1, 0x400]		// A0 priority for SGI0-3
	mov	w2, #0x0f
	str	w2, [x1, #0x100]	// Enable SGI0-3
	mov	w2, #0xf0
	msr	S3_0_c4_c6_0, x2	// PMR
	mov	w2, #1
	msr	S3_0_C12_C12_7, x2	// Enable Group1

1:
	mov	x2, #1
	msr	S3_0_c12_c11_5, x2	// Self SGI0

2:	mrs	x2, S3_0_c12_c12_0	// Read IAR1
	cmp	w2, #0x3ff
	b.ne	3f

	wfi
	b	2b

3:	msr	S3_0_c12_c12_1, x2	// EOI

	sub	x19, x19, #1
	cbnz	x19, 1b

	b	die