PocketMatrix

I want to convert a C function into ARM function. But the arm function doesn't work. Please have a look and tell me what's wrong. Thank!!

C function:

#define MK_RGB565(R,G,B) ((MAX(0,MIN(255, R)) << 8) & 0xf800)
|\((MAX(0,MIN(255, G)) << 3) & 07e0)
|\((MAX(0,MIN(255, B)) >> 3) & 0x001f)

*(uint16_t *) dst = MK_RGB565(r, g, b);

ARM function:
; extern "C" void MKRGB(unsigned char* dst, int r, int g, int b);
;
mov r7, #0

mov r4, r1
mov r5, r2
mov r6, r3

cmp r4, #0
movlt r4, #0
cmp r4, #255
movgt r4, #255

cmp r5, #0
movlt r5, #0
cmp r5, #255
movgt r5, #255

cmp r6, #0
movlt r6, #0
cmp r6, #255
movgt r6, #255

mov r4, r4, LSL #8
and r7, r4, #0x0000f800

mov r5, r5, LSL #3
and r7, r5, #0x000007e0

mov r6, r6, LSR #3
and r7, r6, #0x0000001f

strh r7, [r0]