Thumb->ARM->Thumb in inline-wrapped asm?

[dave madden]

I'm hoping to put together an inline func wrapper for an asm() fragment
that can, for the sake of argument, disable interrupts and return CPSR
to a Thumb function.  I know this sort of thing is traditionally done
with a preprocessor macro, but I thought doing it as a static inline
function defined in a header file would permit GCC to do better
optimization, and also make the return of the current PSR more
robust.  Here's what I have:

	static inline int portDISABLE_INTERRUPTS( void ) {
        int  retval, tmp;
        asm volatile ( "add %0, pc, #(1$ - . - 4)\n"
                       "bx %0\n"
                       ".arm\n"
			// There may be 2 bytes of padding here!
                       "1$: mrs %0, CPSR\n"
                       "orr %1, %0, #0xc0\n"
                       "msr CPSR_c, %1\n"
                       "add %1, pc, #(2$ - . - 8 + 1)\n"
                       "bx %1\n"
                       ".thumb\n"
                       "2$: nop\n"
                       : "=r" (retval), "=r" (tmp) );
        return retval;
    }

Unfortunately, GAS (correctly, AFAICT) decides that it can't tell
whether 1$ will be where it appears to be, or whether there will be an
extra short inserted to word-align the ARM code, so it gives an
"invalid immediate for address calculation" error.  Is there a good
way to handle this automagically?

Thanks!
dave madden