stf_drm.c 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490
  1. // SPDX-License-Identifier: GPL-2.0
  2. /*
  3. * Copyright (C) 2021 StarFive Technology Co., Ltd.
  4. */
  5. #include <sys/mman.h>
  6. #include <libdrm/drm_fourcc.h>
  7. #include "stf_drm.h"
  8. #include "stf_log.h"
  9. static int drm_open(const char *path, int need_dumb, int need_prime)
  10. {
  11. int fd, flags;
  12. uint64_t has_it;
  13. LOG(STF_LEVEL_TRACE, "Enter\n");
  14. if ((fd = open(path, O_RDWR)) < 0) {
  15. LOG(STF_LEVEL_ERR, "cannot open \"%s\"\n", path);
  16. error("open");
  17. }
  18. /* set FD_CLOEXEC flag */
  19. if ((flags = fcntl(fd, F_GETFD)) < 0
  20. || fcntl(fd, F_SETFD, flags | FD_CLOEXEC) < 0)
  21. fatal("fcntl FD_CLOEXEC failed");
  22. if (need_dumb) {
  23. if (drmGetCap(fd, DRM_CAP_DUMB_BUFFER, &has_it) < 0)
  24. error("drmGetCap DRM_CAP_DUMB_BUFFER failed!");
  25. if (has_it == 0)
  26. fatal("can't give us dumb buffers");
  27. }
  28. if (need_prime) {
  29. /* check prime */
  30. if (drmGetCap(fd, DRM_CAP_PRIME, &has_it) < 0)
  31. error("drmGetCap DRM_CAP_PRIME failed!");
  32. if (!(has_it & DRM_PRIME_CAP_EXPORT))
  33. fatal("can't export dmabuf");
  34. }
  35. LOG(STF_LEVEL_TRACE, "Exit\n");
  36. return fd;
  37. }
  38. static struct drm_dev_t *drm_find_dev(int fd, uint32_t width, uint32_t height)
  39. {
  40. int i, m;
  41. struct drm_dev_t *dev = NULL, *dev_head = NULL;
  42. drmModeRes *res;
  43. drmModeConnector *conn;
  44. drmModeEncoder *enc;
  45. drmModeModeInfo *mode = NULL, *preferred = NULL;
  46. LOG(STF_LEVEL_TRACE, "Enter\n");
  47. if ((res = drmModeGetResources(fd)) == NULL) {
  48. fatal("drmModeGetResources() failed");
  49. }
  50. LOG(STF_LEVEL_DEBUG, "count_connectors=%d\n", res->count_connectors);
  51. /* find all available connectors */
  52. for (i = 0; i < res->count_connectors; i++) {
  53. conn = drmModeGetConnector(fd, res->connectors[i]);
  54. if (conn != NULL && conn->connection == DRM_MODE_CONNECTED && conn->count_modes > 0) {
  55. dev = (struct drm_dev_t *) malloc(sizeof(struct drm_dev_t));
  56. memset(dev, 0, sizeof(struct drm_dev_t));
  57. /* find preferred mode */
  58. for (m = 0; m < conn->count_modes; m++) {
  59. mode = &conn->modes[m];
  60. if (mode->hdisplay == width && mode->vdisplay == height) {
  61. preferred = mode;
  62. LOG(STF_LEVEL_INFO, "find the matched mode, modes index=%d, %dx%d\n",
  63. m, width, height);
  64. break;
  65. }
  66. if (mode->type & DRM_MODE_TYPE_PREFERRED) {
  67. preferred = mode;
  68. LOG(STF_LEVEL_INFO, "find perferred mode, modes index=%d\n", m);
  69. }
  70. LOG(STF_LEVEL_DEBUG, "mode: %dx%d %s\n", mode->hdisplay, mode->vdisplay,
  71. mode->type & DRM_MODE_TYPE_PREFERRED ? "*" : "");
  72. }
  73. if (!preferred)
  74. preferred = &conn->modes[0];
  75. dev->conn_id = conn->connector_id;
  76. // dev->next = NULL;
  77. if (conn->count_encoders) {
  78. if (conn->encoder_id) {
  79. dev->enc_id = conn->encoder_id;
  80. } else {
  81. dev->enc_id = conn->encoders[0];
  82. }
  83. } else {
  84. fatal("count_encoders is 0");
  85. }
  86. memcpy(&dev->mode, preferred, sizeof(drmModeModeInfo));
  87. dev->width = preferred->hdisplay;
  88. dev->height = preferred->vdisplay;
  89. /* FIXME: use default encoder/crtc pair */
  90. if ((enc = drmModeGetEncoder(fd, dev->enc_id)) == NULL)
  91. fatal("drmModeGetEncoder() faild");
  92. if (enc->crtc_id) {
  93. dev->crtc_id = enc->crtc_id;
  94. } else {
  95. dev->crtc_id = res->crtcs[0];
  96. }
  97. drmModeFreeEncoder(enc);
  98. dev->saved_crtc = NULL;
  99. /* create dev list */
  100. dev->next = dev_head;
  101. dev_head = dev;
  102. }
  103. drmModeFreeConnector(conn);
  104. }
  105. drmModeFreeResources(res);
  106. LOG(STF_LEVEL_INFO, "selected connector(s)\n");
  107. for (dev = dev_head; dev != NULL; dev = dev->next) {
  108. LOG(STF_LEVEL_INFO, "connector id:%d\n", dev->conn_id);
  109. LOG(STF_LEVEL_INFO, "\tencoder id:%d crtc id:%d\n", dev->enc_id, dev->crtc_id);
  110. LOG(STF_LEVEL_INFO, "\twidth:%d height:%d\n", dev->width, dev->height);
  111. }
  112. LOG(STF_LEVEL_TRACE, "Exit\n");
  113. return dev_head;
  114. }
  115. static void drm_setup_buffer(int fd, struct drm_dev_t *dev,
  116. int width, int height,
  117. struct drm_buffer_t *buffer, int map, int export)
  118. {
  119. struct drm_mode_create_dumb create_req;
  120. struct drm_mode_map_dumb map_req;
  121. LOG(STF_LEVEL_TRACE, "Enter\n");
  122. buffer->dmabuf_fd = -1;
  123. memset(&create_req, 0, sizeof(struct drm_mode_create_dumb));
  124. create_req.width = width;
  125. create_req.height = height;
  126. create_req.bpp = 24;
  127. if (drmIoctl(fd, DRM_IOCTL_MODE_CREATE_DUMB, &create_req) < 0) {
  128. fatal("drmIoctl DRM_IOCTL_MODE_CREATE_DUMB failed");
  129. }
  130. buffer->pitch = create_req.pitch;
  131. buffer->size = create_req.size;
  132. /* GEM buffer handle */
  133. buffer->bo_handle = create_req.handle;
  134. if (export) {
  135. int ret;
  136. ret = drmPrimeHandleToFD(fd, buffer->bo_handle,
  137. DRM_CLOEXEC | DRM_RDWR, &buffer->dmabuf_fd);
  138. if (ret < 0) {
  139. fatal("could not export the dump buffer");
  140. }
  141. LOG(STF_LEVEL_INFO, "drm fd=%d, bo_handle=%d, dmabuf_fd=%d\n",
  142. fd, buffer->bo_handle, buffer->dmabuf_fd);
  143. }
  144. if (map) {
  145. memset(&map_req, 0, sizeof(struct drm_mode_map_dumb));
  146. map_req.handle = buffer->bo_handle;
  147. if (drmIoctl(fd, DRM_IOCTL_MODE_MAP_DUMB, &map_req)) {
  148. fatal("drmIoctl DRM_IOCTL_MODE_MAP_DUMB failed");
  149. }
  150. buffer->buf = (uint8_t *)mmap(0, buffer->size,
  151. PROT_READ | PROT_WRITE, MAP_SHARED,
  152. fd, map_req.offset);
  153. if (buffer->buf == MAP_FAILED) {
  154. error("mmap");
  155. }
  156. }
  157. LOG(STF_LEVEL_TRACE, "Exit\n");
  158. }
  159. static uint32_t drm_get_bpp_from_drm(uint32_t drmfmt)
  160. {
  161. uint32_t bpp;
  162. switch (drmfmt) {
  163. case DRM_FORMAT_YUV420:
  164. case DRM_FORMAT_YVU420:
  165. case DRM_FORMAT_YUV422:
  166. case DRM_FORMAT_NV12:
  167. case DRM_FORMAT_NV21:
  168. case DRM_FORMAT_NV16:
  169. bpp = 8;
  170. break;
  171. case DRM_FORMAT_P010:
  172. bpp = 10;
  173. break;
  174. case DRM_FORMAT_UYVY:
  175. case DRM_FORMAT_YUYV:
  176. case DRM_FORMAT_YVYU:
  177. case DRM_FORMAT_P016:
  178. bpp = 16;
  179. break;
  180. case DRM_FORMAT_BGR888:
  181. case DRM_FORMAT_RGB888:
  182. bpp = 24;
  183. break;
  184. default:
  185. bpp = 32;
  186. break;
  187. }
  188. return bpp;
  189. }
  190. static uint32_t drm_get_height_from_drm (uint32_t drmfmt, uint32_t height)
  191. {
  192. uint32_t ret;
  193. switch (drmfmt) {
  194. case DRM_FORMAT_YUV420:
  195. case DRM_FORMAT_YVU420:
  196. case DRM_FORMAT_YUV422:
  197. case DRM_FORMAT_NV12:
  198. case DRM_FORMAT_NV21:
  199. case DRM_FORMAT_P010:
  200. case DRM_FORMAT_P016:
  201. ret = height * 3 / 2;
  202. break;
  203. case DRM_FORMAT_NV16:
  204. ret = height * 2;
  205. break;
  206. default:
  207. ret = height;
  208. break;
  209. }
  210. return ret;
  211. }
  212. static int drm_setup_buffer2(int fd, struct drm_dev_t *dev,
  213. int width, int height,
  214. struct drm_buffer_t *buffer, int map, int export)
  215. {
  216. struct drm_mode_create_dumb create_req;
  217. struct drm_mode_map_dumb map_req;
  218. int ret = 0;
  219. LOG(STF_LEVEL_TRACE, "Enter\n");
  220. buffer->dmabuf_fd = -1;
  221. memset(&create_req, 0, sizeof(struct drm_mode_create_dumb));
  222. // For NV12 or NV21, bpp is 8, height is height * 3 / 2
  223. create_req.width = width;
  224. create_req.height = drm_get_height_from_drm(dev->drm_format, height);
  225. create_req.bpp = drm_get_bpp_from_drm(dev->drm_format);
  226. if (drmIoctl(fd, DRM_IOCTL_MODE_CREATE_DUMB, &create_req) < 0) {
  227. fatal("drmIoctl DRM_IOCTL_MODE_CREATE_DUMB failed");
  228. }
  229. buffer->pitch = create_req.pitch;
  230. buffer->size = create_req.size;
  231. buffer->bo_handle = create_req.handle; /* GEM buffer handle */
  232. if (export) {
  233. #if 0
  234. memset(&map_req, 0, sizeof(struct drm_mode_map_dumb));
  235. map_req.handle = buffer->bo_handle;
  236. ret = drmIoctl(fd, DRM_IOCTL_MODE_MAP_DUMB, &map_req);
  237. if (ret) {
  238. fatal("drmIoctl DRM_IOCTL_MODE_MAP_DUMB failed");
  239. }
  240. buffer->buf = (uint8_t *) mmap(0, buffer->size,
  241. PROT_READ | PROT_WRITE, MAP_SHARED,
  242. fd, map_req.offset);
  243. if (buffer->buf == MAP_FAILED) {
  244. error("drm mmap");
  245. }
  246. LOG(STF_LEVEL_INFO, "buffer->buf= %p\n", buffer->buf);
  247. #endif
  248. ret = drmPrimeHandleToFD(fd, buffer->bo_handle,
  249. DRM_CLOEXEC | DRM_RDWR, &buffer->dmabuf_fd);
  250. if (ret < 0) {
  251. fatal("could not export the dump buffer");
  252. }
  253. LOG(STF_LEVEL_DEBUG, "drm fd=%d, bo_handle=%d, dmabuf_fd=%d\n",
  254. fd, buffer->bo_handle, buffer->dmabuf_fd);
  255. }
  256. if (map) {
  257. memset(&map_req, 0, sizeof(struct drm_mode_map_dumb));
  258. map_req.handle = buffer->bo_handle;
  259. if (drmIoctl(fd, DRM_IOCTL_MODE_MAP_DUMB, &map_req)) {
  260. fatal("drmIoctl DRM_IOCTL_MODE_MAP_DUMB failed");
  261. }
  262. buffer->buf = (uint8_t *)mmap(0, buffer->size,
  263. PROT_READ | PROT_WRITE, MAP_SHARED,
  264. fd, map_req.offset);
  265. if (buffer->buf == MAP_FAILED) {
  266. error("mmap");
  267. }
  268. }
  269. uint32_t handles[4] = {0}, pitches[4] = {0}, offsets[4] = {0};
  270. handles[0] = buffer->bo_handle;
  271. pitches[0] = buffer->pitch;
  272. offsets[0] = 0;
  273. handles[1] = buffer->bo_handle;
  274. pitches[1] = buffer->pitch;
  275. offsets[1] = buffer->pitch * height;
  276. // planes[0] = virtual;
  277. // planes[1] = virtual + offsets[1];
  278. LOG(STF_LEVEL_INFO, "map=%d, export=%d. width=%d, height=%d, "
  279. "pitch=%d, size=%d, bo_handle=%d, dmabuf_fd=%d\n",
  280. map, export, width, height, buffer->pitch, buffer->size,
  281. buffer->bo_handle, buffer->dmabuf_fd);
  282. //DRM_FORMAT_NV12
  283. ret = drmModeAddFB2(fd, dev->width, dev->height,
  284. dev->drm_format, handles, pitches, offsets, &(buffer->fb_id), 0);
  285. LOG(STF_LEVEL_TRACE, "Exit\n");
  286. return ret;
  287. // return drmModeAddFB(fd, dev->width, dev->height,
  288. // DEPTH, BPP, dev->bufs[i].pitch,
  289. // dev->bufs[i].bo_handle, &dev->bufs[i].fb_id);
  290. }
  291. static void drm_setup_dummy(int fd, struct drm_dev_t *dev, int map, int export)
  292. {
  293. int i;
  294. LOG(STF_LEVEL_TRACE, "Enter\n");
  295. for (i = 0; i < BUFCOUNT; i++) {
  296. drm_setup_buffer(fd, dev, dev->width, dev->height,
  297. &dev->bufs[i], map, export);
  298. }
  299. /* Assume all buffers have the same pitch */
  300. dev->pitch = dev->bufs[0].pitch;
  301. LOG(STF_LEVEL_INFO, "DRM: buffer pitch = %d bytes\n", dev->pitch);
  302. LOG(STF_LEVEL_TRACE, "Exit\n");
  303. }
  304. static void drm_setup_fb(int fd, struct drm_dev_t *dev, int map, int export)
  305. {
  306. int i;
  307. LOG(STF_LEVEL_TRACE, "Enter\n");
  308. #if 0
  309. for (i = 0; i < BUFCOUNT; i++) {
  310. int ret;
  311. drm_setup_buffer(fd, dev, dev->width, dev->height,
  312. &dev->bufs[i], map, export);
  313. ret = drmModeAddFB(fd, dev->width, dev->height,
  314. DEPTH, BPP, dev->bufs[i].pitch,
  315. dev->bufs[i].bo_handle, &dev->bufs[i].fb_id);
  316. if (ret)
  317. fatal("drmModeAddFB failed");
  318. LOG(STF_LEVEL_INFO, "width=%d,height=%d,pitch=%d,bo_handle=%d,fb_id=%d\n",
  319. dev->width, dev->height,dev->bufs[i].pitch,dev->bufs[i].bo_handle, dev->bufs[i].fb_id);
  320. }
  321. #else
  322. for (i = 0; i < BUFCOUNT; i++) {
  323. int ret = -1;
  324. ret = drm_setup_buffer2(fd, dev, dev->width, dev->height,
  325. &dev->bufs[i], map, export);
  326. if (ret) {
  327. fatal("drmModeAddFB failed");
  328. }
  329. }
  330. #endif
  331. /* Assume all buffers have the same pitch */
  332. dev->pitch = dev->bufs[0].pitch;
  333. LOG(STF_LEVEL_DEBUG, "DRM: buffer pitch %d bytes\n", dev->pitch);
  334. dev->saved_crtc = drmModeGetCrtc(fd, dev->crtc_id); /* must store crtc data */
  335. /* First buffer to DRM */
  336. if (drmModeSetCrtc(fd, dev->crtc_id, dev->bufs[0].fb_id, 0, 0,
  337. &dev->conn_id, 1, &dev->mode)) {
  338. fatal("drmModeSetCrtc() failed");
  339. }
  340. /* First flip */
  341. drmModePageFlip(fd, dev->crtc_id,
  342. dev->bufs[0].fb_id, DRM_MODE_PAGE_FLIP_EVENT,
  343. dev);
  344. LOG(STF_LEVEL_TRACE, "Exit\n");
  345. }
  346. static void drm_destroy(int fd, struct drm_dev_t *dev_head)
  347. {
  348. struct drm_dev_t *devp, *devp_tmp;
  349. int i;
  350. LOG(STF_LEVEL_TRACE, "Enter\n");
  351. for (devp = dev_head; devp != NULL;) {
  352. if (devp->saved_crtc) {
  353. drmModeSetCrtc(fd, devp->saved_crtc->crtc_id, devp->saved_crtc->buffer_id,
  354. devp->saved_crtc->x, devp->saved_crtc->y, &devp->conn_id, 1, &devp->saved_crtc->mode);
  355. drmModeFreeCrtc(devp->saved_crtc);
  356. }
  357. for (i = 0; i < BUFCOUNT; i++) {
  358. struct drm_mode_destroy_dumb dreq = { .handle = devp->bufs[i].bo_handle };
  359. if (devp->bufs[i].buf) {
  360. munmap(devp->bufs[i].buf, devp->bufs[i].size);
  361. }
  362. if (devp->bufs[i].dmabuf_fd >= 0) {
  363. close(devp->bufs[i].dmabuf_fd);
  364. }
  365. drmIoctl(fd, DRM_IOCTL_MODE_DESTROY_DUMB, &dreq);
  366. drmModeRmFB(fd, devp->bufs[i].fb_id);
  367. }
  368. devp_tmp = devp;
  369. devp = devp->next;
  370. free(devp_tmp);
  371. }
  372. drmClose(fd);
  373. LOG(STF_LEVEL_TRACE, "Exit\n");
  374. }
  375. void stf_drm_open(DRMParam_t *param, char *device_name, int iomthd)
  376. {
  377. int drm_fd;
  378. LOG(STF_LEVEL_TRACE, "Enter\n");
  379. if (IO_METHOD_MMAP == iomthd) {
  380. drm_fd = drm_open(device_name, 1, 0);
  381. } else if (IO_METHOD_DMABUF == iomthd) {
  382. drm_fd = drm_open(device_name, 1, 1);
  383. } else {
  384. LOG(STF_LEVEL_ERR, "drm: not support io_method %d\n", iomthd);
  385. exit (EXIT_FAILURE);
  386. }
  387. param->fd = drm_fd;
  388. LOG(STF_LEVEL_TRACE, "Exit\n");
  389. }
  390. // try to use v4l2_fmt
  391. void stf_drm_init(DRMParam_t *param, uint32_t width, uint32_t height,
  392. uint32_t v4l2_fmt, int iomthd, int *dmabufs, int nsize)
  393. {
  394. struct drm_dev_t *dev_head = NULL;
  395. int i = 0;
  396. LOG(STF_LEVEL_TRACE, "Enter\n");
  397. dev_head = drm_find_dev(param->fd, width, height);
  398. if (dev_head == NULL) {
  399. LOG(STF_LEVEL_ERR, "available drm_dev not found\n");
  400. exit (EXIT_FAILURE);
  401. }
  402. dev_head->drm_format = v4l2fmt_to_drmfmt(v4l2_fmt);
  403. if (IO_METHOD_MMAP == iomthd) {
  404. drm_setup_fb(param->fd, dev_head, 1, 0);
  405. } else if (IO_METHOD_DMABUF == iomthd) {
  406. drm_setup_fb(param->fd, dev_head, 0, 1);
  407. assert(nsize == BUFCOUNT);
  408. for (i = 0; i < nsize; i++) {
  409. dmabufs[i] = dev_head->bufs[i].dmabuf_fd;
  410. }
  411. } else {
  412. LOG(STF_LEVEL_ERR, "drm: not support io_method %d\n", iomthd);
  413. exit (EXIT_FAILURE);
  414. }
  415. param->dev_head = dev_head;
  416. LOG(STF_LEVEL_TRACE, "Exit\n");
  417. }
  418. void stf_drm_close(DRMParam_t *param)
  419. {
  420. drm_destroy(param->fd, param->dev_head);
  421. }