[PATCH] fbdev: Capture modelist change event

Add capability to change private modelist.  It checks if the entries in the
new modelist are acceptable, and if not, trimmed from the modelist.

Also added a new event, FB_EVENT_NEW_MODELIST, sent by fbmem, and captured by
fbcon to resize all consoles, if need be.

Requires minor changes to new functions, ie, fb_get_color_depth().
Signed-off-by: Antonino Daplas <adaplas@pol.net>
Signed-off-by: Andrew Morton <akpm@osdl.org>
Signed-off-by: Linus Torvalds <torvalds@osdl.org>

drivers/video/console/bitblit.c

@@ -39,7 +39,7 @@ static inline int get_attribute(struct fb_info *info, u16 c)
 {
 	int attribute = 0;
 
-	if (fb_get_color_depth(info) == 1) {
+	if (fb_get_color_depth(&info->var) == 1) {
 		if (attr_underline(c))
 			attribute |= FBCON_ATTRIBUTE_UNDERLINE;
 		if (attr_reverse(c))

drivers/video/console/fbcon.c

@@ -182,8 +182,10 @@ static __inline__ void ypan_up(struct vc_data *vc, int count);
 static __inline__ void ypan_down(struct vc_data *vc, int count);
 static void fbcon_bmove_rec(struct vc_data *vc, struct display *p, int sy, int sx,
 			    int dy, int dx, int height, int width, u_int y_break);
-static void fbcon_set_disp(struct fb_info *info, struct vc_data *vc);
-static void fbcon_preset_disp(struct fb_info *info, int unit);
+static void fbcon_set_disp(struct fb_info *info, struct fb_var_screeninfo *var,
+			   struct vc_data *vc);
+static void fbcon_preset_disp(struct fb_info *info, struct fb_var_screeninfo *var,
+			      int unit);
 static void fbcon_redraw_move(struct vc_data *vc, struct display *p,
 			      int line, int count, int dy);
 
@@ -209,7 +211,7 @@ static inline int fbcon_is_inactive(struct vc_data *vc, struct fb_info *info)
 static inline int get_color(struct vc_data *vc, struct fb_info *info,
 	      u16 c, int is_fg)
 {
-	int depth = fb_get_color_depth(info);
+	int depth = fb_get_color_depth(&info->var);
 	int color = 0;
 
 	if (console_blanked) {
@@ -418,7 +420,7 @@ static void fbcon_prepare_logo(struct vc_data *vc, struct fb_info *info,
 	 * remove underline attribute from erase character
 	 * if black and white framebuffer.
 	 */
-	if (fb_get_color_depth(info) == 1)
+	if (fb_get_color_depth(&info->var) == 1)
 		erase &= ~0x400;
 	logo_height = fb_prepare_logo(info);
 	logo_lines = (logo_height + vc->vc_font.height - 1) /
@@ -595,9 +597,9 @@ static void con2fb_init_display(struct vc_data *vc, struct fb_info *info,
 		info->fbops->fb_set_par(info);
 
 	if (vc)
-		fbcon_set_disp(info, vc);
+		fbcon_set_disp(info, &info->var, vc);
 	else
-		fbcon_preset_disp(info, unit);
+		fbcon_preset_disp(info, &info->var, unit);
 
 	if (show_logo) {
 		struct vc_data *fg_vc = vc_cons[fg_console].d;
@@ -917,7 +919,7 @@ static void fbcon_init(struct vc_data *vc, int init)
 	}
 	if (p->userfont)
 		charcnt = FNTCHARCNT(p->fontdata);
-	vc->vc_can_do_color = (fb_get_color_depth(info) != 1);
+	vc->vc_can_do_color = (fb_get_color_depth(&info->var) != 1);
 	vc->vc_complement_mask = vc->vc_can_do_color ? 0x7700 : 0x0800;
 	if (charcnt == 256) {
 		vc->vc_hi_font_mask = 0;
@@ -1123,13 +1125,14 @@ int update_var(int con, struct fb_info *info)
 /*
  * If no vc is existent yet, just set struct display
  */
-static void fbcon_preset_disp(struct fb_info *info, int unit)
+static void fbcon_preset_disp(struct fb_info *info, struct fb_var_screeninfo *var,
+			      int unit)
 {
 	struct display *p = &fb_display[unit];
 	struct display *t = &fb_display[fg_console];
 
-	info->var.xoffset = info->var.yoffset = p->yscroll = 0;
-	if (var_to_display(p, &info->var, info))
+	var->xoffset = var->yoffset = p->yscroll = 0;
+	if (var_to_display(p, var, info))
 		return;
 
 	p->fontdata = t->fontdata;
@@ -1138,15 +1141,16 @@ static void fbcon_preset_disp(struct fb_info *info, int unit)
 		REFCOUNT(p->fontdata)++;
 }
 
-static void fbcon_set_disp(struct fb_info *info, struct vc_data *vc)
+static void fbcon_set_disp(struct fb_info *info, struct fb_var_screeninfo *var,
+			   struct vc_data *vc)
 {
 	struct display *p = &fb_display[vc->vc_num], *t;
 	struct vc_data **default_mode = vc->vc_display_fg;
 	struct vc_data *svc = *default_mode;
 	int rows, cols, charcnt = 256;
 
-	info->var.xoffset = info->var.yoffset = p->yscroll = 0;
-	if (var_to_display(p, &info->var, info))
+	var->xoffset = var->yoffset = p->yscroll = 0;
+	if (var_to_display(p, var, info))
 		return;
 	t = &fb_display[svc->vc_num];
 	if (!vc->vc_font.data) {
@@ -1160,7 +1164,7 @@ static void fbcon_set_disp(struct fb_info *info, struct vc_data *vc)
 	if (p->userfont)
 		charcnt = FNTCHARCNT(p->fontdata);
 
-	vc->vc_can_do_color = (fb_get_color_depth(info) != 1);
+	vc->vc_can_do_color = (fb_get_color_depth(var) != 1);
 	vc->vc_complement_mask = vc->vc_can_do_color ? 0x7700 : 0x0800;
 	if (charcnt == 256) {
 		vc->vc_hi_font_mask = 0;
@@ -1175,8 +1179,8 @@ static void fbcon_set_disp(struct fb_info *info, struct vc_data *vc)
 	if (!*vc->vc_uni_pagedir_loc)
 		con_copy_unimap(vc, svc);
 
-	cols = info->var.xres / vc->vc_font.width;
-	rows = info->var.yres / vc->vc_font.height;
+	cols = var->xres / vc->vc_font.width;
+	rows = var->yres / vc->vc_font.height;
 	vc_resize(vc, cols, rows);
 	if (CON_IS_VISIBLE(vc)) {
 		update_screen(vc);
@@ -1955,7 +1959,7 @@ static int fbcon_switch(struct vc_data *vc)
 	set_blitting_type(vc, info, p);
 	((struct fbcon_ops *)info->fbcon_par)->cursor_reset = 1;
 
-	vc->vc_can_do_color = (fb_get_color_depth(info) != 1);
+	vc->vc_can_do_color = (fb_get_color_depth(&info->var) != 1);
 	vc->vc_complement_mask = vc->vc_can_do_color ? 0x7700 : 0x0800;
 	updatescrollmode(p, info, vc);
 
@@ -2326,7 +2330,7 @@ static int fbcon_set_palette(struct vc_data *vc, unsigned char *table)
 	if (fbcon_is_inactive(vc, info))
 		return -EINVAL;
 
-	depth = fb_get_color_depth(info);
+	depth = fb_get_color_depth(&info->var);
 	if (depth > 3) {
 		for (i = j = 0; i < 16; i++) {
 			k = table[i];
@@ -2658,6 +2662,31 @@ static void fbcon_fb_blanked(struct fb_info *info, int blank)
 	ops->blank_state = blank;
 }
 
+static void fbcon_new_modelist(struct fb_info *info)
+{
+	int i;
+	struct vc_data *vc;
+	struct fb_var_screeninfo var;
+	struct fb_videomode *mode;
+
+	for (i = 0; i < MAX_NR_CONSOLES; i++) {
+		if (registered_fb[con2fb_map[i]] != info)
+			continue;
+		if (!fb_display[i].mode)
+			continue;
+		vc = vc_cons[i].d;
+		display_to_var(&var, &fb_display[i]);
+		mode = fb_find_nearest_mode(&var, &info->modelist);
+		fb_videomode_to_var(&var, mode);
+
+		if (vc)
+			fbcon_set_disp(info, &var, vc);
+		else
+			fbcon_preset_disp(info, &var, i);
+
+	}
+}
+
 static int fbcon_event_notify(struct notifier_block *self, 
 			      unsigned long action, void *data)
 {
@@ -2696,6 +2725,9 @@ static int fbcon_event_notify(struct notifier_block *self,
 	case FB_EVENT_BLANK:
 		fbcon_fb_blanked(info, *(int *)event->data);
 		break;
+	case FB_EVENT_NEW_MODELIST:
+		fbcon_new_modelist(info);
+		break;
 	}
 
 	return ret;

drivers/video/fbmem.c

@@ -62,10 +62,8 @@ int num_registered_fb;
  * Helpers
  */
 
-int fb_get_color_depth(struct fb_info *info)
+int fb_get_color_depth(struct fb_var_screeninfo *var)
 {
-	struct fb_var_screeninfo *var = &info->var;
-
 	if (var->green.length == var->blue.length &&
 	    var->green.length == var->red.length &&
 	    !var->green.offset && !var->blue.offset &&
@@ -300,7 +298,7 @@ static void fb_set_logo(struct fb_info *info,
 	const u8 *src = logo->data;
 	u8 d, xor = (info->fix.visual == FB_VISUAL_MONO01) ? 0xff : 0;
 
-	if (fb_get_color_depth(info) == 3)
+	if (fb_get_color_depth(&info->var) == 3)
 		fg = 7;
 
 	switch (depth) {
@@ -365,7 +363,7 @@ static struct logo_data {
 
 int fb_prepare_logo(struct fb_info *info)
 {
-	int depth = fb_get_color_depth(info);
+	int depth = fb_get_color_depth(&info->var);
 
 	memset(&fb_logo, 0, sizeof(struct logo_data));
 
@@ -1209,6 +1207,40 @@ fbmem_init(void)
 }
 subsys_initcall(fbmem_init);
 
+int fb_new_modelist(struct fb_info *info)
+{
+	struct fb_event event;
+	struct fb_var_screeninfo var = info->var;
+	struct list_head *pos, *n;
+	struct fb_modelist *modelist;
+	struct fb_videomode *m, mode;
+	int err = 1;
+
+	list_for_each_safe(pos, n, &info->modelist) {
+		modelist = list_entry(pos, struct fb_modelist, list);
+		m = &modelist->mode;
+		fb_videomode_to_var(&var, m);
+		var.activate = FB_ACTIVATE_TEST;
+		err = fb_set_var(info, &var);
+		fb_var_to_videomode(&mode, &var);
+		if (err || !fb_mode_is_equal(m, &mode)) {
+			list_del(pos);
+			kfree(pos);
+		}
+	}
+
+	err = 1;
+
+	if (!list_empty(&info->modelist)) {
+		event.info = info;
+		err = notifier_call_chain(&fb_notifier_list,
+					   FB_EVENT_NEW_MODELIST,
+					   &event);
+	}
+
+	return err;
+}
+
 static char *video_options[FB_MAX];
 static int ofonly;
 
@@ -1320,5 +1352,6 @@ EXPORT_SYMBOL(fb_set_suspend);
 EXPORT_SYMBOL(fb_register_client);
 EXPORT_SYMBOL(fb_unregister_client);
 EXPORT_SYMBOL(fb_get_options);
+EXPORT_SYMBOL(fb_new_modelist);
 
 MODULE_LICENSE("GPL");

drivers/video/fbsysfs.c

@@ -151,13 +151,23 @@ static ssize_t store_modes(struct class_device *class_device, const char * buf,
 {
 	struct fb_info *fb_info =
 		(struct fb_info *)class_get_devdata(class_device);
+	LIST_HEAD(old_list);
 	int i = count / sizeof(struct fb_videomode);
+
 	if (i * sizeof(struct fb_videomode) != count)
 		return -EINVAL;
 
-	fb_destroy_modelist(&fb_info->modelist);
+	acquire_console_sem();
+	list_splice(&fb_info->modelist, &old_list);
 	fb_videomode_to_modelist((struct fb_videomode *)buf, i,
 				 &fb_info->modelist);
+	if (fb_new_modelist(fb_info)) {
+		fb_destroy_modelist(&fb_info->modelist);
+		list_splice(&old_list, &fb_info->modelist);
+	} else
+		fb_destroy_modelist(&old_list);
+
+	release_console_sem();
 
 	return 0;
 }

drivers/video/modedb.c

@@ -721,6 +721,47 @@ struct fb_videomode *fb_find_best_mode(struct fb_var_screeninfo *var,
 	return best;
 }
 
+/**
+ * fb_find_nearest_mode - find mode closest video mode
+ *
+ * @var: pointer to struct fb_var_screeninfo
+ * @head: pointer to modelist
+ *
+ * Finds best matching videomode, smaller or greater in dimension.
+ * If more than 1 videomode is found, will return the videomode with
+ * the closest refresh rate
+ */
+struct fb_videomode *fb_find_nearest_mode(struct fb_var_screeninfo *var,
+					  struct list_head *head)
+{
+	struct list_head *pos;
+	struct fb_modelist *modelist;
+	struct fb_videomode *mode, *best = NULL;
+	u32 diff = -1, diff_refresh = -1;
+
+	list_for_each(pos, head) {
+		u32 d;
+
+		modelist = list_entry(pos, struct fb_modelist, list);
+		mode = &modelist->mode;
+
+		d = abs(mode->xres - var->xres) +
+			abs(mode->yres - var->yres);
+		if (diff > d) {
+			diff = d;
+			best = mode;
+		} else if (diff == d) {
+			d = abs(mode->refresh - best->refresh);
+			if (diff_refresh > d) {
+				diff_refresh = d;
+				best = mode;
+			}
+		}
+	}
+
+	return best;
+}
+
 /**
  * fb_match_mode - find a videomode which exactly matches the timings in var
  * @var: pointer to struct fb_var_screeninfo
@@ -846,5 +887,6 @@ EXPORT_SYMBOL(fb_delete_videomode);
 EXPORT_SYMBOL(fb_destroy_modelist);
 EXPORT_SYMBOL(fb_match_mode);
 EXPORT_SYMBOL(fb_find_best_mode);
+EXPORT_SYMBOL(fb_find_nearest_mode);
 EXPORT_SYMBOL(fb_videomode_to_modelist);
 EXPORT_SYMBOL(fb_find_mode);

drivers/video/nvidia/nvidia.c

@@ -657,7 +657,7 @@ static int nvidia_calc_regs(struct fb_info *info)
 {
 	struct nvidia_par *par = info->par;
 	struct _riva_hw_state *state = &par->ModeReg;
-	int i, depth = fb_get_color_depth(info);
+	int i, depth = fb_get_color_depth(&info->var);
 	int h_display = info->var.xres / 8 - 1;
 	int h_start = (info->var.xres + info->var.right_margin) / 8 - 1;
 	int h_end = (info->var.xres + info->var.right_margin +

include/linux/fb.h

@@ -493,6 +493,8 @@ struct fb_cursor_user {
 #define FB_EVENT_SET_CONSOLE_MAP        0x07
 /*      A display blank is requested       */
 #define FB_EVENT_BLANK                  0x08
+/*      Private modelist is to be replaced */
+#define FB_EVENT_NEW_MODELIST           0x09
 
 struct fb_event {
 	struct fb_info *info;
@@ -826,8 +828,9 @@ extern void fb_sysmove_buf_aligned(struct fb_info *info, struct fb_pixmap *buf,
 				u8 *dst, u32 d_pitch, u8 *src, u32 s_pitch,
 				u32 height);
 extern void fb_set_suspend(struct fb_info *info, int state);
-extern int fb_get_color_depth(struct fb_info *info);
+extern int fb_get_color_depth(struct fb_var_screeninfo *var);
 extern int fb_get_options(char *name, char **option);
+extern int fb_new_modelist(struct fb_info *info);
 
 extern struct fb_info *registered_fb[FB_MAX];
 extern int num_registered_fb;
@@ -882,6 +885,8 @@ extern struct fb_videomode *fb_match_mode(struct fb_var_screeninfo *var,
 					  struct list_head *head);
 extern struct fb_videomode *fb_find_best_mode(struct fb_var_screeninfo *var,
 					      struct list_head *head);
+extern struct fb_videomode *fb_find_nearest_mode(struct fb_var_screeninfo *var,
+						 struct list_head *head);
 extern void fb_destroy_modelist(struct list_head *head);
 extern void fb_videomode_to_modelist(struct fb_videomode *modedb, int num,
 				     struct list_head *head);