Cortexa53 AARCH64 context switch

I have been trying to do a preemptive context switch on interrupt on the Cortexa53 but it isn't working can anyone spot an error in the code.

The code has no FPU use so it is supposed to be just a lazy save and restore registers.

The restore section seems to work because after stack setup I can do restore on a task and it runs into the code as expected, so i am guessing it is the save.

irq_handler_stub:
    /* save x24, x25, x26, x27 to stack (could be irq_stack, or svc_stack) */
    stp x24, x25, [sp, # - 16]!
    stp x26, x27, [sp, # - 16]!
    /* Fetch topofstack from current task pointer */
    ldr  x25, =pxCurrentTCB
    ldr     x25, [x25]
    ldr     x24, [x25]
    /* update pxCurrentTCB stacktop to where we will end */
    mov x26, #(18*16)
    sub x26, x26, x24
    str x26, [x25]
    /* save general registers x0-x29 to the context stack */
    stp x0, x1, [x24, # - 16]!
    stp x2, x3, [x24, # - 16]!
    stp x4, x5, [x24, # - 16]!
    stp x6, x7, [x24, # - 16]!
    stp x8, x9, [x24, # - 16]!
    stp x10, x11, [x24, # - 16]!
    stp x12, x13, [x24, # - 16]!
    stp x14, x15, [x24, # - 16]!
    stp x16, x17, [x24, # - 16]!
    stp x18, x19, [x24, # - 16]!
    stp x20, x21, [x24, # - 16]!
    stp x22, x23, [x24, # - 16]!
    /* Now move x25 to x1 and recover x24,x25,x26, x27 */
    mov x1, x24
    ldp x26, x27, [sp], #16
    ldp x24, x25, [sp], #16
    /* now store last registers */
    stp x24, x25, [x1, # - 16]!
    stp x26, x27, [x1, # - 16]!
    stp x28, x29, [x1, # - 16]!
    stp x30, xzr, [x1, # - 16]!
    /* now save the special registers */
    ldr x1, =ulCriticalNesting
    ldr x2, [x1]
    mov x3, sp
    stp x2, x3, [x0, # - 16]!
    mrs x3, SPSR_EL1
    mrs x2, ELR_EL1
    stp x2, x3, [x0, # - 16]!

    ldr x0, =RPi_IrqFuncAddr                            // Address to IrqFuncAddr
    ldr x0, [x0]                                        // Load IrqFuncAddr value
    blr x0                                                // Call Irqhandler that has been set

    /* Fetch topofstack from current task pointer */
    ldr  x1, =pxCurrentTCB
    ldr  x1, [x1]
    ldr  x0, [x1]
    /* update pxCurrentTCB stacktop to where we will end */
    mov x2, #(18*16)
    add x2, x2, x0
    str x2, [x1]
    ldr x1, =ulCriticalNesting
    /* now restore the special registers */
    ldp x2, x3, [x0], #16
    msr SPSR_EL1, x3
    msr ELR_EL1, x2
    /* now restore thread id and critical nesting count */
    ldp x2, x3, [x0], #16
    mov sp, x3
    str x2, [x1]
    /* restore general registers x2-x30 */
    ldp x30, xzr, [x0], #16
    ldp x28, x29, [x0], #16
    ldp x26, x27, [x0], #16
    ldp x24, x25, [x0], #16
    ldp x22, x23, [x0], #16
    ldp x20, x21, [x0], #16
    ldp x18, x19, [x0], #16
    ldp x16, x17, [x0], #16
    ldp x14, x15, [x0], #16
    ldp x12, x13, [x0], #16
    ldp x10, x11, [x0], #16
    ldp x8, x9, [x0], #16
    ldp x6, x7, [x0], #16
    ldp x4, x5, [x0], #16
    ldp x2, x3, [x0], #16
    ldp x0, x1, [x0]
    eret