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