1.1 --- a/stage2/jzlcd.c Mon Jan 25 18:01:52 2016 +0100
1.2 +++ b/stage2/jzlcd.c Mon Jan 25 18:04:21 2016 +0100
1.3 @@ -25,9 +25,6 @@
1.4 #include "cpu.h"
1.5 #include "board.h"
1.6
1.7 -#define ALIGN(x,a) __ALIGN_MASK((x),(typeof(x))(a)-1)
1.8 -#define __ALIGN_MASK(x,mask) (((x)+(mask))&~(mask))
1.9 -
1.10 #define align2(n) (n)=((((n)+1)>>1)<<1)
1.11 #define align4(n) (n)=((((n)+3)>>2)<<2)
1.12 #define align8(n) (n)=((((n)+7)>>3)<<3)
1.13 @@ -35,44 +32,79 @@
1.14 extern struct jzfb_info jzfb;
1.15 extern vidinfo_t panel_info;
1.16
1.17 +static unsigned short lcd_get_panels()
1.18 +{
1.19 + return ((jzfb.cfg & MODE_MASK) == MODE_STN_MONO_DUAL) ||
1.20 + ((jzfb.cfg & MODE_MASK) == MODE_STN_COLOR_DUAL) ? 2 : 1;
1.21 +}
1.22 +
1.23 static unsigned long lcd_get_size(vidinfo_t *vid)
1.24 {
1.25 - int line_length = (vid->vl_col * NBITS(vid->vl_bpix)) / 8;
1.26 + /* Lines must be aligned to a word boundary. */
1.27 + unsigned long line_length = ALIGN((vid->vl_col * NBITS(vid->vl_bpix)) / 8, sizeof(u32));
1.28 return line_length * vid->vl_row;
1.29 }
1.30
1.31 +static unsigned long lcd_get_aligned_size(vidinfo_t *vid)
1.32 +{
1.33 + /* LCD_CTRL_BST_16 requires 16-word alignment. */
1.34 + return ALIGN(lcd_get_size(vid), 16 * sizeof(u32));
1.35 +}
1.36 +
1.37 +static unsigned long lcd_get_min_size(vidinfo_t *vid)
1.38 +{
1.39 + /* Lines must be aligned to a word boundary. */
1.40 + unsigned long line_length = ALIGN((vid->vl_col * 32) / 8, sizeof(u32));
1.41 + return line_length * vid->vl_row;
1.42 +}
1.43 +
1.44 +static unsigned long lcd_get_aligned_min_size(vidinfo_t *vid)
1.45 +{
1.46 + /* LCD_CTRL_BST_16 requires 16-word alignment. */
1.47 + return ALIGN(lcd_get_min_size(vid), 16 * sizeof(u32));
1.48 +}
1.49 +
1.50 static unsigned long lcd_get_palette_size()
1.51 {
1.52 return 256 * sizeof(u16);
1.53 }
1.54
1.55 +static unsigned long lcd_get_aligned_palette_size()
1.56 +{
1.57 + /* LCD_CTRL_BST_16 requires 16-word alignment. */
1.58 + return ALIGN(lcd_get_palette_size(), 16 * sizeof(u32));
1.59 +}
1.60 +
1.61 static unsigned long lcd_get_descriptors_size()
1.62 {
1.63 return 3 * sizeof(struct jz_fb_dma_descriptor);
1.64 }
1.65
1.66 -static unsigned long lcd_get_aligned_size(vidinfo_t *vid)
1.67 +static unsigned long lcd_get_total_size(vidinfo_t *vid)
1.68 {
1.69 - /* Round up to nearest full page, or MMU section if defined */
1.70 - return ALIGN(lcd_get_size(vid) + lcd_get_palette_size() + lcd_get_descriptors_size(), PAGE_SIZE);
1.71 + unsigned long size = lcd_get_aligned_size(vid) * lcd_get_panels();
1.72 + unsigned long min_size = lcd_get_aligned_min_size(vid);
1.73 +
1.74 + /* Round up to nearest full page, or MMU section if defined. */
1.75 + return ALIGN((size >= min_size ? size : min_size) + lcd_get_aligned_palette_size() + lcd_get_descriptors_size(), PAGE_SIZE);
1.76 }
1.77
1.78 static unsigned long lcd_get_palette(unsigned long addr)
1.79 {
1.80 /* Allocate memory at the end of the region for the palette. */
1.81 - return addr - lcd_get_palette_size();
1.82 + return addr - lcd_get_aligned_palette_size();
1.83 }
1.84
1.85 static unsigned long lcd_get_descriptors(unsigned long addr)
1.86 {
1.87 /* Allocate memory before the palette for the descriptor array. */
1.88 - return addr - lcd_get_palette_size() - lcd_get_descriptors_size();
1.89 + return lcd_get_palette(addr) - lcd_get_descriptors_size();
1.90 }
1.91
1.92 -static unsigned long lcd_get_framebuffer(unsigned long addr)
1.93 +static unsigned long lcd_get_framebuffer(unsigned long addr, unsigned short panel)
1.94 {
1.95 /* Allocate pages for the frame buffer and palette. */
1.96 - return addr - lcd_get_aligned_size(&panel_info);
1.97 + return addr - lcd_get_total_size(&panel_info) + (panel * lcd_get_aligned_size(&panel_info));
1.98 }
1.99
1.100 static void jz_lcd_desc_init(vidinfo_t *vid);
1.101 @@ -84,9 +116,8 @@
1.102
1.103 /* Start from the top of memory and obtain palette and framebuffer regions. */
1.104
1.105 - fbi->screen = lcd_get_framebuffer(get_memory_size());
1.106 + fbi->screen = lcd_get_framebuffer(get_memory_size(), 0);
1.107 fbi->palette = lcd_get_palette(get_memory_size());
1.108 - fbi->palette_size = 256;
1.109
1.110 jz_lcd_desc_init(&panel_info);
1.111 jz_lcd_hw_init(&panel_info);
1.112 @@ -124,22 +155,25 @@
1.113
1.114 /* Populate descriptors. */
1.115
1.116 - fbi->dmadesc_fblow->fdadr = fbi->dmadesc_fblow;
1.117 - fbi->dmadesc_fblow->fsadr = fbi->screen + lcd_get_size(vid);
1.118 - fbi->dmadesc_fblow->fidr = 0;
1.119 - fbi->dmadesc_fblow->ldcmd = lcd_get_size(vid) / 4 ;
1.120 + if (lcd_get_panels() == 2)
1.121 + {
1.122 + fbi->dmadesc_fblow->fdadr = fbi->dmadesc_fblow;
1.123 + fbi->dmadesc_fblow->fsadr = lcd_get_framebuffer(get_memory_size(), 1);
1.124 + fbi->dmadesc_fblow->fidr = 0;
1.125 + fbi->dmadesc_fblow->ldcmd = lcd_get_size(vid) / 4 ;
1.126
1.127 - fbi->fdadr1 = fbi->dmadesc_fblow; /* only used in dual-panel mode */
1.128 + fbi->fdadr1 = fbi->dmadesc_fblow; /* only used in dual-panel mode */
1.129 + }
1.130
1.131 fbi->dmadesc_fbhigh->fsadr = fbi->screen;
1.132 fbi->dmadesc_fbhigh->fidr = 0;
1.133 - fbi->dmadesc_fbhigh->ldcmd = lcd_get_size(vid) / 4; /* length in word */
1.134 + fbi->dmadesc_fbhigh->ldcmd = lcd_get_size(vid) / 4; /* length in words */
1.135
1.136 fbi->dmadesc_palette->fsadr = fbi->palette;
1.137 fbi->dmadesc_palette->fidr = 0;
1.138 - fbi->dmadesc_palette->ldcmd = (fbi->palette_size * 2)/4 | (1<<28);
1.139 + fbi->dmadesc_palette->ldcmd = (lcd_get_palette_size() / 4) | (1<<28);
1.140
1.141 - if(NBITS(vid->vl_bpix) < 12)
1.142 + if (NBITS(vid->vl_bpix) < 12)
1.143 {
1.144 /* assume any mode with <12 bpp is palette driven */
1.145 fbi->dmadesc_palette->fdadr = fbi->dmadesc_fbhigh;