1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <string.h>
5 #include <sys/ioctl.h>
6 #include <fcntl.h>
7 #include <unistd.h>
9 #include <pthread.h>
10 #include <semaphore.h>
12 #include <drm_fourcc.h>
14 #include <list.h>
15 #include <biqueue.h>
16 #include <drm_util.h>
17 #include <swblend.h>
19 #include <errno.h>
20 #include <math.h>
22 #if defined (CONFIG_RPMSG_CHAR_CAN_EMULATION)
23 #define CONFIG_REMOTE_SERVICE_ENDPOINT (27)
24 #define CONFIG_LOCAL_SERVICE_ENDPOINT (8192)
25 #include <rpmsg_char_helper.h>
26 #endif
28 extern bool fps_show;
30 struct needle_image {
31 char *filename;
32 int width;
33 int height;
34 int angle;
35 char *data;
36 };
38 struct needle_asset {
39 int pos_x;
40 int pos_y;
41 int angle;
42 };
44 struct needle_layer_asset {
45 char *feature;
46 int width;
47 int height;
48 int pos_x;
49 int pos_y;
50 char *filename;
51 char *data;
52 int texture;
53 };
55 struct digit_position {
56 char *name;
57 int pos_x;
58 int pos_y;
59 };
61 struct digit_image {
62 char *filename;
63 int width;
64 int height;
65 char value;
66 char *data;
67 };
70 struct needle_param {
71 #if defined (CONFIG_RPMSG_CHAR_CAN_EMULATION)
72 int can_fd;
73 #endif
74 struct biqueue *bq;
75 };
77 struct damage {
78 struct rect *rects;
79 int num_rects;
80 };
82 #include <assets/asset-config.h>
83 #include <assets/needles-assets.h>
84 #include <assets/digit-assets.h>
86 #define NUM_NEEDLE_FEATURES (sizeof(nlassets) / sizeof(struct needle_layer_asset))
87 #define NUM_NEEDLE_ANGLES (sizeof(needle_images) / sizeof(struct needle_image))
88 #define NUM_DIGIT_VALUES (sizeof(dig_images) / sizeof(struct digit_image))
89 #define NUM_FPS_DIGIT_VALUES (sizeof(fps_dig_images) / sizeof(struct digit_image))
91 #define SPEED_RESOLUTION_PER_DEG (1.0f)
92 #define RPM_RESOLUTION_PER_DEG (33.3333333f)
94 #define MPH_NEEDLE_ANGLE_OFFSET (120)
95 #define RPM_NEEDLE_ANGLE_OFFSET (120)
97 #define MAX_SPEED (240)
98 #define MAX_RPM (8000)
100 #define NUM_FPS_ENTRIES (60)
101 struct fps_entry {
102 unsigned long long int timestamp;
103 struct list_head link;
104 };
105 struct list_head fps;
106 #define CONFIG_FPS_REFRESH_INTERVAL (1)
108 static unsigned long long int get_time_ms()
109 {
110 struct timespec t;
111 clock_gettime(CLOCK_MONOTONIC, &t);
112 return (((unsigned long long int)t.tv_sec * 1000) + (t.tv_nsec / 1000000));
113 }
115 static void init_fps()
116 {
117 int i;
119 INIT_LIST_HEAD(&fps);
121 for(i = 0 ; i < NUM_FPS_ENTRIES; i++) {
122 struct fps_entry *entry = calloc(sizeof(*entry), 1);
123 entry->timestamp = get_time_ms();
124 list_add_tail(&entry->link, &fps);
125 }
126 }
128 static int calculate_fps()
129 {
130 struct fps_entry *entry = list_first_entry(&fps, struct fps_entry, link);
131 entry->timestamp = get_time_ms();
132 list_del(&entry->link);
133 list_add_tail(&entry->link, &fps);
135 unsigned long long int total_ms = 0;
136 list_for_each_entry_reverse(entry, &fps, link) {
137 struct list_head *prev_link = entry->link.prev;
138 if(prev_link != &fps) {
139 struct fps_entry *prev = container_of(prev_link, struct fps_entry, link);
140 unsigned long long int diff_ms = entry->timestamp - prev->timestamp;
141 total_ms += diff_ms;
142 }
143 }
145 return total_ms ? (int)(((NUM_FPS_ENTRIES - 1) * 1000) / (float)total_ms) : 0;
146 }
148 static int load_needle_assets(struct needle_layer_asset *d)
149 {
150 int r;
151 int fd;
152 int sz;
154 fd = open(d->filename, O_RDONLY);
155 if(fd < 0) {
156 printf("could not open %s, %d\n", d->filename, errno);
157 goto err1;
158 }
160 sz = d->width * d->height * 4;
161 d->data = calloc(sz, 1);
162 if(!d->data) {
163 printf("could not allocate data for %s\n", d->feature);
164 goto err2;
165 }
167 r = read(fd, d->data, sz);
168 if(r != sz) {
169 printf("could not read data for %s\n", d->feature);
170 goto err3;
171 }
173 close(fd);
174 return 0;
176 err3:
177 free(d->data);
178 err2:
179 close(fd);
180 err1:
181 return -1;
182 }
184 static int load_needle_images(struct needle_image *n)
185 {
186 int r;
187 int fd;
188 int sz;
190 fd = open(n->filename, O_RDONLY);
191 if(fd < 0) {
192 printf("could not open %s, %d\n", n->filename, errno);
193 goto err1;
194 }
196 sz = n->width * n->height * 4;
197 n->data = calloc(sz, 1);
198 if(!n->data) {
199 printf("could not allocate data for needle angle %u\n", n->angle);
200 goto err2;
201 }
203 r = read(fd, n->data, sz);
204 if(r != sz) {
205 printf("could not read data for needle angle %u\n", n->angle);
206 goto err3;
207 }
209 close(fd);
210 return 0;
212 err3:
213 free(n->data);
214 err2:
215 close(fd);
216 err1:
217 return -1;
218 }
220 static int load_digit_images(struct digit_image *d)
221 {
222 int r;
223 int fd;
224 int sz;
226 fd = open(d->filename, O_RDONLY);
227 if(fd < 0) {
228 printf("could not open %s, %d\n", d->filename, errno);
229 goto err1;
230 }
232 sz = d->width * d->height * 4;
233 d->data = calloc(sz, 1);
234 if(!d->data) {
235 printf("could not allocate data for digit \"%c\"\n", d->value);
236 goto err2;
237 }
239 r = read(fd, d->data, sz);
240 if(r != sz) {
241 printf("could not read data for digit \"%c\"\n", d->value);
242 goto err3;
243 }
245 close(fd);
246 return 0;
248 err3:
249 free(d->data);
250 err2:
251 close(fd);
252 err1:
253 return -1;
254 }
256 static int load_fps_digit_images(struct digit_image *d)
257 {
258 int r;
259 int fd;
260 int sz;
262 fd = open(d->filename, O_RDONLY);
263 if(fd < 0) {
264 printf("could not open %s, %d\n", d->filename, errno);
265 goto err1;
266 }
268 sz = d->width * d->height * 4;
269 d->data = calloc(sz, 1);
270 if(!d->data) {
271 printf("could not allocate data for digit \"%c\"\n", d->value);
272 goto err2;
273 }
275 r = read(fd, d->data, sz);
276 if(r != sz) {
277 printf("could not read data for digit \"%c\"\n", d->value);
278 goto err3;
279 }
281 close(fd);
282 return 0;
284 err3:
285 free(d->data);
286 err2:
287 close(fd);
288 err1:
289 return -1;
290 }
292 static void needle_wait_for_next_slot(struct needle_param *prm)
293 {
294 }
296 #if defined (CONFIG_RPMSG_CHAR_CAN_EMULATION)
297 #define REQUEST_CAN_VALUES (0xdeadcafe)
299 struct can_values {
300 uint32_t speed;
301 uint32_t rpm;
302 };
304 static void needle_get_values(struct needle_param *prm, int *mph, int *rpm)
305 {
306 struct can_values v;
307 uint32_t req = REQUEST_CAN_VALUES;
308 int base_mph, base_rpm;
310 write(prm->can_fd, &req, sizeof(req));
311 read(prm->can_fd, &v, sizeof(v));
313 *mph = v.speed;
314 *rpm = v.rpm;
315 }
316 #else
317 static void needle_get_values(struct needle_param *prm, int *mph, int *rpm)
318 {
319 static int i = 0;
320 float speed_val, rpm_val;
322 speed_val = ((sinf(i * 0.01) + 1) / 2) * MAX_SPEED;
323 rpm_val = ((sinf(i * 0.01) + 1) / 2) * MAX_RPM;
325 *mph = (int)speed_val;
326 *rpm = (int)rpm_val;
328 i++;
329 }
330 #endif
332 static struct damage *alloc_zero_damage(int number)
333 {
334 struct damage *d = calloc(sizeof(*d), 1);
335 d->rects = calloc(sizeof(struct rect), number);
336 d->num_rects = 0;
337 return d;
338 }
340 static void add_to_damage(struct damage *d, struct rect *r)
341 {
342 d->rects[d->num_rects].top = r->top;
343 d->rects[d->num_rects].left = r->left;
344 d->rects[d->num_rects].width = r->width;
345 d->rects[d->num_rects].height = r->height;
346 d->num_rects++;
347 }
349 static void free_damage(struct damage *d)
350 {
351 free(d->rects);
352 free(d);
353 }
356 static struct sw_blend_layer *find_layer(struct sw_blend_layer *layers, int n, char *name)
357 {
358 int i;
359 for(i = 0; i < n; i++)
360 if(strcmp(layers[i].name, name) == 0)
361 return &layers[i];
362 return NULL;
363 }
365 static void init_rect_with_layer_bb(struct rect *rect, struct sw_blend_layer *l)
366 {
367 if(!l)
368 return;
369 rect->top = l->y;
370 rect->left = l->x;
371 rect->width = l->buf.width;
372 rect->height = l->buf.height;
373 }
375 static void upd_rect_with_layer_bb(struct rect *rect, struct sw_blend_layer *l)
376 {
377 if(!l)
378 return;
379 if(l->y < rect->top)
380 rect->top = l->y;
381 if(l->x < rect->left)
382 rect->left = l->y;
383 if((l->x + l->buf.width) > (rect->left + rect->width))
384 rect->width = l->x + l->buf.width - rect->left;
385 if((l->y + l->buf.height) > (rect->top + rect->height))
386 rect->height = l->y + l->buf.height - rect->top;
387 }
389 static void add_feature_to_layer(struct sw_blend_layer *layers, int index, char *name, struct needle_layer_asset *d)
390 {
392 layers[index].buf.width = d->width;
393 layers[index].buf.height = d->height;
394 layers[index].buf.format = DRM_FORMAT_ARGB8888;
395 layers[index].buf.stride = d->width * 4;
396 layers[index].buf.vaddr = d->data;
397 layers[index].buf.stride2 = 0;
398 layers[index].buf.vaddr2 = NULL;
399 layers[index].x = d->pos_x;
400 layers[index].y = d->pos_y;
401 layers[index].blend = true;
402 layers[index].blendfuncs = SRC_GL_SRC_ALPHA | DST_GL_ONE_MINUS_SRC_ALPHA;
403 layers[index].name = name;
404 }
406 static void add_needle_to_layer(struct sw_blend_layer *layers, int index, char *name, struct needle_asset *nassets, int nindex, struct rect *d)
407 {
408 layers[index].buf.width = needle_images[nindex].width;
409 layers[index].buf.height = needle_images[nindex].height;
410 layers[index].buf.format = DRM_FORMAT_ARGB8888;
411 layers[index].buf.stride = needle_images[nindex].width * 4;
412 layers[index].buf.vaddr = needle_images[nindex].data;
413 layers[index].buf.stride2 = 0;
414 layers[index].buf.vaddr2 = NULL;
415 layers[index].x = nassets[nindex].pos_x;
416 layers[index].y = nassets[nindex].pos_y;
417 layers[index].blend = true;
418 layers[index].blendfuncs = SRC_GL_SRC_ALPHA | DST_GL_ONE_MINUS_SRC_ALPHA;
419 layers[index].name = name;
421 d->top = layers[index].y;
422 d->left = layers[index].x;
423 d->width = layers[index].buf.width;
424 d->height = layers[index].buf.height;
425 }
427 static void add_fps_digit_to_layer(struct sw_blend_layer *layers, int index, char *name, char *id, int val)
428 {
429 struct digit_image *img;
430 struct digit_position *pos;
432 if(strcmp(id, "fps-digit-0") == 0) {
433 pos = &fps_dig_pos[0];
434 if(val < 100)
435 img = &fps_dig_images[10];
436 else
437 img = &fps_dig_images[val / 100];
438 } else if(strcmp(id, "fps-digit-1") == 0) {
439 pos = &fps_dig_pos[1];
440 if(val < 10)
441 img = &fps_dig_images[val];
442 else
443 img = &fps_dig_images[(val / 10) % 10];
444 } else if(strcmp(id, "fps-digit-2") == 0) {
445 pos = &fps_dig_pos[2];
446 img = &fps_dig_images[val % 10];
447 }
449 layers[index].buf.width = img->width;
450 layers[index].buf.height = img->height;
451 layers[index].buf.format = DRM_FORMAT_ARGB8888;
452 layers[index].buf.stride = img->width * 4;
453 layers[index].buf.vaddr = img->data;
454 layers[index].buf.stride2 = 0;
455 layers[index].buf.vaddr2 = NULL;
456 layers[index].x = pos->pos_x;
457 layers[index].y = pos->pos_y;
458 layers[index].blend = true;
459 layers[index].blendfuncs = SRC_GL_SRC_ALPHA | DST_GL_ONE_MINUS_SRC_ALPHA;
460 layers[index].name = name;
461 }
462 static void add_digit_to_layer(struct sw_blend_layer *layers, int index, char *name, char *id, int val)
463 {
464 struct digit_image *img;
465 struct digit_position *pos;
467 if(strcmp(id, "digit-0") == 0) {
468 pos = &dig_pos[0];
469 if(val < 10)
470 img = &dig_images[10];
471 else if(val < 100)
472 img = &dig_images[10];
473 else
474 img = &dig_images[val / 100];
475 } else if(strcmp(id, "digit-1") == 0) {
476 pos = &dig_pos[1];
477 if(val < 10)
478 img = &dig_images[val];
479 else if(val < 100)
480 img = &dig_images[10];
481 else
482 img = &dig_images[(val % 100) / 10];
483 } else if(strcmp(id, "digit-2") == 0) {
484 pos = &dig_pos[2];
485 if(val < 10)
486 img = &dig_images[10];
487 else if(val < 100)
488 img = &dig_images[10];
489 else
490 img = &dig_images[val % 10];
491 } else if(strcmp(id, "digit-0.5") == 0) {
492 pos = &dig_pos[3];
493 if(val < 10)
494 img = &dig_images[10];
495 else if(val < 100)
496 img = &dig_images[val / 10];
497 else
498 img = &dig_images[10];
499 } else if(strcmp(id, "digit-1.5") == 0) {
500 pos = &dig_pos[4];
501 if(val < 10)
502 img = &dig_images[10];
503 else if(val < 100)
504 img = &dig_images[val % 10];
505 else
506 img = &dig_images[10];
507 }
509 layers[index].buf.width = img->width;
510 layers[index].buf.height = img->height;
511 layers[index].buf.format = DRM_FORMAT_ARGB8888;
512 layers[index].buf.stride = img->width * 4;
513 layers[index].buf.vaddr = img->data;
514 layers[index].buf.stride2 = 0;
515 layers[index].buf.vaddr2 = NULL;
516 layers[index].x = pos->pos_x;
517 layers[index].y = pos->pos_y;
518 layers[index].blend = true;
519 layers[index].blendfuncs = SRC_GL_SRC_ALPHA | DST_GL_ONE_MINUS_SRC_ALPHA;
520 layers[index].name = name;
521 }
523 static void *needle_run(void *arg)
524 {
525 struct needle_param *prm = arg;
526 struct sw_blend_buffer dst;
527 struct sw_blend_layer layers[11];
528 int cfps, fps;
529 int i = 0;
531 while(true) {
532 struct buffer *buf;
533 int mph_val, rpm_val;
534 int mph_angle, rpm_angle;
535 struct rect rect;
536 struct rect digit_rect = {0};
537 struct rect fps_rect = {0};
538 int num_layers = 0;
540 needle_wait_for_next_slot(prm);
542 buf = list_entry(bq_next_empty(prm->bq), struct buffer, link);
543 struct damage *old_damage = drm_buffer_get_priv(buf);
544 struct damage *new_damage = alloc_zero_damage(4);
545 struct damage *store_damage = alloc_zero_damage(3);
547 dst.width = buf->width;
548 dst.height = buf->height;
549 dst.format = buf->format;
550 dst.stride = buf->stride;
551 dst.vaddr = buf->vaddr;
552 dst.stride2 = 0;
553 dst.vaddr2 = NULL;
555 needle_get_values(prm, &mph_val, &rpm_val);
557 //mph value
558 mph_angle = ((int)(mph_val/ SPEED_RESOLUTION_PER_DEG) + 360 - MPH_NEEDLE_ANGLE_OFFSET) % 360;
559 add_needle_to_layer(layers, num_layers, "mph-needle", mphassets, mph_angle, &rect); num_layers++;
560 add_to_damage(new_damage, &rect);
561 add_to_damage(store_damage, &rect);
563 //rpm value
564 rpm_angle = ((int)(rpm_val / RPM_RESOLUTION_PER_DEG) + 360 - RPM_NEEDLE_ANGLE_OFFSET) % 360;
565 add_needle_to_layer(layers, num_layers, "rpm-needle", rpmassets, rpm_angle, &rect); num_layers++;
566 add_to_damage(new_damage, &rect);
567 add_to_damage(store_damage, &rect);
569 add_digit_to_layer(layers, num_layers, "digit-0", "digit-0", mph_val); num_layers++;
570 add_digit_to_layer(layers, num_layers, "digit-1", "digit-1", mph_val); num_layers++;
571 add_digit_to_layer(layers, num_layers, "digit-2", "digit-2", mph_val); num_layers++;
572 add_digit_to_layer(layers, num_layers, "digit-0.5", "digit-0.5", mph_val); num_layers++;
573 add_digit_to_layer(layers, num_layers, "digit-1.5", "digit-1.5", mph_val); num_layers++;
575 init_rect_with_layer_bb(&digit_rect, find_layer(layers, num_layers, "digit-0"));
576 upd_rect_with_layer_bb(&digit_rect, find_layer(layers, num_layers, "digit-1"));
577 upd_rect_with_layer_bb(&digit_rect, find_layer(layers, num_layers, "digit-2"));
578 upd_rect_with_layer_bb(&digit_rect, find_layer(layers, num_layers, "digit-0.5"));
579 upd_rect_with_layer_bb(&digit_rect, find_layer(layers, num_layers, "digit-1.5"));
580 add_to_damage(new_damage, &digit_rect);
582 cfps = calculate_fps();
583 if(cfps > 999)
584 cfps = 999;
585 if((i % CONFIG_FPS_REFRESH_INTERVAL) == 0)
586 fps = cfps;
588 if(fps_show) {
589 struct needle_layer_asset *n = NULL;
590 int c;
591 for(c = 0; c < NUM_NEEDLE_FEATURES; c++)
592 if(strcmp("fps-text", nlassets[c].feature) == 0) {
593 n = &nlassets[c];
594 break;
595 }
596 add_feature_to_layer(layers, num_layers, "fps-text", n); num_layers++;
597 add_fps_digit_to_layer(layers, num_layers, "fps-digit-0", "fps-digit-0", fps); num_layers++;
598 add_fps_digit_to_layer(layers, num_layers, "fps-digit-1", "fps-digit-1", fps); num_layers++;
599 add_fps_digit_to_layer(layers, num_layers, "fps-digit-2", "fps-digit-2", fps); num_layers++;
601 init_rect_with_layer_bb(&fps_rect, find_layer(layers, num_layers, "fps-text"));
602 upd_rect_with_layer_bb(&fps_rect, find_layer(layers, num_layers, "fps-digit-0"));
603 upd_rect_with_layer_bb(&fps_rect, find_layer(layers, num_layers, "fps-digit-1"));
604 upd_rect_with_layer_bb(&fps_rect, find_layer(layers, num_layers, "fps-digit-2"));
606 add_to_damage(new_damage, &fps_rect);
607 add_to_damage(store_damage, &fps_rect);
608 }
610 if(!old_damage)
611 sw_blend(&dst, layers, num_layers, true, 0x00000000, NULL, 0, new_damage->rects, new_damage->num_rects);
612 else
613 sw_blend(&dst, layers, num_layers, true, 0x00000000, old_damage->rects, old_damage->num_rects, new_damage->rects, new_damage->num_rects);
615 if(old_damage)
616 free_damage(old_damage);
618 free_damage(new_damage);
620 drm_buffer_set_priv(buf, store_damage);
622 bq_queue_full(prm->bq, &buf->link);
624 i++;
625 }
626 }
628 void needle_start(struct biqueue *bq)
629 {
630 int i;
631 pthread_t tid;
633 #if defined(CONFIG_RPMSG_CHAR_CAN_EMULATION)
634 char *remote_core_name = "r5f-main-0-core-1";
635 int remote_service_endpt = CONFIG_REMOTE_SERVICE_ENDPOINT;
636 int local_endpt = CONFIG_LOCAL_SERVICE_ENDPOINT;
637 char *local_name = "can-virtual";
638 rproc_device_t *dev;
639 rproc_char_device_t *cdev;
640 rproc_char_endpt_t *ept = NULL;
641 char *path;
642 int can_fd;
644 dev = rproc_device_find_for_name(remote_core_name);
645 if(!dev) {
646 fprintf(stderr, "rproc_device_find_for_name failed\n");
647 return;
648 }
650 cdev = rproc_device_find_chrdev_by_remote_port(dev, remote_service_endpt);
651 if(!cdev) {
652 fprintf(stderr, "rproc_device_find_chrdev_by_remote_port failed\n");
653 return;
654 }
656 if(!ept)
657 ept = rproc_char_device_find_endpt_by_name(cdev, local_name);
658 if(!ept)
659 ept = rproc_char_device_find_endpt_by_local_port(cdev, local_endpt);
660 if(!ept)
661 ept = rproc_char_device_create_endpt(cdev, local_name, local_endpt);
662 if(!ept) {
663 fprintf(stderr, "rproc_char_device_create_endpt failed\n");
664 return;
665 }
667 path = rproc_char_endpt_get_dev_name(ept);
668 if(!path) {
669 fprintf(stderr, "rproc_char_endpt_get_dev_name failed\n");
670 return;
671 }
673 can_fd = open(path, O_RDWR);
674 if(can_fd < 0) {
675 fprintf(stderr, "could not open can_fd\n");
676 return;
677 }
678 #endif
680 init_fps();
682 for(i = 0; i < NUM_NEEDLE_FEATURES; i++) {
683 load_needle_assets(&nlassets[i]);
684 }
685 for(i = 0; i < NUM_NEEDLE_ANGLES; i++) {
686 load_needle_images(&needle_images[i]);
687 }
688 for(i = 0; i < NUM_DIGIT_VALUES; i++) {
689 load_digit_images(&dig_images[i]);
690 }
692 for(i = 0; i < NUM_FPS_DIGIT_VALUES; i++) {
693 load_fps_digit_images(&fps_dig_images[i]);
694 }
696 struct needle_param *prm = calloc(sizeof(*prm), 1);
697 #if defined (CONFIG_RPMSG_CHAR_CAN_EMULATION)
698 prm->can_fd = can_fd;
699 #endif
700 prm->bq = bq;
702 pthread_create(&tid, NULL, needle_run, prm);
703 }