1/*
2 * \brief Driver for face detection on OMAP 4460.
3 *
4 * \see OMAP TRM rev Z
5 */
6/*
7 * Copyright (c) 2013, ETH Zurich.
8 * All rights reserved.
9 *
10 * This file is distributed under the terms in the attached LICENSE file.
11 * If you do not find this file, copies can be found by writing to:
12 * ETH Zurich D-INFK, CAB F.78, Universitaetstr 6, CH-8092 Zurich.
13 */
14
15#include <stdio.h>
16#include <stdlib.h>
17#include <string.h>
18#include <assert.h>
19
20#include <barrelfish/barrelfish.h>
21#include <driverkit/driverkit.h>
22
23
24#include <barrelfish/barrelfish.h>
25#include <barrelfish/waitset.h>
26#include <barrelfish/inthandler.h>
27#include <driverkit/driverkit.h>
28
29#include "fdif.h"
30
31#include <dev/omap/omap44xx_cam_prm_dev.h>
32#include <dev/omap/omap44xx_cam_cm2_dev.h>
33#include <dev/omap/omap44xx_fdif_dev.h>
34
35#define FDIF_IRQ (32+69)
36#define PRINT_BUFFER_SIZE (1024*1024)
37
38extern struct gimage lena_image;
39
40struct fdif_driver_state {
41    uint32_t level;
42    omap44xx_cam_prm_t dev;
43    omap44xx_fdif_t devfdif;
44    omap44xx_cam_cm2_t devclk;
45    char printbuf[PRINT_BUFFER_SIZE];
46};
47
48static void manage_clocks(struct fdif_driver_state* st, struct capref caps[])
49{
50    FDIF_DEBUG("Enable the clocks in domain CD_CAM\n");
51
52    // Clock domain CAM
53    lvaddr_t vbase;
54    errval_t err;
55    err = map_device_cap(caps[0], &vbase); // 0x4A009000, 4096
56    assert(err_is_ok(err));
57
58    omap44xx_cam_cm2_initialize(&st->devclk, (mackerel_addr_t)vbase);
59    //omap44xx_cam_cm2_cm_cam_staticdep_l3_1_statdep_wrf(&st->devclk, 0x1);
60    //omap44xx_cam_cm2_cm_cam_staticdep_memif_statdep_wrf(&st->devclk, 0x1);
61    //omap44xx_cam_cm2_cm_cam_staticdep_ivahd_statdep_wrf(&st->devclk, 0x1);
62
63    // Explicit enable && Force SW wakeup
64    omap44xx_cam_cm2_cm_cam_fdif_clkctrl_modulemode_wrf(&st->devclk, 0x2);
65    omap44xx_cam_cm2_cm_cam_clkstctrl_clktrctrl_wrf(&st->devclk, 0x2);
66}
67
68static void manage_power(struct fdif_driver_state* st, struct capref caps[])
69{
70    FDIF_DEBUG("Power-on the PD_CAM domain for fdif\n");
71
72    // Power domain CAM
73    lvaddr_t vbase;
74    errval_t err;
75    err = map_device_cap(caps[2], &vbase); // 0x4A307000, 4096
76    assert(err_is_ok(err));
77
78    omap44xx_cam_prm_initialize(&st->dev, (mackerel_addr_t)vbase);
79    omap44xx_cam_prm_pm_cam_pwrstctrl_powerstate_wrf(&st->dev, 0x3);
80
81    FDIF_DEBUG("%s:%d: Power OFF -> ON\n", __FUNCTION__, __LINE__);
82    while (omap44xx_cam_prm_pm_cam_pwrstst_powerstatest_rdf(&st->dev)
83            != 0x3);
84    FDIF_DEBUG("%s:%d: Power is on.\n", __FUNCTION__, __LINE__);
85}
86
87static void read_result(struct fdif_driver_state* st)
88{
89    printf("Face detection completed:\n");
90    printf("Read the results...\n");
91
92    int faces = omap44xx_fdif_fd_dnum_dnum_rdf(&st->devfdif);
93    printf("Faces found: %d\n", faces);
94    //omap44xx_fdif_pr(st->printbuf, PRINT_BUFFER_SIZE, &st->devfdif);
95    //printf("%s\n", st->printbuf);
96
97    for (int i = 0; i < faces; i++) {
98        printf("Face %d:\n", i);
99        int x = omap44xx_fdif_fd_centerx_centerx_rdf(&st->devfdif, i);
100        int y = omap44xx_fdif_fd_centery_centery_rdf(&st->devfdif, i);
101        printf("Position (X,Y): %d %d\n", x, y);
102
103        int size = omap44xx_fdif_fd_confsize_size_rdf(&st->devfdif, i);
104        int confidence = omap44xx_fdif_fd_confsize_conf_rdf(&st->devfdif, i);
105        int angle = omap44xx_fdif_fd_angle_angle_rdf(&st->devfdif, i);
106        printf("Size: %d Confidence: %d Angle: %d\n", size, confidence, angle);
107    }
108}
109
110/*
111 * \brief Interrupt handler for "finish" interrupt
112 */
113static void irq_handler(void *args)
114{
115    struct fdif_driver_state* st = (struct fdif_driver_state*) args;
116    read_result(st);
117
118    omap44xx_fdif_fdif_ctrl_pr(st->printbuf, PRINT_BUFFER_SIZE, &st->devfdif);
119    FDIF_DEBUG("%s\n", st->printbuf);
120
121    omap44xx_cam_cm2_pr(st->printbuf, PRINT_BUFFER_SIZE, &st->devclk);
122    FDIF_DEBUG("%s\n", st->printbuf);
123
124    omap44xx_cam_prm_pr(st->printbuf, PRINT_BUFFER_SIZE, &st->dev);
125    FDIF_DEBUG("%s\n", st->printbuf);
126
127
128    omap44xx_fdif_fdif_irqstatus_finish_irq_wrf(&st->devfdif, 2, 1);
129
130    // Go in Standby Mode
131    // Again, module seems to go in standby just fine after its done
132    // processing.
133    //printf("%s:%d: go in standby\n", __FUNCTION__, __LINE__);
134    //omap44xx_fdif_fdif_ctrl_mstandby_wrf(&st->devfdif, 0x1);
135    //while(omap44xx_fdif_fdif_ctrl_mstandby_hdshk_rdf(&st->devfdif) != 0x0);
136
137    // Disable Module Clocks
138    omap44xx_cam_cm2_cm_cam_clkstctrl_clktrctrl_wrf(&st->devclk, 0x1);
139    omap44xx_cam_cm2_cm_cam_fdif_clkctrl_modulemode_wrf(&st->devclk, 0x0);
140
141    // Going Powermode ON-INACTIVE
142    omap44xx_cam_prm_pm_cam_pwrstctrl_powerstate_wrf(&st->dev, 0x2);
143    FDIF_DEBUG("%s:%d: Power ON -> ON-INACTIVE\n", __FUNCTION__, __LINE__);
144    while (omap44xx_cam_prm_pm_cam_pwrstst_powerstatest_rdf(&st->dev)
145            != 0x2);
146
147    // Going Powermode ON-INACTIVE -> OFF
148    // Must use lowpoerstatechange for that
149    FDIF_DEBUG("%s:%d: Power ON-INACTIVE -> OFF\n", __FUNCTION__, __LINE__);
150    omap44xx_cam_prm_pm_cam_pwrstctrl_powerstate_wrf(&st->dev, 0x0);
151    omap44xx_cam_prm_pm_cam_pwrstctrl_lowpowerstatechange_wrf(&st->dev, 0x1);
152    while (omap44xx_cam_prm_pm_cam_pwrstctrl_lowpowerstatechange_rdf(&st->dev)
153            != 0x0);
154
155    omap44xx_cam_cm2_pr(st->printbuf, PRINT_BUFFER_SIZE, &st->devclk);
156    FDIF_DEBUG("%s\n", st->printbuf);
157
158    omap44xx_cam_prm_pr(st->printbuf, PRINT_BUFFER_SIZE, &st->dev);
159    FDIF_DEBUG("%s\n", st->printbuf);
160}
161
162/*
163 * \brief Enable in poll-based mode
164 *
165 * \see OMAP TRM 9.4.1.2.1.1 Main Sequence ��� FDIF Polling Method
166 */
167static void __attribute__((__unused__)) enable_poll_mode(struct fdif_driver_state* st)
168{
169    omap44xx_fdif_fd_ctrl_run_wrf(&st->devfdif, 0x1);
170
171    FDIF_DEBUG("%s:%d: Waiting until fdif is done...\n", __FUNCTION__, __LINE__);
172    while (omap44xx_fdif_fd_ctrl_finish_rdf(&st->devfdif) != 0x1);
173
174    read_result(st);
175}
176
177
178/*
179 * \brief Enable in interrupt-based mode
180 *
181 * See OMAP TRM 9.4.1.2.1.2 Main Sequence ��� FDIF Interrupt Method
182 */
183static void enable_irq_mode(struct fdif_driver_state* st)
184{
185    errval_t err;
186
187    omap44xx_fdif_fdif_irqenable_set_finish_irq_wrf(&st->devfdif, 2, 1);
188
189    // Register interrupt handler in kernel
190    err = inthandler_setup_arm(irq_handler, st, FDIF_IRQ);
191    assert(err_is_ok(err));
192
193    omap44xx_fdif_fd_ctrl_run_wrf(&st->devfdif, 0x1);
194}
195
196/*
197 * \brief Set image parameters
198 *
199 * See OMAP TRM 9.4.1.2.1.3 Subsequence ��� Set Image Parameters
200 */
201static void set_image_params(struct fdif_driver_state* st, genpaddr_t picaddr, genpaddr_t wkaddr)
202{
203    // make sure 5 least significant bits are 0!
204    omap44xx_fdif_fdif_picaddr_wr(&st->devfdif, picaddr);
205    omap44xx_fdif_fdif_wkaddr_wr(&st->devfdif, wkaddr);
206
207    omap44xx_fdif_fd_dcond_min_wrf(&st->devfdif, 0x0); // 40 pixel
208    omap44xx_fdif_fd_dcond_dir_wrf(&st->devfdif, 0x0); // up?
209
210    omap44xx_fdif_fd_startx_startx_wrf(&st->devfdif, 0x0);
211    omap44xx_fdif_fd_starty_starty_wrf(&st->devfdif, 0x0);
212
213    omap44xx_fdif_fd_sizex_sizex_wrf(&st->devfdif, 0x140); // TODO
214    omap44xx_fdif_fd_sizey_sizey_wrf(&st->devfdif, 0xf0); // TODO
215    omap44xx_fdif_fd_lhit_lhit_wrf(&st->devfdif, 0x5);
216}
217
218/**
219 * Driver initialization function. This function is called by the driver domain
220 * (see also 'create_handler' in ddomain_service.c).
221 * Typically through a request from the device manager.
222 *
223 * The init function is supposed to set `dev` to the exported service iref.
224 * The init function may use `bfi->dstate` to store additional state about the device.
225 *
226 * \param[in]   bfi   The instance of this driver.
227 * \param[in]   name  The name of this driver instance.
228 * \param[in]   flags Additional flags (The exact flags supported is device/driver specific).
229 * \param[in]   c     Capabilities (for registers etc.) as provided by the device manager.
230 *                    The exact layout of the `c` is device specific.
231 * \param[out]  dev   The service iref over which the device can be contacted.
232 *
233 * \retval SYS_ERR_OK Device initialized successfully.
234 * \retval LIB_ERR_MALLOC_FAIL Unable to allocate memory for the driver.
235 */
236static errval_t init(struct bfdriver_instance* bfi, const char* name, uint64_t flags,
237                     struct capref* caps, size_t caps_len, char** args, size_t args_len, iref_t* dev) {
238    FDIF_DEBUG("%s:%s:%d: %s\n", __FILE__, __FUNCTION__, __LINE__, bfi->driver->name);
239
240    bfi->dstate = malloc(sizeof(struct fdif_driver_state));
241    if (bfi->dstate == NULL) {
242        return LIB_ERR_MALLOC_FAIL;
243    }
244    assert(bfi->dstate != NULL);
245
246    struct fdif_driver_state* st = (struct fdif_driver_state*)bfi->dstate;
247
248    // 1. Initialize the device:
249    size_t img_size = 320 * 240 * 8; // 75 KB
250    size_t working_size = img_size; // 51.25 KB is enough
251    size_t retbytes;
252    void *workarea;
253    uint8_t *image;
254    errval_t err;
255    lpaddr_t vbase;
256
257    // Face detect Module
258    err = map_device_cap(caps[3], &vbase);
259    assert(err_is_ok(err));
260    omap44xx_fdif_initialize(&st->devfdif, (mackerel_addr_t)vbase);
261
262    FDIF_DEBUG("FDIF Global Initialization\n");
263
264    manage_clocks(st, caps);
265    manage_power(st, caps);
266
267    omap44xx_fdif_fdif_sysconfig_softreset_wrf(&st->devfdif, 1);
268    while (omap44xx_fdif_fdif_sysconfig_softreset_rdf(&st->devfdif) != 0);
269
270    omap44xx_fdif_fdif_sysconfig_pr(st->printbuf, PRINT_BUFFER_SIZE, &st->devfdif);
271    FDIF_DEBUG("%s\n", st->printbuf);
272
273    omap44xx_fdif_fdif_sysconfig_idlemode_wrf(&st->devfdif, 0x2);
274    omap44xx_fdif_fdif_sysconfig_standbymode_wrf(&st->devfdif, 0x2);
275
276    omap44xx_fdif_fdif_sysconfig_pr(st->printbuf, PRINT_BUFFER_SIZE, &st->devfdif);
277    FDIF_DEBUG("%s\n", st->printbuf);
278
279    omap44xx_fdif_fdif_ctrl_max_tags_wrf(&st->devfdif, 0xA);
280
281    struct capref img_cap;
282    struct capref workarea_cap;
283
284    err = frame_alloc(&img_cap, img_size, &retbytes);
285    assert(err_is_ok(err));
286    assert(retbytes >= img_size);
287
288    err = frame_alloc(&workarea_cap, working_size, &retbytes);
289    assert(err_is_ok(err));
290    assert(retbytes >= working_size);
291
292    // Map space for image (as non-cacheable)
293    err = vspace_map_one_frame_attr((void **)&image, img_size, img_cap,
294                                    VREGION_FLAGS_READ_WRITE_NOCACHE,
295                                    NULL, NULL);
296    assert(err_is_ok(err));
297    err = vspace_map_one_frame(&workarea, working_size, workarea_cap,
298                               NULL, NULL);
299    assert(err_is_ok(err));
300
301    FDIF_DEBUG("Image: width:%d height:%d bytes/pixel:%d\n",
302               lena_image.width, lena_image.height, lena_image.bytes_per_pixel);
303    for (int i = 0; i < lena_image.width * lena_image.height; i += 1) {
304        image[i] = lena_image.pixel_data[i];
305    }
306
307    struct frame_identity ret;
308    err = invoke_frame_identify(img_cap, &ret);
309    assert (err_is_ok(err));
310
311    struct frame_identity wkret;
312    err = invoke_frame_identify(workarea_cap, &wkret);
313    assert (err_is_ok(err));
314
315    set_image_params(st, ret.base, wkret.base);
316
317    // The following will set cm_cam_fdif_clkctrl_stdbysy to 0x0 (not in standby)
318    // but it is apparently not needed (because we are in smart standby and
319    // wake-up with magic as soon as we need it...?)
320    //printf("%s:%d: // Wake up from standby\n", __FUNCTION__, __LINE__);
321    //omap44xx_fdif_fdif_ctrl_mstandby_wrf(&st->devfdif, 0x0);
322    //while(omap44xx_fdif_fdif_ctrl_mstandby_hdshk_rdf(&st->devfdif) != 0x1);
323    enable_irq_mode(st);
324
325    // 2. Export service to talk to the device:
326
327    // 3. Set iref of your exported service (this is reported back to Kaluga)
328
329    return SYS_ERR_OK;
330}
331
332/**
333 * Instructs driver to attach to the device.
334 * This function is only called if the driver has previously detached
335 * from the device (see also detach).
336 *
337 * \note After detachment the driver can not assume anything about the
338 * configuration of the device.
339 *
340 * \param[in]   bfi   The instance of this driver.
341 * \retval SYS_ERR_OK Device initialized successfully.
342 */
343static errval_t attach(struct bfdriver_instance* bfi) {
344    FDIF_DEBUG("%s:%s:%d: %s\n", __FILE__, __FUNCTION__, __LINE__, bfi->driver->name);
345
346    return SYS_ERR_OK;
347}
348
349/**
350 * Instructs driver to detach from the device.
351 * The driver must yield any control over to the device after this function returns.
352 * The device may be left in any state.
353 *
354 * \param[in]   bfi   The instance of this driver.
355 * \retval SYS_ERR_OK Device initialized successfully.
356 */
357static errval_t detach(struct bfdriver_instance* bfi) {
358    FDIF_DEBUG("%s:%s:%d: %s\n", __FILE__, __FUNCTION__, __LINE__, bfi->driver->name);
359
360    return SYS_ERR_OK;
361}
362
363/**
364 * Instructs the driver to go in a particular sleep state.
365 * Supported states are platform/device specific.
366 *
367 * \param[in]   bfi   The instance of this driver.
368 * \retval SYS_ERR_OK Device initialized successfully.
369 */
370static errval_t set_sleep_level(struct bfdriver_instance* bfi, uint32_t level) {
371    FDIF_DEBUG("%s:%s:%d: %s\n", __FILE__, __FUNCTION__, __LINE__, bfi->driver->name);
372
373    struct fdif_driver_state* uds = bfi->dstate;
374    uds->level = level;
375
376    return SYS_ERR_OK;
377}
378
379/**
380 * Destroys this driver instance.
381 *
382 * \param[in]   bfi   The instance of this driver.
383 * \retval SYS_ERR_OK Device initialized successfully.
384 */
385static errval_t destroy(struct bfdriver_instance* bfi) {
386    FDIF_DEBUG("%s:%s:%d: %s\n", __FILE__, __FUNCTION__, __LINE__, bfi->driver->name);
387    struct fdif_driver_state* uds = bfi->dstate;
388    free(uds);
389    bfi->dstate = NULL;
390
391    // XXX: Tear-down the service
392    bfi->device = 0x0;
393
394    return SYS_ERR_OK;
395}
396
397DEFINE_MODULE(fdif, init, attach, detach, set_sleep_level, destroy);
398