--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/cs/drivers/drv_app/r2d/r2d_geometry.s	Fri Oct 16 06:23:26 2020 +0000
@@ -0,0 +1,534 @@
+
+
+     .state32
+
+ .if (R2D_ASM = 1)
+
+ .include "r2d_asm_utils.inc"
+ .include "r2d_asm_configs.inc"
+
+
+    ; Main asm code (it is reflecting the C code of
+    ; r2d_blit_lcd_to_lcd)
+
+    .text
+
+ 
+
+    .global _r2d_blit_lb,_r2d_blit_la
+    .global _r2d_sb_down1,_r2d_sb_down2,_r2d_sa_up1,_r2d_sa_up2
+    .global _r2d_scb_down1,_r2d_scb_down2,_r2d_sca_up1,_r2d_sca_up2
+    .global _r2d_sdb_down1,_r2d_sdb_down2,_r2d_sda_up1,_r2d_sda_up2
+    .global _r2d_skip_color_proc_a
+    .global _r2d_end_ynb_color_dst_a
+    .global _r2d_skip_color_proc_b
+    .global _r2d_end_ynb_color_dst_b
+    .global _r2d_skip_color_dst_a
+    .global _r2d_skip_color_dst_b 
+   
+
+    .global _r2d_sconvcolb_down1,_r2d_econvcolb_down1
+    .global _r2d_sconvcolb_down2,_r2d_econvcolb_down2
+    .global _r2d_sconvcola_up1,_r2d_econvcola_up1
+    .global _r2d_sconvcola_up2,_r2d_econvcola_up2
+
+    .global _r2d_swb_down1,_r2d_swb_down2,_r2d_swa_up1,_r2d_swa_up2
+
+
+    ; Patch list for write_shift_pixel_down
+    .global _r2d_sft_dst_n_b_down1,_r2d_sft_dst_n_b_down2
+    .global _r2d_sft_srcb__b_down1,_r2d_sft_srcb__b_down2
+    .global _r2d_sft_dstc__b_down1,_r2d_sft_dstc__b_down2
+
+    .global _r2d_sft_src_c_b_down1,_r2d_sft_src_c_b_down2
+    .global _r2d_sft_b_c_b_down1,_r2d_sft_b_c_b_down2
+    .global _r2d_sft_dstb_c_b_down1,_r2d_sft_dstb_c_b_down2
+    .global _r2d_sft_d_c_b_down1,_r2d_sft_d_c_b_down2
+    .global _r2d_sft_dstc_c_b_down1,_r2d_sft_dstc_c_b_down2
+
+    ; Patch list for write_shift_pixel_up
+    .global _r2d_sft_dst_n_a_up1,_r2d_sft_dst_n_a_up2
+    .global _r2d_sft_srcb__a_up1,_r2d_sft_srcb__a_up2
+    .global _r2d_sft_dstc__a_up1,_r2d_sft_dstc__a_up2
+
+    .global _r2d_sft_src_c_a_up1,_r2d_sft_src_c_a_up2
+    .global _r2d_sft_b_c_a_up1,_r2d_sft_b_c_a_up2
+
+    ; Patch list fro shift_pixel_down
+    .global _r2d_sft_dst_n_downa,_r2d_sft_dst_n_downb,_r2d_sft_dst_n_downc
+    .global _r2d_sft_srcb_n_downa,_r2d_sft_srcb_n_downb,_r2d_sft_srcb_n_downc
+
+    .global _r2d_sft_src_c_downa,_r2d_sft_src_c_downb,_r2d_sft_src_c_downc
+
+    ; Patch list fro shift_pixel_up
+    .global _r2d_sft_dst_n_upa,_r2d_sft_dst_n_upb,_r2d_sft_dst_n_upc
+    .global _r2d_sft_srcb_n_upa,_r2d_sft_srcb_n_upb,_r2d_sft_srcb_n_upc
+
+    .global _r2d_sft_src_c_upa,_r2d_sft_src_c_upb,_r2d_sft_src_c_upc
+    .global _r2d_enda,_r2d_endb
+
+	.global _r2d_skip_load_src_a_up1,_r2d_skip_load_src_a_up2;
+	.global _r2d_skip_load_src_b_down1,_r2d_skip_load_src_b_down2;
+	.global _r2d_skip_shift_down_downa,_r2d_skip_shift_down_downb,_r2d_skip_shift_down_downc;
+    .global _r2d_skip_shift_up_upa,_r2d_skip_shift_up_upb,_r2d_skip_shift_up_upc;
+
+    .global _r2d_no_null_src_counter_a,_r2d_no_null_src_counter_b
+	.global _r2d_end_a_up2,_r2d_end_b_down2,_r2d_end_downa,_r2d_end_downb,_r2d_end_downc;
+    .global _r2d_end_upa,_r2d_end_upb,_r2d_end_upc;
+
+	.global _r2d_always_read_a_up1,_r2d_always_read_a_up2
+	.global _r2d_always_read_b_down1,_r2d_always_read_b_down2
+  
+    .ref $r2d_blit_lcd_to_lcd
+
+ .endif
+
+write_shift_pixel_down .macro label
+       ; Create free space for result
+	   .if (R2D_PIXEL_DEPTH != 32)
+_r2d_sft_dst_n_:label:       MOV dstvalue,dstvalue, LSR #R2D_PIXEL_DEPTH 
+       .else
+_r2d_sft_dst_n_:label:       MOV dstvalue,#0
+	   .endif
+
+       ; Mask to extract pixel from src_current
+_r2d_sft_src_c_:label:       MOV calca,src_current,LSL #((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)
+_r2d_always_read_:label:  MOV src_current,src_current
+_r2d_sft_b_c_:label:       MOV calca,calca,LSR #((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)
+
+       ; Mask to extract pixel from dst_current
+_r2d_sft_dstb_c_:label:       MOV calcb,dst_current,LSL #((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)
+_r2d_sft_d_c_:label:       MOV calcb,calcb,LSR #((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)
+   
+       ; Do foreground detection
+_r2d_sd:label:
+       MOV calca,calca
+       MOV calca,calca
+       MOV calca,calca
+_r2d_ed:label:
+
+       ; Get foreground/background colors if needed
+_r2d_sc:label:
+       MOV calca,calca
+       MOV calca,calca
+       MOV calca,calca
+_r2d_ec:label:
+
+_r2d_sconvcol:label:
+       MOV calca,calca
+       MOV calca,calca
+       MOV calca,calca
+_r2d_econvcol:label:
+
+       ; Compute drawing mode
+_r2d_s:label:
+       MOV calca,calca
+       MOV calcb,calca
+       MOV calcb,calcb
+_r2d_e:label:
+_r2d_sft_dstc_c_:label:       ORR dstvalue,dstvalue,calcb, LSL #((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)
+
+_r2d_sw:label:
+       MOV dstvalue,dstvalue
+_r2d_ew:label:
+
+       ; Read new data if needed
+	   .if (R2D_PIXEL_DEPTH != 32)
+_r2d_sft_srcb__:label:       MOV src_current,src_current, LSR #R2D_PIXEL_DEPTH
+_r2d_sft_dstc__:label:       MOV dst_current,dst_current, LSR #R2D_PIXEL_DEPTH
+       .else
+_r2d_sft_srcb__:label:       MOV src_current,src_current,LSR #0
+_r2d_sft_dstc__:label:       MOV dst_current,dst_current,LSR #0
+	   .endif
+
+       SUBS dstcounter,dstcounter,#1
+       MOVEQ dstcounter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+_r2d_skip_load_src_:label: MOV src_current,src_current
+       SUBS srccounter,srccounter,#1
+       BNE M1END?
+       MOV srccounter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+       LDR src_current,[p_src_current,#4]!
+M1END?
+  .endm
+
+write_shift_pixel_up .macro label
+     .if (R2D_PIXEL_DEPTH != 32)
+_r2d_sft_dst_n_:label:       MOV dstvalue,dstvalue, LSL #R2D_PIXEL_DEPTH 
+     .else
+_r2d_sft_dst_n_:label:       MOV dstvalue,#0
+	 .endif
+
+_r2d_sft_src_c_:label:      MOV calca,src_current,LSR #((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)
+_r2d_always_read_:label:  MOV src_current,src_current
+_r2d_sft_b_c_:label:      MOV calcb,dst_current,LSR #((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)
+
+       ; Do foreground detection
+_r2d_sd:label:
+       MOV calca,calca
+       MOV calca,calca
+       MOV calca,calca
+_r2d_ed:label:
+
+       ; Get foreground/background colors if needed
+_r2d_sc:label:
+       MOV calca,calca
+       MOV calca,calca
+       MOV calca,calca
+_r2d_ec:label:
+
+_r2d_sconvcol:label:
+       MOV calca,calca
+       MOV calca,calca
+       MOV calca,calca
+_r2d_econvcol:label:
+
+       ; Compute drawing mode
+_r2d_s:label:
+       MOV calca,calca
+       MOV calcb,calca
+       MOV calcb,calcb
+_r2d_e:label:
+       ORR dstvalue,dstvalue,calcb
+
+_r2d_sw:label:
+       MOV dstvalue,dstvalue
+_r2d_ew:label:
+
+       .if (R2D_PIXEL_DEPTH != 32)
+_r2d_sft_srcb__:label:       MOV src_current,src_current, LSL #R2D_PIXEL_DEPTH
+_r2d_sft_dstc__:label:       MOV dst_current,dst_current, LSL #R2D_PIXEL_DEPTH
+       .else
+_r2d_sft_srcb__:label:       MOV src_current,src_current,LSL #0
+_r2d_sft_dstc__:label:       MOV dst_current,dst_current,LSL #0
+	   .endif
+
+       SUBS dstcounter,dstcounter,#1
+       MOVEQ dstcounter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+_r2d_skip_load_src_:label: MOV src_current,src_current
+       SUBS srccounter,srccounter,#1
+       BNE M1END?
+       MOV srccounter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+       LDR src_current,[p_src_current,#-4]!
+M1END?
+  .endm
+
+shift_pixel_down .macro arg,reg,label
+_r2d_skip_shift_down_:label:  MOV arg,arg
+
+     .if (R2D_PIXEL_DEPTH != 32)
+_r2d_sft_dst_n_:label:       MOV arg,arg, LSR #R2D_PIXEL_DEPTH 
+     .else
+_r2d_sft_dst_n_:label:       MOV arg,#0
+	 .endif
+
+_r2d_sft_src_c_:label:       ORR arg,arg,:reg:_current, LSL #((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)
+
+     .if (R2D_PIXEL_DEPTH != 32)
+_r2d_sft_srcb_n_:label:       MOV :reg:_current,:reg:_current, LSR #R2D_PIXEL_DEPTH
+     .else
+_r2d_sft_srcb_n_:label:       MOV :reg:_current,#0
+	 .endif
+
+       SUBS :reg:counter,:reg:counter,#1
+       BNE M1END?
+       MOV :reg:counter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+       LDR :reg:_current,[p_:reg:_current,#4]!
+M1END?
+  .endm
+
+shift_pixel_up .macro arg,reg,label
+_r2d_skip_shift_up_:label:  MOV arg,arg
+     .if (R2D_PIXEL_DEPTH != 32)
+_r2d_sft_dst_n_:label:       MOV arg,arg, LSL #R2D_PIXEL_DEPTH 
+     .else
+_r2d_sft_dst_n_:label:       MOV arg,#0
+	 .endif
+
+_r2d_sft_src_c_:label:       ORR arg,arg,:reg:_current, LSR #((1<<R2D_MEMORY_WORD) - R2D_PIXEL_DEPTH)
+
+      .if (R2D_PIXEL_DEPTH != 32)
+_r2d_sft_srcb_n_:label:       MOV :reg:_current,:reg:_current, LSL #R2D_PIXEL_DEPTH
+      .else
+_r2d_sft_srcb_n_:label:       MOV :reg:_current,#0
+	  .endif
+
+
+       SUBS :reg:counter,:reg:counter,#1
+       BNE M1END?
+       MOV :reg:counter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+       LDR :reg:_current,[p_:reg:_current,#-4]!
+M1END?
+  .endm
+
+  .if (R2D_ASM = 1)
+
+;--------------------------------------
+;
+; v_direction == -1
+;
+
+_r2d_blit_la
+	STMFD SP!, {R4,R5,R6,R7,R8,R9,R10,R11,R12,LR}
+    LDR dstvalue, [R0,#S_FRAME.dstvalue_ind]
+    LDR dst_current, [R0,#S_FRAME.dst_current_ind]
+    LDRH dstcounter, [R0,#S_FRAME.dstcounter_ind]
+    LDR src_current, [R0,#S_FRAME.src_current_ind]
+    LDRH ynb, [R0,#S_FRAME.ynb_ind]
+    LDR p_src_current, [R0,#S_FRAME.p_src_current_ind]
+    LDR p_dst_current, [R0,#S_FRAME.p_dst_current_ind]
+    LDR x, [R0,#S_FRAME.x_ind]
+    LDR y, [R0,#S_FRAME.y_ind]
+    LDRH current_dst_dy, [R0,#S_FRAME.current_dst_dy_ind]
+    LDRH current_src_dy, [R0,#S_FRAME.current_src_dy_ind]
+    LDRH dst_dy, [R0,#S_FRAME.dst_dy_ind]
+
+
+;-------------------------------------
+;
+; First while
+;
+; current_dst_dy != dst_dy
+current_dst_a: CMP current_dst_dy,dst_dy
+  BEQ end_current_dst_a
+
+  shift_pixel_up dstvalue,dst,upa
+
+_r2d_end_upa:  SUB current_dst_dy,current_dst_dy,#1
+  SUB y,y,#1
+  B current_dst_a
+
+end_current_dst_a:
+
+  ; Load srcounter(and overwrite dst_dy)
+  LDRH srccounter, [R0,#S_FRAME.srccounter_ind]
+
+  LDRH temp,[R0,#S_FRAME.src_dy_ind]
+  SUB current_src_dy,current_src_dy,temp
+
+;-------------------------------------
+;
+; Second while
+;
+; current_src_dy - src_dy != 0
+current_src_a: CMP current_src_dy,#0
+  BEQ end_current_src_a
+
+  shift_pixel_up temp,src,upb
+
+_r2d_end_upb:  SUB current_src_dy,current_src_dy,#1
+  B current_src_a
+
+end_current_src_a:
+
+;-------------------------------------
+;
+; Third while
+;
+; (ynb!=0)
+_r2d_ynb_loop_a: TEQ ynb,#0
+       BEQ end_ynb_loop_a
+
+      write_shift_pixel_up a_up1 ; was a_up1
+
+_r2d_no_null_src_counter_a:
+       SUB y,y,#1 
+
+       ; When dst is color framebuffer, following instruction is replaced by
+       ; BL no_end_dst_counter_a so, to ynb_loop_a
+_r2d_skip_color_proc_a: MOV y,y
+
+       CMP dstcounter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+       BNE no_end_dst_counter_a
+       
+       SUB ynb,ynb,#1
+       STR dstvalue,[p_dst_current],#-4
+       
+       LDR dst_current,[p_dst_current]
+       MOV dstvalue,#0
+       MOV current_dst_dy,#((1<<R2D_PIXELS_PER_MEMORY_WORD)-1)
+
+no_end_dst_counter_a:       B _r2d_ynb_loop_a
+_r2d_end_ynb_color_dst_a:SUB ynb,ynb,#1
+   B _r2d_ynb_loop_a
+
+end_ynb_loop_a:
+_r2d_skip_color_dst_a: MOV y,y
+
+    ADD current_dst_dy,current_dst_dy,#1
+    LDRH temp,[R0,#S_FRAME.end_dst_dy_ind]
+    SUB current_dst_dy,current_dst_dy,temp
+
+;-------------------------------------
+;
+; Fourth while
+;
+; current_dst_dy - end_dst_dy != 0
+end_dst_loop_a: TEQ current_dst_dy,#0
+    BEQ end_end_dst_loop_a
+    
+    write_shift_pixel_up a_up2
+
+_r2d_end_a_up2:   SUB current_dst_dy,current_dst_dy,#1
+    SUB y,y,#1
+    B end_dst_loop_a
+end_end_dst_loop_a:
+    MOV temp,#0
+    CMP dstcounter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+    MOVEQ temp,#1
+
+;-------------------------------------
+;
+; Last while
+;
+; dstcounter != (1<<R2D_PIXELS_PER_MEMORY_WORD)
+dst_counter_loop_a: CMP dstcounter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+  BEQ end_dst_counter_loop_a
+  shift_pixel_up dstvalue,dst,upc
+_r2d_end_upc:  SUB y,y,#1
+  B dst_counter_loop_a
+end_dst_counter_loop_a:
+    CMP temp,#0
+    STREQ dstvalue,[p_dst_current,#4]!
+
+    CMP temp,#0
+    STRNE dstvalue,[p_dst_current]
+_r2d_enda:
+	LDMFD SP!, {R4,R5,R6,R7,R8,R9,R10,R11,R12,LR}
+	MOV PC,LR
+
+
+;--------------------------------------
+;
+; v_direction != -1
+;
+_r2d_blit_lb
+	STMFD SP!, {R4,R5,R6,R7,R8,R9,R10,R11,R12,LR}
+    LDR dstvalue, [R0,#S_FRAME.dstvalue_ind]
+    LDR dst_current, [R0,#S_FRAME.dst_current_ind]
+    LDRH dstcounter, [R0,#S_FRAME.dstcounter_ind]
+    LDR src_current, [R0,#S_FRAME.src_current_ind]
+    LDRH ynb, [R0,#S_FRAME.ynb_ind]
+    LDR p_src_current, [R0,#S_FRAME.p_src_current_ind]
+    LDR p_dst_current, [R0,#S_FRAME.p_dst_current_ind]
+    LDR x, [R0,#S_FRAME.x_ind]
+    LDR y, [R0,#S_FRAME.y_ind]
+    LDRH current_dst_dy, [R0,#S_FRAME.current_dst_dy_ind]
+    LDRH current_src_dy, [R0,#S_FRAME.current_src_dy_ind]
+    LDRH dst_dy, [R0,#S_FRAME.dst_dy_ind]
+
+
+;-------------------------------------
+;
+; First while
+;
+; current_dst_dy != dst_dy
+current_dst_b: CMP current_dst_dy,dst_dy
+  BEQ end_current_dst_b
+
+  shift_pixel_down dstvalue,dst,downa
+
+_r2d_end_downa:  ADD current_dst_dy,current_dst_dy,#1
+  ADD y,y,#1
+  B current_dst_b
+
+end_current_dst_b:
+
+  ; Load srcounter(and overwrite dst_dy)
+  LDRH srccounter, [R0,#S_FRAME.srccounter_ind]
+
+  LDRH temp,[R0,#S_FRAME.src_dy_ind]
+  SUB current_src_dy,current_src_dy,temp
+
+;-------------------------------------
+;
+; Second while
+;
+; current_src_dy - src_dy != 0
+current_src_b: CMP current_src_dy,#0
+  BEQ end_current_src_b
+
+  shift_pixel_down temp,src,downb
+
+_r2d_end_downb:  ADD current_src_dy,current_src_dy,#1
+  B current_src_b
+
+end_current_src_b:
+
+;-------------------------------------
+;
+; Third while
+;
+; (ynb!=0)
+_r2d_ynb_loop_b: TEQ ynb,#0
+       BEQ end_ynb_loop_b
+
+      write_shift_pixel_down b_down1
+
+_r2d_no_null_src_counter_b:
+       ADD y,y,#1 
+
+       ; When dst is color framebuffer, following instruction is replaced by
+       ; BL no_end_dst_counter_a so, to ynb_loop_a
+_r2d_skip_color_proc_b: MOV y,y
+
+       CMP dstcounter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+       BNE no_end_dst_counter_b
+       
+       SUB ynb,ynb,#1
+       STR dstvalue,[p_dst_current],#4
+       
+       LDR dst_current,[p_dst_current]
+       MOV dstvalue,#0
+       MOV current_dst_dy,#0
+
+no_end_dst_counter_b:       B _r2d_ynb_loop_b
+_r2d_end_ynb_color_dst_b:    SUB ynb,ynb,#1
+   B _r2d_ynb_loop_b
+end_ynb_loop_b:
+_r2d_skip_color_dst_b: MOV y,y
+
+    SUB current_dst_dy,current_dst_dy,#1
+    LDRH temp,[R0,#S_FRAME.end_dst_dy_ind]
+    SUB current_dst_dy,current_dst_dy,temp
+
+;-------------------------------------
+;
+; Fourth while
+;
+; current_dst_dy - end_dst_dy != 0
+end_dst_loop_b: TEQ current_dst_dy,#0
+    BEQ end_end_dst_loop_b
+    
+    write_shift_pixel_down b_down2
+
+_r2d_end_b_down2:    ADD current_dst_dy,current_dst_dy,#1
+    ADD y,y,#1
+    B end_dst_loop_b
+end_end_dst_loop_b:
+    MOV temp,#0
+    CMP dstcounter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+    MOVEQ temp,#1
+
+;-------------------------------------
+;
+; Last while
+;
+; dstcounter != (1<<R2D_PIXELS_PER_MEMORY_WORD)
+dst_counter_loop_b: CMP dstcounter,#(1<<R2D_PIXELS_PER_MEMORY_WORD)
+  BEQ end_dst_counter_loop_b
+  shift_pixel_down dstvalue,dst,downc
+_r2d_end_downc:  ADD y,y,#1
+  B dst_counter_loop_b
+end_dst_counter_loop_b:
+    CMP temp,#0
+    STREQ dstvalue,[p_dst_current,#-4]!
+
+    CMP temp,#0
+    STRNE dstvalue,[p_dst_current]
+_r2d_endb:
+	LDMFD SP!, {R4,R5,R6,R7,R8,R9,R10,R11,R12,LR}
+	MOV PC,LR
+
+ .endif
+
+    .end