Fix bugs in the VESA driver.

diff --git a/drivers/vesa/src/main.c b/drivers/vesa/src/main.c
index a0ba553759beccd5e18994c7da3e764a20c8cb46..c4d6178dbcd925014abc3786ff2155b84d82b146 100644 (file)
--- a/drivers/vesa/src/main.c
+++ b/drivers/vesa/src/main.c
@@ -102,25 +102,28 @@ dword_t vesa_init(list_entry_t *video_devices)
     if (ret != ERR_SUCCESS) goto cleanup;
 
     word_t *modes = (word_t*)((info.videomodes[1] << 4) + info.videomodes[0]);
-    ret = read_physical(modes, available_modes, sizeof(word_t) * MAX_NUM_MODES);
+    word_t *numbers = __builtin_alloca(sizeof(word_t) * MAX_NUM_MODES);
+    ret = read_physical(modes, numbers, sizeof(word_t) * MAX_NUM_MODES);
     if (ret != ERR_SUCCESS) goto cleanup;
 
     num_available_modes = 0;
 
-    for (i = 0; (available_mode_numbers[i] != 0xFFFF) && (i < MAX_NUM_MODES); i++)
+    for (i = 0; (numbers[i] != 0xFFFF) && (i < MAX_NUM_MODES); i++)
     {
         vbe_mode_info_t mode_info;
 
         regs.eax = 0x4F01;
-        regs.ecx = available_mode_numbers[i];
+        regs.ecx = numbers[i];
         regs.es = data_segment;
         regs.edi = 0x0000;
         vm86_interrupt(0x10, &regs);
-
         if (regs.eax != 0x4F) continue;
-        if (mode_info.mem_model_type != 0 && mode_info.mem_model_type != 4 && mode_info.mem_model_type != 6) continue;
 
         ret = read_physical((void*)(data_segment << 4), &mode_info, sizeof(vbe_mode_info_t));
+        if (mode_info.mem_model_type != 0 && mode_info.mem_model_type != 4 && mode_info.mem_model_type != 6) continue;
+
+        available_mode_numbers[num_available_modes] = numbers[i];
+        available_modes[num_available_modes] = mode_info;
         num_available_modes++;
     }
 
@@ -161,6 +164,7 @@ static dword_t vesa_control(video_device_t *device,
             if (ret != ERR_SUCCESS) return ret;
 
             device->current_mode = mode_info;
+            return ERR_SUCCESS;
         }
         else
         {
@@ -175,7 +179,7 @@ static dword_t vesa_control(video_device_t *device,
 
             list[0] = num_available_modes;
 
-            for (i = 0; i < num_available_modes && out_length >= (i * sizeof(dword_t)); i++)
+            for (i = 0; i < num_available_modes && out_length >= ((i + 2) * sizeof(dword_t)); i++)
             {
                 list[i + 1] = available_mode_numbers[i];
             }