Ordenación por inserción de medios palabras uint16_t

Este es el primer código para ordenar palabras con signo completas (int32_t):
aquí está la imagen:

Intenté cambiarlo para ordenar semipalabras sin signo uint16_t. Ya casi he terminado el código, pero falta algo. El problema está en la ordenación, la arquitectura es ARM (en modo ARM, no Thumb); esto es lo que he hecho hasta ahora:

sort:   STMFD   SP!,{R4-R6,LR}
        MOV     R2,#1   // para (unsigned i = 1;
L1:     CMP     R2,R1
        BHS     L4      // i < toSort.size();
        MOV     R3,R2   // para (unsigned j = i
L2:     SUBS    R3,R3,#1// - 1; --j)
        BLO     L3      // j != -1;
        ADD     R6,R0,R3,LSL #2
        LDRH    R4,[R6]
        LDRH    R5,[R6,#2]
        CMP     R5,R4   // if (toSort[j+1] < toSort[j])
        STRHT   R4,[R6,#2]
        STRHT   R5,[R6] // swap(toSort[j], toSort[j+1]);
        BLT     L2
L3:     ADD     R2,R2,#1// else break; ++i)
     B       L1
L4:     LDMFD   SP!,{R4-R6,PC}

c++
void insertionSort(vector<int>& toSort)
{
for (int i = 1; i < toSort.size(); ++i)
for (int j = i - 1; j >= 0; --j)
if (toSort[j+1] < toSort[j])
swap(toSort[j], toSort[j+1]);
else
break;
} */ Este es el código que debería estar en ensamblador