Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
libisp.c
Go to the documentation of this file.
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <stdint.h>
4 #include <fcntl.h>
5 #include <errno.h>
6 #include <string.h>
7 #include <unistd.h>
8 #include <sys/mman.h>
9 #include <sys/ioctl.h>
10 
11 #include "libisp.h"
12 #include "libisp_config.h"
13 
14 #define PRINT(string,...) fprintf(stderr, "[libisp->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
15 
16 #if MT9F002_VERBOSE
17 #define VERBOSE_PRINT PRINT
18 #else
19 #define VERBOSE_PRINT(...)
20 #endif
21 
22 #define AVI_BASE 0x400000
23 #define AVI_SIZE 0x100000
24 #define AVI_MASK (AVI_SIZE - 1)
25 
32 };
33 
34 #define AVI_ISP_STAT_YUV_MAX_WAIT 3
36 
37 /* IOCTL implemented in AVI drivers */
38 #define AVI_ISP_IOGET_OFFSETS _IOR('F', 0x33, struct avi_isp_offsets)
39 
40 /* Raw accesses */
41 #define readl(_addr) (*((volatile uint32_t *)(_addr)))
42 #define writel(_val, _addr) (*((volatile uint32_t *)(_addr)) = _val)
43 
44 /* ISP context */
45 static struct libisp_context isp_ctx = {
46  .devmem = -1
47 };
48 
50 
51 static const unsigned isp_bases[] = {
70  0, /* GAMMA conf */
74  0, /* CHROMA */
75  0, /* STATS YUV*/
84 };
85 
90 static int avi_isp_get_offsets_fd(int fd, struct avi_isp_offsets *off)
91 {
92  if (ioctl(fd, AVI_ISP_IOGET_OFFSETS, off) < 0) {
93  VERBOSE_PRINT("sizeof: %d, %X\n", sizeof(struct avi_isp_offsets), AVI_ISP_IOGET_OFFSETS);
94  perror("ioctl(AVI_ISP_IOGET_OFFSETS) failed");
95  return -1;
96  }
97 
98  return 0;
99 }
100 
105 static int open_isp_fd(struct libisp_context *ctx, int fd)
106 {
107  struct avi_isp_offsets off;
108  int i;
109 
110  ctx->devmem = open("/dev/mem", O_RDWR);
111 
112  if (ctx->devmem < 0) {
113  perror("Can't open /dev/mem");
114  goto open_failed;
115  }
116 
117  ctx->avi_base = (unsigned long) mmap(NULL, AVI_SIZE,
118  PROT_READ | PROT_WRITE,
119  MAP_SHARED, ctx->devmem, AVI_BASE & ~AVI_MASK);
120 
121  if (ctx->avi_base == (unsigned long) MAP_FAILED) {
122  perror("mmap failed");
123  goto mmap_failed;
124  }
125 
126  if (avi_isp_get_offsets_fd(fd, &off) < 0) {
127  goto get_offsets_failed;
128  }
129 
130  /* Compute all the sub-modules offsets */
131  /* Chain Bayer */
132  for (i = chain_bayer_inter ; i < gamma_corrector ; i++) {
133  ctx->offsets[i] = ctx->avi_base + isp_bases[i] + off.chain_bayer;
134  }
135 
136  ctx->offsets[gamma_corrector] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
137  ctx->offsets[gamma_corrector_ry_lut] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
138  ctx->offsets[gamma_corrector_gu_lut] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
139  ctx->offsets[gamma_corrector_bv_lut] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
140  ctx->offsets[chroma] = ctx->avi_base + isp_bases[i++] + off.chroma;
141  ctx->offsets[statistics_yuv] = ctx->avi_base + isp_bases[i++] + off.statistics_yuv;
142  ctx->offsets[statistics_yuv_ae_histogram_y] = ctx->avi_base + isp_bases[i++] + off.statistics_yuv;
143 
144  /* Chain YUV */
145  for (i = chain_yuv_inter ; i < ISP_NODE_NR ; i++) {
146  ctx->offsets[i] = ctx->avi_base + isp_bases[i] + off.chain_yuv;
147  }
148 
149  return 0;
150 
151 get_offsets_failed:
152  munmap((void *) ctx->avi_base, AVI_SIZE);
153 
154 mmap_failed:
155  close(ctx->devmem);
156  ctx->devmem = -1;
157 
158 open_failed:
159  return -1;
160 }
161 
162 /*static int close_isp(struct libisp_context *ctx)
163 {
164  int ret = 0;
165 
166  if (munmap((void *) ctx->avi_base, AVI_SIZE) == -1) {
167  perror("munmap failed");
168  ret = -1;
169  }
170 
171  close(ctx->devmem);
172  ctx->devmem = -1;
173 
174  return ret;
175 }*/
176 
178 {
179  if (open_isp_fd(&isp_ctx, dev->fd) < 0) {
180  return -1;
181  }
182 
183  avi_isp_vlformat_32to40_set_registers(&isp_ctx, &isp_config.vlformat_32to40);
184  avi_isp_chain_bayer_inter_set_registers(&isp_ctx, &isp_config.bayer_inter);
185  avi_isp_pedestal_set_registers(&isp_ctx, &isp_config.pedestal);
186  avi_isp_green_imbalance_set_registers(&isp_ctx, &isp_config.green_imbalance);
187  avi_isp_green_imbalance_green_red_coeff_mem_set_registers(&isp_ctx, &isp_config.grim_gr);
188  avi_isp_green_imbalance_green_blue_coeff_mem_set_registers(&isp_ctx, &isp_config.grim_gb);
189  avi_isp_dead_pixel_correction_set_registers(&isp_ctx, &isp_config.dead_pixel_correction);
190  avi_isp_denoising_set_registers(&isp_ctx, &isp_config.denoising);
191  avi_isp_statistics_bayer_set_registers(&isp_ctx, &isp_config.statistics_bayer);
192  avi_isp_lens_shading_correction_set_registers(&isp_ctx, &isp_config.lens_shading_correction);
193  avi_isp_lens_shading_correction_red_coeff_mem_set_registers(&isp_ctx, &isp_config.lsc_red_coeffs);
194  avi_isp_lens_shading_correction_green_coeff_mem_set_registers(&isp_ctx, &isp_config.lsc_green_coeffs);
195  avi_isp_lens_shading_correction_blue_coeff_mem_set_registers(&isp_ctx, &isp_config.lsc_blue_coeffs);
196  avi_isp_bayer_set_registers(&isp_ctx, &isp_config.bayer);
197  avi_isp_color_correction_set_registers(&isp_ctx, &isp_config.color_correction);
198  avi_isp_vlformat_40to32_set_registers(&isp_ctx, &isp_config.vlformat_40to32);
199  avi_isp_gamma_corrector_set_registers(&isp_ctx, &isp_config.gamma_corrector);
200  avi_isp_gamma_corrector_ry_lut_set_registers(&isp_ctx, &isp_config.gc_ry_lut);
201  avi_isp_gamma_corrector_gu_lut_set_registers(&isp_ctx, &isp_config.gc_gu_lut);
202  avi_isp_gamma_corrector_bv_lut_set_registers(&isp_ctx, &isp_config.gc_bv_lut);
203  avi_isp_chroma_set_registers(&isp_ctx, &isp_config.chroma);
204  avi_isp_statistics_yuv_set_registers(&isp_ctx, &isp_config.statistics_yuv);
205  avi_isp_edge_enhancement_color_reduction_filter_set_registers(&isp_ctx, &isp_config.eecrf);
206  avi_isp_edge_enhancement_color_reduction_filter_ee_lut_set_registers(&isp_ctx, &isp_config.eecrf_lut);
207  avi_isp_chain_yuv_inter_get_registers(&isp_ctx, &isp_config.chain_yuv_inter);
208 
209  //close_isp(&isp_ctx);
210 
211  return 0;
212 }
213 
215  uint16_t x_odd_inc, uint16_t y_odd_inc)
216 {
217  requestWindow[0] = x_start;
218  requestWindow[1] = x_end;
219  requestWindow[2] = y_start;
220  requestWindow[3] = y_end;
221  requestWindow[4] = x_odd_inc;
222  requestWindow[5] = y_odd_inc;
223 
224  VERBOSE_PRINT("[YUV-STAT] Requesting window: [%d %d],[%d %d], [%d %d]\n",
226 
227  return 0;
228 }
229 
231 {
232  VERBOSE_PRINT("[YUV-STAT] Setting window: [%d %d],[%d %d]\n",
234 
241 
242  VERBOSE_PRINT("[YUV-STAT] Current settings: [%d %d] [%d %d] [%d %d] [%d %d] [%d] [%d %d] [%d]\n",
243  isp_config.statistics_yuv.window_pos_x.window_x_start,
245  isp_config.statistics_yuv.window_pos_y.window_y_start,
255  );
256 
257  return 0;
258 }
259 
260 /* Get YUV statistics */
262 {
263  uint16_t i;
264 
265  if (isp_ctx.devmem < 0) {
266  fprintf(stderr, "[YUV-STAT] Error isp_ctx.devmem < 0\n");
267  return -1;
268  }
269 
270  struct avi_isp_statistics_yuv_regs stats_yuv;
271  avi_isp_statistics_yuv_get_registers(&isp_ctx, &stats_yuv);
272 
273  if (!stats_yuv.measure_status.done) {
274  VERBOSE_PRINT("[YUV-STAT] Waiting for YUV stats\n");
275  curWait++;
277  isp_config.statistics_yuv.measure_req.clear = 0; // Clear current results
278  } else {
279  isp_config.statistics_yuv.measure_req.clear = 1; // Clear current results
281  curWait = 0;
282  }
283  avi_isp_statistics_yuv_set_registers(&isp_ctx, &isp_config.statistics_yuv);
284  return -1;
285  } else if (stats_yuv.measure_status.error) {
286  fprintf(stderr, "[YUV-STAT] Error requesting YUV stats\n");
287  isp_config.statistics_yuv.measure_req.clear = 1; // Clear current results?
288  curWait = 0;
290  avi_isp_statistics_yuv_set_registers(&isp_ctx, &isp_config.statistics_yuv);
291  return -1;
292  } else {
293  isp_config.statistics_yuv.measure_req.clear = 1; // Clear current results?
294  }
295  curWait = 0;
296  yuv_stats->awb_sum_Y = stats_yuv.awb_sum_y.awb_sum_y;
297  yuv_stats->awb_sum_U = stats_yuv.awb_sum_u.awb_sum_u;
298  yuv_stats->awb_sum_V = stats_yuv.awb_sum_v.awb_sum_v;
299  yuv_stats->awb_nb_grey_pixels = stats_yuv.awb_nb_grey_pixels.nb_grey_pixels;
300  yuv_stats->nb_valid_Y = stats_yuv.ae_nb_valid_y.nb_valid_y;
301 
302  // Histogram
304  avi_isp_statistics_yuv_ae_histogram_y_get_registers(&isp_ctx, &histogram);
305 
306  for (i = 0; i < 256; ++i) {
307  yuv_stats->ae_histogram_Y[i] = histogram.ae_histogram_y[i].histogram_y;
308  }
310 
311  avi_isp_statistics_yuv_set_registers(&isp_ctx, &isp_config.statistics_yuv);
312  return 0;
313 }
314 
315 static inline void memcpy_to_registers(unsigned long addr,
316  const void *reg_base,
317  size_t s)
318 {
319  const uint32_t *reg = reg_base;
320  unsigned i;
321 
322  s /= sizeof(uint32_t); /* we write one register at a time */
323 
324  for (i = 0; i < s; i++) {
325  writel(reg[i], addr + i * sizeof(uint32_t));
326  }
327 }
328 
329 static inline void memcpy_from_registers(void *reg_base,
330  unsigned long addr,
331  size_t s)
332 {
333  uint32_t *reg = reg_base;
334  unsigned i;
335 
336  s /= sizeof(uint32_t); /* we read one register at a time */
337 
338  for (i = 0; i < s; i++) {
339  reg[i] = readl(addr + i * sizeof(uint32_t));
340  }
341 }
342 
343 #define EXPAND_AS_FUNCTION(_node) \
344  void avi_isp_ ## _node ## _set_registers(struct libisp_context *c, \
345  struct avi_isp_ ## _node ## _regs const *regs) \
346  { \
347  memcpy_to_registers(c->offsets[_node], regs, sizeof(*regs)); \
348  } \
349  \
350  void avi_isp_ ## _node ## _get_registers(struct libisp_context *c, \
351  struct avi_isp_ ## _node ## _regs *regs) \
352  { \
353  memcpy_from_registers(regs, c->offsets[_node], sizeof(*regs)); \
354  }
355 
#define AVI_ISP_DEAD_PIXEL_CORRECTION_LIST_MEM
#define AVI_ISP_DEAD_PIXEL_CORRECTION_CFA
#define AVI_ISP_EDGE_ENHANCEMENT_COLOR_REDUCTION_FILTER_EE_KERNEL_COEFF
#define AVI_ISP_EDGE_ENHANCEMENT_COLOR_REDUCTION_FILTER_EE_LUT
#define AVI_ISP_GAMMA_CORRECTOR_BV_LUT
#define AVI_ISP_GAMMA_CORRECTOR_RY_LUT
#define AVI_ISP_GAMMA_CORRECTOR_GU_LUT
#define AVI_ISP_GREEN_IMBALANCE_GREEN_RED_COEFF_MEM
#define AVI_ISP_GREEN_IMBALANCE_GREEN_BLUE_COEFF_MEM
#define AVI_ISP_I3D_LUT_CLIP_MODE
#define AVI_ISP_I3D_LUT_LUT_OUTSIDE
#define AVI_ISP_I3D_LUT_LUT_INSIDE
#define AVI_ISP_LENS_SHADING_CORRECTION_GREEN_COEFF_MEM
#define AVI_ISP_LENS_SHADING_CORRECTION_RED_COEFF_MEM
#define AVI_ISP_LENS_SHADING_CORRECTION_BLUE_COEFF_MEM
union avi_isp_statistics_yuv_ae_nb_valid_y ae_nb_valid_y
union avi_isp_statistics_yuv_increments_log2 increments_log2
union avi_isp_statistics_yuv_circle_pos_y_center circle_pos_y_center
union avi_isp_statistics_yuv_measure_status measure_status
union avi_isp_statistics_yuv_circle_pos_x_squared circle_pos_x_squared
union avi_isp_statistics_yuv_circle_pos_y_squared circle_pos_y_squared
union avi_isp_statistics_yuv_ae_histogram_y ae_histogram_y[256]
union avi_isp_statistics_yuv_awb_sum_v awb_sum_v
union avi_isp_statistics_yuv_circle_radius_squared circle_radius_squared
union avi_isp_statistics_yuv_awb_sum_y awb_sum_y
union avi_isp_statistics_yuv_awb_sum_u awb_sum_u
union avi_isp_statistics_yuv_circle_pos_x_center circle_pos_x_center
union avi_isp_statistics_yuv_awb_threshold awb_threshold
union avi_isp_statistics_yuv_window_pos_y window_pos_y
union avi_isp_statistics_yuv_measure_req measure_req
union avi_isp_statistics_yuv_awb_nb_grey_pixels awb_nb_grey_pixels
#define AVI_ISP_STATISTICS_YUV_AE_HISTOGRAM_Y
union avi_isp_statistics_yuv_window_pos_x window_pos_x
#define VERBOSE_PRINT(...)
Definition: libisp.c:19
#define EXPAND_AS_FUNCTION(_node)
Definition: libisp.c:343
static const unsigned isp_bases[]
Definition: libisp.c:51
static int open_isp_fd(struct libisp_context *ctx, int fd)
This is taken from libisp.
Definition: libisp.c:105
int configure_isp(struct v4l2_device *dev)
Definition: libisp.c:177
uint32_t chain_bayer
Definition: libisp.c:27
#define AVI_SIZE
Definition: libisp.c:23
static struct libisp_context isp_ctx
Definition: libisp.c:45
uint32_t chroma
Definition: libisp.c:29
uint16_t requestWindow[6]
Definition: libisp.c:49
int isp_get_statistics_yuv(struct isp_yuv_stats_t *yuv_stats)
Definition: libisp.c:261
#define AVI_ISP_IOGET_OFFSETS
Definition: libisp.c:38
static void memcpy_from_registers(void *reg_base, unsigned long addr, size_t s)
Definition: libisp.c:329
#define AVI_ISP_STAT_YUV_MAX_WAIT
Definition: libisp.c:34
static int isp_set_statistics_yuv_window(void)
Definition: libisp.c:230
uint8_t curWait
Definition: libisp.c:35
static int avi_isp_get_offsets_fd(int fd, struct avi_isp_offsets *off)
This is taken from libisp.
Definition: libisp.c:90
static void memcpy_to_registers(unsigned long addr, const void *reg_base, size_t s)
Definition: libisp.c:315
#define writel(_val, _addr)
Definition: libisp.c:42
int isp_request_statistics_yuv_window(uint16_t x_start, uint16_t x_end, uint16_t y_start, uint16_t y_end, uint16_t x_odd_inc, uint16_t y_odd_inc)
Definition: libisp.c:214
#define AVI_BASE
Definition: libisp.c:22
#define readl(_addr)
Definition: libisp.c:41
uint32_t chain_yuv
Definition: libisp.c:31
uint32_t gamma_corrector
Definition: libisp.c:28
uint32_t statistics_yuv
Definition: libisp.c:30
#define AVI_MASK
Definition: libisp.c:24
unsigned long avi_base
Definition: libisp.h:52
struct avi_isp_gamma_corrector_gu_lut_regs gc_gu_lut
Gamma corrector GU lut.
Definition: libisp.h:80
struct avi_isp_edge_enhancement_color_reduction_filter_ee_lut_regs eecrf_lut
Edge enhancement + Color correction lut.
Definition: libisp.h:85
struct avi_isp_lens_shading_correction_regs lens_shading_correction
Lens shade correction.
Definition: libisp.h:67
struct avi_isp_gamma_corrector_ry_lut_regs gc_ry_lut
Gamma corrector RY lut.
Definition: libisp.h:79
struct avi_isp_green_imbalance_green_red_coeff_mem_regs grim_gr
Green imbalance GR coefficients.
Definition: libisp.h:62
uint32_t awb_nb_grey_pixels
Definition: libisp.h:95
uint32_t ae_histogram_Y[256]
Definition: libisp.h:97
uint32_t awb_sum_Y
Definition: libisp.h:92
struct avi_isp_color_correction_regs color_correction
Color correction parameters.
Definition: libisp.h:76
struct avi_isp_lens_shading_correction_red_coeff_mem_regs lsc_red_coeffs
Lens shade correction red coefficients.
Definition: libisp.h:68
struct avi_isp_vlformat_40to32_regs vlformat_40to32
Conversion factor (10bit to 10bit default)
Definition: libisp.h:77
#define AVI_DEFINE_NODE(EXPANDER)
Definition: libisp.h:7
struct avi_isp_gamma_corrector_bv_lut_regs gc_bv_lut
Gamma corrector BV lut.
Definition: libisp.h:81
struct avi_isp_edge_enhancement_color_reduction_filter_regs eecrf
Edge enhancement + Color reduction.
Definition: libisp.h:84
struct avi_isp_statistics_yuv_regs statistics_yuv
YUV statistics parameters.
Definition: libisp.h:83
struct avi_isp_chain_yuv_inter_regs chain_yuv_inter
YUV chain bypass configuration (enable/disable features)
Definition: libisp.h:87
struct avi_isp_statistics_bayer_regs statistics_bayer
Statistics bayer parameters.
Definition: libisp.h:66
struct avi_isp_lens_shading_correction_blue_coeff_mem_regs lsc_blue_coeffs
Lens shade correction blue coefficients.
Definition: libisp.h:72
uint32_t awb_sum_V
Definition: libisp.h:94
struct avi_isp_gamma_corrector_regs gamma_corrector
Gamma corrector (Curves)
Definition: libisp.h:78
struct avi_isp_chroma_regs chroma
Color space conversion.
Definition: libisp.h:82
struct avi_isp_bayer_regs bayer
< Chromatic abberation (Disabled for now)
Definition: libisp.h:75
unsigned offsets[ISP_NODE_NR]
Definition: libisp.h:53
struct avi_isp_green_imbalance_regs green_imbalance
Green imbalance correction.
Definition: libisp.h:61
struct avi_isp_chain_bayer_inter_regs bayer_inter
Enable or disable bayer ISP functions by bypassing them.
Definition: libisp.h:59
@ ISP_NODE_NR
Definition: libisp.h:46
int devmem
Definition: libisp.h:51
struct avi_isp_dead_pixel_correction_regs dead_pixel_correction
Dead pixel correction (disabled)
Definition: libisp.h:64
struct avi_isp_lens_shading_correction_green_coeff_mem_regs lsc_green_coeffs
Lens shade correction green coefficients.
Definition: libisp.h:70
struct avi_isp_denoising_regs denoising
Denoising parameters.
Definition: libisp.h:65
struct avi_isp_green_imbalance_green_blue_coeff_mem_regs grim_gb
Green imbalance GB coefficients.
Definition: libisp.h:63
struct avi_isp_pedestal_regs pedestal
Pedestral parameters (substract from pixels)
Definition: libisp.h:60
struct avi_isp_vlformat_32to40_regs vlformat_32to40
Conversion factor (10bit to 10bit default)
Definition: libisp.h:58
uint32_t nb_valid_Y
Definition: libisp.h:96
uint32_t awb_sum_U
Definition: libisp.h:93
struct libisp_config isp_config
Definition: libisp_config.h:42
static uint32_t s
#define AVI_ISP_I3D_LUT
Definition: reg_avi.h:48
#define AVI_ISP_CHROMATIC_ABERRATION
Definition: reg_avi.h:40
#define AVI_ISP_VLFORMAT_40TO32
Definition: reg_avi.h:43
#define AVI_ISP_DEAD_PIXEL_CORRECTION
Definition: reg_avi.h:36
#define AVI_ISP_STATISTICS_BAYER
Definition: reg_avi.h:38
#define AVI_ISP_DROP
Definition: reg_avi.h:49
#define AVI_ISP_BAYER
Definition: reg_avi.h:41
#define AVI_ISP_VLFORMAT_32TO40
Definition: reg_avi.h:33
#define AVI_ISP_DENOISING
Definition: reg_avi.h:37
#define AVI_ISP_CHAIN_BAYER_INTER
Definition: reg_avi.h:32
#define AVI_ISP_LENS_SHADING_CORRECTION
Definition: reg_avi.h:39
#define AVI_ISP_COLOR_CORRECTION
Definition: reg_avi.h:42
#define AVI_ISP_GREEN_IMBALANCE
Definition: reg_avi.h:35
#define AVI_ISP_EDGE_ENHANCEMENT_COLOR_REDUCTION_FILTER
Definition: reg_avi.h:47
#define AVI_ISP_CHAIN_YUV_INTER
Definition: reg_avi.h:46
#define AVI_ISP_PEDESTAL
Definition: reg_avi.h:34
int fd
Definition: serial.c:26
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98