1#include <stdio.h>
2#include <stdlib.h>
3#include <sys/types.h>
4#include <sys/stat.h>
5#include <sys/mman.h>
6#include <fcntl.h>
7#include <errno.h>
8
9#include <xf86drm.h>
10#include <xf86drmMode.h>
11#include <drm_fourcc.h>
12
13#include "bo.h"
14#include "dev.h"
15
16#define MAKE_YUV_601_Y(r, g, b) \
17	((( 66 * (r) + 129 * (g) +  25 * (b) + 128) >> 8) + 16)
18#define MAKE_YUV_601_U(r, g, b) \
19	(((-38 * (r) -  74 * (g) + 112 * (b) + 128) >> 8) + 128)
20#define MAKE_YUV_601_V(r, g, b) \
21	(((112 * (r) -  94 * (g) -  18 * (b) + 128) >> 8) + 128)
22
23static void draw_rect_yuv(struct sp_bo *bo, uint32_t x, uint32_t y, uint32_t width,
24		uint32_t height, uint8_t a, uint8_t r, uint8_t g, uint8_t b)
25{
26	uint32_t i, j, xmax = x + width, ymax = y + height;
27
28	if (xmax > bo->width)
29		xmax = bo->width;
30	if (ymax > bo->height)
31		ymax = bo->height;
32
33	for (i = y; i < ymax; i++) {
34		uint8_t *luma = bo->map_addr + i * bo->pitch;
35
36		for (j = x; j < xmax; j++)
37			luma[j] = MAKE_YUV_601_Y(r, g, b);
38	}
39
40	for (i = y; i < ymax / 2; i++) {
41		uint8_t *chroma = bo->map_addr + (i + height) * bo->pitch;
42
43		for (j = x; j < xmax / 2; j++) {
44			chroma[j*2] = MAKE_YUV_601_U(r, g, b);
45			chroma[j*2 + 1] = MAKE_YUV_601_V(r, g, b);
46		}
47	}
48}
49
50void fill_bo(struct sp_bo *bo, uint8_t a, uint8_t r, uint8_t g, uint8_t b)
51{
52	if (bo->format == DRM_FORMAT_NV12)
53		draw_rect_yuv(bo, 0, 0, bo->width, bo->height, a, r, g, b);
54	else
55		draw_rect(bo, 0, 0, bo->width, bo->height, a, r, g, b);
56}
57
58void draw_rect(struct sp_bo *bo, uint32_t x, uint32_t y, uint32_t width,
59		uint32_t height, uint8_t a, uint8_t r, uint8_t g, uint8_t b)
60{
61	uint32_t i, j, xmax = x + width, ymax = y + height;
62
63	if (xmax > bo->width)
64		xmax = bo->width;
65	if (ymax > bo->height)
66		ymax = bo->height;
67
68	for (i = y; i < ymax; i++) {
69		uint8_t *row = bo->map_addr + i * bo->pitch;
70
71		for (j = x; j < xmax; j++) {
72			uint8_t *pixel = row + j * 4;
73
74			if (bo->format == DRM_FORMAT_ARGB8888 ||
75			    bo->format == DRM_FORMAT_XRGB8888)
76			{
77				pixel[0] = b;
78				pixel[1] = g;
79				pixel[2] = r;
80				pixel[3] = a;
81			} else if (bo->format == DRM_FORMAT_RGBA8888) {
82				pixel[0] = r;
83				pixel[1] = g;
84				pixel[2] = b;
85				pixel[3] = a;
86			}
87		}
88	}
89}
90
91static int add_fb_sp_bo(struct sp_bo *bo, uint32_t format)
92{
93	int ret;
94	uint32_t handles[4], pitches[4], offsets[4];
95
96	handles[0] = bo->handle;
97	pitches[0] = bo->pitch;
98	offsets[0] = 0;
99	if (bo->format == DRM_FORMAT_NV12) {
100		handles[1] = bo->handle;
101		pitches[1] = pitches[0];
102		offsets[1] = pitches[0] * bo->height;
103	}
104
105	ret = drmModeAddFB2(bo->dev->fd, bo->width, bo->height,
106			format, handles, pitches, offsets,
107			&bo->fb_id, bo->flags);
108	if (ret) {
109		printf("failed to create fb ret=%d\n", ret);
110		return ret;
111	}
112	return 0;
113}
114
115static int map_sp_bo(struct sp_bo *bo)
116{
117	int ret;
118	struct drm_mode_map_dumb md;
119
120	if (bo->map_addr)
121		return 0;
122
123	md.handle = bo->handle;
124	ret = drmIoctl(bo->dev->fd, DRM_IOCTL_MODE_MAP_DUMB, &md);
125	if (ret) {
126		printf("failed to map sp_bo ret=%d\n", ret);
127		return ret;
128	}
129
130	bo->map_addr = mmap(NULL, bo->size, PROT_READ | PROT_WRITE, MAP_SHARED,
131				bo->dev->fd, md.offset);
132	if (bo->map_addr == MAP_FAILED) {
133		printf("failed to map bo ret=%d\n", -errno);
134		return -errno;
135	}
136	return 0;
137}
138
139static int format_to_bpp(uint32_t format)
140{
141	switch (format) {
142	case DRM_FORMAT_NV12:
143		return 8;
144	case DRM_FORMAT_ARGB8888:
145	case DRM_FORMAT_XRGB8888:
146	case DRM_FORMAT_RGBA8888:
147	default:
148		return 32;
149	}
150}
151
152struct sp_bo *create_sp_bo(struct sp_dev *dev, uint32_t width, uint32_t height,
153		uint32_t depth, uint32_t format, uint32_t flags)
154{
155	int ret;
156	struct drm_mode_create_dumb cd;
157	struct sp_bo *bo;
158
159	bo = calloc(1, sizeof(*bo));
160	if (!bo)
161		return NULL;
162
163	if (format == DRM_FORMAT_NV12)
164		cd.height = height * 3 / 2;
165	else
166		cd.height = height;
167
168	cd.width = width;
169	cd.bpp = format_to_bpp(format);
170	cd.flags = flags;
171
172	ret = drmIoctl(dev->fd, DRM_IOCTL_MODE_CREATE_DUMB, &cd);
173	if (ret) {
174		printf("failed to create sp_bo %d\n", ret);
175		goto err;
176	}
177
178	bo->dev = dev;
179	bo->width = width;
180	bo->height = height;
181	bo->depth = depth;
182	bo->bpp = format_to_bpp(format);
183	bo->format = format;
184	bo->flags = flags;
185
186	bo->handle = cd.handle;
187	bo->pitch = cd.pitch;
188	bo->size = cd.size;
189
190	ret = add_fb_sp_bo(bo, format);
191	if (ret) {
192		printf("failed to add fb ret=%d\n", ret);
193		goto err;
194	}
195
196	ret = map_sp_bo(bo);
197	if (ret) {
198		printf("failed to map bo ret=%d\n", ret);
199		goto err;
200	}
201
202	return bo;
203
204err:
205	free_sp_bo(bo);
206	return NULL;
207}
208
209void free_sp_bo(struct sp_bo *bo)
210{
211	int ret;
212	struct drm_mode_destroy_dumb dd;
213
214	if (!bo)
215		return;
216
217	if (bo->map_addr)
218		munmap(bo->map_addr, bo->size);
219
220	if (bo->fb_id) {
221		ret = drmModeRmFB(bo->dev->fd, bo->fb_id);
222		if (ret)
223			printf("Failed to rmfb ret=%d!\n", ret);
224	}
225
226	if (bo->handle) {
227		dd.handle = bo->handle;
228		ret = drmIoctl(bo->dev->fd, DRM_IOCTL_MODE_DESTROY_DUMB, &dd);
229		if (ret)
230			printf("Failed to destroy buffer ret=%d\n", ret);
231	}
232
233	free(bo);
234}
235