src/nx_video_alloc.c

   1 /*
   2  * Copyright (C) 2016  Nexell Co., Ltd.
   3  * Author: SeongO, Park <ray@nexell.co.kr>
   4  */
   5
   6 #include <stdio.h>
   7 #include <stdlib.h>
   8 #include <unistd.h>
   9 #include <fcntl.h>
  10 #include <stdint.h>
  11 #include <errno.h>
  12 #include <stdarg.h>
  13 #include <ctype.h>
  14 #include <string.h>
  15 #include <strings.h>
  16
  17 #include <sys/types.h>
  18 #include <sys/stat.h>
  19 #include <sys/ioctl.h>
  20 #include <sys/mman.h>           //      PROT_READ/PROT_WRITE/MAP_SHARED/mmap/munmap
  21
  22 #include <nexell/nexell_drm.h>
  23 #include <nx_video_alloc.h>
  24
  25 #include <libdrm/drm_fourcc.h>
  26 #include <linux/videodev2.h>
  27 #include <linux/videodev2_nxp_media.h>
  28
  29 #define DRM_DEVICE_NAME "/dev/dri/card0"
  30
  31 #define DRM_IOCTL_NR(n)         _IOC_NR(n)
  32 #define DRM_IOC_VOID            _IOC_NONE
  33 #define DRM_IOC_READ            _IOC_READ
  34 #define DRM_IOC_WRITE           _IOC_WRITE
  35 #define DRM_IOC_READWRITE       _IOC_READ|_IOC_WRITE
  36 #define DRM_IOC(dir, group, nr, size) _IOC(dir, group, nr, size)
  37
  38 static int
  39 drm_ioctl (int32_t drm_fd, uint32_t request, void *arg)
  40 {
  41   int ret;
  42
  43   do {
  44     ret = ioctl (drm_fd, request, arg);
  45   } while (ret == -1 && (errno == EINTR || errno == EAGAIN));
  46   return ret;
  47 }
  48
  49 static int
  50 drm_command_write_read (int fd, uint32_t command_index,
  51     void *data, uint32_t size)
  52 {
  53   uint32_t request;
  54
  55   request = DRM_IOC (DRM_IOC_READ | DRM_IOC_WRITE, DRM_IOCTL_BASE,
  56       DRM_COMMAND_BASE + command_index, size);
  57   if (drm_ioctl (fd, request, data))
  58     return -errno;
  59   return 0;
  60 }
  61
  62 /**
  63  * return gem_fd
  64  */
  65 static int
  66 alloc_gem (int drm_fd, int size, int flags)
  67 {
  68   struct nx_drm_gem_create arg = { 0, };
  69   int ret;
  70
  71   arg.size = (uint32_t) size;
  72   arg.flags = flags;
  73
  74   ret = drm_command_write_read (drm_fd, DRM_NX_GEM_CREATE, &arg, sizeof (arg));
  75   if (ret) {
  76     perror ("drm_command_write_read\n");
  77     return ret;
  78   }
  79   //printf("[DRM ALLOC] gem %d, size %d, flags 0x%x\n", arg.handle, size, flags);
  80
  81   return arg.handle;
  82 }
  83
  84 static void
  85 free_gem (int drm_fd, int gem)
  86 {
  87   struct drm_gem_close arg = { 0, };
  88
  89   arg.handle = gem;
  90   drm_ioctl (drm_fd, DRM_IOCTL_GEM_CLOSE, &arg);
  91 }
  92
  93 /**
  94  * return dmabuf fd
  95  */
  96 static int
  97 gem_to_dmafd (int drm_fd, int gem_fd)
  98 {
  99   int ret;
 100   struct drm_prime_handle arg = { 0, };
 101
 102   arg.handle = gem_fd;
 103   arg.flags = O_CLOEXEC | O_RDWR;
 104   ret = drm_ioctl (drm_fd, DRM_IOCTL_PRIME_HANDLE_TO_FD, &arg);
 105   if (0 != ret) {
 106     printf ("fail : get fd from gem:%d (DRM_IOCTL_PRIME_HANDLE_TO_FD)\n", ret);
 107     return -1;
 108   }
 109   return arg.fd;
 110 }
 111
 112 static uint32_t
 113 get_flink_name (int fd, int gem)
 114 {
 115   struct drm_gem_flink arg = { 0, };
 116
 117   arg.handle = gem;
 118   if (drm_ioctl (fd, DRM_IOCTL_GEM_FLINK, &arg)) {
 119     printf ("fail : get flink from gem:%d (DRM_IOCTL_GEM_FLINK)\n", gem);
 120     return 0;
 121   }
 122   return arg.name;
 123 }
 124
 125 static uint32_t
 126 gem_from_flink (int fd, uint32_t flink_name)
 127 {
 128   struct drm_gem_open arg = { 0, };
 129   /* struct nx_drm_gem_info info = { 0, }; */
 130
 131   arg.name = flink_name;
 132   if (drm_ioctl (fd, DRM_IOCTL_GEM_OPEN, &arg)) {
 133     printf ("fail : cannot open gem name=%d\n", flink_name);
 134     return -EINVAL;
 135   }
 136   return arg.handle;
 137 }
 138
 139
 140 //
 141 //      Nexell Private Video Memory Allocator for DRM
 142 //
 143
 144 #ifndef ALIGN
 145 #define ALIGN(X,N)      ( (X+N-1) & (~(N-1)) )
 146 #endif
 147
 148 #define ALIGNED16(X)    ALIGN(X,16)
 149
 150
 151 //      Nexell Private Memory Allocator
 152 NX_MEMORY_INFO *
 153 NX_AllocateMemory (int size, int align)
 154 {
 155   int gemFd = -1;
 156   int dmaFd = -1;
 157   int32_t flags = 0;
 158   NX_MEMORY_INFO *pMem;
 159
 160   int drmFd = open (DRM_DEVICE_NAME, O_RDWR);
 161   if (drmFd < 0)
 162     return NULL;
 163
 164   drmDropMaster (drmFd);
 165
 166   gemFd = alloc_gem (drmFd, size, flags);
 167   if (gemFd < 0)
 168     goto ErrorExit;
 169
 170   dmaFd = gem_to_dmafd (drmFd, gemFd);
 171   if (dmaFd < 0)
 172     goto ErrorExit;
 173
 174   pMem = (NX_MEMORY_INFO *) calloc (1, sizeof (NX_MEMORY_INFO));
 175   pMem->drmFd = drmFd;
 176   pMem->dmaFd = dmaFd;
 177   pMem->gemFd = gemFd;
 178   pMem->flink = get_flink_name (drmFd, gemFd);
 179   pMem->size = size;
 180   pMem->align = align;
 181   return pMem;
 182
 183 ErrorExit:
 184   if (gemFd > 0) {
 185     free_gem (drmFd, gemFd);
 186   }
 187   if (drmFd > 0)
 188     close (drmFd);
 189   return NULL;
 190 }
 191
 192 void
 193 NX_FreeMemory (NX_MEMORY_INFO * pMem)
 194 {
 195   if (pMem) {
 196     if (pMem->pBuffer) {
 197       munmap (pMem->pBuffer, pMem->size);
 198     }
 199
 200     free_gem (pMem->drmFd, pMem->gemFd);
 201     close (pMem->dmaFd);
 202     close (pMem->drmFd);
 203     free (pMem);
 204   }
 205 }
 206
 207
 208 //      Video Specific Allocator Wrapper
 209 //
 210 //      Suport Format & Planes
 211 //              YUV420 Format :
 212 //                      1 Plane : I420, NV12
 213 //                      2 Plane : NV12
 214 //                      3 Plane : I420
 215 //
 216 NX_VID_MEMORY_INFO *
 217 NX_AllocateVideoMemory (int width, int height, int32_t planes, uint32_t format,
 218     int align)
 219 {
 220   int gemFd[NX_MAX_PLANES] = { 0, };
 221   int dmaFd[NX_MAX_PLANES] = { 0, };
 222   int32_t flags = 0, i = 0;
 223   int32_t luStride, cStride;
 224   int32_t luVStride, cVStride;
 225   int32_t stride[NX_MAX_PLANES];
 226   int32_t size[NX_MAX_PLANES];
 227   uint32_t flink[NX_MAX_PLANES];
 228
 229   NX_VID_MEMORY_INFO *pVidMem;
 230
 231   int drmFd = open (DRM_DEVICE_NAME, O_RDWR);
 232   if (drmFd < 0)
 233     return NULL;
 234
 235   drmDropMaster (drmFd);
 236
 237   //      Luma
 238   luStride = ALIGN (width, 32);
 239   luVStride = ALIGN (height, 16);
 240
 241   //      Chroma
 242   switch (format) {
 243     case DRM_FORMAT_YUV420:
 244     case DRM_FORMAT_NV12:
 245     case DRM_FORMAT_NV21:
 246     case V4L2_PIX_FMT_YUV420M:
 247     case V4L2_PIX_FMT_NV12M:
 248     case V4L2_PIX_FMT_NV21M:
 249       cStride = luStride / 2;
 250       cVStride = ALIGN (height / 2, 16);
 251       break;
 252
 253     case DRM_FORMAT_YUV422:
 254     case DRM_FORMAT_NV16:
 255     case DRM_FORMAT_NV61:
 256     case V4L2_PIX_FMT_YUV422M:
 257 #if 0                           // Disabled by DIGNSYS
 258     case V4L2_PIX_FMT_NV16M:
 259     case V4L2_PIX_FMT_NV61M:
 260 #endif
 261       cStride = luStride / 2;
 262       cVStride = luVStride;
 263       break;
 264
 265     case DRM_FORMAT_YUV444:
 266       //case DRM_FORMAT_NV24:
 267       //case DRM_FORMAT_NV42:
 268     case V4L2_PIX_FMT_YUV444M:
 269     case V4L2_PIX_FMT_NV24M:
 270     case V4L2_PIX_FMT_NV42M:
 271       cStride = luStride;
 272       cVStride = luVStride;
 273       break;
 274
 275     case V4L2_PIX_FMT_GREY:
 276       cStride = 0;
 277       cVStride = 0;
 278       break;
 279     default:
 280       printf ("Unknown format type\n");
 281       goto ErrorExit;
 282   }
 283
 284   //      Decide Memory Size
 285   switch (planes) {
 286     case 1:
 287       size[0] = luStride * luVStride + cStride * cVStride * 2;
 288       stride[0] = luStride;
 289       gemFd[0] = alloc_gem (drmFd, size[0], flags);
 290       if (gemFd[0] < 0)
 291         goto ErrorExit;
 292       dmaFd[0] = gem_to_dmafd (drmFd, gemFd[0]);
 293       if (dmaFd[0] < 0)
 294         goto ErrorExit;
 295       flink[0] = get_flink_name (drmFd, gemFd[0]);
 296       break;
 297     case 2:
 298       //      Buffer 1
 299       size[0] = luStride * luVStride;
 300       stride[0] = luStride;
 301       gemFd[0] = alloc_gem (drmFd, size[0], flags);
 302       if (gemFd[0] < 0)
 303         goto ErrorExit;
 304       dmaFd[0] = gem_to_dmafd (drmFd, gemFd[0]);
 305       if (dmaFd[0] < 0)
 306         goto ErrorExit;
 307       flink[0] = get_flink_name (drmFd, gemFd[0]);
 308
 309       //      Buffer 2
 310       size[1] = cStride * cVStride * 2;
 311       stride[1] = cStride * 2;
 312       gemFd[1] = alloc_gem (drmFd, size[1], flags);
 313       if (gemFd[1] < 0)
 314         goto ErrorExit;
 315       dmaFd[1] = gem_to_dmafd (drmFd, gemFd[1]);
 316       if (dmaFd[1] < 0)
 317         goto ErrorExit;
 318       flink[1] = get_flink_name (drmFd, gemFd[1]);
 319       break;
 320     case 3:
 321       //      Buffer 1
 322       size[0] = luStride * luVStride;
 323       stride[0] = luStride;
 324       gemFd[0] = alloc_gem (drmFd, size[0], flags);
 325       if (gemFd[0] < 0)
 326         goto ErrorExit;
 327       dmaFd[0] = gem_to_dmafd (drmFd, gemFd[0]);
 328       if (dmaFd[0] < 0)
 329         goto ErrorExit;
 330       flink[0] = get_flink_name (drmFd, gemFd[0]);
 331
 332       //      Buffer 2
 333       size[1] = cStride * cVStride;
 334       stride[1] = cStride;
 335       gemFd[1] = alloc_gem (drmFd, size[1], flags);
 336       if (gemFd[1] < 0)
 337         goto ErrorExit;
 338       dmaFd[1] = gem_to_dmafd (drmFd, gemFd[1]);
 339       if (dmaFd[1] < 0)
 340         goto ErrorExit;
 341       flink[1] = get_flink_name (drmFd, gemFd[1]);
 342
 343       //      Buffer 3
 344       size[2] = cStride * cVStride;
 345       stride[2] = cStride;
 346       gemFd[2] = alloc_gem (drmFd, size[2], flags);
 347       if (gemFd[2] < 0)
 348         goto ErrorExit;
 349       dmaFd[2] = gem_to_dmafd (drmFd, gemFd[2]);
 350       if (dmaFd[2] < 0)
 351         goto ErrorExit;
 352       flink[2] = get_flink_name (drmFd, gemFd[2]);
 353       break;
 354       break;
 355   }
 356
 357   pVidMem = (NX_VID_MEMORY_INFO *) calloc (1, sizeof (NX_VID_MEMORY_INFO));
 358   pVidMem->width = width;
 359   pVidMem->height = height;
 360   pVidMem->align = align;
 361   pVidMem->planes = planes;
 362   pVidMem->format = format;
 363   pVidMem->drmFd = drmFd;
 364   for (i = 0; i < planes; i++) {
 365     pVidMem->dmaFd[i] = dmaFd[i];
 366     pVidMem->gemFd[i] = gemFd[i];
 367     pVidMem->size[i] = size[i];
 368     pVidMem->stride[i] = stride[i];
 369     pVidMem->flink[i] = flink[i];
 370   }
 371   return pVidMem;
 372
 373 ErrorExit:
 374   for (i = 0; i < planes; i++) {
 375     if (gemFd[i] > 0) {
 376       free_gem (drmFd, gemFd[i]);
 377     }
 378     if (dmaFd[i] > 0) {
 379       close (dmaFd[i]);
 380     }
 381   }
 382   if (drmFd > 0)
 383     close (drmFd);
 384
 385   return NULL;
 386 }
 387
 388 void
 389 NX_FreeVideoMemory (NX_VID_MEMORY_INFO * pMem)
 390 {
 391   int32_t i;
 392   if (pMem) {
 393     for (i = 0; i < pMem->planes; i++) {
 394       if (pMem->pBuffer[i]) {
 395         munmap (pMem->pBuffer[i], pMem->size[i]);
 396       }
 397       free_gem (pMem->drmFd, pMem->gemFd[i]);
 398       close (pMem->dmaFd[i]);
 399     }
 400     close (pMem->drmFd);
 401     free (pMem);
 402   }
 403 }
 404
 405
 406 //
 407 //              Memory Mapping/Unmapping Memory
 408 //
 409 int
 410 NX_MapMemory (NX_MEMORY_INFO * pMem)
 411 {
 412   void *pBuf;
 413   if (!pMem)
 414     return -1;
 415
 416   //      Already Mapped
 417   if (pMem->pBuffer)
 418     return -1;
 419
 420   pBuf =
 421       mmap (0, pMem->size, PROT_READ | PROT_WRITE, MAP_SHARED, pMem->dmaFd, 0);
 422   if (pBuf == MAP_FAILED) {
 423     printf("Map failed : size %d, fd %d, error : %s\n", pMem->size, pMem->dmaFd, strerror(errno));
 424     return -1;
 425   }
 426   pMem->pBuffer = pBuf;
 427   return 0;
 428 }
 429
 430
 431 int
 432 NX_UnmapMemory (NX_MEMORY_INFO * pMem)
 433 {
 434   if (!pMem)
 435     return -1;
 436
 437   if (!pMem->pBuffer)
 438     return -1;
 439
 440   if (0 != munmap (pMem->pBuffer, pMem->size))
 441     return -1;
 442
 443   pMem->pBuffer = NULL;
 444   return 0;
 445 }
 446
 447 int
 448 NX_MapVideoMemory (NX_VID_MEMORY_INFO * pMem)
 449 {
 450   int32_t i;
 451   void *pBuf;
 452   if (!pMem)
 453     return -1;
 454
 455   //      Already Mapped
 456   for (i = 0; i < pMem->planes; i++) {
 457     if (pMem->pBuffer[i])
 458       return -1;
 459     else {
 460       pBuf =
 461           mmap (0, pMem->size[i], PROT_READ | PROT_WRITE, MAP_SHARED,
 462           pMem->dmaFd[i], 0);
 463       if (pBuf == MAP_FAILED) {
 464         return -1;
 465       }
 466     }
 467     pMem->pBuffer[i] = pBuf;
 468   }
 469   return 0;
 470 }
 471
 472 int
 473 NX_UnmapVideoMemory (NX_VID_MEMORY_INFO * pMem)
 474 {
 475   int32_t i;
 476   if (!pMem)
 477     return -1;
 478   for (i = 0; i < pMem->planes; i++) {
 479     if (pMem->pBuffer[i]) {
 480       munmap (pMem->pBuffer[i], pMem->size[i]);
 481     } else
 482       return -1;
 483   }
 484   return 0;
 485 }
 486
 487 int
 488 NX_GetGEMHandles (int drmFd, NX_VID_MEMORY_INFO * pMem,
 489     uint32_t handles[NX_MAX_PLANES])
 490 {
 491   int32_t i;
 492   memset (handles, 0, sizeof (uint32_t) * NX_MAX_PLANES);
 493
 494   for (i = 0; i < pMem->planes; i++) {
 495     handles[i] = gem_from_flink (drmFd, pMem->flink[i]);
 496     if (0 > (int) handles[i]) {
 497       return -1;
 498     }
 499   }
 500   return 0;
 501 }
 502
 503 int
 504 NX_GetGemHandle (int drmFd, NX_VID_MEMORY_INFO * pMem, int32_t plane)
 505 {
 506   if (plane >= NX_MAX_PLANES || plane < 0)
 507     return -1;
 508
 509   return gem_from_flink (drmFd, pMem->flink[plane]);
 510 }