Help with GBA ARM assembly program

I tried to modify this ARM assembly program (see bottom of the page) to use subroutines. It should display a red screen on the GBA (and compiling the example program it actually does, so it's not a toolchain use problem), but, when I run it, the screen goes black instead. I'm using devkitARM 30

.arm
.text
.global main
main:
    mov r0, #0x4000000
    mov r1, #0x400
    add r1, r1, #3
    str r1, [r0]
    mov r0, #0x6000000
    stmdb sp!, {r0, r2, lr} @push registers before calling
    mov r0, #0xff
    mov r1, #0
    mov r2, #0
    bl set_rgb
    mov r1, r0
    ldmdb sp!, {r0, r2, lr} @restore registers
    mov r2, #0x9600
loop1:
    strh r1, [r0], #2
    subs r2, r2, #1
    bne loop1
infin:
    b infin

set_rgb:
    @r0: R; r1: G; r2: B; r0: RGB

    @truncate everything but the least significant five bits
    and r0, r0, #0x1f
    and r1, r1, #0x1f
    and r2, r2, #0x1f
    @form the color
    orr r0, r0, r1, lsl #5
    orr r0, r0, r2, lsl #10
    mov pc, lr @return

The question is, what's wrong in this program?

I solved it on my own.

The problem was the way I used the stack. Instead of stmdb and ldmdb I needed to use stmfd and ldmfd.