0abef91ca451bfadb33075f74bcf22b0e72f0a40
[barrelfish] / usr / drivers / omap44xx / fdif / fdif.c
1 /*
2  * Copyright (c) 2013, ETH Zurich.
3  * All rights reserved.
4  *
5  * This file is distributed under the terms in the attached LICENSE file.
6  * If you do not find this file, copies can be found by writing to:
7  * ETH Zurich D-INFK, CAB F.78, Universitaetstr 6, CH-8092 Zurich.
8  */
9
10 #include <stdio.h>
11 #include <stdlib.h>
12
13 #include <barrelfish/barrelfish.h>
14 #include <driverkit/driverkit.h>
15
16 #include <dev/omap/omap44xx_cam_prm_dev.h>
17 #include <dev/omap/omap44xx_cam_cm2_dev.h>
18 #include <dev/omap/omap44xx_fdif_dev.h>
19 #include <dev/omap/omap44xx_sr_mpu_dev.h>
20 #include <dev/omap/omap44xx_device_prm_dev.h>
21
22 #include "fdif.h"
23
24 #define PRINT_BUFFER_SIZE (1024*1024)
25 static char printbuf[PRINT_BUFFER_SIZE];
26
27 static omap44xx_cam_prm_t dev;
28 static omap44xx_fdif_t devfdif;
29 static omap44xx_cam_cm2_t devclk;
30 //static omap44xx_sr_mpu_t devvolt;
31 static omap44xx_device_prm_t devprm;
32
33 extern struct gimage lena_image;
34
35 // XXX: you need to have this functions in user space...
36 // not sure about these two, tough
37 //void cp15_invalidate_tlb(void);
38 //void cp15_invalidate_i_and_d_caches(void);
39
40 static lpaddr_t* vbase_glbl;
41
42 static void manage_clocks(void)
43 {
44     printf("Enable the clocks in domain CD_CAM\n");
45
46     // Clock domain CAM
47     lvaddr_t* vbase;
48     errval_t err;
49     err = map_device_register(0x4A009000, 4096, &vbase);
50     assert(err_is_ok(err));
51     FDIF_DEBUG("vbase points to %p\n", vbase);
52
53     omap44xx_cam_cm2_initialize(&devclk, (mackerel_addr_t)vbase);
54     //omap44xx_cam_cm2_pm_cam_pwrstctrl_powerstate_wrf(&dev, omap44xx_cam_prm_POWERSTATE_2);
55     omap44xx_cam_cm2_cm_cam_clkstctrl_clktrctrl_wrf(&devclk, 0x2);
56     omap44xx_cam_cm2_cm_cam_fdif_clkctrl_modulemode_wrf(&devclk, 0x2);
57
58     //omap44xx_cam_cm2_pr(printbuf, PRINT_BUFFER_SIZE, &devclk);
59     //printf("%s\n", printbuf);
60
61     //printf("Enable all the dependencies we can\n");
62     //omap44xx_cam_cm2_cm_cam_staticdep_l3_1_statdep_wrf(&devclk, 0x1);
63     //omap44xx_cam_cm2_cm_cam_staticdep_memif_statdep_wrf(&devclk, 0x1);
64     //omap44xx_cam_cm2_cm_cam_staticdep_ivahd_statdep_wrf(&devclk, 0x1);
65
66     //omap44xx_cam_cm2_pr(printbuf, PRINT_BUFFER_SIZE, &devclk);
67     //printf("%s\n", printbuf);
68
69
70     printf("Handle voltage for domain: VDD_CORE_L\n");
71     
72     // TODO access to smartreflex register not working, why?
73     //offset = (0x4A0DD000 & ARM_L1_SECTION_MASK);
74     //omap44xx_sr_mpu_initialize(&devvolt, (mackerel_addr_t)vbase+offset);
75     //omap44xx_sr_mpu_srstatus_pr(printbuf, PRINT_BUFFER_SIZE-1, &devvolt);
76     //printf("%s\n", printbuf);
77
78     err = map_device_register(0x4A307000, 4096, &vbase_glbl);
79     assert(err_is_ok(err));
80     lpaddr_t offset = (0x4A307B00 & 0xFFF);
81     omap44xx_device_prm_initialize(&devprm, (mackerel_addr_t)vbase_glbl+offset);
82     //omap44xx_device_prm_pr(printbuf, PRINT_BUFFER_SIZE, &devprm);
83     //printf("%s\n", printbuf);
84
85     // Init voltage controller
86     printf("Done handling voltage\n");
87 }
88
89 static void manage_power(void) 
90 {
91     printf("Power-on the PD_CAM domain for fdif\n");
92
93     // Power domain CAM
94     lvaddr_t* vbase;
95     errval_t err;
96     err = map_device_register(0x4A307000, 4096, &vbase);
97     assert(err_is_ok(err));
98
99     omap44xx_cam_prm_initialize(&dev, (mackerel_addr_t)vbase);
100     omap44xx_cam_prm_pm_cam_pwrstctrl_powerstate_wrf(&dev, omap44xx_cam_prm_POWERSTATE_3);
101
102     while(omap44xx_cam_prm_pm_cam_pwrstst_powerstatest_rdf(&dev)
103           != omap44xx_cam_prm_POWERSTATEST_3_r)
104     {}
105
106     //omap44xx_cam_prm_pr(printbuf, PRINT_BUFFER_SIZE, &dev);
107     //printf("%s\n", printbuf);
108
109     // Face detect Module
110     err = map_device_register(0x4A10A000, 4096, &vbase);
111     assert(err_is_ok(err));
112
113     omap44xx_fdif_initialize(&devfdif, (mackerel_addr_t)vbase);
114
115     // Set this to 0x1 to force the device off the standby mode
116     omap44xx_fdif_fdif_sysconfig_standbymode_wrf(&devfdif, 0x2);
117
118     omap44xx_fdif_fdif_sysconfig_pr(printbuf, PRINT_BUFFER_SIZE, &devfdif);
119     printf("%s\n", printbuf);
120
121     omap44xx_cam_cm2_pr(printbuf, PRINT_BUFFER_SIZE, &devclk);
122     printf("%s\n", printbuf);
123
124 }
125
126 int main(void) {
127     //init_memory_manager();
128
129     manage_clocks();
130     manage_power();
131     //manage_voltage();
132
133     printf("FDIF Global Initialization\n");
134     omap44xx_fdif_fdif_sysconfig_softreset_wrf(&devfdif, 1);
135     while (omap44xx_fdif_fdif_sysconfig_softreset_rdf(&devfdif) != 0);
136
137     omap44xx_fdif_fdif_ctrl_max_tags_wrf(&devfdif, 0xA);
138
139     omap44xx_fdif_fdif_irqenable_set_finish_irq_wrf(&devfdif, 0, 1);
140     omap44xx_fdif_fdif_irqenable_set_finish_irq_wrf(&devfdif, 1, 1);
141     omap44xx_fdif_fdif_irqenable_set_finish_irq_wrf(&devfdif, 2, 1);
142
143     printf("Polling Method\n");
144     printf("Set Image Parameters\n");
145
146     size_t img_size = 320*240*8; // 75 KB
147     size_t working_size = img_size; // 51.25 KB is enough
148     size_t retbytes;
149     void* workarea;
150     uint8_t* image;
151     errval_t err;
152
153     struct capref img_cap;
154     struct capref workarea_cap;
155
156     err = frame_alloc(&img_cap, img_size, &retbytes);
157     assert(err_is_ok(err));
158     assert(retbytes >= img_size);
159
160     err = frame_alloc(&workarea_cap, working_size, &retbytes);
161     assert(err_is_ok(err));
162     assert(retbytes >= working_size);
163
164     err = vspace_map_one_frame((void**)&image, img_size, img_cap,
165                               NULL, NULL);
166     assert(err_is_ok(err));
167     err = vspace_map_one_frame(&workarea, working_size, workarea_cap,
168                               NULL, NULL);
169     assert(err_is_ok(err));
170
171     printf("Set up the image ...\n");
172     printf("RGB image type: width:%d height:%d bytes/pixel:%d\n", lena_image.width, lena_image.height, lena_image.bytes_per_pixel);
173     printf("Convert to grayscale, store in image buffer...\n");
174
175     for (int i=0; i<lena_image.width*lena_image.height; i+=1) {
176         image[i] = lena_image.pixel_data[i];
177     }
178
179     // TODO We should make sure here that image is actually fully in memory
180     // and not still hanging around in the cache
181
182     // Does this do cache to mem writeback?
183     //cp15_invalidate_tlb();
184     //cp15_invalidate_i_and_d_caches();
185     struct frame_identity ret;
186     err = invoke_frame_identify(img_cap, &ret);
187     assert (err_is_ok(err));
188     omap44xx_fdif_fdif_picaddr_wr(&devfdif, ret.base); // make sure 5 least significant bits are 0!
189
190     err = invoke_frame_identify(workarea_cap, &ret);
191     assert (err_is_ok(err));
192     omap44xx_fdif_fdif_wkaddr_wr(&devfdif, ret.base); // make sure 5 least significant bits are 0!
193     
194     omap44xx_fdif_fd_dcond_min_wrf(&devfdif, 0x0); // 40 pixel
195     omap44xx_fdif_fd_dcond_dir_wrf(&devfdif, 0x0); // up?
196
197     omap44xx_fdif_fd_startx_startx_wrf(&devfdif, 0x0);
198     omap44xx_fdif_fd_starty_starty_wrf(&devfdif, 0x0);
199
200     omap44xx_fdif_fd_sizex_sizex_wrf(&devfdif, 0x140); // TODO
201     omap44xx_fdif_fd_sizey_sizey_wrf(&devfdif, 0xf0); // TODO
202     omap44xx_fdif_fd_lhit_lhit_wrf(&devfdif, 0x5);
203
204     omap44xx_fdif_fd_ctrl_run_wrf(&devfdif, 0x1);
205
206     printf("%s:%d: Waiting until fdif is done...\n", __FUNCTION__, __LINE__);
207     while(omap44xx_fdif_fd_ctrl_finish_rdf(&devfdif) != 0x1);
208
209     printf("Face detection completed:\n");
210     printf("Read the results...\n");
211     
212     int faces = omap44xx_fdif_fd_dnum_dnum_rdf(&devfdif);
213     printf("Faces found: %d\n", faces);
214     //omap44xx_fdif_pr(printbuf, PRINT_BUFFER_SIZE, &devfdif);
215     //printf("%s\n", printbuf);
216
217     for (int i=0; i<faces; i++) {
218         printf("Face %d:\n", i);
219         int x = omap44xx_fdif_fd_centerx_centerx_rdf(&devfdif, i);
220         int y = omap44xx_fdif_fd_centery_centery_rdf(&devfdif, i);
221         printf("Position (X,Y): %d %d\n", x, y);
222
223         int size = omap44xx_fdif_fd_confsize_size_rdf(&devfdif, i);
224         int confidence = omap44xx_fdif_fd_confsize_conf_rdf(&devfdif, i);
225         int angle = omap44xx_fdif_fd_angle_angle_rdf(&devfdif, i);
226         printf("Size: %d Confidence: %d Angle: %d\n", size, confidence, angle);
227     }
228
229     return 0;
230 }