1 #include <stdio.h>
2 #include <linux/fb.h>
3 #include <fcntl.h>
4 #include <unistd.h>
5 #include <stdlib.h>
6 #include <sys/mman.h>
7 #include <drm_fourcc.h>
8 #include <string.h>
9
10 #include "./drmDsp/dev.h"
11 #include "./drmDsp/bo.h"
12 #include "./drmDsp/modeset.h"
13 #include "./include/xf86drm.h"
14 #include "./include/xf86drmMode.h"
15 #include "drmDsp.h"
16
17 #if ISPDEMO_ENABLE_RGA
18 #include "rkRgaApi.h"
19 #include "rga.h"
20 #endif
21
22 struct drmDsp {
23 struct fb_var_screeninfo vinfo;
24 unsigned long screensize;
25 char* fbp;
26 struct sp_dev* dev;
27 struct sp_plane** plane;
28 struct sp_crtc* test_crtc;
29 struct sp_plane* test_plane;
30 int num_test_planes;
31 struct sp_bo* bo[2];
32 struct sp_bo* nextbo;
33 } gDrmDsp;
34
initDrmDsp()35 int initDrmDsp() {
36 int ret = 0, i = 0;
37 struct drmDsp* pDrmDsp = &gDrmDsp;
38
39 memset(pDrmDsp, 0, sizeof(struct drmDsp));
40
41 pDrmDsp->dev = create_sp_dev();
42 if (!pDrmDsp->dev) {
43 printf("Failed to create sp_dev\n");
44 return -1;
45 }
46
47 ret = initialize_screens(pDrmDsp->dev);
48 if (ret) {
49 printf("Failed to initialize screens\n");
50 return ret;
51 }
52 pDrmDsp->plane = (struct sp_plane**)calloc(pDrmDsp->dev->num_planes, sizeof(struct sp_plane*));
53 if (!pDrmDsp->plane) {
54 printf("Failed to allocate plane array\n");
55 return -1;
56 }
57
58 pDrmDsp->test_crtc = &pDrmDsp->dev->crtcs[3];
59 pDrmDsp->num_test_planes = pDrmDsp->test_crtc->num_planes;
60 for (i = 0; i < pDrmDsp->test_crtc->num_planes; i++) {
61 pDrmDsp->plane[i] = get_sp_plane(pDrmDsp->dev, pDrmDsp->test_crtc);
62 if (is_supported_format(pDrmDsp->plane[i], DRM_FORMAT_NV12)) {
63 pDrmDsp->test_plane = pDrmDsp->plane[i];
64 break;
65 }
66 }
67 if (!pDrmDsp->test_plane)
68 return -1;
69
70 #if ISPDEMO_ENABLE_RGA
71 rkRgaInit();
72 #endif
73 return ret;
74 }
75
deInitDrmDsp()76 void deInitDrmDsp() {
77 struct drmDsp* pDrmDsp = &gDrmDsp;
78 if (pDrmDsp->bo[0])
79 free_sp_bo(pDrmDsp->bo[0]);
80 if (pDrmDsp->bo[1])
81 free_sp_bo(pDrmDsp->bo[1]);
82 destroy_sp_dev(pDrmDsp->dev);
83 memset(pDrmDsp, 0, sizeof(struct drmDsp));
84 }
85
arm_camera_yuv420_scale_arm(char * srcbuf,char * dstbuf,int src_w,int src_h,int dst_w,int dst_h)86 static int arm_camera_yuv420_scale_arm(char *srcbuf, char *dstbuf,int src_w, int src_h,int dst_w, int dst_h) {
87 unsigned char *psY = NULL,*pdY = NULL,*psUV = NULL,*pdUV = NULL;
88 // unsigned char *src,*dst;
89 int srcW,srcH,cropW,cropH,dstW,dstH;
90 long zoomindstxIntInv,zoomindstyIntInv;
91 long x,y;
92 long yCoeff00,yCoeff01,xCoeff00,xCoeff01;
93 long sX,sY;
94 long r0,r1,a,b,c,d;
95 int nv21DstFmt = 0, mirror = 0;
96 int ratio = 0;
97 int top_offset=0,left_offset=0;
98
99 //need crop ?
100 if((src_w*100/src_h) != (dst_w*100/dst_h)){
101 ratio = ((src_w*100/dst_w) >= (src_h*100/dst_h))?(src_h*100/dst_h):(src_w*100/dst_w);
102 cropW = ratio*dst_w/100;
103 cropH = ratio*dst_h/100;
104
105 left_offset=((src_w-cropW)>>1) & (~0x01);
106 top_offset=((src_h-cropH)>>1) & (~0x01);
107 }else{
108 cropW = src_w;
109 cropH = src_h;
110 top_offset=0;
111 left_offset=0;
112 }
113
114 // src = psY = (unsigned char*)(srcbuf)+top_offset*src_w+left_offset;
115 //psUV = psY +src_w*src_h+top_offset*src_w/2+left_offset;
116 psUV = (unsigned char*)(srcbuf) +src_w*src_h+top_offset*src_w/2+left_offset;
117
118
119 srcW =src_w;
120 srcH = src_h;
121 // cropW = src_w;
122 // cropH = src_h;
123
124
125 // dst = pdY = (unsigned char*)dstbuf;
126 pdUV = pdY + dst_w*dst_h;
127 dstW = dst_w;
128 dstH = dst_h;
129
130 zoomindstxIntInv = ((unsigned long)(cropW)<<16)/dstW + 1;
131 zoomindstyIntInv = ((unsigned long)(cropH)<<16)/dstH + 1;
132 //y
133 //for(y = 0; y<dstH - 1 ; y++ ) {
134 for(y = 0; y<dstH; y++ ) {
135 yCoeff00 = (y*zoomindstyIntInv)&0xffff;
136 yCoeff01 = 0xffff - yCoeff00;
137 sY = (y*zoomindstyIntInv >> 16);
138 sY = (sY >= srcH - 1)? (srcH - 2) : sY;
139 for(x = 0; x<dstW; x++ ) {
140 xCoeff00 = (x*zoomindstxIntInv)&0xffff;
141 xCoeff01 = 0xffff - xCoeff00;
142 sX = (x*zoomindstxIntInv >> 16);
143 sX = (sX >= srcW -1)?(srcW- 2) : sX;
144 a = psY[sY*srcW + sX];
145 b = psY[sY*srcW + sX + 1];
146 c = psY[(sY+1)*srcW + sX];
147 d = psY[(sY+1)*srcW + sX + 1];
148
149 r0 = (a * xCoeff01 + b * xCoeff00)>>16 ;
150 r1 = (c * xCoeff01 + d * xCoeff00)>>16 ;
151 r0 = (r0 * yCoeff01 + r1 * yCoeff00)>>16;
152
153 if(mirror)
154 pdY[dstW -1 - x] = r0;
155 else
156 pdY[x] = r0;
157 }
158 pdY += dstW;
159 }
160
161 dstW /= 2;
162 dstH /= 2;
163 srcW /= 2;
164 srcH /= 2;
165
166 //UV
167 //for(y = 0; y<dstH - 1 ; y++ ) {
168 for(y = 0; y<dstH; y++ ) {
169 yCoeff00 = (y*zoomindstyIntInv)&0xffff;
170 yCoeff01 = 0xffff - yCoeff00;
171 sY = (y*zoomindstyIntInv >> 16);
172 sY = (sY >= srcH -1)? (srcH - 2) : sY;
173 for(x = 0; x<dstW; x++ ) {
174 xCoeff00 = (x*zoomindstxIntInv)&0xffff;
175 xCoeff01 = 0xffff - xCoeff00;
176 sX = (x*zoomindstxIntInv >> 16);
177 sX = (sX >= srcW -1)?(srcW- 2) : sX;
178 //U
179 a = psUV[(sY*srcW + sX)*2];
180 b = psUV[(sY*srcW + sX + 1)*2];
181 c = psUV[((sY+1)*srcW + sX)*2];
182 d = psUV[((sY+1)*srcW + sX + 1)*2];
183
184 r0 = (a * xCoeff01 + b * xCoeff00)>>16 ;
185 r1 = (c * xCoeff01 + d * xCoeff00)>>16 ;
186 r0 = (r0 * yCoeff01 + r1 * yCoeff00)>>16;
187
188 if(mirror && nv21DstFmt)
189 pdUV[dstW*2-1- (x*2)] = r0;
190 else if(mirror)
191 pdUV[dstW*2-1-(x*2+1)] = r0;
192 else if(nv21DstFmt)
193 pdUV[x*2 + 1] = r0;
194 else
195 pdUV[x*2] = r0;
196 //V
197 a = psUV[(sY*srcW + sX)*2 + 1];
198 b = psUV[(sY*srcW + sX + 1)*2 + 1];
199 c = psUV[((sY+1)*srcW + sX)*2 + 1];
200 d = psUV[((sY+1)*srcW + sX + 1)*2 + 1];
201
202 r0 = (a * xCoeff01 + b * xCoeff00)>>16 ;
203 r1 = (c * xCoeff01 + d * xCoeff00)>>16 ;
204 r0 = (r0 * yCoeff01 + r1 * yCoeff00)>>16;
205
206 if(mirror && nv21DstFmt)
207 pdUV[dstW*2-1- (x*2+1) ] = r0;
208 else if(mirror)
209 pdUV[dstW*2-1-(x*2)] = r0;
210 else if(nv21DstFmt)
211 pdUV[x*2] = r0;
212 else
213 pdUV[x*2 + 1] = r0;
214 }
215 pdUV += dstW*2;
216 }
217 return 0;
218 }
219
drmDspFrame(int srcWidth,int srcHeight,int dispWidth,int dispHeight,int dmaFd,void * srcAddr,int fmt)220 int drmDspFrame(int srcWidth, int srcHeight, int dispWidth, int dispHeight,
221 int dmaFd, void* srcAddr, int fmt)
222 {
223 int ret;
224 // struct drm_mode_create_dumb cd;
225 struct sp_bo* bo;
226 struct drmDsp* pDrmDsp = &gDrmDsp;
227
228 int wAlign16 = ((dispWidth+ 15) & (~15));
229 int hAlign16 = (dispHeight + 15) & (~15);
230 // int frameSize = wAlign16 * hAlign16 * 3 / 2;
231 uint32_t handles[4], pitches[4], offsets[4];
232
233 if (DRM_FORMAT_NV12 != fmt) {
234 printf("%s just support NV12 to display\n", __func__);
235 return -1;
236 }
237 //create bo
238 #if 1
239 if (!pDrmDsp->bo[0]) {
240 printf("%s:bo widthxheight:%dx%d\n", __func__, wAlign16, hAlign16);
241 pDrmDsp->bo[0] = create_sp_bo(pDrmDsp->dev, wAlign16, hAlign16,
242 16, 32, DRM_FORMAT_NV12, 0);
243 pDrmDsp->bo[1] = create_sp_bo(pDrmDsp->dev, wAlign16, hAlign16,
244 16, 32, DRM_FORMAT_NV12, 0);
245 if (!pDrmDsp->bo[0] || !pDrmDsp->bo[1]) {
246 printf("%s:create bo failed ! \n", __func__);
247 return -1;
248 }
249 pDrmDsp->nextbo = pDrmDsp->bo[0];
250 }
251
252 if (!pDrmDsp->nextbo) {
253 printf("%s:no available bo ! \n", __func__);
254 return -1;
255 }
256
257 bo = pDrmDsp->nextbo;
258 #else
259 bo = create_sp_bo(pDrmDsp->dev, wAlign16, hAlign16,
260 16, 32, DRM_FORMAT_NV12, 0);
261 if (!bo)
262 printf("%s:create bo failed ! \n", __func__);
263 #endif
264
265 handles[0] = bo->handle;
266 pitches[0] = wAlign16;
267 offsets[0] = 0;
268 handles[1] = bo->handle;
269 pitches[1] = wAlign16;
270 offsets[1] = dispWidth * dispHeight; //wAlign16 * hAlign16;
271
272 #if ISPDEMO_ENABLE_RGA
273 struct rkRgaCfg src_cfg, dst_cfg;
274
275 src_cfg.fd = dmaFd;
276 src_cfg.addr = 0;
277 src_cfg.fmt = RK_FORMAT_YCrCb_420_SP;
278 src_cfg.width = srcWidth;
279 src_cfg.height = srcHeight;
280
281 dst_cfg.fd = bo->fd;
282 dst_cfg.addr = 0;
283 dst_cfg.fmt = RK_FORMAT_YCrCb_420_SP;
284 dst_cfg.width = dispWidth;
285 dst_cfg.height = dispHeight;
286
287 rkRgaBlit(&src_cfg, &dst_cfg);
288 #else
289 //copy src data to bo
290 if (srcWidth == dispWidth)
291 memcpy(bo->map_addr, srcAddr, wAlign16 * hAlign16 * 3 / 2);
292 else
293 arm_camera_yuv420_scale_arm(srcAddr, bo->map_addr, srcWidth, srcHeight, dispWidth, dispHeight);
294 #endif
295
296 ret = drmModeAddFB2(bo->dev->fd, bo->width, bo->height,
297 bo->format, handles, pitches, offsets,
298 &bo->fb_id, bo->flags);
299 if (ret) {
300 printf("%s:failed to create fb ret=%d\n", __func__, ret);
301 printf("fd:%d ,wxh:%ux%u,format:%u,handles:%u,%u,pictches:%u,%u,offsets:%u,%u,fb_id:%u,flags:%u \n",
302 bo->dev->fd, bo->width, bo->height, bo->format,
303 handles[0], handles[1], pitches[0], pitches[1],
304 offsets[0], offsets[1], bo->fb_id, bo->flags);
305 return ret;
306 }
307
308 ret = drmModeSetPlane(pDrmDsp->dev->fd, pDrmDsp->test_plane->plane->plane_id,
309 pDrmDsp->test_crtc->crtc->crtc_id, bo->fb_id, 0, 0, 0,
310 //pDrmDsp->test_crtc->crtc->mode.hdisplay,
311 wAlign16, hAlign16,
312 //pDrmDsp->test_crtc->crtc->mode.vdisplay,
313 0, 0, dispWidth << 16, dispHeight << 16);
314 if (ret) {
315 printf("failed to set plane to crtc ret=%d\n", ret);
316 return ret;
317 }
318 //free_sp_bo(bo);
319 #if 0
320 if (pDrmDsp->test_plane->bo) {
321 if (pDrmDsp->test_plane->bo->fb_id) {
322 ret = drmModeRmFB(pDrmDsp->dev->fd, pDrmDsp->test_plane->bo->fb_id);
323 if (ret)
324 printf("Failed to rmfb ret=%d!\n", ret);
325 }
326 if (pDrmDsp->test_plane->bo->handle) {
327 struct drm_gem_close req = {
328 .handle = pDrmDsp->test_plane->bo->handle,
329 };
330
331 drmIoctl(bo->dev->fd, DRM_IOCTL_GEM_CLOSE, &req);
332 printf("%s:close bo success!\n", __func__);
333 }
334
335 if (!pDrmDsp->nextbo)
336 free_sp_bo(pDrmDsp->test_plane->bo);
337 }
338 pDrmDsp->test_plane->bo = bo; //last po
339 #endif
340 #if 1
341 //switch bo
342 if (pDrmDsp->nextbo == pDrmDsp->bo[0])
343 pDrmDsp->nextbo = pDrmDsp->bo[1];
344 else
345 pDrmDsp->nextbo = pDrmDsp->bo[0];
346 #endif
347 return ret;
348 }
349