video_in_hal/nsframework/common_library/client/src/cl_monitor.c

   1 /*
   2  * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION.
   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 #include <stdlib.h>
  18 #include <string.h>
  19 #include <sys/mman.h>
  20 #include <sys/stat.h>
  21 #include <fcntl.h>
  22 #include <time.h>
  23 #include <errno.h>
  24 #include <semaphore.h>
  25
  26 #include "cl_error.h"
  27 #include <native_service/cl_region.h>
  28 #include <native_service/cl_monitor.h>
  29
  30 #define CL_MONITOR_SHM_NAME "/cl_monitor"
  31
  32 #define CL_MONITOR_ID_REAL_GENERIC_START (0)
  33 #define CL_MONITOR_ID_REAL_GENERIC_END   (1023)
  34 #define CL_MONITOR_ID_REAL_RPC_START     (49952)
  35 #define CL_MONITOR_ID_REAL_RPC_END       (59999)
  36
  37 #define CL_MONITOR_ID_FLAT_GENERIC_START (0)
  38 #define CL_MONITOR_ID_FLAT_GENERIC_END \
  39   (CL_MONITOR_ID_FLAT_GENERIC_START + (CL_MONITOR_ID_REAL_GENERIC_END - CL_MONITOR_ID_REAL_GENERIC_START))
  40 #define CL_MONITOR_ID_FLAT_RPC_START (CL_MONITOR_ID_FLAT_GENERIC_END + 1)
  41 #define CL_MONITOR_ID_FLAT_RPC_END \
  42   (CL_MONITOR_ID_FLAT_RPC_START + (CL_MONITOR_ID_REAL_RPC_END - CL_MONITOR_ID_REAL_RPC_START))
  43 #define CL_MONITOR_ENTRY_NUM (CL_MONITOR_ID_FLAT_RPC_END + 1)
  44
  45 #define CL_MONITOR_BITMAP_LENGTH ((CL_MONITOR_ENTRY_NUM + 63) / 64)  // aligned
  46
  47 #define CL_ALIGN(x, a) (((x) + (a - 1)) / (a) * (a))
  48
  49 struct cl_monitor_header {
  50   char signature[4];
  51   uint64_t bitmap[CL_MONITOR_BITMAP_LENGTH];
  52   sem_t sem;
  53 };
  54
  55 #define CL_MONITOR_OBJECT_SIZE \
  56   CL_ALIGN((sizeof(struct cl_monitor_header) + (sizeof(CL_MonitorEntry_t) * CL_MONITOR_ENTRY_NUM)), 4096)
  57
  58 #define CL_MONITOR_SET_BIT(a, i) (a[i / 64] |= (1ULL << (i % 64)))
  59 #define CL_MONITOR_CLEAR_BIT(a, i) (a[i / 64] &= ~(1ULL << (i % 64)))
  60
  61 static struct cl_monitor_header *cl_monitor_obj;
  62 static CL_MonitorEntry_t *cl_monitor_entry_head;
  63
  64 int CL_MonitorInit(CL_MonitorInit_t init_type) {
  65   int fd;
  66   int oflag;
  67   mode_t mode;
  68
  69   if (init_type != CL_MONITOR_INIT_SYSTEM && init_type != CL_MONITOR_INIT_USER) {
  70     errno = EINVAL;
  71     return -1;
  72   }
  73
  74   if (cl_monitor_obj != NULL) {
  75     return 0;
  76   }
  77
  78   if (init_type == CL_MONITOR_INIT_SYSTEM) {
  79     oflag = O_RDWR | O_CREAT | O_EXCL;
  80     mode = 0666;
  81   } else {
  82     oflag = O_RDWR;
  83     mode = 0;
  84   }
  85
  86   if ((fd = shm_open(CL_MONITOR_SHM_NAME, oflag, mode)) == -1) { // LCOV_EXCL_BR_LINE 5: fail safe for glibc function shm_open
  87     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
  88
  89     return -1;// LCOV_EXCL_LINE 5: fail safe for glibc function shm_open
  90   }
  91
  92   if (init_type == CL_MONITOR_INIT_SYSTEM) {
  93     if (ftruncate(fd, CL_MONITOR_OBJECT_SIZE) == -1) { // LCOV_EXCL_BR_LINE 5:  fail safe for glibc function ftruncate
  94       AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
  95
  96       return -1; // LCOV_EXCL_LINE 5:  fail safe for glibc function ftruncate
  97     }
  98   } else {
  99     struct stat st_buf;
 100     if (fstat(fd, &st_buf) == -1) { // LCOV_EXCL_BR_LINE 5:  fail safe for glibc function fstat
 101       AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 102
 103       return -1; // LCOV_EXCL_LINE 5:  fail safe for glibc function fstat
 104     }
 105
 106     if (st_buf.st_size != CL_MONITOR_OBJECT_SIZE) {
 107       errno = EAGAIN;
 108       return -1;
 109     }
 110   }
 111
 112   // LCOV_EXCL_BR_START 5:  fail safe for glibc function mmap
 113   if ((cl_monitor_obj = mmap(NULL, CL_MONITOR_OBJECT_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0)) == MAP_FAILED) {
 114     // LCOV_EXCL_BR_STOP
 115     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 116
 117     return -1; // LCOV_EXCL_LINE 5:  fail safe for glibc function mmap
 118   }
 119
 120   if (init_type == CL_MONITOR_INIT_SYSTEM) { // LCOV_EXCL_BR_LINE 11:  out branch
 121     memcpy(cl_monitor_obj->signature, "CLAM", 4);
 122     memset(cl_monitor_obj->bitmap, 0, sizeof(cl_monitor_obj->bitmap));
 123     sem_init(&cl_monitor_obj->sem, 1, 1);
 124   }
 125   cl_monitor_entry_head = (CL_MonitorEntry_t *)(((char *)cl_monitor_obj) + sizeof(struct cl_monitor_header));
 126
 127   return 0;
 128 }
 129
 130 static inline int cl_monitor_check_type_id(CL_MonitorType_t type, uint32_t id, int *offset) {
 131   if (type == CL_MONITOR_TYPE_GENERIC) {
 132     if (id > CL_MONITOR_ID_REAL_GENERIC_END) {
 133       errno = ENOENT;
 134       return -1;
 135     }
 136     *offset = (int)((id - CL_MONITOR_ID_REAL_GENERIC_START) + CL_MONITOR_ID_FLAT_GENERIC_START);
 137   } else if (type == CL_MONITOR_TYPE_RPC) {
 138     if (id < CL_MONITOR_ID_REAL_RPC_START || id > CL_MONITOR_ID_REAL_RPC_END) {
 139       errno = ENOENT;
 140       return -1;
 141     }
 142     *offset = (int)((id - CL_MONITOR_ID_REAL_RPC_START) + CL_MONITOR_ID_FLAT_RPC_START);
 143   } else {
 144     errno = ENOENT;
 145     return -1;
 146   }
 147
 148   return 0;
 149 }
 150
 151 static inline int cl_monitor_sem_wait(sem_t *sem) {
 152   int ret;
 153
 154 retry:
 155   ret = sem_wait(sem);
 156   if (ret == -1) { // LCOV_EXCL_BR_LINE 5: fail safe for libc   sem_wait
 157     // LCOV_EXCL_START 5: fail safe for libc   sem_wait
 158
 159     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 160
 161     if (errno == EINTR) {
 162       goto retry;
 163     }
 164     CL_PERROR("sem_wait");
 165   }
 166   // LCOV_EXCL_STOP
 167
 168   return ret;
 169 }
 170
 171 static inline int cl_monitor_sem_post(sem_t *sem) {
 172   int ret;
 173
 174   if ((ret = sem_post(sem)) == -1) { // LCOV_EXCL_BR_LINE 5: fail safe for libc sem_post
 175     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 176
 177     CL_PERROR("sem_post");  // LCOV_EXCL_LINE 5: fail safe for libc sem_post
 178   }
 179
 180   return ret;
 181 }
 182
 183 int CL_MonitorSetEntry(CL_MonitorType_t type, uint32_t id, CL_MonitorState_t state, uint32_t timeout,
 184                        uint32_t user_data) {
 185   int offset;
 186   CL_MonitorEntry_t *e;
 187   struct timespec ts;
 188
 189   if (cl_monitor_obj == NULL) {
 190     errno = ENOENT;
 191     return -1;
 192   }
 193
 194   if (cl_monitor_check_type_id(type, id, &offset) == -1) {
 195     return -1;
 196   }
 197
 198   if (clock_gettime(CLOCK_MONOTONIC, &ts) == -1) { // LCOV_EXCL_BR_LINE 5: fail safe for libc clock_gettime
 199     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 200
 201     return -1; // LCOV_EXCL_LINE 5: fail safe for libc clock_gettime
 202   }
 203
 204   e = cl_monitor_entry_head + offset;
 205   cl_monitor_sem_wait(&cl_monitor_obj->sem);
 206
 207   if (state == CL_MONITOR_STATE_SLEEP) {
 208     memset(e, 0, sizeof(CL_MonitorEntry_t));
 209     CL_MONITOR_CLEAR_BIT(cl_monitor_obj->bitmap, offset);
 210   } else {
 211     e->pid = (uint16_t)getpid();
 212     e->type = type;
 213     e->state = state;
 214
 215 //    e->timeout = (uint32_t)ts.tv_sec + timeout;
 216     e->timeout = ts.tv_sec + timeout;
 217     e->id = id;
 218     e->user_data = user_data;
 219     CL_MONITOR_SET_BIT(cl_monitor_obj->bitmap, offset);
 220   }
 221
 222   cl_monitor_sem_post(&cl_monitor_obj->sem);
 223
 224   return 0;
 225 }
 226
 227 int CL_MonitorGetEntry(CL_MonitorType_t type, uint32_t id, CL_MonitorEntry_t *entry) {
 228   int offset;
 229   CL_MonitorEntry_t *e;
 230
 231   if (cl_monitor_obj == NULL) {
 232     errno = ENOENT;
 233     return -1;
 234   }
 235
 236   if (entry == NULL) {
 237     errno = EINVAL;
 238     return -1;
 239   }
 240
 241   if (cl_monitor_check_type_id(type, id, &offset) == -1) {
 242     return -1;
 243   }
 244
 245   e = cl_monitor_entry_head + offset;
 246   cl_monitor_sem_wait(&cl_monitor_obj->sem);
 247
 248   memcpy(entry, e, sizeof(CL_MonitorEntry_t));
 249
 250   cl_monitor_sem_post(&cl_monitor_obj->sem);
 251
 252   return 0;
 253 }
 254
 255 int CL_MonitorSearchInit(CL_MonitorSearch_t *serch) {
 256   if (serch == NULL) {
 257     errno = EINVAL;
 258     return -1;
 259   }
 260
 261   serch->entry_list = NULL;
 262   serch->entry_num = 0;
 263   return 0;
 264 }
 265
 266 int CL_MonitorSearchDestroy(CL_MonitorSearch_t *serch) {
 267   if (serch == NULL) {
 268     errno = EINVAL;
 269     return -1;
 270   }
 271
 272   free(serch->entry_list);
 273   serch->entry_num = 0;
 274   return 0;
 275 }
 276
 277 static inline int cl_monitor_popcnt64(uint64_t x) {
 278   uint64_t n;
 279
 280   n = (x >> 1) & 0x7777777777777777ULL;
 281   x = x - n;
 282   n = (n >> 1) & 0x7777777777777777ULL;
 283   x = x - n;
 284   n = (n >> 1) & 0x7777777777777777ULL;
 285   x = x - n;
 286   x = (x + (x >> 4)) & 0x0f0f0f0f0f0f0f0fULL;
 287   x = x * 0x0101010101010101ULL;
 288   return (int)(x >> 56);
 289 }
 290
 291 typedef struct cl_monitor_offset_s cl_monitor_offset_t;
 292 struct cl_monitor_offset_s {
 293   cl_monitor_offset_t *next;
 294   int offset;
 295 };
 296
 297 int CL_MonitorSearchTimeout(CL_MonitorSearch_t *search) {
 298   int i;
 299   int offset;
 300   CL_MonitorEntry_t *e;
 301   struct timespec ts;
 302   int timeout_entry_num = 0;
 303   cl_region_t *r;
 304   cl_monitor_offset_t *o, *head = NULL, *tail = NULL;
 305
 306   if (cl_monitor_obj == NULL) {
 307     errno = ENOENT;
 308     return -1;
 309   }
 310
 311   if (search == NULL) {
 312     errno = EINVAL;
 313     return -1;
 314   }
 315
 316   if ((r = CL_RegionCreate(CL_REGION_DEFAULT_SIZE)) == NULL) {
 317     errno = ENOMEM;
 318     return -1;
 319   }
 320
 321   free(search->entry_list);
 322   search->entry_list = NULL;
 323
 324   if (clock_gettime(CLOCK_MONOTONIC, &ts) == -1) {// LCOV_EXCL_BR_LINE 5: fail safe for libc clock_gettime
 325     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 326
 327     return -1;// LCOV_EXCL_LINE 5: fail safe for libc clock_gettime
 328   }
 329
 330   cl_monitor_sem_wait(&cl_monitor_obj->sem);
 331
 332   for (i = 0; i < CL_MONITOR_BITMAP_LENGTH; i++) {
 333     if (cl_monitor_obj->bitmap[i] != 0) {
 334       uint64_t bits, mrb1;  // most right bit 1
 335
 336       bits = cl_monitor_obj->bitmap[i];
 337       while (bits) {
 338         mrb1 = bits & (-bits);
 339         offset = i * 64 + cl_monitor_popcnt64(mrb1 - 1);
 340         e = cl_monitor_entry_head + offset;
 341
 342         if (e->timeout <= ts.tv_sec) {
 343           timeout_entry_num++;
 344           if ((o = CL_RegionAlloc(r, cl_monitor_offset_t, 1)) == NULL) {  // LCOV_EXCL_BR_LINE 5: fail safe for mmap
 345             // LCOV_EXCL_START 5: fail safe for libc mmap
 346             AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 347             errno = ENOMEM;
 348             timeout_entry_num = -1;
 349             goto exit;
 350             // LCOV_EXCL_STOP
 351           }
 352
 353           o->offset = offset;
 354           o->next = NULL;
 355
 356           if (head == NULL) {
 357             head = tail = o;
 358           } else {
 359             tail->next = o;
 360             tail = o;
 361           }
 362         }
 363         bits &= ~mrb1;
 364       }
 365     }
 366   }
 367
 368   if (timeout_entry_num) {
 369     CL_MonitorEntry_t *src, *dst;
 370     dst = malloc(sizeof(CL_MonitorEntry_t) * (size_t)timeout_entry_num);
 371     if (dst == NULL) {  // LCOV_EXCL_LINE 5: fail safe for libc malloc
 372       AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 373
 374       errno = ENOMEM;  // LCOV_EXCL_LINE 5: fail safe for libc malloc
 375       timeout_entry_num = -1;  // LCOV_EXCL_LINE 5: fail safe for libc malloc
 376       goto exit;  // LCOV_EXCL_LINE 5: fail safe for libc malloc
 377     }
 378
 379     search->entry_list = dst;
 380     o = head;
 381     for (i = 0; i < timeout_entry_num; i++) {
 382       src = cl_monitor_entry_head + o->offset;
 383       memcpy(dst, src, sizeof(CL_MonitorEntry_t));
 384
 385       o = o->next;
 386       dst++;
 387     }
 388   }
 389
 390 exit:
 391   cl_monitor_sem_post(&cl_monitor_obj->sem);
 392
 393   CL_RegionDestroy(r);
 394   search->entry_num = (timeout_entry_num == -1) ? 0 : timeout_entry_num;  // LCOV_EXCL_BR_LINE 11:  out branch
 395   return timeout_entry_num;
 396 }
 397
 398 int cl_monitor_cleanup(int pid) {
 399   int ret = 0;
 400   int i;
 401   int offset;
 402   CL_MonitorEntry_t *e;
 403
 404   if (cl_monitor_obj == NULL) {
 405     errno = ENOENT;
 406     return -1;
 407   }
 408
 409   if (pid <= 0) { // LCOV_EXCL_LINE 5: fail safe for glibc function waitid
 410     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 411
 412     errno = EINVAL;// LCOV_EXCL_LINE 5: fail safe for glibc function waitid
 413     return -1;// LCOV_EXCL_LINE 5: fail safe for glibc function waitid
 414   }
 415
 416   cl_monitor_sem_wait(&cl_monitor_obj->sem);
 417
 418   for (i = 0; i < CL_MONITOR_BITMAP_LENGTH; i++) {
 419     if (cl_monitor_obj->bitmap[i] != 0) {
 420       uint64_t bits, mrb1;  // most right bit 1
 421
 422       bits = cl_monitor_obj->bitmap[i];
 423       while (bits) {
 424         mrb1 = bits & (-bits);
 425         offset = i * 64 + cl_monitor_popcnt64(mrb1 - 1);
 426
 427         e = cl_monitor_entry_head + offset;
 428         if (e->pid == pid) {
 429           memset(e, 0, sizeof(CL_MonitorEntry_t));
 430           CL_MONITOR_CLEAR_BIT(cl_monitor_obj->bitmap, offset);
 431           ret++;
 432         }
 433
 434         bits &= ~mrb1;
 435       }
 436     }
 437   }
 438
 439   cl_monitor_sem_post(&cl_monitor_obj->sem);
 440
 441   return ret;
 442 }