Print number to bitmap bytearray in assembly

print_num_asm.py

from LCD_3inch5 import LCD_3inch5
from machine import Pin, SPI, PWM, WDT
import framebuf
from time import sleep_ms, sleep_us, ticks_diff, ticks_us, sleep
from micropython import const
import array
from usys import exit
import gc
from math import sin,cos,pi,radians,sqrt,tan


MAXSCREEN_X = const(240)
MAXSCREEN_Y = const(200)

char_map=array.array('b',(
     0x3E, 0x63, 0x73, 0x7B, 0x6F, 0x67, 0x3E, 0x00,   # U+0030 (0)
     0x0C, 0x0E, 0x0C, 0x0C, 0x0C, 0x0C, 0x3F, 0x00,   # U+0031 (1)
     0x1E, 0x33, 0x30, 0x1C, 0x06, 0x33, 0x3F, 0x00,   # U+0032 (2)
     0x1E, 0x33, 0x30, 0x1C, 0x30, 0x33, 0x1E, 0x00,   # U+0033 (3)
     0x38, 0x3C, 0x36, 0x33, 0x7F, 0x30, 0x78, 0x00,   # U+0034 (4)
     0x3F, 0x03, 0x1F, 0x30, 0x30, 0x33, 0x1E, 0x00,   # U+0035 (5)
     0x1C, 0x06, 0x03, 0x1F, 0x33, 0x33, 0x1E, 0x00,   # U+0036 (6)
     0x3F, 0x33, 0x30, 0x18, 0x0C, 0x0C, 0x0C, 0x00,   # U+0037 (7)
     0x1E, 0x33, 0x33, 0x1E, 0x33, 0x33, 0x1E, 0x00,   # U+0038 (8)
     0x1E, 0x33, 0x33, 0x3E, 0x30, 0x18, 0x0E, 0x00))  # U+0039 (9)



ROW_ASM=const(0)
COL_ASM=const(2)
OFFSET_ASM=const(4)
COLOR_ASM=const(6)

@micropython.asm_thumb
def printnum_asm(r0,r1,r2,r3): # screen addr, char map addr, num ,x_offset         
    mov(r4,pc)             # r4=addr data
    b(SKIP2)
    data(2,0,2,4,0xffff)   #row,col,offset,color     
    align(2)
    label(SKIP2)
    mov(r5,r3)
    lsl(r5,r5,4)          # screen offset
    add(r0,r0,r5)          # screen offset
    label(MAIN_LOOP)
    push({r0, r1})
    mov(r0,r2)               # num 
    mov(r1,10)               # num//10
    bl(DIVIDE10)             # do divide
    mov(r6,r0)               # r6=num//10
    pop({r1, r0}) 
    mov(r5,10)
    mul(r5,r6)              # 10*(num//10)
    sub(r3,r2,r5)           # r3=num-(10*(num//10))   
    label(CHAR_LOOP)
    mov(r2,r6)             # r2=num//10   
    mov(r5,7)
    strh(r5,[r4,ROW_ASM])  # row=8    
    label(ROW)
    mov(r5,7)               
    strh(r5,[r4,COL_ASM])  # col=8
    label(COL)
    bl(PRINT)              # print    
    ldrh(r5,[r4,COL_ASM])
    sub(r5,1)              # col-=1
    strh(r5,[r4,COL_ASM])
    bpl(COL)
    ldrh(r5,[r4,ROW_ASM])
    sub(r5,1)
    strh(r5,[r4,ROW_ASM])
    bpl(ROW)
    sub(r0,16)                # add one char
    cmp(r2,0)
    beq(EXIT)              # if num=0, exit
    b(MAIN_LOOP)
    
    b(EXIT)
    # ---------------------# SUBROUTINE -------------------
    label(PRINT)           # r3=digit, uses r5,r6,r7
    ldrh(r5,[r4,ROW_ASM]) 
    lsl(r7,r3,3)           # r7=num*8
    add(r7,r7,r5)          # r7=num*8 + row
    add(r7,r7,r1)          # c_map[(num*8)+row]
    ldrb(r7,[r7,0])        # r7=c_map[(num*8)+row]
    mov(r5,1)              # r5=1
    ldrh(r6,[r4,COL_ASM])  # r6=col
    lsl(r5,r6)             # r5=1<<col
    and_(r5,r7)            # c_map[(num*8)+row] & 1<<col
    beq(NOBIT)             # no pixel, skip
    ldrh(r5,[r4,ROW_ASM])
    mov(r7,MAXSCREEN_X)    # screen width
    add(r7,r7,r7)####
    mul(r7,r5)             # row*width
    add(r7,r7,r6)          # r7 = row*width+col
    add(r7,r7,r7)
    add(r7,r7,r0)          # r7 = dest[row*200+col]
    ldrh(r5,[r4,COLOR_ASM])
    strh(r5,[r7,0])        # dest[row*200+col] = white
    add(r7,MAXSCREEN_X)
    add(r7,MAXSCREEN_X)
    strh(r5,[r7,0])        # dest[row*200+col] = white
    label(NOBIT)
    bx(lr)
    
    # -------------------divide routine----------r0=r0//r1-------------
    label(DIVIDE10)
    align(2)
    mov(r7,pc)        # address of next statement will go in r7
    b(SIOSKIP)
    data(2,0xd000)    #  SIO_BASE _u(0xd0000000)
    align(2)    
    label(SIOSKIP)
    ldrh(r7, [r7, 0]) # read variable
    lsl(r6,r7,16)
    add(r6,0x60)      # offset so strh will work
    str(r0, [r6, 8])  # SIO_DIV_SDIVIDEND_OFFSET _u(0x00000068)8
    str(r1, [r6, 12]) # SIO_DIV_SDIVISOR_OFFSET _u(0x0000006c)12    
    nop()
    nop()
    nop()
    nop()
    nop()
    nop()
    nop()
    nop()
    nop()
    ldr(r1, [r6, 20])  #SIO_DIV_REMAINDER_OFFSET _u(0x00000074)20
    ldr(r0, [r6, 16])  #SIO_DIV_QUOTIENT_OFFSET _u(0x00000070)16
    bx(lr)
    #----------------end divide-------------------------------------

    #--------------------End Subs---------------------   
    label(EXIT)

num=123456789
if __name__=='__main__':
    lcd = LCD_3inch5()
    lcd.bl_ctrl(100)
    lcd.Fill(lcd.BLACK)
    display_buffer=bytearray(MAXSCREEN_X * MAXSCREEN_Y * 2)
    screen=framebuf.FrameBuffer(display_buffer, MAXSCREEN_X , MAXSCREEN_Y, framebuf.RGB565)
    gticks=ticks_us()
    printnum_asm(screen,char_map,987654321,39) # screen addr, char map addr, fps,x_offset
    #screen.text('123456789',10,20,0xffff)
    print(ticks_diff(ticks_us(), gticks)) 
    lcd.show_xy(0,0,MAXSCREEN_X-1,MAXSCREEN_Y-1,screen)