static void ffb_loadcmap (struct fb_info_sbusfb *fb, struct display *p, int index, int count)
{
struct ffb_dac *dac = fb->s.ffb.dac;
unsigned long flags;
int i, j = count;
spin_lock_irqsave(&fb->lock, flags);
upa_writel(0x2000 | index, &dac->type);
for (i = index; j--; i++) {
u32 val;
/* Feed the colors in :)) */
val = ((fb->color_map CM(i,0))) |
((fb->color_map CM(i,1)) << 8) |
((fb->color_map CM(i,2)) << 16);
upa_writel(val, &dac->value);
}
if (!p)
goto out;
for (i = index, j = count; i < 16 && j--; i++)
((u32 *)p->dispsw_data)[i] = ((fb->color_map CM(i,0))) |
((fb->color_map CM(i,1)) << 8) |
((fb->color_map CM(i,2)) << 16);
out:
spin_unlock_irqrestore(&fb->lock, flags);
}
static void ffb_setcursormap (struct fb_info_sbusfb *fb, u8 *red, u8 *green, u8 *blue)
{
struct ffb_dac *dac = fb->s.ffb.dac;
unsigned long flags;
spin_lock_irqsave(&fb->lock, flags);
__ffb_curs_enable (fb, 0);
upa_writel(0x102, &dac->type2);
upa_writel((red[0] | (green[0]<<8) | (blue[0]<<16)), &dac->value2);
upa_writel((red[1] | (green[1]<<8) | (blue[1]<<16)), &dac->value2);
spin_unlock_irqrestore(&fb->lock, flags);
}
static __inline__ void __ffb_curs_enable (struct fb_info_sbusfb *fb, int enable)
{
struct ffb_dac *dac = fb->s.ffb.dac;
u32 val;
upa_writel(0x100, &dac->type2);
if (fb->s.ffb.dac_rev <= 2) {
val = enable ? 3 : 0;
} else {
val = enable ? 0 : 3;
}
upa_writel(val, &dac->value2);
}
static void ffb_unblank(struct fb_info_sbusfb *fb)
{
struct ffb_dac *dac = fb->s.ffb.dac;
unsigned long flags;
u32 tmp;
spin_lock_irqsave(&fb->lock, flags);
upa_writel(0x6000, &dac->type);
tmp = (upa_readl(&dac->value) | 0x1);
upa_writel(0x6000, &dac->type);
upa_writel(tmp, &dac->value);
spin_unlock_irqrestore(&fb->lock, flags);
}
static void ffb_putc(struct vc_data *conp, struct display *p, int c, int yy, int xx)
{
struct fb_info_sbusfb *fb = (struct fb_info_sbusfb *)p->fb_info;
register struct ffb_fbc *fbc = fb->s.ffb.fbc;
unsigned long flags;
int i, xy;
u8 *fd;
u64 fgbg;
spin_lock_irqsave(&fb->lock, flags);
if (fontheightlog(p)) {
xy = (yy << (16 + fontheightlog(p)));
i = ((c & p->charmask) << fontheightlog(p));
} else {
xy = ((yy * fontheight(p)) << 16);
i = (c & p->charmask) * fontheight(p);
}
if (fontwidth(p) <= 8)
fd = p->fontdata + i;
else
fd = p->fontdata + (i << 1);
if (fontwidthlog(p))
xy += (xx << fontwidthlog(p)) + fb->s.ffb.xy_margin;
else
xy += (xx * fontwidth(p)) + fb->s.ffb.xy_margin;
fgbg = (((u64)(((u32 *)p->dispsw_data)[attr_fgcol(p,c)])) << 32) |
((u32 *)p->dispsw_data)[attr_bgcol(p,c)];
if (fgbg != *(u64 *)&fb->s.ffb.fg_cache) {
FFBFifo(fb, 2);
upa_writeq(fgbg, &fbc->fg);
*(u64 *)&fb->s.ffb.fg_cache = fgbg;
}
FFBFifo(fb, 2 + fontheight(p));
upa_writel(xy, &fbc->fontxy);
upa_writel(fontwidth(p), &fbc->fontw);
if (fontwidth(p) <= 8) {
for (i = 0; i < fontheight(p); i++) {
u32 val = *fd++ << 24;
upa_writel(val, &fbc->font);
}
} else {
for (i = 0; i < fontheight(p); i++) {
u32 val = *(u16 *)fd << 16;
upa_writel(val, &fbc->font);
fd += 2;
}
}
spin_unlock_irqrestore(&fb->lock, flags);
}
static void ffb_clear(struct vc_data *conp, struct display *p, int sy, int sx,
int height, int width)
{
struct fb_info_sbusfb *fb = (struct fb_info_sbusfb *)p->fb_info;
register struct ffb_fbc *fbc = fb->s.ffb.fbc;
unsigned long flags;
u64 yx, hw;
int fg;
spin_lock_irqsave(&fb->lock, flags);
fg = ((u32 *)p->dispsw_data)[attr_bgcol_ec(p,conp)];
if (fg != fb->s.ffb.fg_cache) {
FFBFifo(fb, 5);
upa_writel(fg, &fbc->fg);
fb->s.ffb.fg_cache = fg;
} else
FFBFifo(fb, 4);
if (fontheightlog(p)) {
yx = (u64)sy << (fontheightlog(p) + 32); hw = (u64)height << (fontheightlog(p) + 32);
} else {
yx = (u64)(sy * fontheight(p)) << 32; hw = (u64)(height * fontheight(p)) << 32;
}
if (fontwidthlog(p)) {
yx += sx << fontwidthlog(p); hw += width << fontwidthlog(p);
} else {
yx += sx * fontwidth(p); hw += width * fontwidth(p);
}
upa_writeq(yx + fb->s.ffb.yx_margin, &fbc->by);
upa_writeq(hw, &fbc->bh);
spin_unlock_irqrestore(&fb->lock, flags);
}
/* Load cursor information */
static void ffb_setcursor (struct fb_info_sbusfb *fb)
{
struct ffb_dac *dac = fb->s.ffb.dac;
struct cg_cursor *c = &fb->cursor;
unsigned long flags;
u32 val;
spin_lock_irqsave(&fb->lock, flags);
upa_writel(0x104, &dac->type2);
/* Should this be just 0x7ff??
Should I do some margin handling and setcurshape in that case? */
val = (((c->cpos.fby - c->chot.fby) & 0xffff) << 16)
|((c->cpos.fbx - c->chot.fbx) & 0xffff);
upa_writel(val, &dac->value2);
__ffb_curs_enable (fb, fb->cursor.enable);
spin_unlock_irqrestore(&fb->lock, flags);
}
static __inline__ void FFBWait(struct ffb_fbc *ffb)
{
int limit = 10000;
do {
if ((upa_readl(&ffb->ucsr) & FFB_UCSR_ALL_BUSY) == 0)
break;
if ((upa_readl(&ffb->ucsr) & FFB_UCSR_ALL_ERRORS) != 0) {
upa_writel(FFB_UCSR_ALL_ERRORS, &ffb->ucsr);
}
} while(--limit > 0);
}
/* Set cursor shape */
static void ffb_setcurshape (struct fb_info_sbusfb *fb)
{
struct ffb_dac *dac = fb->s.ffb.dac;
unsigned long flags;
int i, j;
spin_lock_irqsave(&fb->lock, flags);
__ffb_curs_enable (fb, 0);
for (j = 0; j < 2; j++) {
u32 val = j ? 0 : 0x80;
upa_writel(val, &dac->type2);
for (i = 0; i < 0x40; i++) {
if (fb->cursor.size.fbx <= 32) {
upa_writel(fb->cursor.bits [j][i], &dac->value2);
upa_writel(0, &dac->value2);
} else {
upa_writel(fb->cursor.bits [j][2*i], &dac->value2);
upa_writel(fb->cursor.bits [j][2*i+1], &dac->value2);
}
}
}
spin_unlock_irqrestore(&fb->lock, flags);
}
static void ffb_fill(struct fb_info_sbusfb *fb, struct display *p, int s,
int count, unsigned short *boxes)
{
register struct ffb_fbc *fbc = fb->s.ffb.fbc;
unsigned long flags;
int fg;
spin_lock_irqsave(&fb->lock, flags);
fg = ((u32 *)p->dispsw_data)[attr_bgcol(p,s)];
if (fg != fb->s.ffb.fg_cache) {
FFBFifo(fb, 1);
upa_writel(fg, &fbc->fg);
fb->s.ffb.fg_cache = fg;
}
while (count-- > 0) {
FFBFifo(fb, 4);
upa_writel(boxes[1], &fbc->by);
upa_writel(boxes[0], &fbc->bx);
upa_writel(boxes[3] - boxes[1], &fbc->bh);
upa_writel(boxes[2] - boxes[0], &fbc->bw);
boxes += 4;
}
spin_unlock_irqrestore(&fb->lock, flags);
}
static void ffb_switch_from_graph (struct fb_info_sbusfb *fb)
{
register struct ffb_fbc *fbc = fb->s.ffb.fbc;
unsigned long flags;
spin_lock_irqsave(&fb->lock, flags);
FFBWait(fbc);
fb->s.ffb.fifo_cache = 0;
FFBFifo(fb, 8);
upa_writel(FFB_PPC_VCE_DISABLE|FFB_PPC_TBE_OPAQUE|
FFB_PPC_APE_DISABLE|FFB_PPC_CS_CONST,
&fbc->ppc);
upa_writel(0x2000707f, &fbc->fbc);
upa_writel(FFB_ROP_NEW, &fbc->rop);
upa_writel(FFB_DRAWOP_RECTANGLE, &fbc->drawop);
upa_writel(0xffffffff, &fbc->pmask);
upa_writel(0x10000, &fbc->fontinc);
upa_writel(fb->s.ffb.fg_cache, &fbc->fg);
upa_writel(fb->s.ffb.bg_cache, &fbc->bg);
FFBWait(fbc);
spin_unlock_irqrestore(&fb->lock, flags);
}
static void __fhc_set(struct led_classdev *led_cdev,
enum led_brightness led_val, u32 bit)
{
struct sunfire_led *p = to_sunfire_led(led_cdev);
u32 reg = upa_readl(p->reg);
switch (bit) {
case FHC_CONTROL_LLED:
if (led_val)
reg &= ~bit;
else
reg |= bit;
break;
default:
if (led_val)
reg |= bit;
else
reg &= ~bit;
break;
}
upa_writel(reg, p->reg);
}
static void ffb_restore_context(ffb_dev_priv_t *fpriv, int old, int idx)
{
ffb_fbcPtr ffb = fpriv->regs;
struct ffb_hw_context *ctx;
int i;
ctx = fpriv->hw_state[idx - 1];
if (idx == 0 || ctx == NULL)
return;
if (ctx->is_2d_only) {
/* 2D applications only care about certain pieces
* of state.
*/
upa_writel(ctx->drawop, &ffb->drawop);
/* If we were restoring the vertex registers, this is where
* we would do it. We would restore 32 32-bit words starting
* at ffb->suvtx.
*/
upa_writel(ctx->ppc, &ffb->ppc);
upa_writel(ctx->wid, &ffb->wid);
upa_writel(ctx->fg, &ffb->fg);
upa_writel(ctx->bg, &ffb->bg);
upa_writel(ctx->xclip, &ffb->xclip);
upa_writel(ctx->fbc, &ffb->fbc);
upa_writel(ctx->rop, &ffb->rop);
upa_writel(ctx->cmp, &ffb->cmp);
upa_writel(ctx->matchab, &ffb->matchab);
upa_writel(ctx->magnab, &ffb->magnab);
upa_writel(ctx->pmask, &ffb->pmask);
upa_writel(ctx->xpmask, &ffb->xpmask);
upa_writel(ctx->lpat, &ffb->lpat);
upa_writel(ctx->fontxy, &ffb->fontxy);
upa_writel(ctx->fontw, &ffb->fontw);
upa_writel(ctx->fontinc, &ffb->fontinc);
/* stencil/stencilctl only exists on FFB2+ and later
* due to the introduction of 3DRAM-III.
*/
if (fpriv->ffb_type == ffb2_vertical_plus ||
fpriv->ffb_type == ffb2_horizontal_plus) {
upa_writel(ctx->stencil, &ffb->stencil);
upa_writel(ctx->stencilctl, &ffb->stencilctl);
upa_writel(0x80000000, &ffb->fbc);
upa_writel((ctx->stencilctl | 0x80000),
&ffb->rawstencilctl);
upa_writel(ctx->fbc, &ffb->fbc);
}
for (i = 0; i < 32; i++)
upa_writel(ctx->area_pattern[i], &ffb->pattern[i]);
upa_writel((ctx->ucsr & 0xf0000), &ffb->ucsr);
return;
}
/* Restore drawop. */
upa_writel(ctx->drawop, &ffb->drawop);
/* If we were restoring the vertex registers, this is where
* we would do it. We would restore 32 32-bit words starting
* at ffb->suvtx.
*/
/* Restore rendering attributes. */
upa_writel(ctx->ppc, &ffb->ppc); /* Pixel Processor Control */
upa_writel(ctx->wid, &ffb->wid); /* Current WID */
upa_writel(ctx->fg, &ffb->fg); /* Constant FG color */
upa_writel(ctx->bg, &ffb->bg); /* Constant BG color */
upa_writel(ctx->consty, &ffb->consty); /* Constant Y */
upa_writel(ctx->constz, &ffb->constz); /* Constant Z */
upa_writel(ctx->xclip, &ffb->xclip); /* X plane clip */
upa_writel(ctx->dcss, &ffb->dcss); /* Depth Cue Scale Slope */
upa_writel(ctx->vclipmin, &ffb->vclipmin); /* Primary XY clip, minimum */
upa_writel(ctx->vclipmax, &ffb->vclipmax); /* Primary XY clip, maximum */
upa_writel(ctx->vclipzmin, &ffb->vclipzmin); /* Primary Z clip, minimum */
upa_writel(ctx->vclipzmax, &ffb->vclipzmax); /* Primary Z clip, maximum */
upa_writel(ctx->dcsf, &ffb->dcsf); /* Depth Cue Scale Front Bound */
upa_writel(ctx->dcsb, &ffb->dcsb); /* Depth Cue Scale Back Bound */
upa_writel(ctx->dczf, &ffb->dczf); /* Depth Cue Scale Z Front */
upa_writel(ctx->dczb, &ffb->dczb); /* Depth Cue Scale Z Back */
upa_writel(ctx->blendc, &ffb->blendc); /* Alpha Blend Control */
upa_writel(ctx->blendc1, &ffb->blendc1); /* Alpha Blend Color 1 */
upa_writel(ctx->blendc2, &ffb->blendc2); /* Alpha Blend Color 2 */
upa_writel(ctx->fbc, &ffb->fbc); /* Frame Buffer Control */
upa_writel(ctx->rop, &ffb->rop); /* Raster Operation */
upa_writel(ctx->cmp, &ffb->cmp); /* Compare Controls */
upa_writel(ctx->matchab, &ffb->matchab); /* Buffer A/B Match Ops */
upa_writel(ctx->matchc, &ffb->matchc); /* Buffer C Match Ops */
upa_writel(ctx->magnab, &ffb->magnab); /* Buffer A/B Magnitude Ops */
upa_writel(ctx->magnc, &ffb->magnc); /* Buffer C Magnitude Ops */
upa_writel(ctx->pmask, &ffb->pmask); /* RGB Plane Mask */
upa_writel(ctx->xpmask, &ffb->xpmask); /* X Plane Mask */
upa_writel(ctx->ypmask, &ffb->ypmask); /* Y Plane Mask */
upa_writel(ctx->zpmask, &ffb->zpmask); /* Z Plane Mask */
/* Auxiliary Clips. */
upa_writel(ctx->auxclip0min, &ffb->auxclip[0].min);
upa_writel(ctx->auxclip0max, &ffb->auxclip[0].max);
upa_writel(ctx->auxclip1min, &ffb->auxclip[1].min);
upa_writel(ctx->auxclip1max, &ffb->auxclip[1].max);
upa_writel(ctx->auxclip2min, &ffb->auxclip[2].min);
upa_writel(ctx->auxclip2max, &ffb->auxclip[2].max);
upa_writel(ctx->auxclip3min, &ffb->auxclip[3].min);
upa_writel(ctx->auxclip3max, &ffb->auxclip[3].max);
upa_writel(ctx->lpat, &ffb->lpat); /* Line Pattern */
upa_writel(ctx->fontxy, &ffb->fontxy); /* XY Font Coordinate */
upa_writel(ctx->fontw, &ffb->fontw); /* Font Width */
upa_writel(ctx->fontinc, &ffb->fontinc); /* Font X/Y Increment */
/* These registers/features only exist on FFB2 and later chips. */
if (fpriv->ffb_type >= ffb2_prototype) {
upa_writel(ctx->dcss1, &ffb->dcss1); /* Depth Cue Scale Slope 1 */
upa_writel(ctx->dcss2, &ffb->dcss2); /* Depth Cue Scale Slope 2 */
upa_writel(ctx->dcss3, &ffb->dcss2); /* Depth Cue Scale Slope 3 */
upa_writel(ctx->dcs2, &ffb->dcs2); /* Depth Cue Scale 2 */
upa_writel(ctx->dcs3, &ffb->dcs3); /* Depth Cue Scale 3 */
upa_writel(ctx->dcs4, &ffb->dcs4); /* Depth Cue Scale 4 */
upa_writel(ctx->dcd2, &ffb->dcd2); /* Depth Cue Depth 2 */
upa_writel(ctx->dcd3, &ffb->dcd3); /* Depth Cue Depth 3 */
upa_writel(ctx->dcd4, &ffb->dcd4); /* Depth Cue Depth 4 */
/* And stencil/stencilctl only exists on FFB2+ and later
* due to the introduction of 3DRAM-III.
*/
if (fpriv->ffb_type == ffb2_vertical_plus ||
fpriv->ffb_type == ffb2_horizontal_plus) {
/* Unfortunately, there is a hardware bug on
* the FFB2+ chips which prevents a normal write
* to the stencil control register from working
* as it should.
*
* The state controlled by the FFB stencilctl register
* really gets transferred to the per-buffer instances
* of the stencilctl register in the 3DRAM chips.
*
* The bug is that FFB does not update buffer C correctly,
* so we have to do it by hand for them.
*/
/* This will update buffers A and B. */
upa_writel(ctx->stencil, &ffb->stencil);
upa_writel(ctx->stencilctl, &ffb->stencilctl);
/* Force FFB to use buffer C 3dram regs. */
upa_writel(0x80000000, &ffb->fbc);
upa_writel((ctx->stencilctl | 0x80000),
&ffb->rawstencilctl);
/* Now restore the correct FBC controls. */
upa_writel(ctx->fbc, &ffb->fbc);
}
}
/* Restore the 32x32 area pattern. */
for (i = 0; i < 32; i++)
upa_writel(ctx->area_pattern[i], &ffb->pattern[i]);
/* Finally, stash away the User Constol/Status Register.
* The only state we really preserve here is the picking
* control.
*/
upa_writel((ctx->ucsr & 0xf0000), &ffb->ucsr);
}
char __init *creatorfb_init(struct fb_info_sbusfb *fb)
{
struct fb_fix_screeninfo *fix = &fb->fix;
struct fb_var_screeninfo *var = &fb->var;
struct display *disp = &fb->disp;
struct fbtype *type = &fb->type;
struct linux_prom64_registers regs[2*PROMREG_MAX];
int i, afb = 0;
unsigned int btype;
char name[64];
struct fb_ops *fbops;
if (prom_getproperty(fb->prom_node, "reg", (void *) regs, sizeof(regs)) <= 0)
return NULL;
if (creator_apply_upa_parent_ranges(fb->prom_parent, ®s[0]))
return NULL;
disp->dispsw_data = (void *)kmalloc(16 * sizeof(u32), GFP_KERNEL);
if (disp->dispsw_data == NULL)
return NULL;
memset(disp->dispsw_data, 0, 16 * sizeof(u32));
fbops = kmalloc(sizeof(*fbops), GFP_KERNEL);
if (fbops == NULL) {
kfree(disp->dispsw_data);
return NULL;
}
*fbops = *fb->info.fbops;
fbops->fb_rasterimg = ffb_rasterimg;
fb->info.fbops = fbops;
prom_getstring(fb->prom_node, "name", name, sizeof(name));
if (!strcmp(name, "SUNW,afb"))
afb = 1;
btype = prom_getintdefault(fb->prom_node, "board_type", 0);
strcpy(fb->info.modename, "Creator");
if (!afb) {
if ((btype & 7) == 3)
strcpy(fix->id, "Creator 3D");
else
strcpy(fix->id, "Creator");
} else
strcpy(fix->id, "Elite 3D");
fix->visual = FB_VISUAL_TRUECOLOR;
fix->line_length = 8192;
fix->accel = FB_ACCEL_SUN_CREATOR;
var->bits_per_pixel = 32;
var->green.offset = 8;
var->blue.offset = 16;
var->accel_flags = FB_ACCELF_TEXT;
disp->scrollmode = SCROLL_YREDRAW;
disp->screen_base = (char *)__va(regs[0].phys_addr) + FFB_DFB24_POFF + 8192 * fb->y_margin + 4 * fb->x_margin;
fb->s.ffb.xy_margin = (fb->y_margin << 16) + fb->x_margin;
fb->s.ffb.yx_margin = (((u64)fb->y_margin) << 32) + fb->x_margin;
fb->s.ffb.fbc = (struct ffb_fbc *)(regs[0].phys_addr + FFB_FBC_REGS_POFF);
fb->s.ffb.dac = (struct ffb_dac *)(regs[0].phys_addr + FFB_DAC_POFF);
fb->dispsw = ffb_dispsw;
fb->margins = ffb_margins;
fb->loadcmap = ffb_loadcmap;
fb->setcursor = ffb_setcursor;
fb->setcursormap = ffb_setcursormap;
fb->setcurshape = ffb_setcurshape;
fb->switch_from_graph = ffb_switch_from_graph;
fb->fill = ffb_fill;
#if 0
/* XXX Can't enable this for now, I've seen cases
* XXX where the VC was blanked, and Xsun24 was started
* XXX via a remote login, the sunfb code did not
* XXX unblank creator when it was mmap'd for some
* XXX reason, investigate later... -DaveM
*/
fb->blank = ffb_blank;
fb->unblank = ffb_unblank;
#endif
/* If there are any read errors or fifo overflow conditions,
* clear them now.
*/
if((upa_readl(&fb->s.ffb.fbc->ucsr) & FFB_UCSR_ALL_ERRORS) != 0)
upa_writel(FFB_UCSR_ALL_ERRORS, &fb->s.ffb.fbc->ucsr);
ffb_switch_from_graph(fb);
fb->physbase = regs[0].phys_addr;
fb->mmap_map = ffb_mmap_map;
fb->cursor.hwsize.fbx = 64;
fb->cursor.hwsize.fby = 64;
type->fb_depth = 24;
upa_writel(0x8000, &fb->s.ffb.dac->type);
fb->s.ffb.dac_rev = (upa_readl(&fb->s.ffb.dac->value) >> 0x1c);
i = prom_getintdefault (fb->prom_node, "board_type", 8);
sprintf(idstring, "%s at %016lx type %d DAC %d",
fix->id, regs[0].phys_addr, i, fb->s.ffb.dac_rev);
/* Elite3D has different DAC revision numbering, and no DAC revisions
have the reversed meaning of cursor enable */
if (afb)
fb->s.ffb.dac_rev = 10;
/* Unblank it just to be sure. When there are multiple
* FFB/AFB cards in the system, or it is not the OBP
* chosen console, it will have video outputs off in
* the DAC.
*/
ffb_unblank(fb);
return idstring;
}
static void ffb_putcs(struct vc_data *conp, struct display *p, const unsigned short *s,
int count, int yy, int xx)
{
struct fb_info_sbusfb *fb = (struct fb_info_sbusfb *)p->fb_info;
register struct ffb_fbc *fbc = fb->s.ffb.fbc;
unsigned long flags;
int i, xy;
u8 *fd1, *fd2, *fd3, *fd4;
u16 c;
u64 fgbg;
spin_lock_irqsave(&fb->lock, flags);
c = scr_readw(s);
fgbg = (((u64)(((u32 *)p->dispsw_data)[attr_fgcol(p, c)])) << 32) |
((u32 *)p->dispsw_data)[attr_bgcol(p, c)];
if (fgbg != *(u64 *)&fb->s.ffb.fg_cache) {
FFBFifo(fb, 2);
upa_writeq(fgbg, &fbc->fg);
*(u64 *)&fb->s.ffb.fg_cache = fgbg;
}
xy = fb->s.ffb.xy_margin;
if (fontwidthlog(p))
xy += (xx << fontwidthlog(p));
else
xy += xx * fontwidth(p);
if (fontheightlog(p))
xy += (yy << (16 + fontheightlog(p)));
else
xy += ((yy * fontheight(p)) << 16);
if (fontwidth(p) <= 8) {
while (count >= 4) {
count -= 4;
FFBFifo(fb, 2 + fontheight(p));
upa_writel(4 * fontwidth(p), &fbc->fontw);
upa_writel(xy, &fbc->fontxy);
if (fontheightlog(p)) {
fd1 = p->fontdata + ((scr_readw(s++) & p->charmask) << fontheightlog(p));
fd2 = p->fontdata + ((scr_readw(s++) & p->charmask) << fontheightlog(p));
fd3 = p->fontdata + ((scr_readw(s++) & p->charmask) << fontheightlog(p));
fd4 = p->fontdata + ((scr_readw(s++) & p->charmask) << fontheightlog(p));
} else {
fd1 = p->fontdata + ((scr_readw(s++) & p->charmask) * fontheight(p));
fd2 = p->fontdata + ((scr_readw(s++) & p->charmask) * fontheight(p));
fd3 = p->fontdata + ((scr_readw(s++) & p->charmask) * fontheight(p));
fd4 = p->fontdata + ((scr_readw(s++) & p->charmask) * fontheight(p));
}
if (fontwidth(p) == 8) {
for (i = 0; i < fontheight(p); i++) {
u32 val;
val = ((u32)*fd4++) | ((((u32)*fd3++) | ((((u32)*fd2++) | (((u32)*fd1++)
<< 8)) << 8)) << 8);
upa_writel(val, &fbc->font);
}
xy += 32;
} else {
for (i = 0; i < fontheight(p); i++) {
u32 val = (((u32)*fd4++) | ((((u32)*fd3++) | ((((u32)*fd2++) | (((u32)*fd1++)
<< fontwidth(p))) << fontwidth(p))) << fontwidth(p))) << (24 - 3 * fontwidth(p));
upa_writel(val, &fbc->font);
}
xy += 4 * fontwidth(p);
}
}
} else {
while (count >= 2) {
count -= 2;
FFBFifo(fb, 2 + fontheight(p));
upa_writel(2 * fontwidth(p), &fbc->fontw);
upa_writel(xy, &fbc->fontxy);
if (fontheightlog(p)) {
fd1 = p->fontdata + ((scr_readw(s++) & p->charmask) << (fontheightlog(p) + 1));
fd2 = p->fontdata + ((scr_readw(s++) & p->charmask) << (fontheightlog(p) + 1));
} else {
fd1 = p->fontdata + (((scr_readw(s++) & p->charmask) * fontheight(p)) << 1);
fd2 = p->fontdata + (((scr_readw(s++) & p->charmask) * fontheight(p)) << 1);
}
for (i = 0; i < fontheight(p); i++) {
u32 val = ((((u32)*(u16 *)fd1) << fontwidth(p)) | ((u32)*(u16 *)fd2)) << (16 - fontwidth(p));
upa_writel(val, &fbc->font);
fd1 += 2; fd2 += 2;
}
xy += 2 * fontwidth(p);
}
}
while (count) {
count--;
FFBFifo(fb, 2 + fontheight(p));
upa_writel(fontwidth(p), &fbc->fontw);
upa_writel(xy, &fbc->fontxy);
if (fontheightlog(p))
i = ((scr_readw(s++) & p->charmask) << fontheightlog(p));
else
i = ((scr_readw(s++) & p->charmask) * fontheight(p));
if (fontwidth(p) <= 8) {
fd1 = p->fontdata + i;
for (i = 0; i < fontheight(p); i++) {
u32 val = *fd1++ << 24;
upa_writel(val, &fbc->font);
}
} else {
fd1 = p->fontdata + (i << 1);
for (i = 0; i < fontheight(p); i++) {
u32 val = *(u16 *)fd1 << 16;
upa_writel(val, &fbc->font);
fd1 += 2;
}
}
xy += fontwidth(p);
}
spin_unlock_irqrestore(&fb->lock, flags);
}