Pi Pico rotate bytearray for framebuffer in inline assembly

rotate_asm.py

SCALE3=const(13)
CTL_SIN=const(0)
CTL_COS=const(4)
CTL_WIDTH=const(8)
CTL_HEIGHT=const(12)
CTL_DEG=const(16)
rot_control = array.array('i',[0,0,MAXSCREEN_X,MAXSCREEN_Y,1]) # sin,cos,w,h,deg
#rotate_asm(source,destination,rot_control)
#example:
#rot_control[4]=degrees # 0-359
#rotate_asm(display_buffer,display_buffer2,rot_control)
@micropython.asm_thumb       #                                              0         4      8     12     16     
def rotate_asm(r0,r1,r2)->int: # r0=source bytearray, r1=dest bytearray,r2 (addr sin, addr cos,width, height, deg)    
    label(START)
    bl(CLS)                    # zero out sprite        
    ldr(r3, [r2, CTL_HEIGHT])  # r3 = height or y
    sub(r3,1)
    label(HLOOP)
    ldr(r4, [r2, CTL_WIDTH])   # r4 = width or x
    sub(r4,1)                  # dec x by 1
    label(WLOOP)
    
    ldr(r7, [r2, CTL_HEIGHT])  # r7 = max height
    mov(r5,r3)                 # r5 = r3 = y
    mul(r5,r7)                 # y*height
    add(r5,r5,r4)              # add x
    add(r5,r5,r5)              # double (2 for pixel color)

    add(r5,r5,r0)              # source addr + y(height)   
    ldrh(r6, [r5,0])           # load pixel color from source    
    cmp(r6,0)                  # compare with zero
    beq(SKIP)                  # skip if zero (black)
    push({r6})                 # save color
    
    bl(OFFSET_Y)               # jump offset_y
    sub(r5,r3,r5)              # r5 =   adjusted_y = (y - offsety)
    bl(SIN)                    # jump sin
    mul(r5,r6)                 # r5 = adjusted_y * sin
    mov(r6,SCALE3)             # r6 = 12
    asr(r5,r6)                 # r5 >> 12      
    push({r5})                 # save r5 (adjusted_y * sin)>>12
    
    bl(OFFSET_X)               # jump offset_x
    sub(r5,r4,r5)              # r5 =  adjusted_x = (x - offsetx)    
    bl(COS)                    # jump cos
    mul(r5,r6)                 # r5 = adjusted_x * cos    
    mov(r6,SCALE3)             # r6 = 1
    asr(r5,r6)                 # r5 >> 12  (good)   
    push({r5})                 # save r5 (adjusted_x * cos)>>12
    
    bl(OFFSET_X)               # jump adjusted_x     
    pop({r6,r7})               # retrieve (cos_rad * adjusted_x)>>12 , (sin_rad * adjusted_y)>>12        
    add(r5,r5,r6)              # +(cos_rad * adjusted_x)>>12
    add(r5,r5,r7)              # +(sin_rad * adjusted_y)>>12   
    bmi(POPSKIP)               # branch if negative
    ldr(r7, [r2, CTL_WIDTH])   # r4 = width or x
    sub(r7,2)                  # width-2
    cmp(r7, r5)                # is qx > width-1?
    bmi(POPSKIP)               # too big skip        
    push({r5})                 # save qx
    
    bl(OFFSET_Y)               # jump offset_y
    sub(r5,r3,r5)              # r5 =   adjusted_y = (y - offsety)
    bl(COS)                    # jump cos
    mul(r5,r6)                 # r5 = adjusted_y * cos
    mov(r6,SCALE3)             # r6 = 1
    asr(r5,r6)                 # r5 >> 12
    push({r5})                 # save r5 (adjusted_y * cos)>>12
    
    bl(OFFSET_X)               # jump offset_x
    sub(r5,r4,r5)              # r5 =  adjusted_x = (x - offsetx)
    bl(SIN)                    # jump sin
    mul(r5,r6)                 # r5 = adjusted_x * sin
    mov(r6,SCALE3)             # r6 = 1
    asr(r5,r6)                 # r5 >> 12
    push({r5})                 # save r5 (adjusted_x * sin)>>12

    bl(OFFSET_Y)               # jump offset_y
    pop({r6,r7})               # retrieve (cos_rad * adjusted_y)>>12 , (sin_rad * adjusted_x)>>12
    sub(r5,r5,r6)              # -(adjusted_x * sin)>>12
    add(r5,r5,r7)              # +(adjusted_y * cos)>>12
    bmi(POPSKIP2)              # branch if negative
    ldr(r7, [r2, CTL_HEIGHT])  # r7 = max height
    sub(r7,1)                  # height-1
    cmp(r7, r5)                # is qy > height-1?
    bmi(POPSKIP2)              # too big skip
        
    ldr(r7, [r2, CTL_HEIGHT])  # r7 = max height
    mul(r5,r7)                 # qy*height
    pop({r6})                  # get qx
    add(r5,r5,r6)              # qy*height + qx
    add(r5,r5,r5)              # double
    add(r5,r5,r1)              # dest addr + qy(height)    
    
    pop({r6})                  # get color    
    strh(r6, [r5,0])           # store pixel color to dest
    strh(r6, [r5,2])           # store pixel color to dest 
     
    label(SKIP)
    sub(r4,1)                  # dec x by 1
    bgt(WLOOP)
    sub(r3,1)                  # dec y by 1
    bgt(HLOOP)
    
    b(EXIT)                    # exit
    
    # ----------------------------------------------------------------------- subroutines   
    label(POPSKIP2)           # pop then skip
    pop({r7})
    label(POPSKIP)            # pop then skip
    pop({r7})
    b(SKIP)
    
    label(POPEXIT)            # pop then exit
    pop({r7})
    b(EXIT)
    
    label(OFFSET_X)           # uses r5,r6 and returns offset_x in r5
    ldr(r5, [r2, CTL_WIDTH])  # r5 = width 
    mov(r6, 1)                # r6 = 1
    asr(r5,r6)                # r5 >> 1  (offsetx)
    bx(lr)
    
    label(OFFSET_Y)           # uses r5,r6 and returns offset_y in r5
    ldr(r5, [r2, CTL_HEIGHT]) # r5 = height 
    mov(r6, 1)                # r6 = 1
    asr(r5,r6)                # r5 >> 1  (offsety)
    bx(lr)
   
    label(SIN)              # uses r6,r7 and returns sin in r6 
    ldr(r6, [r2, CTL_SIN])  # r6 = sin addr
    ldr(r7, [r2, CTL_DEG])  # r7 = degrees
    add(r7,r7,r7)
    add(r7,r7,r7)           # x4 for word aligned
    add(r6,r6,r7)           # sin addr + deg offset 
    ldr(r6, [r6,  0])       # r3 = sin(degrees)
    bx(lr)
    
    label(COS)              # uses r6,r7 and returns cos in r6 
    ldr(r6, [r2, CTL_COS])  # r5 = cos addr
    ldr(r7, [r2, CTL_DEG])  # r7 = degrees
    add(r7,r7,r7)
    add(r7,r7,r7)           # x4 for word aligned
    add(r6,r6,r7)           # cos addr + deg offset 
    ldr(r6, [r6,  0])       # r6 = cos(degrees)
    bx(lr)

    label(CLS)        # clear the screen, uses r3,r4,r5
    ldr(r3, [r2, CTL_HEIGHT])  # r3 = height or y
    mul(r3,r3)        # r3 = number of words to clear    
    mov(r4,r1)        # r4 = address
    mov(r5,0)         # r5 = data to load
    label(CLS_LOOP)
    strh(r5, [r4, 0]) # store data in address
    add(r4, 2)        # add 2 to address (next word)
    sub(r3, 1)        # dec number of words
    bgt(CLS_LOOP)         # branch if not done
    bx(lr)

    label(EXIT) 

def init_isin():            # integer sin lookup table    
    for i in range(0,361):
        isin[i]=int(sin(radians(i))*SCALE) # 2<<12
    rot_control[0]=addressof(isin)  

def init_icos():            # integer cos lookup table 
    for i in range(0,361):
        icos[i]=int(cos(radians(i))*SCALE)
    rot_control[1]=addressof(icos)

init_isin()
init_icos()