Hi, i just took a glance at your routine and i think some points need to be improved obviously.
1. When you write a C function (e.g. something to be called by callasm script command), you never need to push r0-r3, as they're assumed to be 'temporary variables' and other functions won't need...