1 /*
2 *
3 * SPDX-License-Identifier: GPL-2.0
4 *
5 * Copyright (C) 2011-2018 ARM or its affiliates
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; version 2.
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
12 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
13 * for more details.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17 *
18 */
19 #include <linux/vmalloc.h>
20 #include "acamera_types.h"
21
22 #include "acamera_fw.h"
23 #include "acamera_metering_stats_mem_config.h"
24 #include "acamera_math.h"
25 #include "acamera_lens_api.h"
26 #include "system_stdlib.h"
27 #include "acamera_isp_core_nomem_settings.h"
28 #include "af_manual_fsm.h"
29
30 #include "sbuf.h"
31
32 #if defined( ISP_METERING_OFFSET_AF )
33 #define ISP_METERING_OFFSET_AUTO_FOCUS ISP_METERING_OFFSET_AF
34 #elif defined( ISP_METERING_OFFSET_AF2W )
35 #define ISP_METERING_OFFSET_AUTO_FOCUS ISP_METERING_OFFSET_AF2W
36 #else
37 #error "The AF metering offset is not defined in acamera_metering_mem_config.h"
38 #endif
39
40 #ifdef LOG_MODULE
41 #undef LOG_MODULE
42 #define LOG_MODULE LOG_MODULE_AF_MANUAL
43 #endif
44
af_update_lens_position(AF_fsm_ptr_t p_fsm)45 static void af_update_lens_position( AF_fsm_ptr_t p_fsm )
46 {
47 const lens_param_t *lens_param = p_fsm->lens_ctrl.get_parameters( p_fsm->lens_ctx );
48 af_lms_param_t *param = (af_lms_param_t *)_GET_USHORT_PTR( ACAMERA_FSM2CTX_PTR( p_fsm ), CALIBRATION_AF_LMS );
49
50 /* the new AF position is updated in sbuf FSM */
51 if ( p_fsm->last_position != p_fsm->new_pos ) {
52 int fw_id = p_fsm->cmn.ctx_id;
53
54 p_fsm->lens_ctrl.move( p_fsm->lens_ctx, p_fsm->new_pos );
55 p_fsm->frame_skip_start = 1;
56 LOG( LOG_INFO, "ctx: %d, new af applied, position: %u, last_position: %u.", fw_id, p_fsm->new_pos, p_fsm->last_position );
57
58 if ( param->print_debug )
59 LOG( LOG_CRIT, "ctx: %d, new af applied, position: %u, last_position: %u.", fw_id, p_fsm->new_pos, p_fsm->last_position );
60
61 /* update the done position and sharpness when sharpness value changed */
62 if ( ( p_fsm->last_sharp_done != p_fsm->new_last_sharp ) && ( p_fsm->new_last_sharp != 0 ) ) {
63 p_fsm->last_pos_done = p_fsm->last_position;
64 p_fsm->last_sharp_done = p_fsm->new_last_sharp;
65
66 if ( AF_MODE_CALIBRATION == p_fsm->mode && param->print_debug )
67 LOG( LOG_CRIT, "new af calibration, pos: %u, sharp: %d.", p_fsm->last_pos_done / lens_param->min_step, p_fsm->last_sharp_done );
68 }
69
70 p_fsm->last_position = p_fsm->new_pos;
71
72 status_info_param_t *p_status_info = (status_info_param_t *)_GET_UINT_PTR( ACAMERA_FSM2CTX_PTR( p_fsm ), CALIBRATION_STATUS_INFO );
73 p_status_info->af_info.lens_pos = p_fsm->last_position;
74 p_status_info->af_info.focus_value = p_fsm->last_sharp_done;
75 } else {
76 LOG( LOG_DEBUG, "last position(%u) not changed.", p_fsm->last_position );
77 }
78 }
79
af_set_new_param(AF_fsm_ptr_t p_fsm,sbuf_af_t * p_sbuf_af)80 void af_set_new_param( AF_fsm_ptr_t p_fsm, sbuf_af_t *p_sbuf_af )
81 {
82 p_fsm->new_pos = p_sbuf_af->af_position;
83 p_fsm->new_last_sharp = p_sbuf_af->af_last_sharp;
84 p_fsm->skip_frame = p_sbuf_af->frame_to_skip;
85
86 af_update_lens_position( p_fsm );
87 }
88
af_read_stats_data(AF_fsm_ptr_t p_fsm)89 void af_read_stats_data( AF_fsm_ptr_t p_fsm )
90 {
91 uint8_t zones_horiz, zones_vert, x, y;
92 uint32_t( *stats )[2];
93 sbuf_af_t *p_sbuf_af = NULL;
94 struct sbuf_item sbuf;
95 int fw_id = p_fsm->cmn.ctx_id;
96
97 memset( &sbuf, 0, sizeof( sbuf ) );
98 sbuf.buf_type = SBUF_TYPE_AF;
99 sbuf.buf_status = SBUF_STATUS_DATA_EMPTY;
100
101 if ( p_fsm->frame_num )
102 p_fsm->frame_num--;
103
104 if ( sbuf_get_item( fw_id, &sbuf ) ) {
105 LOG( LOG_ERR, "Error: Failed to get sbuf, return." );
106 return;
107 }
108
109 p_sbuf_af = (sbuf_af_t *)sbuf.buf_base;
110 LOG( LOG_DEBUG, "Get sbuf ok, idx: %u, status: %u, addr: %p.", sbuf.buf_idx, sbuf.buf_status, sbuf.buf_base );
111
112 zones_horiz = acamera_isp_metering_af_nodes_used_horiz_read( p_fsm->cmn.isp_base );
113 zones_vert = acamera_isp_metering_af_nodes_used_vert_read( p_fsm->cmn.isp_base );
114 stats = p_sbuf_af->stats_data;
115 p_sbuf_af->frame_num = p_fsm->frame_num;
116
117 // we need to skip frames after lens movement to get stable stats for next AF step calculation.
118 if ( p_fsm->skip_frame ) {
119 p_sbuf_af->skip_cur_frame = 1;
120 p_fsm->skip_frame--;
121 } else {
122 p_sbuf_af->skip_cur_frame = 0;
123 }
124
125 for ( y = 0; y < zones_vert; y++ ) {
126 uint32_t inx = (uint32_t)y * zones_horiz;
127 for ( x = 0; x < zones_horiz; x++ ) {
128 uint32_t full_inx = inx + x;
129 stats[full_inx][0] = acamera_metering_stats_mem_array_data_read( p_fsm->cmn.isp_base, ISP_METERING_OFFSET_AUTO_FOCUS + ( ( full_inx ) << 1 ) + 0 );
130 stats[full_inx][1] = acamera_metering_stats_mem_array_data_read( p_fsm->cmn.isp_base, ISP_METERING_OFFSET_AUTO_FOCUS + ( ( full_inx ) << 1 ) + 1 );
131 }
132 }
133
134 /* read done, set the buffer back for future using */
135 sbuf.buf_status = SBUF_STATUS_DATA_DONE;
136
137 if ( sbuf_set_item( fw_id, &sbuf ) ) {
138 LOG( LOG_ERR, "Error: Failed to set sbuf, return." );
139 return;
140 }
141 LOG( LOG_DEBUG, "Set sbuf ok, idx: %u, status: %u, addr: %p.", sbuf.buf_idx, sbuf.buf_status, sbuf.buf_base );
142 }
143
AF_fsm_process_interrupt(AF_fsm_const_ptr_t p_fsm,uint8_t irq_event)144 void AF_fsm_process_interrupt( AF_fsm_const_ptr_t p_fsm, uint8_t irq_event )
145 {
146 if ( acamera_fsm_util_is_irq_event_ignored( (fsm_irq_mask_t *)( &p_fsm->mask ), irq_event ) )
147 return;
148
149 //check if lens was initialised properly
150 if ( p_fsm->lens_driver_ok == 0 ) {
151 LOG( LOG_INFO, "lens driver is not OK, return" );
152 return;
153 }
154
155 switch ( irq_event ) {
156 case ACAMERA_IRQ_AF2_STATS: // read out the statistic
157 af_read_stats_data( (AF_fsm_ptr_t)p_fsm );
158 fsm_raise_event( p_fsm, event_id_af_stats_ready );
159
160
161 break;
162 }
163 }
164 //================================================================================
AF_init(AF_fsm_ptr_t p_fsm)165 void AF_init( AF_fsm_ptr_t p_fsm )
166 {
167 int32_t result = 0;
168 af_lms_param_t *param = NULL;
169
170 p_fsm->zone_weight = vmalloc(AF_ZONES_COUNT_MAX*sizeof(uint8_t));
171 if(p_fsm->zone_weight == NULL){
172 LOG(LOG_ERR, "Failed to malloc mem");
173 return;
174 }
175
176 p_fsm->zone_process_statistic = vmalloc(AF_ZONES_COUNT_MAX*sizeof(uint64_t));
177 if(p_fsm->zone_process_statistic == NULL){
178 LOG(LOG_ERR, "Failed to malloc mem");
179 return;
180 }
181
182 p_fsm->zone_process_reliablility = vmalloc(AF_ZONES_COUNT_MAX*sizeof(uint32_t));
183 if(p_fsm->zone_process_reliablility == NULL){
184 LOG(LOG_ERR, "Failed to malloc mem");
185 return;
186 }
187
188 //check if lens was initialised properly
189 if ( !p_fsm->lens_driver_ok ) {
190 p_fsm->lens_ctx = NULL;
191
192 if ( ACAMERA_FSM2CTX_PTR( p_fsm )->settings.lens_init != NULL ) {
193 result = ACAMERA_FSM2CTX_PTR( p_fsm )->settings.lens_init( &p_fsm->lens_ctx, &p_fsm->lens_ctrl );
194 if ( result != -1 && p_fsm->lens_ctx != NULL ) {
195 //only on lens init success populate param
196 param = (af_lms_param_t *)_GET_USHORT_PTR( ACAMERA_FSM2CTX_PTR( p_fsm ), CALIBRATION_AF_LMS );
197 p_fsm->lens_driver_ok = 1;
198 } else {
199 p_fsm->lens_driver_ok = 0;
200 return;
201 }
202 } else {
203 p_fsm->lens_driver_ok = 0;
204 return;
205 }
206 }
207
208 if ( param ) {
209 p_fsm->pos_min = p_fsm->def_pos_min = param->pos_min;
210 p_fsm->pos_inf = p_fsm->def_pos_inf = param->pos_inf;
211 p_fsm->pos_macro = p_fsm->def_pos_macro = param->pos_macro;
212 p_fsm->pos_max = p_fsm->def_pos_max = param->pos_max;
213 p_fsm->def_pos_min_down = param->pos_min_down;
214 p_fsm->def_pos_inf_down = param->pos_inf_down;
215 p_fsm->def_pos_macro_down = param->pos_macro_down;
216 p_fsm->def_pos_max_down = param->pos_max_down;
217 p_fsm->def_pos_min_up = param->pos_min_up;
218 p_fsm->def_pos_inf_up = param->pos_inf_up;
219 p_fsm->def_pos_macro_up = param->pos_macro_up;
220 p_fsm->def_pos_max_up = param->pos_max_up;
221
222 p_fsm->mask.repeat_irq_mask = ACAMERA_IRQ_MASK( ACAMERA_IRQ_AF2_STATS );
223 AF_request_interrupt( p_fsm, p_fsm->mask.repeat_irq_mask );
224 }
225 }
226
AF_deinit(AF_fsm_ptr_t p_fsm)227 void AF_deinit( AF_fsm_ptr_t p_fsm )
228 {
229 if ( ACAMERA_FSM2CTX_PTR( p_fsm )->settings.lens_deinit )
230 ACAMERA_FSM2CTX_PTR( p_fsm )
231 ->settings.lens_deinit( p_fsm->lens_ctx );
232
233 if(p_fsm->zone_weight)
234 {
235 vfree(p_fsm->zone_weight);
236 p_fsm->zone_weight = NULL;
237 }
238
239 if(p_fsm->zone_process_statistic)
240 {
241 vfree(p_fsm->zone_process_statistic);
242 p_fsm->zone_process_statistic = NULL;
243 }
244
245 if(p_fsm->zone_process_reliablility)
246 {
247 vfree(p_fsm->zone_process_reliablility);
248 p_fsm->zone_process_reliablility = NULL;
249 }
250 }
251