1 /*
2 * Copyright (C) 2011 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 /*
18 * Contains implementation of a class EmulatedFakeRotatingCameraDevice that encapsulates
19 * fake camera device.
20 */
21
22 #define GL_GLEXT_PROTOTYPES
23 #define LOG_NDEBUG 0
24 #define LOG_TAG "EmulatedCamera_FakeDevice"
25 #define FAKE_CAMERA_SENSOR "FakeRotatingCameraSensor"
26 #include <cutils/log.h>
27 #include "EmulatedFakeCamera.h"
28 #include "EmulatedFakeRotatingCameraDevice.h"
29 #include "qemud.h"
30
31 #include <EGL/egl.h>
32 #include <GLES/gl.h>
33 #include <GLES/glext.h>
34 #include <stdio.h>
35 #include <stdlib.h>
36 #include <math.h>
37 #include <ui/DisplayInfo.h>
38 #include <fcntl.h>
39
40 #undef min
41 #undef max
42 #include <algorithm>
43
44 namespace android {
45
46 // include the dots pattern directly, it is NV21 format
47 #include "acircles_pattern_1280_720.c"
48
49 // ----------------------------------------------------------------------------
50
checkEglError(const char * op,EGLBoolean returnVal=EGL_TRUE)51 static void checkEglError(const char* op, EGLBoolean returnVal = EGL_TRUE) {
52 if (returnVal != EGL_TRUE) {
53 ALOGE("%s() returned %d\n", op, returnVal);
54 }
55
56 for (EGLint error = eglGetError(); error != EGL_SUCCESS; error
57 = eglGetError()) {
58 ALOGE("after %s() eglError (0x%x)\n", op, error);
59 }
60 }
61
clamp_rgb(signed value)62 static signed clamp_rgb(signed value) {
63 if (value > 255) {
64 value = 255;
65 } else if (value < 0) {
66 value = 0;
67 }
68 return value;
69 }
70
rgba8888_to_nv21(uint8_t * input,uint8_t * output,int width,int height)71 static void rgba8888_to_nv21(uint8_t* input, uint8_t* output, int width, int height) {
72 int align = 16;
73 int yStride = (width + (align -1)) & ~(align-1);
74 uint8_t* outputVU = output + height*yStride;
75 for (int j = 0; j < height; ++j) {
76 uint8_t* outputY = output + j*yStride;
77 for (int i = 0; i < width; ++i) {
78 uint8_t R = input[j*width*4 + i*4];
79 uint8_t G = input[j*width*4 + i*4 + 1];
80 uint8_t B = input[j*width*4 + i*4 + 2];
81 uint8_t Y = clamp_rgb((77 * R + 150 * G + 29 * B) >> 8);
82 *outputY++ = Y;
83 bool jeven = (j & 1) == 0;
84 bool ieven = (i & 1) == 0;
85 if (jeven && ieven) {
86 uint8_t V = clamp_rgb((( 128 * R - 107 * G - 21 * B) >> 8) + 128);
87 uint8_t U = clamp_rgb((( -43 * R - 85 * G + 128 * B) >> 8) + 128);
88 *outputVU++ = V;
89 *outputVU++ = U;
90 }
91 }
92 }
93 }
94
nv21_to_rgba8888(uint8_t * input,uint32_t * output,int width,int height)95 static void nv21_to_rgba8888(uint8_t* input, uint32_t * output, int width, int height) {
96 int align = 16;
97 uint32_t* output0 = output;
98 int yStride = (width + (align -1)) & ~(align-1);
99 uint8_t* inputVU = input + height*yStride;
100 uint8_t Y, U, V;
101 for (int j = 0; j < height; ++j) {
102 uint8_t* inputY = input + j*yStride;
103 for (int i = 0; i < width; ++i) {
104 Y = *inputY++;
105 bool jeven = (j & 1) == 0;
106 bool ieven = (i & 1) == 0;
107 if (jeven && ieven) {
108 V = *inputVU++;
109 U = *inputVU++;
110 }
111 *output++ = YUVToRGB32(Y,U,V);
112 }
113 }
114 }
115
render(int width,int height)116 void EmulatedFakeRotatingCameraDevice::render(int width, int height)
117 {
118 update_scene((float)width, (float)height);
119 create_texture_dotx(1280, 720);
120 int i, j;
121 int quads = 1;
122
123 int w= 992/2;
124 int h = 1280/2;
125 const GLfloat verticesfloat[] = {
126 -w, -h, 0,
127 w, -h, 0,
128 w, h, 0,
129 -w, h, 0
130 };
131
132 const GLfloat texCoordsfloat[] = {
133 0, 0,
134 1.0f, 0,
135 1.0f, 1.0f,
136 0, 1.0f
137 };
138
139 const GLushort indices[] = { 0, 1, 2, 0, 2, 3 };
140
141 glVertexPointer(3, GL_FLOAT, 0, verticesfloat);
142 glTexCoordPointer(2, GL_FLOAT, 0, texCoordsfloat);
143 glClearColor(0.5, 0.5, 0.5, 1.0);
144 int nelem = sizeof(indices)/sizeof(indices[0]);
145 glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
146 glDrawElements(GL_TRIANGLES, nelem, GL_UNSIGNED_SHORT, indices);
147 glFinish();
148 glReadPixels(0, 0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, mPixelBuf);
149 }
150
get_color(uint32_t * img,int i,int j,int w,int h,int dw,uint32_t * color)151 static void get_color(uint32_t* img, int i, int j, int w, int h, int dw, uint32_t * color) {
152 int mini = dw/2 - w/2;
153 int minj = dw/2 - h/2;
154 int maxi = mini + w -1;
155 int maxj = minj + h -1;
156
157 if ( i >= mini && i <= maxi && j >= minj && j <= maxj) {
158 *color = img[i-mini + dw*(j-minj)];
159 }
160 }
161
convert_to_square(uint32_t * src,uint32_t * dest,int sw,int sh,int dw)162 static void convert_to_square(uint32_t* src, uint32_t* dest, int sw, int sh, int dw) {
163 for (int i=0; i < dw; ++i) {
164 for (int j=0; j < dw; ++j) {
165 uint32_t color=0;
166 get_color(src, i, j, sw, sh, dw, &color);
167 dest[i+j*dw] = color;
168 }
169 }
170 }
171
create_texture_dotx(int width,int height)172 void EmulatedFakeRotatingCameraDevice::create_texture_dotx(int width, int height) {
173 uint32_t* myrgba = new uint32_t[width * height];
174 nv21_to_rgba8888(rawData, myrgba, width, height);
175 uint32_t* myrgba2 = new uint32_t[width * width];
176 convert_to_square(myrgba, myrgba2, width, height, width);
177
178 glGenTextures(1, &mTexture);
179 glBindTexture(GL_TEXTURE_2D, mTexture);
180 glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, width, 0, GL_RGBA, GL_UNSIGNED_BYTE, myrgba2);
181 //glGenerateMipmapOES does not work on mac, dont use it.
182 //glGenerateMipmapOES(GL_TEXTURE_2D);
183 // need to use linear, otherwise the dots will have sharp edges
184 glTexParameterx(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
185 glTexParameterx(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
186 glTexEnvx(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);
187 delete[] myrgba;
188 delete[] myrgba2;
189 }
190
191
gluLookAt(float eyeX,float eyeY,float eyeZ,float centerX,float centerY,float centerZ,float upX,float upY,float upZ)192 static void gluLookAt(float eyeX, float eyeY, float eyeZ,
193 float centerX, float centerY, float centerZ, float upX, float upY,
194 float upZ)
195 {
196 // See the OpenGL GLUT documentation for gluLookAt for a description
197 // of the algorithm. We implement it in a straightforward way:
198
199 float fx = centerX - eyeX;
200 float fy = centerY - eyeY;
201 float fz = centerZ - eyeZ;
202 float flf = 1.0f / sqrt(fx * fx + fy * fy + fz * fz);
203 fx *= flf;
204 fy *= flf;
205 fz *= flf;
206
207 // compute s = f x up (x means "cross product")
208
209 float sx = fy * upZ - fz * upY;
210 float sy = fz * upX - fx * upZ;
211 float sz = fx * upY - fy * upX;
212 float slf = 1.0f / sqrt(sx * sx + sy * sy + sz * sz);
213 sx *= slf;
214 sy *= slf;
215 sz *= slf;
216
217 // compute u = s x f
218 float ux = sy * fz - sz * fy;
219 float uy = sz * fx - sx * fz;
220 float uz = sx * fy - sy * fx;
221 float ulf = 1.0f / sqrt(ux * ux + uy * uy + uz * uz);
222 ux *= ulf;
223 uy *= ulf;
224 uz *= ulf;
225
226 float m[16] ;
227 m[0] = sx;
228 m[1] = ux;
229 m[2] = -fx;
230 m[3] = 0.0f;
231
232 m[4] = sy;
233 m[5] = uy;
234 m[6] = -fy;
235 m[7] = 0.0f;
236
237 m[8] = sz;
238 m[9] = uz;
239 m[10] = -fz;
240 m[11] = 0.0f;
241
242 m[12] = 0.0f;
243 m[13] = 0.0f;
244 m[14] = 0.0f;
245 m[15] = 1.0f;
246
247 glMultMatrixf(m);
248 glTranslatef(-eyeX, -eyeY, -eyeZ);
249 }
250
update_scene(float width,float height)251 void EmulatedFakeRotatingCameraDevice::update_scene(float width, float height)
252 {
253 float ratio = width / height;
254 glViewport(0, 0, width, height);
255 glMatrixMode(GL_PROJECTION);
256 glLoadIdentity();
257 glFrustumf(-ratio/2.0, ratio/2.0, -1/2.0, 1/2.0, 1, 40000);
258 glMatrixMode(GL_MODELVIEW);
259 glLoadIdentity();
260 float up_x=-1;
261 float up_y=0;
262 float up_z=0;
263 get_yawing(&up_x, &up_y, &up_z);
264 float eye_x=0;
265 float eye_y=0;
266 float eye_z=2000;
267 get_eye_x_y_z(&eye_x, &eye_y, &eye_z);
268 gluLookAt( eye_x, eye_y, eye_z, 0, 0, 0, up_x, up_y, up_z);
269 glEnable(GL_TEXTURE_2D);
270 glEnableClientState(GL_VERTEX_ARRAY);
271 glEnableClientState(GL_TEXTURE_COORD_ARRAY);
272 }
273
free_gl_surface(void)274 void EmulatedFakeRotatingCameraDevice::free_gl_surface(void)
275 {
276 if (mEglDisplay != EGL_NO_DISPLAY)
277 {
278 eglMakeCurrent( EGL_NO_DISPLAY, EGL_NO_SURFACE,
279 EGL_NO_SURFACE, EGL_NO_CONTEXT );
280 eglDestroyContext( mEglDisplay, mEglContext );
281 eglDestroySurface( mEglDisplay, mEglSurface );
282 eglTerminate( mEglDisplay );
283 mEglDisplay = EGL_NO_DISPLAY;
284 }
285 }
286
init_sensor()287 void EmulatedFakeRotatingCameraDevice::init_sensor() {
288 if (mSensorPipe >=0) return;
289 // create a sensor pipe
290 mSensorPipe = qemu_pipe_open(FAKE_CAMERA_SENSOR);
291 if (mSensorPipe < 0) {
292 ALOGE("cannot open %s", FAKE_CAMERA_SENSOR);
293 } else {
294 ALOGD("successfully opened %s", FAKE_CAMERA_SENSOR);
295 }
296 }
297
read_sensor()298 void EmulatedFakeRotatingCameraDevice::read_sensor() {
299 if (mSensorPipe < 0) return;
300 char get[] = "get";
301 int pipe_command_length = sizeof(get);
302 WriteFully(mSensorPipe, &pipe_command_length, sizeof(pipe_command_length));
303 WriteFully(mSensorPipe, get, pipe_command_length);
304 ReadFully(mSensorPipe, &pipe_command_length, sizeof(pipe_command_length));
305 ReadFully(mSensorPipe, &mSensorValues, pipe_command_length);
306 assert(pipe_command_length == 9*sizeof(float));
307 ALOGD("accel: %g %g %g; magnetic %g %g %g orientation %g %g %g",
308 mSensorValues[SENSOR_VALUE_ACCEL_X], mSensorValues[SENSOR_VALUE_ACCEL_Y],
309 mSensorValues[SENSOR_VALUE_ACCEL_Z],
310 mSensorValues[SENSOR_VALUE_MAGNETIC_X], mSensorValues[SENSOR_VALUE_MAGNETIC_Y],
311 mSensorValues[SENSOR_VALUE_MAGNETIC_Y],
312 mSensorValues[SENSOR_VALUE_ROTATION_X], mSensorValues[SENSOR_VALUE_ROTATION_Y],
313 mSensorValues[SENSOR_VALUE_ROTATION_Z]);
314 }
315
read_rotation_vector(double * yaw,double * pitch,double * roll)316 void EmulatedFakeRotatingCameraDevice::read_rotation_vector(double *yaw, double* pitch, double* roll) {
317 read_sensor();
318 *yaw = mSensorValues[SENSOR_VALUE_ROTATION_Z];
319 *pitch = mSensorValues[SENSOR_VALUE_ROTATION_X];
320 *roll = mSensorValues[SENSOR_VALUE_ROTATION_Y];
321 return;
322 }
323
get_yawing(float * x,float * y,float * z)324 void EmulatedFakeRotatingCameraDevice::get_yawing(float* x, float* y, float*z) {
325 double yaw, pitch, roll;
326 read_rotation_vector(&yaw, &pitch, &roll);
327 *x = sin((180+yaw)*3.14/180);
328 *y = cos((180+yaw)*3.14/180);
329 *z = 0;
330 ALOGD("%s: yaw is %g, x %g y %g z %g", __func__, yaw, *x, *y, *z);
331 }
332
get_eye_x_y_z(float * x,float * y,float * z)333 void EmulatedFakeRotatingCameraDevice::get_eye_x_y_z(float* x, float* y, float*z) {
334 const float R=3500;
335 //the coordinate of real camera is rotated (x-y swap)
336 //and reverted (+/- swap)
337 //
338 //so rotation y is clockwise around x axis;
339 //and rotation x is clockwise around y axis.
340 const float theta_around_x = -mSensorValues[SENSOR_VALUE_ROTATION_Y];
341 const float theta_around_y = -mSensorValues[SENSOR_VALUE_ROTATION_X];
342 //apply x rotation first
343 float y1 = -R*sin(theta_around_x*3.14/180);
344 float z1 = R*cos(theta_around_x*3.14/180);
345 //apply y rotation second
346 float xz2 = z1 * sin(theta_around_y*3.14/180);
347 float zz2 = z1 * cos(theta_around_y*3.14/180);
348 *x = xz2;
349 *y = y1;
350 *z = zz2;
351
352 }
353
init_gl_surface(int width,int height)354 int EmulatedFakeRotatingCameraDevice::init_gl_surface(int width, int height)
355 {
356 EGLint numConfigs = 1;
357 EGLConfig myConfig = {0};
358
359 if ( (mEglDisplay = eglGetDisplay(EGL_DEFAULT_DISPLAY)) == EGL_NO_DISPLAY )
360 {
361 ALOGE("eglGetDisplay failed\n");
362 return 0;
363 }
364
365 if ( eglInitialize(mEglDisplay, NULL, NULL) != EGL_TRUE )
366 {
367 ALOGE("eglInitialize failed\n");
368 return 0;
369 }
370
371 {
372 EGLint s_configAttribs[] = {
373 EGL_SURFACE_TYPE, EGL_PBUFFER_BIT|EGL_WINDOW_BIT,
374 EGL_RED_SIZE, 5,
375 EGL_GREEN_SIZE, 6,
376 EGL_BLUE_SIZE, 5,
377 EGL_NONE
378 };
379 eglChooseConfig(mEglDisplay, s_configAttribs, &myConfig, 1, &numConfigs);
380 EGLint attribs[] = { EGL_WIDTH, width, EGL_HEIGHT, height, EGL_NONE };
381 mEglSurface = eglCreatePbufferSurface(mEglDisplay, myConfig, attribs);
382 if (mEglSurface == EGL_NO_SURFACE) {
383 ALOGE("eglCreatePbufferSurface error %x\n", eglGetError());
384 }
385 }
386
387 if ( (mEglContext = eglCreateContext(mEglDisplay, myConfig, 0, 0)) == EGL_NO_CONTEXT )
388 {
389 ALOGE("eglCreateContext failed\n");
390 return 0;
391 }
392
393 if ( eglMakeCurrent(mEglDisplay, mEglSurface, mEglSurface, mEglContext) != EGL_TRUE )
394 {
395 ALOGE("eglMakeCurrent failed\n");
396 return 0;
397 }
398
399 int w, h;
400
401 eglQuerySurface(mEglDisplay, mEglSurface, EGL_WIDTH, &w);
402 checkEglError("eglQuerySurface");
403 eglQuerySurface(mEglDisplay, mEglSurface, EGL_HEIGHT, &h);
404 checkEglError("eglQuerySurface");
405 GLint dim = w < h ? w : h;
406
407 ALOGD("Window dimensions: %d x %d\n", w, h);
408
409 glDisable(GL_DITHER);
410 glEnable(GL_CULL_FACE);
411
412 return 1;
413 }
414
EmulatedFakeRotatingCameraDevice(EmulatedFakeCamera * camera_hal)415 EmulatedFakeRotatingCameraDevice::EmulatedFakeRotatingCameraDevice(EmulatedFakeCamera* camera_hal)
416 : EmulatedCameraDevice(camera_hal), mOpenglReady(false)
417 {
418 }
419
~EmulatedFakeRotatingCameraDevice()420 EmulatedFakeRotatingCameraDevice::~EmulatedFakeRotatingCameraDevice()
421 {
422 }
423
424 /****************************************************************************
425 * Emulated camera device abstract interface implementation.
426 ***************************************************************************/
427
connectDevice()428 status_t EmulatedFakeRotatingCameraDevice::connectDevice()
429 {
430 ALOGV("%s", __FUNCTION__);
431
432 Mutex::Autolock locker(&mObjectLock);
433 if (!isInitialized()) {
434 ALOGE("%s: Fake camera device is not initialized.", __FUNCTION__);
435 return EINVAL;
436 }
437 if (isConnected()) {
438 ALOGW("%s: Fake camera device is already connected.", __FUNCTION__);
439 return NO_ERROR;
440 }
441
442 /* There is no device to connect to. */
443 mState = ECDS_CONNECTED;
444
445 return NO_ERROR;
446 }
447
disconnectDevice()448 status_t EmulatedFakeRotatingCameraDevice::disconnectDevice()
449 {
450 ALOGV("%s", __FUNCTION__);
451
452 Mutex::Autolock locker(&mObjectLock);
453 if (!isConnected()) {
454 ALOGW("%s: Fake camera device is already disconnected.", __FUNCTION__);
455 return NO_ERROR;
456 }
457 if (isStarted()) {
458 ALOGE("%s: Cannot disconnect from the started device.", __FUNCTION__);
459 return EINVAL;
460 }
461
462 /* There is no device to disconnect from. */
463 mState = ECDS_INITIALIZED;
464
465 return NO_ERROR;
466 }
467
startDevice(int width,int height,uint32_t pix_fmt)468 status_t EmulatedFakeRotatingCameraDevice::startDevice(int width,
469 int height,
470 uint32_t pix_fmt)
471 {
472 ALOGE("%s width %d height %d", __FUNCTION__, width, height);
473
474 Mutex::Autolock locker(&mObjectLock);
475 if (!isConnected()) {
476 ALOGE("%s: Fake camera device is not connected.", __FUNCTION__);
477 return EINVAL;
478 }
479 if (isStarted()) {
480 ALOGE("%s: Fake camera device is already started.", __FUNCTION__);
481 return EINVAL;
482 }
483
484 /* Initialize the base class. */
485 const status_t res =
486 EmulatedCameraDevice::commonStartDevice(width, height, pix_fmt);
487
488 mState = ECDS_STARTED;
489
490 return res;
491 }
492
stopDevice()493 status_t EmulatedFakeRotatingCameraDevice::stopDevice()
494 {
495 ALOGV("%s", __FUNCTION__);
496
497 Mutex::Autolock locker(&mObjectLock);
498 if (!isStarted()) {
499 ALOGW("%s: Fake camera device is not started.", __FUNCTION__);
500 return NO_ERROR;
501 }
502
503 EmulatedCameraDevice::commonStopDevice();
504 mState = ECDS_CONNECTED;
505
506 if (mOpenglReady) {
507 free_gl_surface();
508 delete mPixelBuf;
509 mOpenglReady=false;
510 }
511 if (mSensorPipe >= 0) {
512 close(mSensorPipe);
513 mSensorPipe = -1;
514 }
515
516 return NO_ERROR;
517 }
518
519 /****************************************************************************
520 * Worker thread management overrides.
521 ***************************************************************************/
522
produceFrame(void * buffer,int64_t * timestamp)523 bool EmulatedFakeRotatingCameraDevice::produceFrame(void* buffer,
524 int64_t* timestamp)
525 {
526 if (mOpenglReady == false) {
527 init_gl_surface(mFrameWidth, mFrameHeight);
528 mOpenglReady = true;
529 int width=mFrameWidth;
530 int height = mFrameHeight;
531 int kGlBytesPerPixel = 4;
532 mPixelBuf = new uint8_t[width * height * kGlBytesPerPixel];
533 init_sensor();
534 }
535 render(mFrameWidth, mFrameHeight);
536 fillBuffer(buffer);
537 return true;
538 }
539
540 /****************************************************************************
541 * Fake camera device private API
542 ***************************************************************************/
543
fillBuffer(void * buffer)544 void EmulatedFakeRotatingCameraDevice::fillBuffer(void* buffer)
545 {
546 uint8_t* currentFrame = reinterpret_cast<uint8_t*>(buffer);
547 rgba8888_to_nv21(mPixelBuf, currentFrame, mFrameWidth, mFrameHeight);
548 return;
549 }
550
551 }; /* namespace android */
552