radeonfb: Implement proper workarounds for PLL accesses

Index: linux-work/drivers/video/aty/radeonfb.h
===================================================================
@@ -77,6 +77,15 @@
 	CHIP_HAS_CRTC2		= 0x00040000UL,	
 };
 
+/*
+ * Errata workarounds
+ */
+enum radeon_errata {
+	CHIP_ERRATA_R300_CG		= 0x00000001,
+	CHIP_ERRATA_PLL_DUMMYREADS	= 0x00000002,
+	CHIP_ERRATA_PLL_DELAY		= 0x00000004,
+};
+
 
 /*
  * Monitor types

@@ -295,6 +304,7 @@
 	int			chipset;
 	u8			family;
 	u8			rev;
+	unsigned int		errata;
 	unsigned long		video_ram;
 	unsigned long		mapped_vram;
 	int			vram_width;

@@ -305,7 +315,6 @@
 	int			has_CRTC2;
 	int			is_mobility;
 	int			is_IGP;
-	int			R300_cg_workaround;
 	int			reversed_DAC;
 	int			reversed_TMDS;
 	struct panel_info	panel_info;

@@ -369,6 +378,21 @@
  * IO macros
  */
 
+/* Note about this function: we have some rare cases where we must not schedule,
+ * this typically happen with our special "wake up early" hook which allows us to
+ * wake up the graphic chip (and thus get the console back) before everything else
+ * on some machines that support that mecanism. At this point, interrupts are off
+ * and scheduling is not permitted
+ */
+static inline void _radeon_msleep(struct radeonfb_info *rinfo, unsigned long ms)
+{
+	if (rinfo->no_schedule || oops_in_progress)
+		mdelay(ms);
+	else
+		msleep(ms);
+}
+
+
 #define INREG8(addr)		readb((rinfo->mmio_base)+addr)
 #define OUTREG8(addr,val)	writeb(val, (rinfo->mmio_base)+addr)
 #define INREG(addr)		readl((rinfo->mmio_base)+addr)

@@ -390,107 +414,85 @@
 
 #define OUTREGP(addr,val,mask)	_OUTREGP(rinfo, addr, val,mask)
 
-/* This function is required to workaround a hardware bug in some (all?)
- * revisions of the R300.  This workaround should be called after every
- * CLOCK_CNTL_INDEX register access.  If not, register reads afterward
- * may not be correct.
- */
-static inline void R300_cg_workardound(struct radeonfb_info *rinfo)
-{
-	u32 save, tmp;
-	save = INREG(CLOCK_CNTL_INDEX);
-	tmp = save & ~(0x3f | PLL_WR_EN);
-	OUTREG(CLOCK_CNTL_INDEX, tmp);
-	tmp = INREG(CLOCK_CNTL_DATA);
-	OUTREG(CLOCK_CNTL_INDEX, save);
-}
-
 /*
- * PLL accesses suffer from various HW issues on the different chip
- * families. Some R300's need the above workaround, rv200 & friends
- * need a couple of dummy reads after any write of CLOCK_CNTL_INDEX,
- * and some RS100/200 need a dummy read before writing to
- * CLOCK_CNTL_INDEX as well. Instead of testing every chip revision,
- * we just unconditionally do  the workarounds at once since PLL
- * accesses are far from beeing performance critical. Except for R300
- * one which stays separate for now
+ * Note about PLL register accesses:
+ *
+ * I have removed the spinlock on them on purpose. The driver now
+ * expects that it will only manipulate the PLL registers in normal
+ * task environment, where radeon_msleep() will be called, protected
+ * by a semaphore (currently the console semaphore) so that no conflict
+ * will happen on the PLL register index.
+ *
+ * With the latest changes to the VT layer, this is guaranteed for all
+ * calls except the actual drawing/blits which aren't supposed to use
+ * the PLL registers anyway
+ *
+ * This is very important for the workarounds to work properly. The only
+ * possible exception to this rule is the call to unblank(), which may
+ * be done at irq time if an oops is in progress.
  */
-
-static inline void radeon_pll_workaround_before(struct radeonfb_info *rinfo)
+static inline void radeon_pll_errata_after_index(struct radeonfb_info *rinfo)
 {
+	if (!(rinfo->errata & CHIP_ERRATA_PLL_DUMMYREADS))
+		return;
+
+	(void)INREG(CLOCK_CNTL_DATA);
 	(void)INREG(CRTC_GEN_CNTL);
 }
 
-static inline void radeon_pll_workaround_after(struct radeonfb_info *rinfo)
+static inline void radeon_pll_errata_after_data(struct radeonfb_info *rinfo)
 {
-	(void)INREG(CLOCK_CNTL_DATA);
-	(void)INREG(CRTC_GEN_CNTL);
+	if (rinfo->errata & CHIP_ERRATA_PLL_DELAY) {
+		/* we can't deal with posted writes here ... */
+		_radeon_msleep(rinfo, 5);
+	}
+	if (rinfo->errata & CHIP_ERRATA_R300_CG) {
+		u32 save, tmp;
+		save = INREG(CLOCK_CNTL_INDEX);
+		tmp = save & ~(0x3f | PLL_WR_EN);
+		OUTREG(CLOCK_CNTL_INDEX, tmp);
+		tmp = INREG(CLOCK_CNTL_DATA);
+		OUTREG(CLOCK_CNTL_INDEX, save);
+	}
 }
 
 static inline u32 __INPLL(struct radeonfb_info *rinfo, u32 addr)
 {
 	u32 data;
 
-	radeon_pll_workaround_before(rinfo);
 	OUTREG8(CLOCK_CNTL_INDEX, addr & 0x0000003f);
-	radeon_pll_workaround_after(rinfo);
+	radeon_pll_errata_after_index(rinfo);
 	data = INREG(CLOCK_CNTL_DATA);
-	if (rinfo->R300_cg_workaround)
-		R300_cg_workardound(rinfo);
+	radeon_pll_errata_after_data(rinfo);
 	return data;
 }
 
-static inline u32 _INPLL(struct radeonfb_info *rinfo, u32 addr)
-{
-       	unsigned long flags;
-	u32 data;
-
-	spin_lock_irqsave(&rinfo->reg_lock, flags);
-	data = __INPLL(rinfo, addr);
-	spin_unlock_irqrestore(&rinfo->reg_lock, flags);
-	return data;
-}
-
-#define INPLL(addr)		_INPLL(rinfo, addr)
-
-
 static inline void __OUTPLL(struct radeonfb_info *rinfo, unsigned int index,
 			    u32 val)
 {
 
-	radeon_pll_workaround_before(rinfo);
 	OUTREG8(CLOCK_CNTL_INDEX, (index & 0x0000003f) | 0x00000080);
-	radeon_pll_workaround_after(rinfo);
+	radeon_pll_errata_after_index(rinfo);
 	OUTREG(CLOCK_CNTL_DATA, val);
-	if (rinfo->R300_cg_workaround)
-		R300_cg_workardound(rinfo);
+	radeon_pll_errata_after_data(rinfo);
 }
 
-static inline void _OUTPLL(struct radeonfb_info *rinfo, unsigned int index, u32 val)
-{
-       	unsigned long flags;
-	spin_lock_irqsave(&rinfo->reg_lock, flags);
-	__OUTPLL(rinfo, index, val);
-	spin_unlock_irqrestore(&rinfo->reg_lock, flags);
-}
 
-static inline void _OUTPLLP(struct radeonfb_info *rinfo, unsigned int index,
-			    u32 val, u32 mask)
+static inline void __OUTPLLP(struct radeonfb_info *rinfo, unsigned int index,
+			     u32 val, u32 mask)
 {
-	unsigned long flags;
 	unsigned int tmp;
 
-	spin_lock_irqsave(&rinfo->reg_lock, flags);
 	tmp  = __INPLL(rinfo, index);
 	tmp &= (mask);
 	tmp |= (val);
 	__OUTPLL(rinfo, index, tmp);
-	spin_unlock_irqrestore(&rinfo->reg_lock, flags);
 }
 
 
-#define OUTPLL(index, val)		_OUTPLL(rinfo, index, val)
-#define OUTPLLP(index, val, mask)	_OUTPLLP(rinfo, index, val, mask)
+#define INPLL(addr)			__INPLL(rinfo, addr)
+#define OUTPLL(index, val)		__OUTPLL(rinfo, index, val)
+#define OUTPLLP(index, val, mask)	__OUTPLLP(rinfo, index, val, mask)
 
 
 #define BIOS_IN8(v)  	(readb(rinfo->bios_seg + (v)))

@@ -582,20 +584,6 @@
 	printk(KERN_ERR "radeonfb: Idle Timeout !\n");
 }
 
-/* Note about this function: we have some rare cases where we must not schedule,
- * this typically happen with our special "wake up early" hook which allows us to
- * wake up the graphic chip (and thus get the console back) before everything else
- * on some machines that support that mecanism. At this point, interrupts are off
- * and scheduling is not permitted
- */
-static inline void _radeon_msleep(struct radeonfb_info *rinfo, unsigned long ms)
-{
-	if (rinfo->no_schedule)
-		mdelay(ms);
-	else
-		msleep(ms);
-}
-
 
 #define radeon_engine_idle()		_radeon_engine_idle(rinfo)
 #define radeon_fifo_wait(entries)	_radeon_fifo_wait(rinfo,entries)

Index: linux-work/drivers/video/aty/radeon_monitor.c
===================================================================
@@ -657,11 +657,8 @@
 	    && rinfo->is_mobility) {
 		int ppll_div_sel;
 		u32 ppll_divn;
-		radeon_pll_workaround_before(rinfo);
 		ppll_div_sel = INREG8(CLOCK_CNTL_INDEX + 1) & 0x3;
-		radeon_pll_workaround_after(rinfo);
-		if (rinfo->R300_cg_workaround)
-			R300_cg_workardound(rinfo);
+		radeon_pll_errata_after_index(rinfo);
 		ppll_divn = INPLL(PPLL_DIV_0 + ppll_div_sel);
 		rinfo->panel_info.ref_divider = rinfo->pll.ref_div;
 		rinfo->panel_info.fbk_divider = ppll_divn & 0x7ff;

101 lines not on this page · raw message

111 lines not on this page · raw message

Index: linux-work/drivers/video/aty/radeon_accel.c
===================================================================
@@ -189,32 +189,6 @@
 
 	radeon_engine_flush (rinfo);
 
-    	/* Some ASICs have bugs with dynamic-on feature, which are  
-     	 * ASIC-version dependent, so we force all blocks on for now
-     	 * -- from XFree86
-     	 * We don't do that on macs, things just work here with dynamic
-     	 * clocking... --BenH
-	 */
-#ifdef CONFIG_ALL_PPC
-	if (_machine != _MACH_Pmac && rinfo->hasCRTC2)
-#else
-	if (rinfo->has_CRTC2)
-#endif	
-	{
-		u32 tmp;
-
-		tmp = INPLL(SCLK_CNTL);
-		OUTPLL(SCLK_CNTL, ((tmp & ~DYN_STOP_LAT_MASK) |
-				   CP_MAX_DYN_STOP_LAT |
-				   SCLK_FORCEON_MASK));
-
-		if (rinfo->family == CHIP_FAMILY_RV200)
-		{
-			tmp = INPLL(SCLK_MORE_CNTL);
-			OUTPLL(SCLK_MORE_CNTL, tmp | SCLK_MORE_FORCEON);
-		}
-	}
-
 	clock_cntl_index = INREG(CLOCK_CNTL_INDEX);
 	mclk_cntl = INPLL(MCLK_CNTL);
 

@@ -274,8 +248,6 @@
 
 	OUTREG(CLOCK_CNTL_INDEX, clock_cntl_index);
 	OUTPLL(MCLK_CNTL, mclk_cntl);
-	if (rinfo->R300_cg_workaround)
-		R300_cg_workardound(rinfo);
 }
 
 void radeonfb_engine_init (struct radeonfb_info *rinfo)

Index: linux-work/drivers/video/aty/radeonfb.h
===================================================================
@@ -77,6 +77,15 @@
 	CHIP_HAS_CRTC2		= 0x00040000UL,	
 };
 
+/*
+ * Errata workarounds
+ */
+enum radeon_errata {
+	CHIP_ERRATA_R300_CG		= 0x00000001,
+	CHIP_ERRATA_PLL_DUMMYREADS	= 0x00000002,
+	CHIP_ERRATA_PLL_DELAY		= 0x00000004,
+};
+
 
 /*
  * Monitor types

@@ -295,6 +304,7 @@
 	int			chipset;
 	u8			family;
 	u8			rev;
+	unsigned int		errata;
 	unsigned long		video_ram;
 	unsigned long		mapped_vram;
 	int			vram_width;

@@ -305,7 +315,6 @@
 	int			has_CRTC2;
 	int			is_mobility;
 	int			is_IGP;
-	int			R300_cg_workaround;
 	int			reversed_DAC;
 	int			reversed_TMDS;
 	struct panel_info	panel_info;

@@ -369,6 +378,21 @@
  * IO macros
  */
 
+/* Note about this function: we have some rare cases where we must not schedule,
+ * this typically happen with our special "wake up early" hook which allows us to
+ * wake up the graphic chip (and thus get the console back) before everything else
+ * on some machines that support that mecanism. At this point, interrupts are off
+ * and scheduling is not permitted
+ */
+static inline void _radeon_msleep(struct radeonfb_info *rinfo, unsigned long ms)
+{
+	if (rinfo->no_schedule || oops_in_progress)
+		mdelay(ms);
+	else
+		msleep(ms);
+}
+
+
 #define INREG8(addr)		readb((rinfo->mmio_base)+addr)
 #define OUTREG8(addr,val)	writeb(val, (rinfo->mmio_base)+addr)
 #define INREG(addr)		readl((rinfo->mmio_base)+addr)

@@ -390,107 +414,85 @@
 
 #define OUTREGP(addr,val,mask)	_OUTREGP(rinfo, addr, val,mask)
 
-/* This function is required to workaround a hardware bug in some (all?)
- * revisions of the R300.  This workaround should be called after every
- * CLOCK_CNTL_INDEX register access.  If not, register reads afterward
- * may not be correct.
- */
-static inline void R300_cg_workardound(struct radeonfb_info *rinfo)
-{
-	u32 save, tmp;
-	save = INREG(CLOCK_CNTL_INDEX);
-	tmp = save & ~(0x3f | PLL_WR_EN);
-	OUTREG(CLOCK_CNTL_INDEX, tmp);
-	tmp = INREG(CLOCK_CNTL_DATA);
-	OUTREG(CLOCK_CNTL_INDEX, save);
-}
-
 /*
- * PLL accesses suffer from various HW issues on the different chip
- * families. Some R300's need the above workaround, rv200 & friends
- * need a couple of dummy reads after any write of CLOCK_CNTL_INDEX,
- * and some RS100/200 need a dummy read before writing to
- * CLOCK_CNTL_INDEX as well. Instead of testing every chip revision,
- * we just unconditionally do  the workarounds at once since PLL
- * accesses are far from beeing performance critical. Except for R300
- * one which stays separate for now
+ * Note about PLL register accesses:
+ *
+ * I have removed the spinlock on them on purpose. The driver now
+ * expects that it will only manipulate the PLL registers in normal
+ * task environment, where radeon_msleep() will be called, protected
+ * by a semaphore (currently the console semaphore) so that no conflict
+ * will happen on the PLL register index.
+ *
+ * With the latest changes to the VT layer, this is guaranteed for all
+ * calls except the actual drawing/blits which aren't supposed to use
+ * the PLL registers anyway
+ *
+ * This is very important for the workarounds to work properly. The only
+ * possible exception to this rule is the call to unblank(), which may
+ * be done at irq time if an oops is in progress.
  */
-
-static inline void radeon_pll_workaround_before(struct radeonfb_info *rinfo)
+static inline void radeon_pll_errata_after_index(struct radeonfb_info *rinfo)
 {
+	if (!(rinfo->errata & CHIP_ERRATA_PLL_DUMMYREADS))
+		return;
+
+	(void)INREG(CLOCK_CNTL_DATA);
 	(void)INREG(CRTC_GEN_CNTL);
 }
 
-static inline void radeon_pll_workaround_after(struct radeonfb_info *rinfo)
+static inline void radeon_pll_errata_after_data(struct radeonfb_info *rinfo)
 {
-	(void)INREG(CLOCK_CNTL_DATA);
-	(void)INREG(CRTC_GEN_CNTL);
+	if (rinfo->errata & CHIP_ERRATA_PLL_DELAY) {
+		/* we can't deal with posted writes here ... */
+		_radeon_msleep(rinfo, 5);
+	}
+	if (rinfo->errata & CHIP_ERRATA_R300_CG) {
+		u32 save, tmp;
+		save = INREG(CLOCK_CNTL_INDEX);
+		tmp = save & ~(0x3f | PLL_WR_EN);
+		OUTREG(CLOCK_CNTL_INDEX, tmp);
+		tmp = INREG(CLOCK_CNTL_DATA);
+		OUTREG(CLOCK_CNTL_INDEX, save);
+	}
 }
 
 static inline u32 __INPLL(struct radeonfb_info *rinfo, u32 addr)
 {
 	u32 data;
 
-	radeon_pll_workaround_before(rinfo);
 	OUTREG8(CLOCK_CNTL_INDEX, addr & 0x0000003f);
-	radeon_pll_workaround_after(rinfo);
+	radeon_pll_errata_after_index(rinfo);
 	data = INREG(CLOCK_CNTL_DATA);
-	if (rinfo->R300_cg_workaround)
-		R300_cg_workardound(rinfo);
+	radeon_pll_errata_after_data(rinfo);
 	return data;
 }
 
-static inline u32 _INPLL(struct radeonfb_info *rinfo, u32 addr)
-{
-       	unsigned long flags;
-	u32 data;
-
-	spin_lock_irqsave(&rinfo->reg_lock, flags);
-	data = __INPLL(rinfo, addr);
-	spin_unlock_irqrestore(&rinfo->reg_lock, flags);
-	return data;
-}
-
-#define INPLL(addr)		_INPLL(rinfo, addr)
-
-
 static inline void __OUTPLL(struct radeonfb_info *rinfo, unsigned int index,
 			    u32 val)
 {
 
-	radeon_pll_workaround_before(rinfo);
 	OUTREG8(CLOCK_CNTL_INDEX, (index & 0x0000003f) | 0x00000080);
-	radeon_pll_workaround_after(rinfo);
+	radeon_pll_errata_after_index(rinfo);
 	OUTREG(CLOCK_CNTL_DATA, val);
-	if (rinfo->R300_cg_workaround)
-		R300_cg_workardound(rinfo);
+	radeon_pll_errata_after_data(rinfo);
 }
 
-static inline void _OUTPLL(struct radeonfb_info *rinfo, unsigned int index, u32 val)
-{
-       	unsigned long flags;
-	spin_lock_irqsave(&rinfo->reg_lock, flags);
-	__OUTPLL(rinfo, index, val);
-	spin_unlock_irqrestore(&rinfo->reg_lock, flags);
-}
 
-static inline void _OUTPLLP(struct radeonfb_info *rinfo, unsigned int index,
-			    u32 val, u32 mask)
+static inline void __OUTPLLP(struct radeonfb_info *rinfo, unsigned int index,
+			     u32 val, u32 mask)
 {
-	unsigned long flags;
 	unsigned int tmp;
 
-	spin_lock_irqsave(&rinfo->reg_lock, flags);
 	tmp  = __INPLL(rinfo, index);
 	tmp &= (mask);
 	tmp |= (val);
 	__OUTPLL(rinfo, index, tmp);
-	spin_unlock_irqrestore(&rinfo->reg_lock, flags);
 }
 
 
-#define OUTPLL(index, val)		_OUTPLL(rinfo, index, val)
-#define OUTPLLP(index, val, mask)	_OUTPLLP(rinfo, index, val, mask)
+#define INPLL(addr)			__INPLL(rinfo, addr)
+#define OUTPLL(index, val)		__OUTPLL(rinfo, index, val)
+#define OUTPLLP(index, val, mask)	__OUTPLLP(rinfo, index, val, mask)
 
 
 #define BIOS_IN8(v)  	(readb(rinfo->bios_seg + (v)))

@@ -582,20 +584,6 @@
 	printk(KERN_ERR "radeonfb: Idle Timeout !\n");
 }
 
-/* Note about this function: we have some rare cases where we must not schedule,
- * this typically happen with our special "wake up early" hook which allows us to
- * wake up the graphic chip (and thus get the console back) before everything else
- * on some machines that support that mecanism. At this point, interrupts are off
- * and scheduling is not permitted
- */
-static inline void _radeon_msleep(struct radeonfb_info *rinfo, unsigned long ms)
-{
-	if (rinfo->no_schedule)
-		mdelay(ms);
-	else
-		msleep(ms);
-}
-
 
 #define radeon_engine_idle()		_radeon_engine_idle(rinfo)
 #define radeon_fifo_wait(entries)	_radeon_fifo_wait(rinfo,entries)

Index: linux-work/drivers/video/aty/radeon_monitor.c
===================================================================
@@ -657,11 +657,8 @@
 	    && rinfo->is_mobility) {
 		int ppll_div_sel;
 		u32 ppll_divn;
-		radeon_pll_workaround_before(rinfo);
 		ppll_div_sel = INREG8(CLOCK_CNTL_INDEX + 1) & 0x3;
-		radeon_pll_workaround_after(rinfo);
-		if (rinfo->R300_cg_workaround)
-			R300_cg_workardound(rinfo);
+		radeon_pll_errata_after_index(rinfo);
 		ppll_divn = INPLL(PPLL_DIV_0 + ppll_div_sel);
 		rinfo->panel_info.ref_divider = rinfo->pll.ref_div;
 		rinfo->panel_info.fbk_divider = ppll_divn & 0x7ff;

103 lines not on this page · raw message

111 lines not on this page · raw message

Index: linux-work/drivers/video/aty/radeon_accel.c
===================================================================
@@ -189,32 +189,6 @@
 
 	radeon_engine_flush (rinfo);
 
-    	/* Some ASICs have bugs with dynamic-on feature, which are  
-     	 * ASIC-version dependent, so we force all blocks on for now
-     	 * -- from XFree86
-     	 * We don't do that on macs, things just work here with dynamic
-     	 * clocking... --BenH
-	 */
-#ifdef CONFIG_ALL_PPC
-	if (_machine != _MACH_Pmac && rinfo->hasCRTC2)
-#else
-	if (rinfo->has_CRTC2)
-#endif	
-	{
-		u32 tmp;
-
-		tmp = INPLL(SCLK_CNTL);
-		OUTPLL(SCLK_CNTL, ((tmp & ~DYN_STOP_LAT_MASK) |
-				   CP_MAX_DYN_STOP_LAT |
-				   SCLK_FORCEON_MASK));
-
-		if (rinfo->family == CHIP_FAMILY_RV200)
-		{
-			tmp = INPLL(SCLK_MORE_CNTL);
-			OUTPLL(SCLK_MORE_CNTL, tmp | SCLK_MORE_FORCEON);
-		}
-	}
-
 	clock_cntl_index = INREG(CLOCK_CNTL_INDEX);
 	mclk_cntl = INPLL(MCLK_CNTL);
 

@@ -274,8 +248,6 @@
 
 	OUTREG(CLOCK_CNTL_INDEX, clock_cntl_index);
 	OUTPLL(MCLK_CNTL, mclk_cntl);
-	if (rinfo->R300_cg_workaround)
-		R300_cg_workardound(rinfo);
 }
 
 void radeonfb_engine_init (struct radeonfb_info *rinfo)

Index: linux-work/include/linux/fb.h
===================================================================
@@ -563,6 +563,9 @@
 	int (*fb_setcolreg)(unsigned regno, unsigned red, unsigned green,
 			    unsigned blue, unsigned transp, struct fb_info *info);
 
+	/* set color registers in batch */
+	int (*fb_setcmap)(struct fb_cmap *cmap, struct fb_info *info);
+
 	/* blank display */
 	int (*fb_blank)(int blank, struct fb_info *info);
 

163 lines not on this page · raw message

Index: linux-work/drivers/video/fbcmap.c
===================================================================
@@ -222,8 +222,11 @@
 	transp = cmap->transp;
 	start = cmap->start;
 
-	if (start < 0 || !info->fbops->fb_setcolreg)
+	if (start < 0 || (!info->fbops->fb_setcolreg &&
+			  !info->fbops->fb_setcmap))
 		return -EINVAL;
+	if (info->fbops->fb_setcmap)
+		return info->fbops->fb_setcmap(cmap, info);
 	for (i = 0; i < cmap->len; i++) {
 		hred = *red++;
 		hgreen = *green++;

@@ -243,15 +246,40 @@
 	int i, start;
 	u16 __user *red, *green, *blue, *transp;
 	u_int hred, hgreen, hblue, htransp = 0xffff;
-
+	
 	red = cmap->red;
 	green = cmap->green;
 	blue = cmap->blue;
 	transp = cmap->transp;
 	start = cmap->start;
 
-	if (start < 0 || !info->fbops->fb_setcolreg)
+	if (start < 0 || (!info->fbops->fb_setcolreg &&
+			  !info->fbops->fb_setcmap))
 		return -EINVAL;
+	
+	/* If we can batch, do it */
+	if (info->fbops->fb_setcmap && cmap->len > 1) {
+		struct fb_cmap umap;
+		int size = cmap->len * sizeof(u16);
+		int rc;
+
+		memset(&umap, 0, sizeof(struct fb_cmap));
+		rc = fb_alloc_cmap(&umap, cmap->len, transp != NULL);
+		if (rc)
+			return rc;
+		if (copy_from_user(umap.red, red, size) ||
+		    copy_from_user(umap.green, green, size) ||
+		    copy_from_user(umap.blue, blue, size) ||
+		    (transp && copy_from_user(umap.transp, transp, size))) {
+			rc = -EFAULT;
+		}
+		umap.start = start;
+		if (rc == 0)
+			rc = info->fbops->fb_setcmap(&umap, info);
+		fb_dealloc_cmap(&umap);
+		return rc;
+	}
+
 	for (i = 0; i < cmap->len; i++, red++, blue++, green++) {
 		if (get_user(hred, red) ||
 		    get_user(hgreen, green) ||