← Back to branch summary

~drgeo-developers/drgeo/trunk

« back to all changes in this revision

Viewing changes to VMs/iPad/source/unix/plugins/CameraPlugin/sqCamera.c

  • Committer: Hilaire Fernandes
  • Date: 2012-01-27 21:15:40 UTC
  • Revision ID: hilaire.fernandes@gmail.com-20120127211540-912spf97bhpx6mve
Initial additions

Show diffs side-by-side

added added

removed removed

Lines of Context:
VMs/iPad/source/unix/plugins/CameraPlugin/sqCamera.c
 
1
/*
 
2
 *  V4L2 for Scratch (Derek O'Connell, 2009)
 
3
 *
 
4
 *  This code can be used and distributed without restrictions.
 
5
 *
 
6
 */
 
7
 
 
8
#include "sq.h"
 
9
 
 
10
#include <stdio.h>
 
11
#include <stdlib.h>
 
12
#include <string.h>
 
13
#include <assert.h>
 
14
 
 
15
#include <getopt.h>   /* getopt_long() */
 
16
 
 
17
#include <fcntl.h>    /* low-level i/o */
 
18
#include <unistd.h>
 
19
#include <errno.h>
 
20
#include <malloc.h>
 
21
#include <sys/stat.h>
 
22
#include <sys/types.h>
 
23
#include <sys/time.h>
 
24
#include <sys/mman.h>
 
25
#include <sys/ioctl.h>
 
26
#include <dlfcn.h>
 
27
 
 
28
#include <asm/types.h>    /* for videodev2.h */
 
29
 
 
30
#include <linux/videodev2.h>
 
31
 
 
32
/* >>>>>>>>>>>>>>>>>>>>>>>>>>>>>> */
 
33
/*
 
34
#define USE_LIBV4L2x
 
35
#ifdef USE_LIBV4L2
 
36
#include <libv4l2.h>
 
37
#endif
 
38
*/
 
39
 
 
40
void *hLibv4l2 = NULL;
 
41
 
 
42
int (*vd_open)(const char *, int, ...);
 
43
int (*vd_close)(int);
 
44
int (*vd_dup)(int);
 
45
int (*vd_ioctl)(int, unsigned long int, ...);
 
46
ssize_t (*vd_read)(int, void *, size_t);
 
47
void * (*vd_mmap)(void *, size_t, int, int, int, int64_t);
 
48
int (*vd_munmap)(void *, size_t);
 
49
 
 
50
/* <<<<<<<<<<<<<<<<<<<<<<<<<<<<<< */
 
51
 
 
52
 
 
53
#define CLEAR(x) memset (&(x), 0, sizeof (x))
 
54
 
 
55
typedef enum {
 
56
        IO_METHOD_READ,
 
57
        IO_METHOD_MMAP,
 
58
        IO_METHOD_USERPTR,
 
59
} io_method;
 
60
 
 
61
struct buffer {
 
62
        void *  start;
 
63
        size_t  length;
 
64
};
 
65
 
 
66
 
 
67
struct camInfo_t {
 
68
        int devNum;
 
69
        int     fileDesc;
 
70
        int bmWidth, bmHeight;
 
71
        
 
72
        io_method ioMethod;
 
73
        int pixelformat;
 
74
        struct buffer * buffers;
 
75
        unsigned int nBuffers;
 
76
 
 
77
        struct v4l2_buffer read_buf;
 
78
 
 
79
        void *sqBuffer;
 
80
        long sqBufferBytes;
 
81
        long sqPixels;
 
82
 
 
83
        long frameCount;
 
84
} camInfo[10];
 
85
 
 
86
typedef struct camInfo_t *camPtr;
 
87
 
 
88
static char * videoDevName0 = "/dev/video0";
 
89
 
 
90
/* ================================== FUNCTION PROTOTYPES */
 
91
 
 
92
/* LIBRARY CONSTRUCTOR/DESCTRUCTOR */
 
93
 
 
94
void __attribute__ ((constructor)) libCon(void);
 
95
void __attribute__ ((destructor)) libDes(void);
 
96
 
 
97
 
 
98
/* UTILITY */
 
99
 
 
100
inline int camIsOpen(  camPtr cam) { return (-1 != cam->fileDesc); }
 
101
inline int camIsClosed(camPtr cam) { return (-1 == cam->fileDesc); }
 
102
 
 
103
 
 
104
/* V4L ACCESS */
 
105
 
 
106
 
 
107
/* SQUEAK INTERFACE */
 
108
 
 
109
sqInt CameraGetParam(int camNum, int paramNum);
 
110
sqInt CameraGetFrame(int camNum, unsigned char* buf, int pixelCount);
 
111
sqInt CameraExtent(int camNum);
 
112
char* CameraName(int camNum);
 
113
void CameraClose(int camNum);
 
114
sqInt CameraOpen(int camNum, int frameWidth, int frameHeight);
 
115
 
 
116
 
 
117
/* ================================== ??? */
 
118
 
 
119
 
 
120
/* LIBRARY CONSTRUCTOR/DESCTRUCTOR */
 
121
 
 
122
void __attribute__ ((constructor)) 
 
123
libCon(void)
 
124
{
 
125
  int devNum;
 
126
  camPtr cam;
 
127
 
 
128
  vd_open = open;
 
129
  vd_close = close;
 
130
  vd_dup = dup;
 
131
  vd_ioctl = ioctl;
 
132
  vd_read = read;
 
133
  vd_mmap = mmap;
 
134
  vd_munmap = munmap;
 
135
 
 
136
/*  printf("libv4l2: use if available...");
 
137
*/
 
138
hLibv4l2 = dlopen("libv4l2.so", RTLD_LAZY);
 
139
  if (hLibv4l2) 
 
140
  {
 
141
/*        printf("yay!\n");
 
142
*/  
 
143
          vd_open       = dlsym(hLibv4l2, "v4l2_open");
 
144
          vd_close      = dlsym(hLibv4l2, "v4l2_close");
 
145
          vd_dup        = dlsym(hLibv4l2, "v4l2_dup");
 
146
          vd_ioctl      = dlsym(hLibv4l2, "v4l2_ioctl");
 
147
          vd_read       = dlsym(hLibv4l2, "v4l2_read");
 
148
          vd_mmap       = dlsym(hLibv4l2, "v4l2_mmap");
 
149
          vd_munmap = dlsym(hLibv4l2, "v4l2_munmap");
 
150
  } else {
 
151
/*        printf("nay, %s\n", dlerror());
 
152
*/
 
153
  }
 
154
 
 
155
  for (devNum = 0; devNum < 10; ++devNum) {
 
156
        cam = &camInfo[devNum];
 
157
 
 
158
        CLEAR(*cam);
 
159
 
 
160
        cam->devNum                             = devNum;
 
161
        cam->fileDesc                   = -1;
 
162
        cam->ioMethod                   = IO_METHOD_MMAP;
 
163
        cam->read_buf.type              = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
164
        cam->read_buf.memory    = V4L2_MEMORY_MMAP;
 
165
        cam->nBuffers                   = 1;
 
166
 
 
167
/*      Pixel format now auto selected according to ease/speed of conversion
 
168
        cam->pixelformat                = V4L2_PIX_FMT_YUYV;
 
169
        cam->pixelformat                = V4L2_PIX_FMT_RGB24;
 
170
*/
 
171
 
 
172
/*
 
173
        cam->fileDesc = 0;
 
174
        cam->bmWidth = 0;
 
175
        cam->bmHeight = 0;
 
176
        cam->buffers = NULL;
 
177
        cam->nBuffers = 0;
 
178
        cam->read_buf = NULL;
 
179
        cam->sqBuffer = 0;
 
180
        cam->sqBufferBytes = 0;
 
181
        cam->sqPixels = 0;
 
182
        cam->frameCount = 0;
 
183
*/
 
184
  }
 
185
}
 
186
 
 
187
void __attribute__ ((destructor)) 
 
188
libDes(void)
 
189
{
 
190
  int camNum;
 
191
  for (camNum = 1; camNum < 11; ++camNum)
 
192
          CameraClose(camNum);
 
193
/*  
 
194
  if (hLibv4l2)
 
195
          dlclose(hLibv4l2);
 
196
*/
 
197
}
 
198
 
 
199
 
 
200
/* V4L ACCESS */
 
201
 
 
202
static int 
 
203
xioctl (camPtr cam, int request, void * arg) 
 
204
{
 
205
        int r;
 
206
 
 
207
        do r = vd_ioctl (cam->fileDesc, request, arg);
 
208
          while (-1 == r && EINTR == errno);
 
209
 
 
210
        return r;
 
211
}
 
212
 
 
213
 
 
214
/* >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
 
215
  from: palettes.c in VideoForLinuxPlugin
 
216
  from: http://en.wikipedia.org/wiki/YUV422
 
217
 
 
218
  Originally (here) a quick hack for XO-1 but libv4l
 
219
  version worked anyway.
 
220
  >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> */
 
221
 
 
222
inline unsigned char clipPixel(const int pixel) {
 
223
    int result;
 
224
 
 
225
    result = ((pixel < 0) ? 0 : pixel);
 
226
    return (unsigned char) ((result > 255) ? 255: result);
 
227
}
 
228
 
 
229
inline void 
 
230
convertPixelYUV444toARGB32(
 
231
                           const unsigned char y,
 
232
               const unsigned char u,
 
233
               const unsigned char v,
 
234
               unsigned char* dest)
 
235
{
 
236
    const int C = (y - 16) * 298 + 128;
 
237
    const int D = u - 128;
 
238
    const int E = v - 128;
 
239
 
 
240
    /* ARGB */
 
241
    dest[0] = clipPixel(( C + 516 * D          ) >> 8);
 
242
    dest[1] = clipPixel(( C - 100 * D - 208 * E) >> 8);
 
243
    dest[2] = clipPixel(( C           + 409 * E) >> 8);
 
244
    dest[3] = 255;
 
245
 
 
246
}
 
247
 
 
248
inline void 
 
249
convertImageYUYVToARGB32 (camPtr cam, int bufIdx)
 
250
{
 
251
        int i;
 
252
 
 
253
        const unsigned char* src = cam->buffers[bufIdx].start;
 
254
        unsigned char* dst = cam->sqBuffer;
 
255
        unsigned long int *pdst;
 
256
        unsigned long int pixelCount = cam->sqPixels;
 
257
 
 
258
        unsigned char u, y1, v, y2;
 
259
 
 
260
        for (i = 0; i < pixelCount; i += 2) {
 
261
                y1 = *src++;
 
262
                u  = *src++;
 
263
                y2 = *src++;
 
264
                v  = *src++;
 
265
 
 
266
                convertPixelYUV444toARGB32(y1, u, v, dst);
 
267
                pdst = (unsigned long *)dst;
 
268
                dst += 4;
 
269
 
 
270
                if (y2 == y1)
 
271
                  *(unsigned long *)dst = *pdst;
 
272
                else
 
273
                  convertPixelYUV444toARGB32(y2, u, v, dst);
 
274
                
 
275
                dst += 4;
 
276
        }
 
277
}
 
278
 
 
279
/* <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< */
 
280
 
 
281
static void 
 
282
convertImageRGB24toARGB32 (camPtr cam, int bufIdx) /* const void *src, const void *dst) */
 
283
{
 
284
        unsigned char     *src = cam->buffers[bufIdx].start;
 
285
        unsigned long int *dst = cam->sqBuffer;
 
286
        unsigned long int pixelCount = cam->sqPixels;
 
287
        unsigned long int pixel;
 
288
        int i;
 
289
 
 
290
        if (0 == dst) return;
 
291
 
 
292
        for ( i = 0; i < pixelCount; i++) {
 
293
                pixel = 0xFF000000 | (*src++ << 16);
 
294
                pixel = pixel | (*src++ << 8);
 
295
                *dst++  = pixel | *src++;
 
296
        }
 
297
}
 
298
 
 
299
 
 
300
static void 
 
301
convertImageRGB444toARGB32 (camPtr cam, int bufIdx) /* const void *src, const void *dst) */
 
302
{
 
303
        unsigned char     *src = cam->buffers[bufIdx].start;
 
304
        unsigned long int *dst = cam->sqBuffer;
 
305
        unsigned long int pixelCount = cam->sqPixels;
 
306
        unsigned long int r,g,b,pixel;
 
307
        int i;
 
308
 
 
309
        if (0 == dst) return;
 
310
 
 
311
        /* Byte0: (g)ggg(b)bbb, Byte1: xxxx(r)rrr */
 
312
 
 
313
        for ( i = 0; i < pixelCount; i++) {
 
314
          r = *src << 4;
 
315
          g = *src++ & 0xF0;
 
316
          b = (*src++ & 0x0F) << 4;
 
317
          pixel = 0xFF000000;
 
318
          pixel |= (r << 16);
 
319
          pixel |= (g <<  8);
 
320
          pixel |= b;
 
321
          *dst++ = pixel;
 
322
        }
 
323
}
 
324
 
 
325
 
 
326
static void 
 
327
convertImageRGB565toARGB32 (camPtr cam, int bufIdx) /* const void *src, const void *dst) */
 
328
{
 
329
        unsigned char     *src = cam->buffers[bufIdx].start;
 
330
        unsigned long int *dst = cam->sqBuffer;
 
331
        unsigned long int pixelCount = cam->sqPixels;
 
332
        unsigned long int r,g,b,pixel;
 
333
        int i;
 
334
 
 
335
        if (0 == dst) return;
 
336
 
 
337
        /* Byte0: ggg(r)rrrr, Byte1: (b)bbbb(g)gg */
 
338
 
 
339
        for ( i = 0; i < pixelCount; i++) {
 
340
          r = (*src & 0x1F) << 3;
 
341
          g = (*src++ & 0xE0) >> 5;
 
342
          g |= (*src & 0x07) << 5;
 
343
          b = *src++ & 0xF8;
 
344
          pixel = 0xFF000000;
 
345
          pixel |= (b << 16);
 
346
          pixel |= (g <<  8);
 
347
          pixel |= r;
 
348
          *dst++ = pixel;
 
349
        }
 
350
}
 
351
 
 
352
 
 
353
void
 
354
convertImage (camPtr cam, int bufIdx)
 
355
{
 
356
        /* func pts to be used at later date */
 
357
  
 
358
        if (cam->pixelformat == V4L2_PIX_FMT_YUYV) {
 
359
                convertImageYUYVToARGB32 (cam, bufIdx);
 
360
                return;
 
361
        }
 
362
 
 
363
        if (cam->pixelformat == V4L2_PIX_FMT_RGB565) {
 
364
                convertImageRGB565toARGB32 (cam, bufIdx);
 
365
                return;
 
366
        }
 
367
        
 
368
        if (cam->pixelformat == V4L2_PIX_FMT_RGB444) {
 
369
                convertImageRGB444toARGB32 (cam, bufIdx);
 
370
                return;
 
371
        }
 
372
        
 
373
        if (cam->pixelformat == V4L2_PIX_FMT_RGB24) {
 
374
                convertImageRGB24toARGB32 (cam, bufIdx);
 
375
                return;
 
376
        }
 
377
}
 
378
 
 
379
 
 
380
static int 
 
381
read_frame (camPtr cam) 
 
382
{
 
383
        struct v4l2_buffer buf;
 
384
 
 
385
        cam->frameCount += 1;
 
386
 
 
387
        CLEAR (buf);
 
388
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
389
        buf.memory = V4L2_MEMORY_MMAP;
 
390
 
 
391
        if (-1 == xioctl (cam, VIDIOC_DQBUF, &buf)) {
 
392
                switch (errno) {
 
393
                        case EAGAIN:
 
394
                        case EIO:
 
395
                                return 0;
 
396
                        default:
 
397
                                return -1;
 
398
                }
 
399
        }
 
400
 
 
401
        if (buf.index < cam->nBuffers)
 
402
                convertImage (cam, buf.index);
 
403
 
 
404
        if (-1 == xioctl (cam, VIDIOC_QBUF, &buf)) return -1;
 
405
 
 
406
        return 0;
 
407
}
 
408
 
 
409
 
 
410
static int 
 
411
getFrame(camPtr cam) 
 
412
{
 
413
        int fd = cam->fileDesc;
 
414
        unsigned int retry;
 
415
        fd_set fds;
 
416
        struct timeval tv;
 
417
        int r;
 
418
 
 
419
        retry = 1;
 
420
 
 
421
        while (retry-- > 0) {
 
422
                FD_ZERO (&fds);
 
423
                FD_SET (fd, &fds);
 
424
 
 
425
                /* Timeout. */
 
426
                tv.tv_sec = 1;
 
427
                tv.tv_usec = 0;
 
428
 
 
429
                r = select (fd + 1, &fds, NULL, NULL, &tv);
 
430
 
 
431
                if (-1 == r) {
 
432
                        if (EINTR == errno)
 
433
                                continue;
 
434
 
 
435
                        return -1;
 
436
                }
 
437
 
 
438
                if (0 == r) return -1;
 
439
                if (0 == read_frame (cam)) return 0;
 
440
 
 
441
                /* EAGAIN - retry */
 
442
        }
 
443
}
 
444
 
 
445
 
 
446
static int 
 
447
stream_off (camPtr cam) 
 
448
{
 
449
        enum v4l2_buf_type type;
 
450
        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
451
        xioctl (cam, VIDIOC_STREAMOFF, &type);
 
452
        return 0;
 
453
}
 
454
 
 
455
 
 
456
static int 
 
457
stream_on (camPtr cam) 
 
458
{
 
459
        enum v4l2_buf_type type;
 
460
        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
461
        if (-1 == xioctl (cam, VIDIOC_STREAMON, &type)) return -1;
 
462
        return 0;
 
463
}
 
464
 
 
465
 
 
466
static int 
 
467
uninit_device (camPtr cam) 
 
468
{
 
469
        unsigned int i;
 
470
 
 
471
        for (i = 0; i < cam->nBuffers; ++i)
 
472
                if (-1 == vd_munmap (cam->buffers[i].start, cam->buffers[i].length))
 
473
                        return -1;
 
474
 
 
475
        free (cam->buffers);
 
476
        return 0;
 
477
}
 
478
 
 
479
 
 
480
static int 
 
481
queue_buffers (camPtr cam) 
 
482
{
 
483
        unsigned int i;
 
484
 
 
485
        for (i = 0; i < cam->nBuffers; ++i) {
 
486
                struct v4l2_buffer buf;
 
487
 
 
488
                CLEAR (buf);
 
489
                buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
490
                buf.memory      = V4L2_MEMORY_MMAP;
 
491
                buf.index       = i;
 
492
 
 
493
                if (-1 == xioctl (cam, VIDIOC_QBUF, &buf)) return -1;
 
494
        }
 
495
 
 
496
        return 0;
 
497
}
 
498
 
 
499
 
 
500
static int 
 
501
init_mmap (camPtr cam) 
 
502
{
 
503
        struct v4l2_requestbuffers req;
 
504
        int bufIdx;
 
505
        
 
506
        CLEAR (req);
 
507
        req.count       = cam->nBuffers;
 
508
        req.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
509
        req.memory      = V4L2_MEMORY_MMAP;
 
510
 
 
511
        if (-1 == xioctl (cam, VIDIOC_REQBUFS, &req)) {
 
512
                if (EINVAL == errno) {
 
513
                        return -1;
 
514
                } else {
 
515
                        return -1;
 
516
                }
 
517
        }
 
518
 
 
519
        if (req.count < 1) return -1;
 
520
 
 
521
        cam->buffers = calloc (req.count, sizeof (*(cam->buffers)));
 
522
        if (!cam->buffers) return -1;
 
523
 
 
524
        for (bufIdx = 0; bufIdx < req.count; ++bufIdx) {
 
525
                struct v4l2_buffer buf;
 
526
 
 
527
                CLEAR (buf);
 
528
                buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
529
                buf.memory      = V4L2_MEMORY_MMAP;
 
530
                buf.index       = bufIdx;
 
531
 
 
532
                if (-1 == xioctl (cam, VIDIOC_QUERYBUF, &buf)) return -1;
 
533
 
 
534
                cam->buffers[bufIdx].length = buf.length;
 
535
                cam->buffers[bufIdx].start  = vd_mmap (NULL /* start anywhere */,
 
536
                                                  buf.length,
 
537
                                                  PROT_READ | PROT_WRITE /* required */,
 
538
                                                  MAP_SHARED /* recommended */,
 
539
                                                  cam->fileDesc,
 
540
                                                  buf.m.offset);
 
541
 
 
542
                if (MAP_FAILED == cam->buffers[bufIdx].start) return -1;
 
543
        }
 
544
 
 
545
        return 0;
 
546
}
 
547
 
 
548
static int
 
549
set_format (camPtr cam, struct v4l2_format *fmt, int pixelformat, int w, int h)
 
550
{
 
551
/*
 
552
        fmt->fmt.pix.field      = V4L2_FIELD_INTERLACED;
 
553
        fmt->fmt.pix.field      = V4L2_FIELD_TOP;
 
554
*/
 
555
 
 
556
        fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
557
        fmt->fmt.pix.width      = w;
 
558
        fmt->fmt.pix.height     = h;
 
559
        fmt->fmt.pix.pixelformat = pixelformat;
 
560
        fmt->fmt.pix.field      = V4L2_FIELD_NONE; /* V4L2_FIELD_INTERLACED; */
 
561
        if (-1 == xioctl (cam, VIDIOC_S_FMT, fmt)) return -1;
 
562
 
 
563
        /* Note VIDIOC_S_FMT may change width and height. */
 
564
 
 
565
        if ((w != fmt->fmt.pix.width)  | 
 
566
            (h != fmt->fmt.pix.height) | 
 
567
            (fmt->fmt.pix.pixelformat != pixelformat))
 
568
        {
 
569
                return -1;
 
570
        }
 
571
 
 
572
        cam->pixelformat = pixelformat;
 
573
        
 
574
        return 0;
 
575
}
 
576
 
 
577
static int 
 
578
init_device (camPtr cam, int w, int h) 
 
579
{
 
580
        struct v4l2_capability cap;
 
581
        struct v4l2_cropcap cropcap;
 
582
        struct v4l2_crop crop;
 
583
        struct v4l2_format fmt;
 
584
        int bpp;
 
585
        unsigned int min;
 
586
 
 
587
        if (-1 == xioctl (cam, VIDIOC_QUERYCAP, &cap)) {
 
588
                if (EINVAL == errno) {
 
589
                        return -1;
 
590
                } else {
 
591
                        return -1;
 
592
                }
 
593
        }
 
594
 
 
595
        if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) return -1;
 
596
        if (!(cap.capabilities & V4L2_CAP_STREAMING)) return -1;
 
597
 
 
598
        /* Select video input, video standard and tune here. */
 
599
 
 
600
        CLEAR (cropcap);
 
601
        cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
602
 
 
603
        if (0 == xioctl (cam, VIDIOC_CROPCAP, &cropcap)) {
 
604
                crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
605
                crop.c = cropcap.defrect; /* reset to default */
 
606
 
 
607
                if (-1 == xioctl (cam, VIDIOC_S_CROP, &crop)) {
 
608
                        if (EINVAL == errno) {
 
609
                                /* Cropping not supported (ignored) */
 
610
                        } else {
 
611
                                /* Errors ignored. */
 
612
                        }
 
613
                }
 
614
        } else {
 
615
                /* Errors ignored. */
 
616
        }
 
617
 
 
618
        CLEAR (fmt);
 
619
        /* The order of preference of formats... */
 
620
        if (-1 == set_format(cam, &fmt, V4L2_PIX_FMT_RGB24, w, h))
 
621
          if (-1 == set_format(cam, &fmt, V4L2_PIX_FMT_YUYV, w, h))
 
622
                if (-1 == set_format(cam, &fmt, V4L2_PIX_FMT_RGB565, w, h))
 
623
                  if (-1 == set_format(cam, &fmt, V4L2_PIX_FMT_RGB444, w, h))
 
624
                        return -1;
 
625
                
 
626
        /* For reference:
 
627
                V4L2_PIX_FMT_RGB24 : 3 bytes == 1 dst pixel
 
628
                V4L2_PIX_FMT_RGB565: 2 bytes == 1 dst pixel
 
629
                V4L2_PIX_FMT_RGB444: 2 bytes == 1 dst pixel
 
630
                V4L2_PIX_FMT_YUYV  : 4 bytes == 2 dst pixels
 
631
        */
 
632
        
 
633
        switch (fmt.fmt.pix.pixelformat) {
 
634
          case V4L2_PIX_FMT_RGB24: /* printf("V4L2_PIX_FMT_RGB24\n"); */
 
635
                bpp = 3; 
 
636
                break;
 
637
          case V4L2_PIX_FMT_RGB565: /* printf("V4L2_PIX_FMT_RGB565\n"); */
 
638
                bpp = 2; 
 
639
                break;
 
640
          case V4L2_PIX_FMT_RGB444: /* printf("V4L2_PIX_FMT_RGB444\n"); */
 
641
                bpp = 2;
 
642
                break;
 
643
          case V4L2_PIX_FMT_YUYV: /* printf("V4L2_PIX_FMT_YUYV\n"); */
 
644
                bpp = 4; 
 
645
                break;
 
646
        }
 
647
        
 
648
        /* Buggy driver paranoia >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> */    
 
649
        min = fmt.fmt.pix.width * bpp;
 
650
        if (fmt.fmt.pix.bytesperline < min) fmt.fmt.pix.bytesperline = min;
 
651
        min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
 
652
        if (fmt.fmt.pix.sizeimage < min) fmt.fmt.pix.sizeimage = min;
 
653
        /* <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< */
 
654
        
 
655
        if (0 > init_mmap(cam)) return -1;
 
656
        if (0 > queue_buffers(cam)) return -1;
 
657
 
 
658
        /* cache returned dims (make fmt a global?) */
 
659
        cam->bmWidth = fmt.fmt.pix.width;
 
660
        cam->bmHeight = fmt.fmt.pix.height;
 
661
        cam->sqPixels = cam->bmWidth * cam->bmHeight;
 
662
        cam->sqBufferBytes = cam->sqPixels * 4; /* Bytes to tx to Squeak (always RGB32) */
 
663
 
 
664
        return 0;
 
665
}
 
666
 
 
667
 
 
668
static int 
 
669
close_device (camPtr cam) 
 
670
{
 
671
        if (-1 == vd_close (cam->fileDesc)) return -1;
 
672
        cam->fileDesc = -1;
 
673
        return 0;
 
674
}
 
675
 
 
676
 
 
677
static int 
 
678
open_device (camPtr cam) 
 
679
{
 
680
        char deviceName[12];
 
681
        struct stat st;
 
682
 
 
683
        strcpy(deviceName, videoDevName0);
 
684
        deviceName[10] = cam->devNum + '0';
 
685
 
 
686
        if (-1 == stat (deviceName, &st)) return -1;
 
687
        if (!S_ISCHR (st.st_mode)) return -1;
 
688
 
 
689
        cam->fileDesc = vd_open (deviceName, O_RDWR /* required */ | O_NONBLOCK, 0);
 
690
 
 
691
        if (camIsClosed(cam)) return -1;
 
692
 
 
693
        return 0;
 
694
}
 
695
 
 
696
 
 
697
int 
 
698
InitCamera(camPtr cam, int w, int h) 
 
699
{
 
700
    cam->read_buf.type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
701
    cam->read_buf.memory = V4L2_MEMORY_MMAP;
 
702
        cam->ioMethod           = IO_METHOD_MMAP;
 
703
 
 
704
    if (0 > open_device(cam)) return -1;
 
705
 
 
706
    if (0 > init_device(cam, w, h)) {
 
707
        close_device(cam);
 
708
        return -1;
 
709
    }
 
710
 
 
711
    if (0 > stream_on(cam)) {
 
712
        uninit_device(cam);
 
713
        close_device(cam);
 
714
        return -1;
 
715
    }
 
716
 
 
717
    return 0;
 
718
}
 
719
 
 
720
 
 
721
 
 
722
/* ============================================= SCRATCH I/F ==================================================== */
 
723
 
 
724
 
 
725
sqInt 
 
726
CameraGetParam(int camNum, int paramNum) 
 
727
{
 
728
        camPtr cam = &camInfo[camNum-1];
 
729
        
 
730
        return 0;
 
731
}
 
732
 
 
733
/*
 
734
        "Copy a camera frame into the given Bitmap. The Bitmap should be for a Form 
 
735
        of depth 32 that is the same width and height as the current camera frame. 
 
736
        Fail if the camera is not open or if the bitmap is not the right size. If 
 
737
        successful, answer the number of frames received from the camera since the 
 
738
        last call. If this is zero, then there has been no change."
 
739
        
 
740
        ???
 
741
*/
 
742
sqInt 
 
743
CameraGetFrame(int camNum, unsigned char* buf, int pixelCount) 
 
744
{
 
745
        camPtr cam = &camInfo[camNum-1];
 
746
        
 
747
        if (camIsClosed(cam)) return false;
 
748
        if (pixelCount != cam->sqPixels) return false;
 
749
        cam->sqBuffer = (void *)buf;
 
750
        if (0 != getFrame(cam)) return 0;
 
751
        return 1;
 
752
}
 
753
 
 
754
 
 
755
sqInt 
 
756
CameraExtent(int camNum) 
 
757
{
 
758
        camPtr cam = &camInfo[camNum-1];
 
759
        
 
760
        if (camIsClosed(cam)) return 0;
 
761
        return (cam->bmWidth << 16) + cam->bmHeight;
 
762
}
 
763
 
 
764
 
 
765
char* 
 
766
CameraName(int camNum) 
 
767
{
 
768
        camPtr cam = &camInfo[camNum-1];
 
769
        
 
770
        if (camIsClosed(cam)) return "camera not open";
 
771
        return "default camera";
 
772
}
 
773
 
 
774
 
 
775
void 
 
776
CameraClose(int camNum) 
 
777
{
 
778
        camPtr cam = &camInfo[camNum-1];
 
779
        
 
780
        if (camIsClosed(cam)) return;
 
781
        stream_off(cam);
 
782
        uninit_device(cam);
 
783
        close_device(cam);
 
784
}
 
785
 
 
786
 
 
787
sqInt 
 
788
CameraOpen(int camNum, int frameWidth, int frameHeight) 
 
789
{
 
790
        camPtr cam = &camInfo[camNum-1];
 
791
        
 
792
        if (camIsOpen(cam)) return false;
 
793
        if (0 != InitCamera(cam, frameWidth, frameHeight)) return false;
 
794
        return true;
 
795
}
 
796