mtk_drm_fbconsole.c 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580
  1. /*
  2. * Copyright (C) 2015 MediaTek Inc.
  3. *
  4. * This program is free software: you can redistribute it and/or modify
  5. * it under the terms of the GNU General Public License version 2 as
  6. * published by the Free Software Foundation.
  7. *
  8. * This program is distributed in the hope that it will be useful,
  9. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  11. * GNU General Public License for more details.
  12. */
  13. #include <linux/font.h>
  14. #include <linux/string.h>
  15. #include <linux/semaphore.h>
  16. #include <linux/slab.h>
  17. #include <linux/font.h>
  18. #include "mtk_drm_fbconsole.h"
  19. /* ------------------------------------------------------------------------- */
  20. #define MFC_WIDTH (ctxt->fb_width)
  21. #define MFC_HEIGHT (ctxt->fb_height)
  22. #define MFC_BPP (ctxt->fb_bpp)
  23. #define MFC_PITCH (MFC_WIDTH * MFC_BPP)
  24. #define MFC_FG_COLOR (ctxt->fg_color)
  25. #define MFC_BG_COLOR (ctxt->bg_color)
  26. #define MFC_FONT font_vga_8x16
  27. #define MFC_FONT_WIDTH (MFC_FONT.width)
  28. #define MFC_FONT_HEIGHT (MFC_FONT.height)
  29. #define MFC_FONT_DATA (MFC_FONT.data)
  30. #define MFC_ROW_SIZE (MFC_FONT_HEIGHT * MFC_PITCH * ctxt->scale)
  31. #define MFC_ROW_FIRST ((BYTE *)(ctxt->fb_addr))
  32. #define MFC_ROW_SECOND (MFC_ROW_FIRST + MFC_ROW_SIZE)
  33. #define MFC_ROW_LAST (MFC_ROW_FIRST + MFC_SIZE - MFC_ROW_SIZE)
  34. #define MFC_SIZE (MFC_ROW_SIZE * ctxt->rows)
  35. #define MFC_SCROLL_SIZE (MFC_SIZE - MFC_ROW_SIZE)
  36. #define MAKE_TWO_RGB565_COLOR(high, low) (((low) << 16) | (high))
  37. /* ------------------------------------------------------------------------- */
  38. UINT32 MFC_Get_Cursor_Offset(MFC_HANDLE handle)
  39. {
  40. struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;
  41. UINT32 offset = ctxt->cursor_col * MFC_FONT_WIDTH * MFC_BPP +
  42. ctxt->cursor_row * MFC_FONT_HEIGHT * MFC_PITCH;
  43. offset *= ctxt->scale;
  44. return offset;
  45. }
  46. static void _mfc_draw_row(struct MFC_CONTEXT *ctxt, BYTE *dest, BYTE raw_color)
  47. {
  48. UINT32 pixel_row, i;
  49. uint16_t color;
  50. int cols, index;
  51. for (pixel_row = 0; pixel_row < ctxt->scale; pixel_row++) {
  52. for (cols = 7; cols >= 0; cols--) {
  53. if (raw_color >> (unsigned int)cols & 1)
  54. color = MFC_FG_COLOR;
  55. else
  56. color = MFC_BG_COLOR;
  57. for (i = 0; i < ctxt->scale; i++) {
  58. index = i + (7 - cols) * ctxt->scale;
  59. ((uint16_t *)dest)[index] = color;
  60. }
  61. }
  62. dest += MFC_PITCH;
  63. }
  64. }
  65. static void _mfc_draw_char(struct MFC_CONTEXT *ctxt, UINT32 x, UINT32 y, char c)
  66. {
  67. BYTE ch = *((BYTE *)&c);
  68. const BYTE *cdat = (const BYTE *)MFC_FONT_DATA + ch * MFC_FONT_HEIGHT;
  69. BYTE *dest = NULL;
  70. INT32 rows, cols, offset;
  71. int font_draw_table16[4];
  72. if (x > (MFC_WIDTH - MFC_FONT_WIDTH)) {
  73. pr_info("draw width too large,x=%d\n", x);
  74. return;
  75. }
  76. if (y > (MFC_HEIGHT - MFC_FONT_HEIGHT)) {
  77. pr_info("draw hight too large,y=%d\n", y);
  78. return;
  79. }
  80. offset = y * MFC_PITCH + x * MFC_BPP;
  81. offset *= ctxt->scale;
  82. dest = (MFC_ROW_FIRST + offset);
  83. switch (MFC_BPP) {
  84. case 2:
  85. font_draw_table16[0] =
  86. MAKE_TWO_RGB565_COLOR(MFC_BG_COLOR, MFC_BG_COLOR);
  87. font_draw_table16[1] =
  88. MAKE_TWO_RGB565_COLOR(MFC_BG_COLOR, MFC_FG_COLOR);
  89. font_draw_table16[2] =
  90. MAKE_TWO_RGB565_COLOR(MFC_FG_COLOR, MFC_BG_COLOR);
  91. font_draw_table16[3] =
  92. MAKE_TWO_RGB565_COLOR(MFC_FG_COLOR, MFC_FG_COLOR);
  93. for (rows = MFC_FONT_HEIGHT; rows--;) {
  94. BYTE bits = *cdat++;
  95. if (ctxt->scale >= 2) {
  96. _mfc_draw_row(ctxt, dest, bits);
  97. dest += (MFC_PITCH * ctxt->scale);
  98. } else {
  99. ((UINT32 *)dest)[0] =
  100. font_draw_table16[bits >> 6];
  101. ((UINT32 *)dest)[1] =
  102. font_draw_table16[bits >> 4 & 3];
  103. ((UINT32 *)dest)[2] =
  104. font_draw_table16[bits >> 2 & 3];
  105. ((UINT32 *)dest)[3] =
  106. font_draw_table16[bits & 3];
  107. dest += MFC_PITCH;
  108. }
  109. }
  110. break;
  111. case 3:
  112. for (rows = MFC_FONT_HEIGHT; rows--; dest += MFC_PITCH) {
  113. BYTE bits = *cdat++;
  114. BYTE *tmp = dest;
  115. for (cols = 0; cols < 8; ++cols) {
  116. UINT32 color = (((unsigned int)bits >>
  117. (unsigned int)(7 -
  118. cols)) & 0x1)
  119. ? MFC_FG_COLOR
  120. : MFC_BG_COLOR;
  121. ((BYTE *)tmp)[0] = color & 0xff;
  122. ((BYTE *)tmp)[1] = (color >> 8) & 0xff;
  123. ((BYTE *)tmp)[2] = (color >> 16) & 0xff;
  124. tmp += 3;
  125. }
  126. }
  127. break;
  128. case 4:
  129. for (rows = MFC_FONT_HEIGHT; rows--; dest += MFC_PITCH) {
  130. BYTE bits = *cdat++;
  131. BYTE *tmp = dest;
  132. for (cols = 0; cols < 8; ++cols) {
  133. UINT32 color = ((bits >> (7 - cols)) & 0x1)
  134. ? MFC_FG_COLOR
  135. : MFC_BG_COLOR;
  136. ((BYTE *)tmp)[1] = color & 0xff;
  137. ((BYTE *)tmp)[2] = (color >> 8) & 0xff;
  138. ((BYTE *)tmp)[3] = (color >> 16) & 0xff;
  139. ((BYTE *)tmp)[0] = (color >> 16) & 0xff;
  140. tmp += 4;
  141. }
  142. }
  143. break;
  144. default:
  145. pr_info("draw char fail,MFC_BPP=%d\n", MFC_BPP);
  146. break;
  147. }
  148. }
  149. static void _mfc_scroll_up(struct MFC_CONTEXT *ctxt)
  150. {
  151. const UINT32 bg_color =
  152. MAKE_TWO_RGB565_COLOR(MFC_BG_COLOR, MFC_BG_COLOR);
  153. UINT32 *ptr = (UINT32 *)MFC_ROW_LAST;
  154. int i = MFC_ROW_SIZE / sizeof(UINT32);
  155. memcpy(MFC_ROW_FIRST, MFC_ROW_SECOND, MFC_SCROLL_SIZE);
  156. while (--i >= 0)
  157. *ptr++ = bg_color;
  158. }
  159. static void _mfc_newline(struct MFC_CONTEXT *ctxt)
  160. {
  161. /* Bin:add for filling the color for the blank of this column */
  162. while (ctxt->cursor_col < ctxt->cols) {
  163. _mfc_draw_char(ctxt, ctxt->cursor_col * MFC_FONT_WIDTH,
  164. ctxt->cursor_row * MFC_FONT_HEIGHT, ' ');
  165. ++ctxt->cursor_col;
  166. }
  167. ++ctxt->cursor_row;
  168. ctxt->cursor_col = 0;
  169. /* check if we need to scroll the terminal */
  170. if (ctxt->cursor_row >= ctxt->rows) {
  171. /* Scroll everything up */
  172. _mfc_scroll_up(ctxt);
  173. /* decrement row number */
  174. --ctxt->cursor_row;
  175. }
  176. }
  177. #define check_newline() \
  178. do { \
  179. if (ctxt->cursor_col >= ctxt->cols) \
  180. _mfc_newline(ctxt); \
  181. } while (0)
  182. static void _mfc_putc(struct MFC_CONTEXT *ctxt, const char c)
  183. {
  184. check_newline();
  185. switch (c) {
  186. case '\n': /* next line */
  187. _mfc_newline(ctxt);
  188. break;
  189. case '\r': /* carriage return */
  190. ctxt->cursor_col = 0;
  191. break;
  192. case '\t': /* tab 8 */
  193. ctxt->cursor_col += 8;
  194. ctxt->cursor_col &= ~0x0007;
  195. check_newline();
  196. break;
  197. default: /* draw the char */
  198. _mfc_draw_char(ctxt, ctxt->cursor_col * MFC_FONT_WIDTH,
  199. ctxt->cursor_row * MFC_FONT_HEIGHT, c);
  200. ++ctxt->cursor_col;
  201. check_newline();
  202. break;
  203. }
  204. }
  205. static int MFC_GetScale(unsigned int fb_width, unsigned int fb_height,
  206. unsigned int fb_bpp)
  207. {
  208. if (fb_bpp == 2 && fb_width * fb_height >= 1080 * 1920)
  209. return 2;
  210. return 1;
  211. }
  212. enum MFC_STATUS MFC_Open(MFC_HANDLE *handle, void *fb_addr,
  213. unsigned int fb_width, unsigned int fb_height,
  214. unsigned int fb_bpp, unsigned int fg_color,
  215. unsigned int bg_color, struct file *filp)
  216. {
  217. struct MFC_CONTEXT *ctxt = NULL;
  218. if (!handle || !fb_addr)
  219. return MFC_STATUS_INVALID_ARGUMENT;
  220. /* if (fb_bpp != 2) */
  221. /* return MFC_STATUS_NOT_IMPLEMENTED; */
  222. ctxt = kzalloc(sizeof(struct MFC_CONTEXT), GFP_KERNEL);
  223. if (!ctxt)
  224. return MFC_STATUS_OUT_OF_MEMORY;
  225. ctxt->scale = MFC_GetScale(fb_width, fb_height, fb_bpp);
  226. if (ctxt->scale == 0)
  227. ctxt->scale = 1;
  228. sema_init(&ctxt->sem, 1);
  229. ctxt->fb_addr = fb_addr;
  230. ctxt->fb_width = fb_width;
  231. ctxt->fb_height = fb_height;
  232. ctxt->fb_bpp = fb_bpp;
  233. ctxt->fg_color = fg_color;
  234. ctxt->bg_color = bg_color;
  235. ctxt->rows = fb_height / (MFC_FONT_HEIGHT * ctxt->scale);
  236. ctxt->cols = fb_width / (MFC_FONT_WIDTH * ctxt->scale);
  237. ctxt->font_width = MFC_FONT_WIDTH;
  238. ctxt->font_height = MFC_FONT_HEIGHT;
  239. ctxt->filp = filp;
  240. *handle = ctxt;
  241. return MFC_STATUS_OK;
  242. }
  243. enum MFC_STATUS MFC_Open_Ex(MFC_HANDLE *handle, void *fb_addr,
  244. unsigned int fb_width, unsigned int fb_height,
  245. unsigned int fb_pitch, unsigned int fb_bpp,
  246. unsigned int fg_color, unsigned int bg_color,
  247. struct file *filp)
  248. {
  249. struct MFC_CONTEXT *ctxt = NULL;
  250. if (!handle || !fb_addr)
  251. return MFC_STATUS_INVALID_ARGUMENT;
  252. if (fb_bpp != 2)
  253. return MFC_STATUS_NOT_IMPLEMENTED; /* only support RGB565 */
  254. ctxt = kzalloc(sizeof(struct MFC_CONTEXT), GFP_KERNEL);
  255. if (!ctxt)
  256. return MFC_STATUS_OUT_OF_MEMORY;
  257. ctxt->scale = MFC_GetScale(fb_width, fb_height, fb_bpp);
  258. if (ctxt->scale == 0)
  259. ctxt->scale = 1;
  260. sema_init(&ctxt->sem, 1);
  261. ctxt->fb_addr = fb_addr;
  262. ctxt->fb_width = fb_pitch;
  263. ctxt->fb_height = fb_height;
  264. ctxt->fb_bpp = fb_bpp;
  265. ctxt->fg_color = fg_color;
  266. ctxt->bg_color = bg_color;
  267. ctxt->rows = fb_height / (MFC_FONT_HEIGHT * ctxt->scale);
  268. ctxt->cols = fb_width / (MFC_FONT_WIDTH * ctxt->scale);
  269. ctxt->font_width = MFC_FONT_WIDTH;
  270. ctxt->font_height = MFC_FONT_HEIGHT;
  271. ctxt->filp = filp;
  272. *handle = ctxt;
  273. return MFC_STATUS_OK;
  274. }
  275. enum MFC_STATUS MFC_Close(MFC_HANDLE handle)
  276. {
  277. if (!handle)
  278. return MFC_STATUS_INVALID_ARGUMENT;
  279. kfree(handle);
  280. return MFC_STATUS_OK;
  281. }
  282. enum MFC_STATUS MFC_SetColor(MFC_HANDLE handle, unsigned int fg_color,
  283. unsigned int bg_color)
  284. {
  285. struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;
  286. if (!ctxt)
  287. return MFC_STATUS_INVALID_ARGUMENT;
  288. if (down_interruptible(&ctxt->sem)) {
  289. pr_err("[MFC] ERROR: Can't get semaphore in %s()\n", __func__);
  290. return MFC_STATUS_LOCK_FAIL;
  291. }
  292. ctxt->fg_color = fg_color;
  293. ctxt->bg_color = bg_color;
  294. up(&ctxt->sem);
  295. return MFC_STATUS_OK;
  296. }
  297. enum MFC_STATUS MFC_ResetCursor(MFC_HANDLE handle)
  298. {
  299. struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;
  300. if (!ctxt)
  301. return MFC_STATUS_INVALID_ARGUMENT;
  302. if (down_interruptible(&ctxt->sem)) {
  303. pr_err("[MFC] ERROR: Can't get semaphore in %s()\n", __func__);
  304. return MFC_STATUS_LOCK_FAIL;
  305. }
  306. ctxt->cursor_row = ctxt->cursor_col = 0;
  307. up(&ctxt->sem);
  308. return MFC_STATUS_OK;
  309. }
  310. enum MFC_STATUS MFC_Print(MFC_HANDLE handle, const char *str)
  311. {
  312. struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;
  313. int count = 0;
  314. if (!ctxt || !str)
  315. return MFC_STATUS_INVALID_ARGUMENT;
  316. if (down_interruptible(&ctxt->sem)) {
  317. pr_err("[MFC] ERROR: Can't get semaphore in %s()\n", __func__);
  318. return MFC_STATUS_LOCK_FAIL;
  319. }
  320. count = strlen(str);
  321. while (count--)
  322. _mfc_putc(ctxt, *str++);
  323. up(&ctxt->sem);
  324. return MFC_STATUS_OK;
  325. }
  326. enum MFC_STATUS MFC_SetMem(MFC_HANDLE handle, const char *str, UINT32 color)
  327. {
  328. struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;
  329. int count = 0;
  330. int i, j;
  331. UINT32 *ptr = NULL;
  332. if (!ctxt || !str)
  333. return MFC_STATUS_INVALID_ARGUMENT;
  334. if (down_interruptible(&ctxt->sem)) {
  335. pr_err("[MFC] ERROR: Can't get semaphore in %s()\n", __func__);
  336. return MFC_STATUS_LOCK_FAIL;
  337. }
  338. count = strlen(str);
  339. count = count * MFC_FONT_WIDTH;
  340. for (j = 0; j < MFC_FONT_HEIGHT; j++) {
  341. ptr = (UINT32 *)(ctxt->fb_addr + (j + 1) * MFC_PITCH -
  342. count * ctxt->fb_bpp);
  343. for (i = 0; i < count * ctxt->fb_bpp / sizeof(UINT32); i++)
  344. *ptr++ = color;
  345. }
  346. up(&ctxt->sem);
  347. return MFC_STATUS_OK;
  348. }
  349. enum MFC_STATUS MFC_LowMemory_Printf(MFC_HANDLE handle, const char *str,
  350. UINT32 fg_color, UINT32 bg_color)
  351. {
  352. struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;
  353. int count = 0;
  354. unsigned int col, row, fg_color_mfc, bg_color_mfc;
  355. if (!ctxt || !str)
  356. return MFC_STATUS_INVALID_ARGUMENT;
  357. if (down_interruptible(&ctxt->sem)) {
  358. pr_err("[MFC] ERROR: Can't get semaphore in %s()\n", __func__);
  359. return MFC_STATUS_LOCK_FAIL;
  360. }
  361. count = strlen(str);
  362. /* store cursor_col and row for printf low memory char temply */
  363. row = ctxt->cursor_row;
  364. col = ctxt->cursor_col;
  365. ctxt->cursor_row = 0;
  366. ctxt->cursor_col = ctxt->cols - count;
  367. fg_color_mfc = ctxt->fg_color;
  368. bg_color_mfc = ctxt->bg_color;
  369. ctxt->fg_color = fg_color;
  370. ctxt->bg_color = bg_color;
  371. while (count--)
  372. _mfc_putc(ctxt, *str++);
  373. /* restore cursor_col and row for printf low memory char temply */
  374. ctxt->cursor_row = row;
  375. ctxt->cursor_col = col;
  376. ctxt->fg_color = fg_color_mfc;
  377. ctxt->bg_color = bg_color_mfc;
  378. up(&ctxt->sem);
  379. return MFC_STATUS_OK;
  380. }
  381. /* ---------- screen logger ---------- */
  382. static struct screen_logger logger_head;
  383. static int screen_logger_inited;
  384. void screen_logger_init(void)
  385. {
  386. if (screen_logger_inited == 0) {
  387. INIT_LIST_HEAD(&logger_head.list);
  388. screen_logger_inited = 1;
  389. }
  390. }
  391. void screen_logger_add_message(char *obj, enum message_mode mode, char *message)
  392. {
  393. struct screen_logger *p = NULL;
  394. int add_new = 1;
  395. char *new = NULL, *old = NULL;
  396. unsigned int len = 0;
  397. screen_logger_init();
  398. list_for_each_entry(p, &logger_head.list, list) {
  399. if (strcmp(p->obj, obj) != 0)
  400. continue;
  401. switch (mode) {
  402. case MESSAGE_REPLACE:
  403. old = p->message;
  404. new = kstrdup(message, GFP_KERNEL);
  405. p->message = new;
  406. kfree(old);
  407. break;
  408. case MESSAGE_APPEND:
  409. len = strlen(p->message) + strlen(message);
  410. new = kmalloc(sizeof(char) * (len + 1), GFP_KERNEL);
  411. strncpy(new, p->message, strlen(p->message));
  412. if (strlen(message) + strlen(p->message) < len)
  413. strncat(new, message, strlen(message));
  414. else
  415. strncat(new, message, len - strlen(p->message));
  416. old = p->message;
  417. p->message = new;
  418. kfree(old);
  419. break;
  420. default:
  421. break;
  422. }
  423. add_new = 0;
  424. }
  425. if (add_new == 1) {
  426. struct screen_logger *logger =
  427. kmalloc(sizeof(struct screen_logger), GFP_KERNEL);
  428. if (logger == NULL)
  429. return;
  430. logger->obj = kstrdup(obj, GFP_KERNEL);
  431. logger->message = kstrdup(message, GFP_KERNEL);
  432. list_add_tail(&logger->list, &logger_head.list);
  433. }
  434. }
  435. void screen_logger_remove_message(const char *obj)
  436. {
  437. struct screen_logger *p = NULL;
  438. list_for_each_entry(p, &logger_head.list, list) {
  439. if (strcmp(p->obj, obj) == 0) {
  440. kfree(p->obj);
  441. kfree(p->message);
  442. /* memory leak. TODO: delete completely. */
  443. list_del(&p->list);
  444. kfree(p);
  445. break;
  446. }
  447. }
  448. p = NULL;
  449. }
  450. void screen_logger_print(MFC_HANDLE handle)
  451. {
  452. struct screen_logger *p = NULL;
  453. list_for_each_entry(p, &logger_head.list, list)
  454. MFC_Print(handle, p->message);
  455. }
  456. void screen_logger_empty(void)
  457. {
  458. struct screen_logger *p = NULL;
  459. p = list_entry(logger_head.list.prev, typeof(*p), list);
  460. while (p != &logger_head) {
  461. screen_logger_remove_message(p->obj);
  462. p = list_entry(logger_head.list.prev, typeof(*p), list);
  463. }
  464. }
  465. void screen_logger_test_case(MFC_HANDLE handle)
  466. {
  467. screen_logger_add_message("message1", MESSAGE_REPLACE,
  468. "print message1\n");
  469. screen_logger_add_message("message1", MESSAGE_REPLACE,
  470. "print message2\n");
  471. screen_logger_print(handle);
  472. screen_logger_empty();
  473. }