;=================================================
;  Copyright(C) 2024 Advanced Micro Devices, Inc. All rights Reserved.
;=================================================

; Camera sensor I2C group control API
;
; [void] SensorGroupHold(void)
; [void] SensorGroupRelease(void)
;=================================================
.function SensorGroupHold
WI2C $0x3208, $0x00, $1 ;Group Hold
.endfunction

.function SensorGroupRelease
WI2C $0x3208, $0x10, $1 ;Group Release
WI2C $0x3208, $0xA0, $1  
.endfunction

;=================================================
; Camera sensor exposure control API
;
; [uint32 setGain]  SensorConvertAGain(uint32 aGain, uint32 *aGainReal, uint32 *aGainSetting)
;=================================================
.function SensorConvertAGain
;R0 is the aGain
;gain_reg_coarse = aGain / 1000;
;gain_reg_fine = (aGain*10-(gain_reg_coarse*10000)) / 625;
;if(gain_reg_coarse<2): gain_reg_fine
;elseif(gain_reg_coarse<4):gain_reg_fine = gain_reg_fine&0xe
;else:gain_reg_fine = gain_reg_fine&0xc
;R1 = gain_reg_coarse << 8 | gain_reg_fine << 4
;R0 = gain_reg_coarse*1000+gain_reg_fine*625/10
;return R0&R1
START:
;calculate gain_reg_coarse
DIV R8, R0, $1000           ;R8 = R0/1000;	gain_reg_coarse

;calculate gain_reg_fine
MUL R9, R8, $10000          ;R9 = R8 * 10000
MUL R10, R0, $10            ;R10 = R0 * 10
SUB R11, R10,R9				;R11 = R10 - R9
DIV R12, R11, $625          ;R12 = R11/625;	gain_reg_fine

;dispatch
DIV R13,R8,$2				;R13 = R8 / 2
JEZ R13,@FL_AGAIN2			;if (R8<2),jump to FL_AGAIN2
DIV R13,R8,$4				;R13 = R8 / 4
JEZ R13,@FL_AGAIN4			;if (2<=R8<4),jump to FL_AGAIN4
DIV R13,R8,$8				;R13 = R8 / 8
JEZ R13,@FL_AGAIN8			;if (4<=R8<8),jump to FL_AGAIN8
DIV R13,R8,$16				;R13 = R8 / 16
JEZ R13,@FL_AGAIN16			;if (8<=R8<16),jump to FL_AGAIN16
MOVDW R0,$15500				;Exception->set Again = 15.5 max
JEZ $0,@START				;RE calculate again = 15.5*1000

FL_AGAIN2:
JEZ $0,@END					;

FL_AGAIN4:
AND R12,R12,$0xe			;
JEZ $0,@END					;

FL_AGAIN8:
AND R12,R12,$0xc			;
JEZ $0,@END					;

FL_AGAIN16:
AND R12,R12,$0x8			;
JEZ $0,@END					;

END:
;create aGainSetting
SHL R13,R8,$8				;R13 = R8 << 8
SHL R14,R12,$4				;R14 = R12 << 4
OR R1,R13,R14				;R1 = R13 | R14

;create aGainReal
MUL R15,R8,$1000			;R15 = R8 * 1000
MUL R16,R12,$625			;R16 = R12 * 625
DIV R17,R16,$10				;R17 = R16 / 10	;may lose 0.0005 AGain
ADD R0, R15,R17				;R0 = R15 + R17


.endfunction

;=================================================
; Camera sensor exposure control API
;
; [uint32 setGain]  SensorSetAGain(uint32 aGainSetting)
;=================================================
.function SensorSetAGain
;R0 is the aGainSetting

SHR R10, R0, $8            ;R10 = aGainSetting >> 8
AND R10, R10, $0xff        ;R10 = (aGainSetting >> 8) & 0xff, high 8 bit
AND R11, R0, $0xff         ;R11 = aGainSetting & 0xff, low 8 bit

SHL R11, R11, $8
OR  R10, R10, R11     	   ; R10 = (R11 << 8) | R10

WI2C $0x3508, R10, $2
.endfunction

;=================================================
; Camera sensor exposure control API
;
;=================================================
.function SensorGetAGain
;R0 is the realGain
;Gain = (1000 * regGain) / 256;
RI2C R8, $0x3508, $1   ;
RI2C R9, $0x3509, $1   ;
SHL R10, R8, $8        ;
OR R10, R10, R9        ;

MUL R10, R10, $1000    ;
DIV R0, R10, $256      ;
.endfunction

;=================================================
; Camera sensor exposure control API
;
; [uint32 setItime] SensorSetItime(uint32 iTimeSetting,
;                                  uint32 durationSetting)
;=================================================
.function SensorSetItime
;R0 is the iTimeSetting,exposure line
;R1 is the durationSetting,vts,frame_length_line
;R6: first_frame_length_lines
;R31: save the current_frame_length_lines
;calculate******************
SHR R9, R1, $8             ;R1 = frame_length_line
AND R9, R9, $0xff          ;R9 = frame_length_line_high = (frame_length_line >> 8) & 0xff
AND R10, R1, $0xff         ;R10 = frame_length_line_l = frame_length_line & 0xff
SHL R10, R10, $8
OR  R9,  R9,  R10          ;R9 = (R10 << 8) | R9. final, R9 is frame_length_line/vts setting

SHR R15, R0, $8            ;R0  = line_count
AND R15, R15, $0xff        ;R15 = line_count_h = (line_count >> 8) & 0xff
AND R16, R0, $0xff         ;R16 = line_count_l = line_count & 0xff
SHL R16, R16, $8           ; R16 = R16 << 8
OR  R15, R15, R16          ; R15 = R16 | R15. final, R15 is exposure line reg setting
;calculate******************

JNZ R31 @FL_UPDATED			;if current_frame_length_lines !=0, jump to FL_UPDATED
MOVDW R31, R6               ;R31 = R6;

FL_UPDATED:
SUB R30, R1, R31			;R30 = R1 - R31 = durationSetting(frame_length_line) - current_frame_length_lines
MOVDW R31, R1				;R31 = R1

JLZ R30 @LINE_COUNT_FIRST  ;if frame_length_line_set < current_frame_length_lines, jump to LINE_COUNT_FIRST
	WI2C $0x380e, R9, $2
	WI2C $0x3501, R15, $2
	JGZ R6 @END            ; if R6 >0 , jump to end

LINE_COUNT_FIRST:
	WI2C $0x3501, R15, $2
	WI2C $0x380e, R9, $2

END:
;Note, the setted new Gain and new Itime must be return in R0 and R1
;Here, we just return the input value.
MOVDW R0, R0
.endfunction

;=================================================
; Camera sensor exposure control API
;
;=================================================
 .function SensorGetItime 
;R0 is the newItime
;R1: t_line

 RI2C R9, $0x3501, $1   ;line_count_h
 RI2C R10, $0x3502, $1  ;line_count_l
 SHL R12, R9, $8        ;
 OR R11, R10, R12       ;
 MUL R0, R11, R1        ;
 .endfunction
 
 ;===================================================
 ; Camera sensor Digital Gain control API
 ; Empty for now
 ;===================================================
 .function SensorSetDGain
 .endfunction

 ;===================================================
 ; Camera sensor Digital Gain control API
 ; Empty for now
 ;===================================================
 .function SensorGetDGain
 .endfunction

;=================================================
; VCM control API
;
; LensSetPos
;=================================================
.function LensSetPos
;R0 is lens position
;R1 is vcm mode
JNZ R1, @LSC_MODE      	; if(R1 != 0) jump

DIRECT:
SHR R10, R0, $4         ; R10 = R0 >> 4
AND R10, R10, $0x3F     ; R10 = R10 & 0x3F
AND R11, R0, $0x0F      ; R11 = R0 & 0x0F
SHL R11, R11, $4        ; R11 = R11 << 4
WI2C R10, R11, $1       ; write byte1 and byte2
JEZ $0, @END

LSC_MODE:
SUB R2, R1, $1          ; R2 = R1 - 1
JNZ R2, @SAC_MODE       ; if(R1 != 1) jump
SHR R10, R0, $4         ; R10 = R0 >> 4
AND R10, R10, $0x3F     ; R10 = R10 & 0x3F
AND R11, R0, $0x0F      ; R11 = R0 & 0x0F
SHL R11, R11, $4        ; R11 = R11 << 4
OR R11, R11, $0x04      ; R11 = R11 | 0x4, S=0b0100
WI2C R10, R11, $1       ; write byte1 and byte2
JEZ $0, @END

SAC_MODE:
SHR R10, R0, $8         ; R10 = R0 >> 8
AND R10, R10, $0x03    ; R10 = R10 & 0x03
AND R11, R0, $0xFF     ; R11 = R0 & 0xFF
SHL R12, R11, $8        ; R12 = R11 << 8
OR  R12, R12, R10       ; R12 = R12 | R10
GET_STATUS:
RI2C R15, $0x05, $1     ; Get STATUS
AND  R15, R15, $0x01    ; R15 = R15 & 0x01
JNZ  R15, @GET_STATUS   ; if(R15 != 0) jump
WI2C $0x03, R12, $2     ; Write MSB -> LSB

END:
MOVDW R0, R0
.endfunction
