embox

1
/**
2
 * @file
3
 * @brief
4
 *
5
 * @date 30.01.13
6
 * @author Ilia Vaprol
7
 */
8

9
#include <assert.h>
10
#include <stdio.h>
11
#include <stdlib.h>
12
#include <string.h>
13
#include <limits.h>
14
#include <errno.h>
15

16
#include <kernel/thread/sync/mutex.h>
17
#include <kernel/printk.h>
18
#include <lib/libds/dlist.h>
19
#include <util/log.h>
20
#include <util/math.h>
21

22
#include <drivers/video/fb.h>
23

24
#include <framework/mod/options.h>
25
#include <mem/misc/pool.h>
26

27
#define MODOPS_FB_AMOUNT OPTION_GET(NUMBER, fb_amount)
28

29
struct fb_dev {
30
	struct dlist_head link;
31
	struct fb_info info;
32
};
33

34
static int fb_update_current_var(struct fb_info *info);
35

36
static void fb_default_copyarea(struct fb_info *info, const struct fb_copyarea *area);
37
static void fb_default_cursor(struct fb_info *info, const struct fb_cursor *cursor);
38
static void fb_default_imageblit(struct fb_info *info, const struct fb_image *image);
39
static void fb_default_fillrect(struct fb_info *info, const struct fb_fillrect *rect);
40

41
struct mutex fb_static = MUTEX_INIT_STATIC;
42
POOL_DEF(fb_pool, struct fb_dev, MODOPS_FB_AMOUNT);
43
static DLIST_DEFINE(fb_list);
44
static unsigned int fb_count = 0;
45

46
#define fb_readb(addr)       (*(uint8_t *) (addr))
47
#define fb_readw(addr)       (*(uint16_t *) (addr))
48
#define fb_readl(addr)       (*(uint32_t *) (addr))
49
#define fb_writeb(val, addr) (*(uint8_t *) (addr) = (val))
50
#define fb_writew(val, addr) (*(uint16_t *) (addr) = (val))
51
#define fb_writel(val, addr) (*(uint32_t *) (addr) = (val))
52
#define fb_memset            memset
53
#define fb_memcpy_fromfb     memcpy
54
#define fb_memcpy_tofb       memcpy
55

56
static void fb_ops_fixup(struct fb_ops *ops) {
57
	if (!ops->fb_copyarea) {
58
		ops->fb_copyarea = fb_default_copyarea;
59
	}
60
	if (!ops->fb_imageblit) {
61
		ops->fb_imageblit = fb_default_imageblit;
62
	}
63
	if (!ops->fb_fillrect) {
64
		ops->fb_fillrect = fb_default_fillrect;
65
	}
66
	if (!ops->fb_cursor) {
67
		ops->fb_cursor = fb_default_cursor;
68
	}
69
}
70

71
struct fb_info *fb_create(const struct fb_ops *ops, char *map_base, size_t map_size) {
72
	struct fb_dev *dev;
73
	struct fb_info *info = NULL;
74

75
	assert(ops);
76

77
	mutex_lock(&fb_static);
78
	{
79
		dev = pool_alloc(&fb_pool);
80
		if (!dev) {
81
			log_error("Failed to create framebuffer");
82
			goto out;
83
		}
84

85
		info = &dev->info;
86

87
		info->id = fb_count++;
88
		dlist_init(&dev->link);
89
		dlist_add_next(&dev->link, &fb_list);
90
		info->screen_base = map_base;
91
		info->screen_size = map_size;
92

93
		memcpy(&info->ops, ops, sizeof(struct fb_ops));
94
		fb_ops_fixup(&info->ops);
95
		fb_update_current_var(info);
96
		fb_devfs_create(info, map_base, map_size);
97
	}
98
out:
99
	mutex_unlock(&fb_static);
100

101
	return info;
102
}
103

104
void fb_delete(struct fb_info *info) {
105
	struct fb_dev *dev = member_cast_out(info, struct fb_dev, info);
106

107
	if (info) {
108
		mutex_lock(&fb_static);
109
		{
110
			dlist_del(&dev->link);
111
			pool_free(&fb_pool, dev);
112
		}
113
		mutex_unlock(&fb_static);
114
	}
115
}
116

117
struct fb_info *fb_lookup(int id) {
118
	struct fb_info *ret;
119

120
	ret = NULL;
121
	mutex_lock(&fb_static);
122
	{
123
		struct fb_dev *fb_dev;
124
		dlist_foreach_entry(fb_dev, &fb_list, link) {
125
			if (fb_dev->info.id == id) {
126
				ret = &fb_dev->info;
127
				break;
128
			}
129
		}
130
	}
131
	mutex_unlock(&fb_static);
132
	return ret;
133
}
134

135
int fb_set_var(struct fb_info *info, const struct fb_var_screeninfo *var) {
136
	int ret;
137

138
	assert(info != NULL);
139
	assert(var != NULL);
140

141
	if (info->ops.fb_set_var != NULL) {
142
		ret = info->ops.fb_set_var(info, var);
143
		if (ret < 0) {
144
			return ret;
145
		}
146
		memcpy(&info->var, var, sizeof(struct fb_var_screeninfo));
147
	}
148

149
	return 0;
150
}
151

152
int fb_get_var(struct fb_info *info, struct fb_var_screeninfo *var) {
153
	memcpy(var, &info->var, sizeof(struct fb_var_screeninfo));
154
	return 0;
155
}
156

157
void fb_copyarea(struct fb_info *info, const struct fb_copyarea *area) {
158
	info->ops.fb_copyarea(info, area);
159
}
160

161
void fb_cursor(struct fb_info *info, const struct fb_cursor *cursor) {
162
	info->ops.fb_cursor(info, cursor);
163
}
164

165
void fb_imageblit(struct fb_info *info, const struct fb_image *image) {
166
	info->ops.fb_imageblit(info, image);
167
}
168

169
#define _val_fixup(x, low, high) (min((high), max((low), (x))))
170
void fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect) {
171
	assert(info);
172
	assert(rect);
173

174
	/* Don't touch original values if fixup needed */
175
	struct fb_fillrect r = *rect;
176
	r.dx     = _val_fixup(rect->dx, 0, info->var.xres);
177
	r.dy     = _val_fixup(rect->dy, 0, info->var.yres);
178
	r.width  = _val_fixup(rect->width, 0, info->var.xres - r.dx);
179
	r.height = _val_fixup(rect->height, 0, info->var.yres - r.dy);
180

181
	info->ops.fb_fillrect(info, &r);
182
}
183

184
static int fb_update_current_var(struct fb_info *info) {
185
	if (info->ops.fb_get_var != NULL) {
186
		return info->ops.fb_get_var(info, &info->var);
187
	}
188
	return -ENOENT;
189
}
190

191
static void bitcpy(uint32_t *dst, uint32_t dstn, uint32_t *src, uint32_t srcn,
192
		uint32_t len) {
193
	uint32_t mask1, mask2, loff, roff, lval, rval, left;
194
	int32_t shift;
195

196
	assert(dst != NULL);
197
	assert(src != NULL);
198

199
	if (len == 0) return;
200

201
	mask1 = ~(uint32_t)0 << dstn;
202
	mask2 = ~(~(uint32_t)0 << (dstn + len) % (sizeof(*dst) * CHAR_BIT));
203
	shift = dstn - srcn;
204

205
	if (shift == 0) {
206
		if (dstn + len <= sizeof(*dst) * CHAR_BIT) {
207
			mask1 &= mask2 != 0 ? mask2 : mask1;
208
			fb_writel((fb_readl(src) & mask1) | (fb_readl(dst) & ~mask1), dst);
209
		}
210
		else {
211
			if (mask1 != 0) {
212
				fb_writel((fb_readl(src) & mask1) | (fb_readl(dst) & ~mask1), dst);
213
				++dst, ++src;
214
				len -= sizeof(*dst) * CHAR_BIT - dstn;
215
			}
216

217
			len /= sizeof(*dst) * CHAR_BIT;
218
			while (len-- != 0) {
219
				fb_writel(fb_readl(src), dst);
220
				++dst;
221
				++src;
222
			}
223

224
			fb_writel((fb_readl(src) & mask2) | (fb_readl(dst) & ~mask2), dst);
225
		}
226
	}
227
	else {
228
		loff = (uint32_t)-shift % (sizeof(*dst) * CHAR_BIT);
229
		roff = (uint32_t)shift % (sizeof(*dst) * CHAR_BIT);
230

231
		if (dstn + len <= sizeof(*dst) * CHAR_BIT) {
232
			mask1 &= mask2 != 0 ? mask2 : mask1;
233
			if (shift > 0)
234
				fb_writel(((fb_readl(src) >> roff) & mask1) | (fb_readl(dst) & ~mask1), dst);
235
			else if (srcn + len <= sizeof(*dst) * CHAR_BIT)
236
				fb_writel(((fb_readl(src) << loff) & mask1) | (fb_readl(dst) & ~mask1), dst);
237
			else {
238
				lval = fb_readl(src);
239
				++src;
240
				rval = fb_readl(src);
241
				fb_writel((((lval << loff) | (rval >> roff)) & mask1) | (fb_readl(dst) & ~mask1), dst);
242
			}
243
		}
244
		else {
245
			lval = fb_readl(src);
246
			++src;
247
			if (shift > 0) {
248
				fb_writel(((lval >> roff) & mask1) | (fb_readl(dst) & ~mask1), dst);
249
				++dst;
250
				len -= sizeof(*dst) * CHAR_BIT - dstn;
251
			}
252
			else {
253
				rval = fb_readl(src);
254
				++src;
255
				fb_writel((((lval << loff) | (fb_readl(dst) >> roff)) & mask1) | (fb_readl(dst) & ~mask1), dst);
256
				lval = rval;
257
				++dst;
258
				len -= sizeof(*dst) * CHAR_BIT - dstn;
259
			}
260

261
			left = len % (sizeof(*dst) * CHAR_BIT);
262
			len /= sizeof(*dst) * CHAR_BIT;
263
			while (len-- != 0) {
264
				rval = fb_readl(src);
265
				++src;
266
				fb_writel((lval << loff) | (rval >> roff), dst);
267
				++dst;
268
				lval = rval;
269
			}
270

271
			if (left <= roff)
272
				fb_writel(((lval << loff) & mask2) | (fb_readl(dst) & ~mask2), dst);
273
			else {
274
				rval = fb_readl(src);
275
				fb_writel((((lval << loff) | (rval >> roff)) & mask2) | (fb_readl(dst) & ~mask2), dst);
276
			}
277
		}
278
	}
279
}
280

281
static void bitcpy_rev(uint32_t *dst, uint32_t dstn, uint32_t *src,
282
		uint32_t srcn, uint32_t len) {
283
	uint32_t mask1, mask2, roff, loff, rval, lval, left;
284
	int32_t shift;
285

286
	assert(dst != NULL);
287
	assert(src != NULL);
288

289
	if (len == 0) return;
290

291
	dst += (len - 1) / (sizeof(*dst) * CHAR_BIT);
292
	src += (len - 1) / (sizeof(*src) * CHAR_BIT);
293
	if ((len - 1) % (sizeof(*dst) * CHAR_BIT) != 0) {
294
		dstn += (len - 1) % (sizeof(*dst) * CHAR_BIT);
295
		dst += dstn / (sizeof(*dst) * CHAR_BIT);
296
		dstn %= sizeof(*dst) * CHAR_BIT;
297
		srcn += (len - 1) % (sizeof(*src) * CHAR_BIT);
298
		src += srcn / (sizeof(*src) * CHAR_BIT);
299
		srcn %= sizeof(*src) * CHAR_BIT;
300
	}
301

302
	mask1 = ~(uint32_t)0 >> (sizeof(*dst) * CHAR_BIT - 1 - dstn);
303
	mask2 = ~(~(uint32_t)0 >> (sizeof(*dst) * CHAR_BIT - 1 - (dstn - len) % (sizeof(*dst) * CHAR_BIT)));
304
	shift = dstn - srcn;
305

306
	if (shift == 0) {
307
		if (dstn + 1 >= len) {
308
			mask1 &= mask2 != 0 ? mask2 : mask1;
309
			fb_writel((fb_readl(src) & mask1) | (fb_readl(dst) & ~mask1), dst);
310
		}
311
		else {
312
			if (mask1 != 0) {
313
				fb_writel((fb_readl(src) & mask1) | (fb_readl(dst) & ~mask1), dst);
314
				--dst;
315
				--src;
316
				len -= dstn + 1;
317
			}
318

319
			len /= sizeof(*dst) * CHAR_BIT;
320
			while (len-- != 0) {
321
				fb_writel(fb_readl(src), dst);
322
				--dst;
323
				--src;
324
			}
325

326
			fb_writel((fb_readl(src) & mask2) | (fb_readl(dst) & ~mask2), dst);
327
		}
328
	}
329
	else {
330
		roff = (uint32_t)shift % (sizeof(*dst) * CHAR_BIT);
331
		loff = (uint32_t)-shift % (sizeof(*dst) * CHAR_BIT);
332

333
		if (dstn + 1 >= len) {
334
			mask1 &= mask2 != 0 ? mask2 : mask1;
335
			if (shift < 0)
336
				fb_writel(((fb_readl(src) << loff) & mask1) | (fb_readl(dst) & ~mask1), dst);
337
			else if (srcn + 1 >= len)
338
				fb_writel(((fb_readl(src) >> roff) & mask1) | (fb_readl(dst) & ~mask1), dst);
339
			else {
340
				rval = fb_readl(src);
341
				--src;
342
				lval = fb_readl(src);
343
				fb_writel((((rval >> roff) | (lval << loff)) & mask1) | (fb_readl(dst) & ~mask1), dst);
344
			}
345
		}
346
		else {
347
			rval = fb_readl(src);
348
			--src;
349
			if (shift < 0) {
350
				fb_writel(((rval << loff) & mask1) | (fb_readl(dst) & ~mask1), dst);
351
				--dst;
352
				len -= dstn + 1;
353
			}
354
			else {
355
				lval = fb_readl(src);
356
				--src;
357
				fb_writel((((rval >> roff) | (lval << loff)) & mask1) | (fb_readl(dst) & ~mask1), dst);
358
				rval = lval;
359
				--dst;
360
				len -= dstn + 1;
361
			}
362

363
			left = len % (sizeof(*dst) * CHAR_BIT);
364
			len /= sizeof(*dst) * CHAR_BIT;
365
			while (len-- != 0) {
366
				lval = fb_readl(src);
367
				--src;
368
				fb_writel((rval >> roff) | (lval << loff), dst);
369
				--dst;
370
				rval = lval;
371
			}
372

373
			if (left <= loff)
374
				fb_writel(((rval >> roff) & mask2) | (fb_readl(dst) & ~mask2), dst);
375
			else {
376
				lval = fb_readl(src);
377
				fb_writel((((rval << roff) | (lval >> loff)) & mask2) | (fb_readl(dst) & ~mask2), dst);
378
			}
379
		}
380
	}
381
}
382

383
static void fb_default_copyarea(struct fb_info *info, const struct fb_copyarea *area) {
384
	uint32_t width, height, *dst, dstn, *src, srcn;
385

386
	assert(info != NULL);
387
	assert(area != NULL);
388

389
	if ((area->dx >= info->var.xres) || (area->dy >= info->var.yres)
390
			|| (area->sx >= info->var.xres) || (area->sy >= info->var.yres)) return;
391

392
	width = min(area->width, info->var.xres - max(area->sx, area->dx));
393
	height = min(area->height, info->var.yres - max(area->sy, area->dy));
394

395
	assert(info->screen_base != NULL);
396
	dstn = srcn = (uintptr_t)info->screen_base % sizeof(*dst);
397
	dst = src = (uint32_t *)((uintptr_t)info->screen_base - dstn);
398
	dstn = dstn * CHAR_BIT + (area->dy * info->var.xres
399
			+ area->dx) * info->var.bits_per_pixel;
400
	srcn = srcn * CHAR_BIT + (area->sy * info->var.xres
401
			+ area->sx) * info->var.bits_per_pixel;
402

403
	if (((area->dy == area->sy) && (area->dx > area->sx))
404
			|| (area->dy > area->sy)) {
405
		dstn += height * info->var.xres * info->var.bits_per_pixel;
406
		srcn += height * info->var.xres * info->var.bits_per_pixel;
407
		while (height-- != 0) {
408
			dstn -= info->var.xres * info->var.bits_per_pixel;
409
			dst += dstn / (sizeof(*dst) * CHAR_BIT);
410
			dstn %= sizeof(*dst) * CHAR_BIT;
411
			srcn -= info->var.xres * info->var.bits_per_pixel;
412
			src += srcn / (sizeof(*src) * CHAR_BIT);
413
			srcn %= sizeof(*src) * CHAR_BIT;
414
			bitcpy_rev(dst, dstn, src, srcn, width * info->var.bits_per_pixel);
415
		}
416
	}
417
	else {
418
		while (height-- != 0) {
419
			dst += dstn / (sizeof(*dst) * CHAR_BIT);
420
			dstn %= sizeof(*dst) * CHAR_BIT;
421
			src += srcn / (sizeof(*src) * CHAR_BIT);
422
			srcn %= sizeof(*src) * CHAR_BIT;
423
			bitcpy(dst, dstn, src, srcn, width * info->var.bits_per_pixel);
424
			dstn += info->var.xres * info->var.bits_per_pixel;
425
			srcn += info->var.xres * info->var.bits_per_pixel;
426
		}
427
	}
428
}
429

430
static uint32_t pixel_to_pat(uint32_t bpp, uint32_t pixel) {
431
	pixel &= ~((uint32_t )-1 << bpp);
432
	return bpp == 1 ? 0xffffffffUL * pixel
433
			: bpp == 2 ? 0x55555555UL * pixel
434
			: bpp == 4 ? 0x11111111UL * pixel
435
			: bpp == 8 ? 0x01010101UL * pixel
436
			: bpp == 12 ? 0x01001001UL * pixel
437
			: bpp == 16 ? 0x00010001UL * pixel
438
			: bpp == 24 ? 0x01000001UL * pixel
439
			: bpp == 32 ? 0x00000001UL * pixel
440
			: 0xbadffbad;
441
}
442

443
static void bitfill(uint32_t *dst, uint32_t dstn, uint32_t pat,
444
		uint32_t loff, uint32_t roff, uint32_t len) {
445
	uint32_t mask1, mask2;
446

447
	assert(dst != NULL);
448

449
	if (len == 0) return;
450

451
	mask1 = ~(uint32_t)0 << dstn;
452
	mask2 = ~(~(uint32_t)0 << (dstn + len) % (sizeof(*dst) * CHAR_BIT));
453

454
	if (dstn + len <= sizeof(*dst) * CHAR_BIT) {
455
		mask1 &= mask2 != 0 ? mask2 : mask1;
456
		fb_writel((pat & mask1) | (fb_readl(dst) & ~mask1), dst);
457
	}
458
	else {
459
		if (mask1 != 0) {
460
			fb_writel((pat & mask1) | (fb_readl(dst) & ~mask1), dst);
461
			++dst;
462
			pat = (pat << loff) | (pat >> roff);
463
			len -= sizeof(*dst) * CHAR_BIT - dstn;
464
		}
465

466
		len /= sizeof(*dst) * CHAR_BIT;
467
		while (len-- != 0) {
468
			fb_writel(pat, dst);
469
			++dst;
470
			pat = (pat << loff) | (pat >> roff);
471
		}
472

473
		fb_writel((pat & mask2) | (fb_readl(dst) & ~mask2), dst);
474
	}
475
}
476

477
static void bitfill_rev(uint32_t *dst, uint32_t dstn, uint32_t pat,
478
		uint32_t loff, uint32_t roff, uint32_t len) {
479
	uint32_t mask1, mask2;
480

481
	assert(dst != NULL);
482

483
	if (len == 0) return;
484

485
	mask1 = ~(uint32_t)0 << dstn;
486
	mask2 = ~(~(uint32_t)0 << (dstn + len) % (sizeof(*dst) * CHAR_BIT));
487

488
	if (dstn + len <= sizeof(*dst) * CHAR_BIT) {
489
		mask1 &= mask2 != 0 ? mask2 : mask1;
490
		fb_writel(((fb_readl(dst) ^ pat) & mask1) | (fb_readl(dst) & ~mask1), dst);
491
	}
492
	else {
493
		if (mask1 != 0) {
494
			fb_writel(((fb_readl(dst) ^ pat) & mask1) | (fb_readl(dst) & ~mask1), dst);
495
			++dst;
496
			pat = (pat << loff) | (pat >> roff);
497
			len -= sizeof(*dst) * CHAR_BIT - dstn;
498
		}
499

500
		len /= sizeof(*dst) * CHAR_BIT;
501
		while (len >= 4) {
502
			fb_writel(fb_readl(dst) ^ pat, dst);
503
			++dst;
504
			pat = (pat << loff) | (pat >> roff);
505
			fb_writel(fb_readl(dst) ^ pat, dst);
506
			++dst;
507
			pat = (pat << loff) | (pat >> roff);
508
			fb_writel(fb_readl(dst) ^ pat, dst);
509
			++dst;
510
			pat = (pat << loff) | (pat >> roff);
511
			fb_writel(fb_readl(dst) ^ pat, dst);
512
			++dst;
513
			pat = (pat << loff) | (pat >> roff);
514
			len -= 4;
515
		}
516
		while (len-- != 0) {
517
			fb_writel(fb_readl(dst) ^ pat, dst);
518
			pat = (pat << loff) | (pat >> roff);
519
		}
520

521
		fb_writel(((fb_readl(dst) ^ pat) & mask2) | (fb_readl(dst) & ~mask2), dst);
522
	}
523
}
524

525
static void fb_default_fillrect(struct fb_info *info, const struct fb_fillrect *rect) {
526
	uint32_t width, height, pat_orig, pat, *dst, dstn, loff, roff;
527
	void (*fill_op)(uint32_t *dst, uint32_t dstn, uint32_t pat,
528
		uint32_t loff, uint32_t roff, uint32_t len);
529

530
	assert(info != NULL);
531
	assert(rect != NULL);
532

533
	if ((rect->dx >= info->var.xres) || (rect->dy >= info->var.yres)) return;
534

535
	width = min(rect->width, info->var.xres - rect->dx);
536
	height = min(rect->height, info->var.yres - rect->dy);
537

538
	pat_orig = pixel_to_pat(info->var.bits_per_pixel, rect->color);
539

540
	assert(info->screen_base != NULL);
541
	dstn = (uintptr_t)info->screen_base % sizeof(*dst);
542
	dst = (uint32_t *)((uintptr_t)info->screen_base - dstn);
543
	dstn = dstn * CHAR_BIT + (rect->dy * info->var.xres
544
			+ rect->dx) * info->var.bits_per_pixel;
545
	roff = sizeof(*dst) * CHAR_BIT % info->var.bits_per_pixel;
546
	loff = info->var.bits_per_pixel - roff;
547

548
	fill_op = rect->rop == ROP_COPY ? &bitfill : &bitfill_rev;
549

550
	while (height-- != 0) {
551
		dst += dstn / (sizeof(*dst) * CHAR_BIT);
552
		dstn %= sizeof(*dst) * CHAR_BIT;
553

554
		pat = roff == 0 ? pat_orig : (pat_orig << (dstn % info->var.bits_per_pixel))
555
				| (pat_orig >> (info->var.bits_per_pixel - dstn % info->var.bits_per_pixel));
556

557
		fill_op(dst, dstn, pat, loff, roff, width * info->var.bits_per_pixel);
558
		dstn += info->var.xres * info->var.bits_per_pixel;
559
	}
560
}
561

562
static void fb_default_imageblit(struct fb_info *info, const struct fb_image *image) {
563
	/* TODO it's slow version:) */
564
	uint32_t i, j;
565
	struct fb_fillrect rect;
566

567
	assert(info != NULL);
568
	assert(image != NULL);
569
	assert(image->depth == 1);
570
	assert(image->width == 8);
571

572
	rect.width = rect.height = 1;
573
	rect.rop = ROP_COPY;
574
	for (j = 0; j < image->height; ++j) {
575
		rect.dy = image->dy + j * rect.height;
576
		for (i = 0; i < image->width; ++i) {
577
			rect.dx = image->dx + i * rect.width;
578
			rect.color = *(uint8_t *)(image->data + j) & (1 << ((image->width - i - 1)))
579
					? image->fg_color : image->bg_color;
580
			info->ops.fb_fillrect(info, &rect);
581
		}
582
	}
583
}
584

585
static void fb_default_cursor(struct fb_info *info, const struct fb_cursor *cursor) {
586
	uint32_t i, j;
587
	struct fb_fillrect rect;
588

589
	assert(info != NULL);
590
	assert(cursor != NULL);
591

592
	if (!cursor->enable) return;
593

594
	rect.width = rect.height = 1;
595
	rect.rop = cursor->rop;
596
	rect.color = cursor->image.fg_color;
597
	for (j = 0; j < cursor->image.height; ++j) {
598
		rect.dy = cursor->hot.y * cursor->image.height + j * rect.height;
599
		for (i = 0; i < cursor->image.width; ++i) {
600
			rect.dx = cursor->hot.x * cursor->image.width + i * rect.width;
601
			info->ops.fb_fillrect(info, &rect);
602
		}
603
	}
604
}
605

606
int pix_fmt_has_alpha(enum pix_fmt fmt) {
607
	return (fmt == RGBA8888) || (fmt == BGRA8888);
608
}
609

610
int pix_fmt_bpp(enum pix_fmt fmt) {
611
	switch (fmt) {
612
	case RGB888:
613
	case BGR888:
614
	case RGBA8888:
615
	case BGRA8888:
616
		return 32;
617
	case RGB565:
618
	case BGR565:
619
		return 16;
620
	default:
621
		log_error("Wrong pixel format=%d, assume bpp=1", fmt);
622
		return 8;
623
	}
624
}
625

626
int pix_fmt_chan_bits(enum pix_fmt fmt, enum pix_chan chan) {
627
	switch (fmt) {
628
	case BGR888:
629
	case RGB888:
630
		if (chan == ALPHA_CHAN)
631
			return 0;
632
		return 8;
633
	case BGRA8888:
634
	case RGBA8888:
635
		return 8;
636
	case RGB565:
637
	case BGR565:
638
		if (chan == ALPHA_CHAN)
639
			return 0;
640
		if (chan == GREEN_CHAN)
641
			return 6;
642
		return 5;
643
	default:
644
		log_error("Wrong pixel format=%d", fmt);
645
		return 0;
646
	}
647
}
648

649
/* sr - shift of red component, sg - for green, etc.
650
 * dr - bits of red componen, dg - for green, etc. */
651
static const struct fb_rgb_conv {
652
	uint8_t sr, sg, sb, sa, dr, dg, db, da;
653
} rgb_conv[] = {
654
	[RGB888]   = { 0,  8,  16, 24, 8, 8, 8, 0, },
655
	[BGR888]   = { 16, 8,  0,  24, 8, 8, 8, 0, },
656
	[RGBA8888] = { 0,  8,  16, 24, 8, 8, 8, 8, },
657
	[BGRA8888] = { 16, 8,  0,  24, 8, 8, 8, 8, },
658
	[RGB565]   = { 0,  5,  11, 16, 5, 6, 5, 0, },
659
	[BGR565]   = { 11, 5,  0,  16, 5, 6, 5, 0, },
660
};
661

662
static inline int fb_fmt_supported(enum pix_fmt fmt) {
663
	return
664
		(fmt == RGB888)   || (fmt == BGR888) || (fmt == RGBA8888) ||
665
		(fmt == BGRA8888) || (fmt == RGB565) || (fmt == BGR565);
666
}
667

668
static inline int fb_component_conv(int c, int in_dx, int out_dx) {
669
	if (in_dx > out_dx) {
670
		return c >> (in_dx - out_dx);
671
	} else {
672
		return c << (out_dx - in_dx);
673
	}
674
}
675

676
static inline void fb_copy_pixel(char *dst, int px, int bpp) {
677
	int i;
678

679
	switch (bpp) {
680
	case 1:
681
		*(uint8_t *)dst = px;
682
		break;
683
	case 2:
684
		*(uint16_t *)dst = px;
685
		break;
686
	case 4:
687
		*(uint32_t *)dst = px;
688
		break;
689
	default:
690
		for (i = 0; i < bpp; i++) {
691
			dst[i] = px & 0xff;
692
			px >>= bpp;
693
		}
694
		break;
695
	}
696
}
697

698
int pix_fmt_convert(void *src, void *dst, int n,
699
		enum pix_fmt in, enum pix_fmt out) {
700
	int i, a, r, g, b, src_bpp, dst_bpp;
701
	uint32_t in_px, out_px;
702
	struct fb_rgb_conv in_cv, out_cv;
703

704
	if (!fb_fmt_supported(in) || !fb_fmt_supported(out)) {
705
		return -1;
706
	}
707

708
	in_cv = rgb_conv[in];
709
	out_cv = rgb_conv[out];
710

711
	src_bpp = (in_cv.da + in_cv.dr + in_cv.dg + in_cv.db) / 8;
712
	dst_bpp = (out_cv.da + out_cv.dr + out_cv.dg + out_cv.db) / 8;
713

714
	for (i = 0; i < n; i++) {
715
		in_px = *(uint32_t *)src;
716

717
		a = ((in_px >> in_cv.sa) & 0xff) >> (8 - in_cv.da);
718
		r = ((in_px >> in_cv.sr) & 0xff) >> (8 - in_cv.dr);
719
		g = ((in_px >> in_cv.sg) & 0xff) >> (8 - in_cv.dg);
720
		b = ((in_px >> in_cv.sb) & 0xff) >> (8 - in_cv.db);
721

722
		if (!a) {
723
			/* For the formats which do not support alpha, set
724
			 * alpha to the maximum value. */
725
			a = 0xff;
726
		} else {
727
			a = fb_component_conv(a, in_cv.da, out_cv.da);
728
		}
729
		r = fb_component_conv(r, in_cv.dr, out_cv.dr);
730
		g = fb_component_conv(g, in_cv.dg, out_cv.dg);
731
		b = fb_component_conv(b, in_cv.db, out_cv.db);
732

733
		out_px = (a << out_cv.sa) | (r << out_cv.sr) |
734
		         (g << out_cv.sg) | (b << out_cv.sb);
735

736
		fb_copy_pixel(dst, out_px, dst_bpp);
737

738
		dst += dst_bpp;
739
		src += src_bpp;
740
	}
741

742
	return 0;
743
}
744