xtensa: add double exception fixup handler for fast_unaligned

fast_unaligned_fixup restores user registers and runs normal exception
handler in the current stack frame. Unaligned load/store is retried
after that.

Signed-off-by: Max Filippov <[email protected]>

arch/xtensa/kernel/align.S

@@ -8,6 +8,7 @@
  * this archive for more details.
  *
  * Copyright (C) 2001 - 2005 Tensilica, Inc.
+ * Copyright (C) 2014 Cadence Design Systems Inc.
  *
  * Rewritten by Chris Zankel <[email protected]>
  *
@@ -174,6 +175,10 @@ ENTRY(fast_unaligned)
 	s32i	a0, a2, PT_AREG2
 	s32i	a3, a2, PT_AREG3
 
+	rsr	a3, excsave1
+	movi	a4, fast_unaligned_fixup
+	s32i	a4, a3, EXC_TABLE_FIXUP
+
 	/* Keep value of SAR in a0 */
 
 	rsr	a0, sar
@@ -430,6 +435,10 @@ ENTRY(fast_unaligned)
 .Linvalid_instruction_store:
 .Linvalid_instruction:
 
+	movi	a4, 0
+	rsr	a3, excsave1
+	s32i	a4, a3, EXC_TABLE_FIXUP
+
 	/* Restore a4...a8 and SAR, set SP, and jump to default exception. */
 
 	l32i	a8, a2, PT_AREG8
@@ -451,4 +460,38 @@ ENTRY(fast_unaligned)
 
 ENDPROC(fast_unaligned)
 
+ENTRY(fast_unaligned_fixup)
+
+	l32i	a2, a3, EXC_TABLE_DOUBLE_SAVE
+	wsr	a3, excsave1
+
+	l32i	a8, a2, PT_AREG8
+	l32i	a7, a2, PT_AREG7
+	l32i	a6, a2, PT_AREG6
+	l32i	a5, a2, PT_AREG5
+	l32i	a4, a2, PT_AREG4
+	l32i	a0, a2, PT_AREG2
+	xsr	a0, depc			# restore depc and a0
+	wsr	a0, sar
+
+	rsr	a0, exccause
+	s32i	a0, a2, PT_DEPC			# mark as a regular exception
+
+	rsr	a0, ps
+	bbsi.l  a0, PS_UM_BIT, 1f		# jump if user mode
+
+	rsr	a0, exccause
+	addx4	a0, a0, a3              	# find entry in table
+	l32i	a0, a0, EXC_TABLE_FAST_KERNEL   # load handler
+	l32i	a3, a2, PT_AREG3
+	jx	a0
+1:
+	rsr	a0, exccause
+	addx4	a0, a0, a3              	# find entry in table
+	l32i	a0, a0, EXC_TABLE_FAST_USER     # load handler
+	l32i	a3, a2, PT_AREG3
+	jx	a0
+
+ENDPROC(fast_unaligned_fixup)
+
 #endif /* XCHAL_UNALIGNED_LOAD_EXCEPTION || XCHAL_UNALIGNED_STORE_EXCEPTION */