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