Bug-fixing and keystroke

[Serbinenko Vladimir <serbinenko.vova@list.ru>]

[-- Attachment #1: Type: text/plain, Size: 1599 bytes --]


I wrote some bugfixing patch + new feature.

Bug-fixing was mainly about FAT bug (unalloc magic on not-fat fs).

The new feature is sending keystroke to OS (imitating keypress +
changing keyboard flags).
it works like:
    keystroke [flags] [keys]

[flags] are like
--capslock=0 --rshift=1 ... (documentation in patch in keystroke.c at
the end)
0 - means set flag to 0, 1 to 1 and 2 means keep value.
Some flags are the modes (like caps, num, scroll). Others are keys (like
ctrl and shift)
If you set 0/1 to mode it will result like  turning  off/on  the mode
If you set  key to 1 it will imitate keypressing until you really press it
[keys] are key1 key2 ... like
h e l l o or
down down enter.
keylist is taken from GRUB Legacy + some new keys.

Known bugs (can smb help fix them):
LEDs in qemu are not set
help doesn't contain all useful informations.

                                                                        
            Serbinenko Vladimir


2005-07-30  Vladimir Serbinenko <serbinenko.vova@list.ru>
   
    Some bugfixes
   
    * commands/ls.c (grub_ls_list_disks): fixed segmentation fault
    when fs == 0 or fs->label == 0
    (grub_ls_list_files): Added label showing
    * kern/disk.c (grub_print_partinfo): fixed segmentation fault when
    fs->label == 0
    * fs/fat.c (grub_fat_dir): fixed segmentetaion fault by freeing

    Keystroke module(pc) and preboots
   
    * commands/boot.c: Added support for preboot functions
    * conf/i386.rmk: new module keystroke
    * include/grub/normal.h: new headers and types for preboots
    * keystroke/keystroke.c: new file

[-- Attachment #2: key+bugfix.patch --]
[-- Type: text/x-patch, Size: 23076 bytes --]

diff -urN grub2um/commands/boot.c grub2m/commands/boot.c
--- grub2um/commands/boot.c	2005-07-25 15:23:26.000000000 +0200
+++ grub2m/commands/boot.c	2005-07-25 16:58:11.000000000 +0200
@@ -23,13 +23,23 @@
 #include <grub/arg.h>
 #include <grub/misc.h>
 #include <grub/loader.h>
+#include <grub/mm.h>
+#include <grub/err.h>
+
+static grub_prebootfn_t *grub_preboots = 0;
+static int grub_preboot_cnt = 0;
 
 static grub_err_t
 grub_cmd_boot (struct grub_arg_list *state __attribute__ ((unused)),
 	       int argc, char **args __attribute__ ((unused)))
 {
+  int i;
+
   if (argc)
     return grub_error (GRUB_ERR_BAD_ARGUMENT, "too many arguments");
+
+  for (i = 0; i < grub_preboot_cnt; i++)
+    grub_preboots[i] ();
   
   grub_loader_boot ();
   
@@ -38,6 +48,46 @@
 
 \f
 
+grub_err_t
+grub_preboot_add (grub_prebootfn_t fn)
+{
+  grub_prebootfn_t *tmp;
+  tmp = (grub_prebootfn_t *) grub_realloc (grub_preboots, (grub_preboot_cnt + 1) * sizeof(grub_prebootfn_t));
+  if (!tmp)
+    return grub_errno;
+  grub_preboots = tmp;
+  grub_preboots[grub_preboot_cnt] = fn;
+  grub_preboot_cnt++;
+
+  return 0;
+}
+
+\f
+
+grub_err_t
+grub_preboot_remove (grub_prebootfn_t fn)
+{
+  int i;
+  for (i = 0; i < grub_preboot_cnt; i++)
+    if (grub_preboots[i] == fn)
+      break;
+
+  if (i == grub_preboot_cnt)
+    {
+      return grub_error (GRUB_ERR_BAD_ARGUMENT, "fn not found");
+    }
+
+  for (; i < grub_preboot_cnt - 1; i++)
+    grub_preboots[i] = grub_preboots[i + 1];
+
+  grub_preboots = (grub_prebootfn_t *) grub_realloc (grub_preboots, (--grub_preboot_cnt) * sizeof(grub_prebootfn_t));
+
+  return 0;
+
+}
+
+\f
+
 #ifdef GRUB_UTIL
 void
 grub_boot_init (void)
diff -urN grub2um/commands/ls.c grub2m/commands/ls.c
--- grub2um/commands/ls.c	2005-07-25 15:23:26.000000000 +0200
+++ grub2m/commands/ls.c	2005-07-30 13:42:04.000000000 +0200
@@ -84,16 +84,18 @@
 
 		  grub_printf (", Filesystem type %s",
 			       fs ? fs->name : "Unknown");
-		  
-		  (fs->label) (dev, &label);
-		  if (grub_errno == GRUB_ERR_NONE)
+		  if (fs && fs->label)
 		    {
-		      if (label && grub_strlen (label))
-			grub_printf (", Label: %s", label);
-		      grub_free (label);
+		      (fs->label) (dev, &label);
+		      if (grub_errno == GRUB_ERR_NONE)
+			{
+			  if (label && grub_strlen (label))
+			    grub_printf (", Label: %s", label);
+			  grub_free (label);
+			}
+		      else
+			grub_errno = GRUB_ERR_NONE;
 		    }
-		  else
-		    grub_errno = GRUB_ERR_NONE;
 		}
 
 	      grub_putchar ('\n');
@@ -108,6 +110,7 @@
 	    }
 
 	  grub_device_close (dev);
+	 
 	}
   
       return 0;
@@ -223,6 +226,19 @@
 	  
       grub_printf ("(%s): Filesystem is %s.\n",
 		   device_name, fs ? fs->name : "unknown");
+      if (fs && fs->label)
+	{
+	  char *label;
+	  (fs->label) (dev, &label);
+	  if (grub_errno == GRUB_ERR_NONE)
+	    {
+	      if (label && grub_strlen (label))
+		grub_printf (", Label: %s", label);
+	      grub_free (label);
+	    }
+	  else
+	    grub_errno = GRUB_ERR_NONE;
+	}
     }
   else if (fs)
     {
@@ -267,6 +283,7 @@
     }
 
  fail:
+
   if (dev)
     grub_device_close (dev);
       
diff -urN grub2um/conf/i386-pc.mk grub2m/conf/i386-pc.mk
--- grub2um/conf/i386-pc.mk	2005-07-25 15:23:26.000000000 +0200
+++ grub2m/conf/i386-pc.mk	2005-07-25 15:25:29.000000000 +0200
@@ -989,7 +989,7 @@
 	font.mod _multiboot.mod ls.mod boot.mod cmp.mod cat.mod		\
 	terminal.mod fshelp.mod chain.mod multiboot.mod amiga.mod	\
 	apple.mod pc.mod sun.mod loopback.mod reboot.mod halt.mod	\
-	help.mod default.mod timeout.mod configfile.mod
+	help.mod default.mod timeout.mod configfile.mod keystroke.mod
 
 # For _chain.mod.
 _chain_mod_SOURCES = loader/i386/pc/chainloader.c
@@ -1781,6 +1781,51 @@
 
 hello_mod_CFLAGS = $(COMMON_CFLAGS)
 
+# For keystroke.mod.
+keystroke_mod_SOURCES = keystroke/keystroke.c
+CLEANFILES += keystroke.mod mod-keystroke.o mod-keystroke.c pre-keystroke.o keystroke_mod-keystroke_keystroke.o def-keystroke.lst und-keystroke.lst
+MOSTLYCLEANFILES += keystroke_mod-keystroke_keystroke.d
+DEFSYMFILES += def-keystroke.lst
+UNDSYMFILES += und-keystroke.lst
+
+keystroke.mod: pre-keystroke.o mod-keystroke.o
+	-rm -f $@
+	$(LD) -r -d -o $@ $^
+	$(STRIP) --strip-unneeded -K grub_mod_init -K grub_mod_fini -R .note -R .comment $@
+
+pre-keystroke.o: keystroke_mod-keystroke_keystroke.o
+	-rm -f $@
+	$(LD) -r -d -o $@ $^
+
+mod-keystroke.o: mod-keystroke.c
+	$(CC) $(CPPFLAGS) $(CFLAGS) $(keystroke_mod_CFLAGS) -c -o $@ $<
+
+mod-keystroke.c: moddep.lst genmodsrc.sh
+	sh $(srcdir)/genmodsrc.sh 'keystroke' $< > $@ || (rm -f $@; exit 1)
+
+def-keystroke.lst: pre-keystroke.o
+	$(NM) -g --defined-only -P -p $< | sed 's/^\([^ ]*\).*/\1 keystroke/' > $@
+
+und-keystroke.lst: pre-keystroke.o
+	echo 'keystroke' > $@
+	$(NM) -u -P -p $< | cut -f1 -d' ' >> $@
+
+keystroke_mod-keystroke_keystroke.o: keystroke/keystroke.c
+	$(CC) -Ikeystroke -I$(srcdir)/keystroke $(CPPFLAGS) $(CFLAGS) $(keystroke_mod_CFLAGS) -c -o $@ $<
+
+keystroke_mod-keystroke_keystroke.d: keystroke/keystroke.c
+	set -e; 	  $(CC) -Ikeystroke -I$(srcdir)/keystroke $(CPPFLAGS) $(CFLAGS) $(keystroke_mod_CFLAGS) -M $< 	  | sed 's,keystroke\.o[ :]*,keystroke_mod-keystroke_keystroke.o $@ : ,g' > $@; 	  [ -s $@ ] || rm -f $@
+
+-include keystroke_mod-keystroke_keystroke.d
+
+CLEANFILES += cmd-keystroke.lst
+COMMANDFILES += cmd-keystroke.lst
+
+cmd-keystroke.lst: keystroke/keystroke.c gencmdlist.sh
+	set -e; 	  $(CC) -Ikeystroke -I$(srcdir)/keystroke $(CPPFLAGS) $(CFLAGS) $(keystroke_mod_CFLAGS) -E $< 	  | sh $(srcdir)/gencmdlist.sh keystroke > $@ || (rm -f $@; exit 1)
+
+keystroke_mod_CFLAGS = $(COMMON_CFLAGS)
+
 # For boot.mod.
 boot_mod_SOURCES = commands/boot.c
 CLEANFILES += boot.mod mod-boot.o mod-boot.c pre-boot.o boot_mod-commands_boot.o def-boot.lst und-boot.lst
diff -urN grub2um/conf/i386-pc.rmk grub2m/conf/i386-pc.rmk
--- grub2um/conf/i386-pc.rmk	2005-07-25 15:23:26.000000000 +0200
+++ grub2m/conf/i386-pc.rmk	2005-07-25 15:25:29.000000000 +0200
@@ -102,7 +102,7 @@
 	font.mod _multiboot.mod ls.mod boot.mod cmp.mod cat.mod		\
 	terminal.mod fshelp.mod chain.mod multiboot.mod amiga.mod	\
 	apple.mod pc.mod sun.mod loopback.mod reboot.mod halt.mod	\
-	help.mod default.mod timeout.mod configfile.mod
+	help.mod default.mod timeout.mod configfile.mod keystroke.mod
 
 # For _chain.mod.
 _chain_mod_SOURCES = loader/i386/pc/chainloader.c
@@ -163,6 +163,10 @@
 hello_mod_SOURCES = hello/hello.c
 hello_mod_CFLAGS = $(COMMON_CFLAGS)
 
+# For keystroke.mod.
+keystroke_mod_SOURCES = keystroke/keystroke.c
+keystroke_mod_CFLAGS = $(COMMON_CFLAGS)
+
 # For boot.mod.
 boot_mod_SOURCES = commands/boot.c
 boot_mod_CFLAGS = $(COMMON_CFLAGS)
diff -urN grub2um/fs/fat.c grub2m/fs/fat.c
--- grub2um/fs/fat.c	2005-07-25 15:23:26.000000000 +0200
+++ grub2m/fs/fat.c	2005-07-30 14:26:32.000000000 +0200
@@ -629,7 +629,7 @@
   struct grub_fat_data *data = 0;
   grub_disk_t disk = device->disk;
   grub_size_t len;
-  char *dirname;
+  char *dirname = 0;
   char *p;
 
 #ifndef GRUB_UTIL
@@ -660,8 +660,10 @@
 
  fail:
 
-  grub_free (dirname);
-  grub_free (data);
+  if (dirname)
+    grub_free (dirname);
+  if (data)
+    grub_free (data);
   
 #ifndef GRUB_UTIL
   grub_dl_unref (my_mod);
diff -urN grub2um/include/grub/normal.h grub2m/include/grub/normal.h
--- grub2um/include/grub/normal.h	2005-07-25 15:23:26.000000000 +0200
+++ grub2m/include/grub/normal.h	2005-07-25 15:25:29.000000000 +0200
@@ -44,6 +44,9 @@
 /* Not loaded yet. Used for auto-loading.  */
 #define GRUB_COMMAND_FLAG_NOT_LOADED	0x20
 
+/* Preboot function declaration.  */
+typedef void (*grub_prebootfn_t) (void); 
+
 /* The command description.  */
 struct grub_command
 {
@@ -178,6 +181,8 @@
 grub_menu_t grub_context_get_current_menu (void);
 grub_menu_t grub_context_push_menu (grub_menu_t menu);
 void grub_context_pop_menu (void);
+grub_err_t grub_preboot_add (grub_prebootfn_t fn);
+grub_err_t grub_preboot_remove (grub_prebootfn_t fn);
 
 #ifdef GRUB_UTIL
 void grub_normal_init (void);
diff -urN grub2um/kern/disk.c grub2m/kern/disk.c
--- grub2um/kern/disk.c	2005-07-25 15:23:26.000000000 +0200
+++ grub2m/kern/disk.c	2005-07-30 12:14:34.000000000 +0200
@@ -535,7 +535,7 @@
       grub_printf ("\tPartition num:%s, Filesystem type %s",
 		   partname, fs ? fs->name : "Unknown");
 	  
-      if (fs)
+      if (fs && fs->label)
 	{
 	  (fs->label) (part, &label);
 	  if (grub_errno == GRUB_ERR_NONE)
diff -urN grub2um/keystroke/keystroke.c grub2m/keystroke/keystroke.c
--- grub2um/keystroke/keystroke.c	1970-01-01 01:00:00.000000000 +0100
+++ grub2m/keystroke/keystroke.c	2005-07-28 16:59:19.000000000 +0200
@@ -0,0 +1,406 @@
+/* keystroke.c - test module for dynamic loading */
+/*
+ *  GRUB  --  GRand Unified Bootloader
+ *  Copyright (C) 2005  Free Software Foundation, Inc.
+ *  Copyright (C) 2005  Vladimir Serbinenko serbinenko.vova@bk.ru
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <grub/types.h>
+#include <grub/misc.h>
+#include <grub/mm.h>
+#include <grub/err.h>
+#include <grub/dl.h>
+#include <grub/normal.h>
+#include <grub/arg.h>
+
+#define RAW_ADDR(a) ((void *)(a))
+struct 
+keysym
+{
+  char *unshifted_name;			/* the name in unshifted state */
+  char *shifted_name;			/* the name in shifted state */
+  unsigned char unshifted_ascii;	/* the ascii code in unshifted state */
+  unsigned char shifted_ascii;		/* the ascii code in shifted state */
+  unsigned char keycode;		/* keyboard scancode */
+};
+
+char keystroke[32];
+int keylen=0;
+/* The sum of:
+   x << y x - action y - flag
+   x: 0x0 - turn off, 0x1  - turn on , 0x3  - keep
+   y:
+   0x0 - numlock    mode, 
+   0x1 - capslock   mode, 
+   0x2 - scrolllock mode,
+   0x3 - insert     mode,
+   0x4 - wait       mode,
+   0x5 - left  shift key,
+   0x6 - right shift key,
+   0x7 - left  alt   key,
+   0x8 - right alt   key,
+   0x9 - left  ctrl  key,
+   0xa - right ctrl  key,
+   0xb - sysreq      key,
+   0xc - numlock     key,
+   0xd - capslock    key,
+   0xe - scrolllock  key,
+   0xf - insert      key
+ */
+unsigned long kbflags=0;
+int noled = 1;
+
+/* The table for key symbols. If the "shifted" member of an entry is
+   NULL, the entry does not have shifted state. Copied from GRUB Legacy setkey fuction  */
+static struct keysym keysym_table[] =
+{
+  {"escape",		0,		0x1b,	0,	0x01},
+  {"1",			"exclam",	'1',	'!',	0x02},
+  {"2",			"at",		'2',	'@',	0x03},
+  {"3",			"numbersign",	'3',	'#',	0x04},
+  {"4",			"dollar",	'4',	'$',	0x05},
+  {"5",			"percent",	'5',	'%',	0x06},
+  {"6",			"caret",	'6',	'^',	0x07},
+  {"7",			"ampersand",	'7',	'&',	0x08},
+  {"8",			"asterisk",	'8',	'*',	0x09},
+  {"9",			"parenleft",	'9',	'(',	0x0a},
+  {"0",			"parenright",	'0',	')',	0x0b},
+  {"minus",		"underscore",	'-',	'_',	0x0c},
+  {"equal",		"plus",		'=',	'+',	0x0d},
+  {"backspace",		0,		'\b',	0,	0x0e},
+  {"tab",		0,		'\t',	0,	0x0f},
+  {"q",			"Q",		'q',	'Q',	0x10},
+  {"w",			"W",		'w',	'W',	0x11},
+  {"e",			"E",		'e',	'E',	0x12},
+  {"r",			"R",		'r',	'R',	0x13},
+  {"t",			"T",		't',	'T',	0x14},
+  {"y",			"Y",		'y',	'Y',	0x15},
+  {"u",			"U",		'u',	'U',	0x16},
+  {"i",			"I",		'i',	'I',	0x17},
+  {"o",			"O",		'o',	'O',	0x18},
+  {"p",			"P",		'p',	'P',	0x19},
+  {"bracketleft",	"braceleft",	'[',	'{',	0x1a},
+  {"bracketright",	"braceright",	']',	'}',	0x1b},
+  {"enter",		0,		'\r',	0,	0x1c},
+  {"control",		0,		0,	0,	0x1d},
+  {"a",			"A",		'a',	'A',	0x1e},
+  {"s",			"S",		's',	'S',	0x1f},
+  {"d",			"D",		'd',	'D',	0x20},
+  {"f",			"F",		'f',	'F',	0x21},
+  {"g",			"G",		'g',	'G',	0x22},
+  {"h",			"H",		'h',	'H',	0x23},
+  {"j",			"J",		'j',	'J',	0x24},
+  {"k",			"K",		'k',	'K',	0x25},
+  {"l",			"L",		'l',	'L',	0x26},
+  {"semicolon",		"colon",	';',	':',	0x27},
+  {"quote",		"doublequote",	'\'',	'"',	0x28},
+  {"backquote",		"tilde",	'`',	'~',	0x29},
+  {"shift",		0,		0,	0,	0x2a},
+  {"backslash",		"bar",		'\\',	'|',	0x2b},
+  {"z",			"Z",		'z',	'Z',	0x2c},
+  {"x",			"X",		'x',	'X',	0x2d},
+  {"c",			"C",		'c',	'C',	0x2e},
+  {"v",			"V",		'v',	'V',	0x2f},
+  {"b",			"B",		'b',	'B',	0x30},
+  {"n",			"N",		'n',	'N',	0x31},
+  {"m",			"M",		'm',	'M',	0x32},
+  {"comma",		"less",		',',	'<',	0x33},
+  {"period",		"greater",	'.',	'>',	0x34},
+  {"slash",		"question",	'/',	'?',	0x35},
+  {"rshift",		0,		0,	0,	0x36},
+  {"numasterisk",		0,		'*',	0,	0x37},
+  {"alt",		0,		0,	0,	0x38},
+  {"space",		0,		' ',	0,	0x39},
+  {"capslock",		0,		0,	0,	0x3a},
+  {"F1",		0,		0,	0,	0x3b},
+  {"F2",		0,		0,	0,	0x3c},
+  {"F3",		0,		0,	0,	0x3d},
+  {"F4",		0,		0,	0,	0x3e},
+  {"F5",		0,		0,	0,	0x3f},
+  {"F6",	 	0,		0,	0,	0x40},
+  {"F7",		0,		0,	0,	0x41},
+  {"F8",		0,		0,	0,	0x42},
+  {"F9",		0,		0,	0,	0x43},
+  {"F10",		0,		0,	0,	0x44},
+  {"num7",		"numhome",		'7',	0,	0x47},
+  {"num8",		"numup",		'8',	0,	0x48},
+  {"num9",		"numpgup",		'9',	0,	0x49},
+  {"numminus",		0,		'-',	0,	0x4a},
+  {"num4",		"numleft",		'4',	0,	0x4b},
+  {"num5",		"num5numlock",		'5',	0,	0x4c},
+  {"num6",		"numright",		'6',	0,	0x4d},
+  {"numplus",		0,		'-',	0,	0x4e},
+  {"num1",		"numend",		'1',	0,	0x4f},
+  {"num2",		"numdown",		'2',	0,	0x50},
+  {"num3",		"numpgdown",		'3',	0,	0x51},
+  {"num0",		"numinsert",		'0',	0,	0x52},
+  {"numperiod",	"numdelete", 0,	0x7f,		0x53},
+  {"F11",		0,		0,	0,	0x57},
+  {"F12",		0,		0,	0,	0x58},
+  {"numenter",		0,		'\r',	0,	0xe0},
+  {"numslash",		0,		'/',	0,	0xe0},
+  {"delete",		0,		0x7f,	0,	0xe0},
+  {"insert",		0,		0xe0,	0,	0x52},
+  {"home",		0,		0xe0,	0,	0x47},
+  {"end",		0,		0xe0,	0,	0x4f},
+  {"pgdown",		0,		0xe0,	0,	0x51},
+  {"pgup",		0,		0xe0,	0,	0x49},
+  {"down",		0,		0xe0,	0,	0x50},
+  {"up",		0,		0xe0,	0,	0x48},
+  {"left",		0,		0xe0,	0,	0x4b},
+  {"right",		0,		0xe0,	0,	0x4d}
+};
+
+/* Send a character VALUE to port PORT  */
+static void 
+outportb (char value, int port) {
+  asm volatile ("outb %%al,%%dx": :"a" (value),"d" (port));
+  return;
+}
+
+/* Read a byte from port PORT  */
+static unsigned char 
+inb (unsigned int port)
+{
+  unsigned char ret;
+  asm volatile ("inb %%dx,%%al":"=a" (ret):"d" (port));
+  return ret;
+
+}
+
+/* Set a simple flag in flags variable  
+   flags - where to set,
+   outoffset - offset of flag in FLAGS,
+   inoffset  - offset of flag in kbflags
+*/
+static void
+grub_keystroke_set_simple_flag (unsigned long *flags, int outoffset, int inoffset)
+{
+  /* previous state of flag  */
+  int prevstat = (*flags >> outoffset) & 1;
+  /* what to do with flag*/
+  int operation = (kbflags >> inoffset) & 3;
+  /* new state */
+  int newstat = (operation == 1) || (operation == 2 && prevstat);
+  /* Set new state  */
+  *flags = (*flags & (~(1 << outoffset))) | (newstat << outoffset);
+}
+
+/* Set a double flag (ctrl/alt) in flags variable  
+   flags - where to set,
+   outoffsetc - offset of common flag in FLAGS,
+   outoffsetl - offset of "left" flag in FLAGS,
+   inoffsetr  - offset of "right" flag in kbflags,
+   inoffsetl - offset of "left" flag in kbflags,
+*/
+static void
+grub_keystroke_set_double_flag (unsigned long *flags, int outoffsetc, int outoffsetl, int inoffsetr, int inoffsetl)
+{
+  /* previous state of flag  */
+  int prevstatc = (*flags >> outoffsetc) & 1;
+  int prevstatl = (*flags >> outoffsetl) & 1;
+  int prevstatr = prevstatc && (!prevstatl);
+  /* what to do with flag*/
+  int operationr = (kbflags >> inoffsetr) & 3;
+  int operationl = (kbflags >> inoffsetl) & 3;
+  /* new state */
+  int newstatl = (operationl == 1) || (operationl == 2 && prevstatl);
+  int newstatr = (operationr == 1) || (operationr == 2 && prevstatr);
+  int newstatc = newstatr || newstatr;
+  /* Set new state  */
+  *flags = (*flags & (~(1 << outoffsetl))) | (newstatl << outoffsetl);
+  *flags = (*flags & (~(1 << outoffsetc))) | (newstatc << outoffsetc);
+}
+
+
+/* Set keyboard buffer to our keystroke  */
+static void
+grub_keystroke_preboot (void)
+{
+
+  int i;
+  /* For convenion: pointer to flags  */
+  unsigned long *flags = (unsigned long *) RAW_ADDR (0x417);
+
+  /* Set the keystroke  */
+  *((char *) RAW_ADDR (0x41a)) = 0x1e;
+  *((char *) RAW_ADDR (0x41c)) = keylen+0x1e;
+  for(i = 0; i < 0x20; i++)
+    ((char *) RAW_ADDR (0x41e))[i] = keystroke[i];
+
+  /* Set the flags. For more information reffer to technical specification*/
+  grub_keystroke_set_simple_flag (flags,  5,   0 * 2); // numlock mode
+  grub_keystroke_set_simple_flag (flags,  6,   1 * 2); // capslock mode
+  grub_keystroke_set_simple_flag (flags,  4,   2 * 2); // scrolllock mode
+  grub_keystroke_set_simple_flag (flags,  7,   3 * 2); // insert mode
+  grub_keystroke_set_simple_flag (flags, 11,   4 * 2); // wait mode
+  grub_keystroke_set_simple_flag (flags,  1,   5 * 2); // left shift
+  grub_keystroke_set_simple_flag (flags,  0,   6 * 2); // right shift
+  grub_keystroke_set_simple_flag (flags, 10, 0xb * 2); // sysreq
+  grub_keystroke_set_simple_flag (flags, 13, 0xc * 2); // numlock key
+  grub_keystroke_set_simple_flag (flags, 14, 0xd * 2); // capslock key
+  grub_keystroke_set_simple_flag (flags, 12, 0xe * 2); // scrolllock key
+  grub_keystroke_set_simple_flag (flags, 15, 0xf * 2); // insert key
+
+  /*Set ctrl and alt*/
+  grub_keystroke_set_double_flag (flags, 2, 8, 0xa * 2, 9 * 2); //Ctrl
+  grub_keystroke_set_double_flag (flags, 3, 9,   8 * 2, 7 * 2); //Alt
+
+  /* Write new LED state  */
+  if (!noled)
+    {
+      int value = 0;
+      int failed;
+      /* Try 5 times  */
+      for (failed = 0; failed < 5; failed++)
+	{
+	  value = 0;
+	  /* Send command change LEDs  */
+	  outportb (0xed, 0x60);
+
+	  /* Wait */
+	  while ((value != 0xfa) && (value != 0xfe))
+	    value = inb (0x60);
+
+	  if (value == 0xfa)
+	    {
+	      /* Set new LEDs*/
+	      outportb ((flags[0] >> 4) & 7, 0x60);
+	      break;
+	    }
+	}
+    }
+}
+
+  /*   0x0 - numlock    mode, 
+   0x1 - capslock   mode, 
+   0x2 - scrolllock mode,
+   0x3 - insert     mode,
+   0x4 - wait       mode,
+   0x5 - left  shift key,
+   0x6 - right shift key,
+   0x7 - left  alt   key,
+   0x8 - right alt   key,
+   0x9 - left  ctrl  key,
+   0xa - right ctrl  key,
+   0xb - sysreq      key,
+   0xc - numlock     key,
+   0xd - capslock    key,
+   0xe - scrolllock  key,
+   0xf - insert      key*/
+
+/*Parse keystroke  */
+static grub_err_t
+grub_cmd_keystroke (struct grub_arg_list *state,
+		int argc,
+		char **args)
+{
+
+  /* To stop warning  */ 
+  auto int find_key_code (char *key); 
+  auto int find_ascii_code (char *key);
+
+  auto int find_key_code (char *key)
+    {
+      unsigned i;
+
+      for (i = 0; i < sizeof (keysym_table) / sizeof (keysym_table[0]); i++)
+	{
+	  if (keysym_table[i].unshifted_name && grub_strcmp (key, keysym_table[i].unshifted_name) == 0)
+	    return keysym_table[i].keycode;
+	  else if (keysym_table[i].shifted_name && grub_strcmp (key, keysym_table[i].shifted_name) == 0)
+	    return keysym_table[i].keycode;
+	}
+
+      return 0;
+    }
+
+  auto int find_ascii_code (char *key)
+    {
+      unsigned i;
+
+      for (i = 0; i < sizeof (keysym_table) / sizeof (keysym_table[0]); i++)
+	{
+	  if (keysym_table[i].unshifted_name && grub_strcmp (key, keysym_table[i].unshifted_name) == 0)
+	    return keysym_table[i].unshifted_ascii;
+	  else if (keysym_table[i].shifted_name && grub_strcmp (key, keysym_table[i].shifted_name) == 0)
+	    return keysym_table[i].shifted_ascii;
+	}
+
+      return 0;
+    }
+
+  int i;
+
+  /* Set keystroke and keylen variables*/
+  keylen = 0;
+
+  for (i = 0; i < argc && keylen < 0x20; i++)
+    {
+      if (find_key_code (args[i]))
+	{
+	  keystroke[keylen++] = find_ascii_code (args[i]);
+	  keystroke[keylen++] = find_key_code (args[i]);
+	}
+    }
+
+  /* Set kbflags */
+  kbflags = 0;
+  for (i = 0; i <= 15; i++)
+    kbflags |= (state[i].set ? grub_strtoul (state[i].arg, 0, 0) : 2) << (2*i);
+
+  noled = state[16].set;
+
+  return 0;
+}
+
+static const struct grub_arg_option options[] =
+  {
+    {"numlock",    'n', GRUB_ARG_OPTION_OPTIONAL, "set numlock    mode   (2=keep, 0=off, 1=on)", "2", ARG_TYPE_INT},
+    {"capslock",   'c', GRUB_ARG_OPTION_OPTIONAL, "set capslock   mode   (2=keep, 0=off, 1=on)", "2", ARG_TYPE_INT},
+    {"scrolllock", 's', GRUB_ARG_OPTION_OPTIONAL, "set scrolllock mode   (2=keep, 0=off, 1=on)", "2", ARG_TYPE_INT},
+    {"insert",     'i', GRUB_ARG_OPTION_OPTIONAL, "set insert     mode   (2=keep, 0=off, 1=on)", "2", ARG_TYPE_INT},
+    {"wait",        0 , GRUB_ARG_OPTION_OPTIONAL, "set wait mode (pause) (2=keep, 0=off, 1=on)", "2", ARG_TYPE_INT},
+    {"lshift",     'l', GRUB_ARG_OPTION_OPTIONAL, "block left  shift key (2=no, 0=unpress, 1=press)", "2", ARG_TYPE_INT},
+    {"rshift",     'r', GRUB_ARG_OPTION_OPTIONAL, "block right shift key (2=no, 0=unpress, 1=press)", "2", ARG_TYPE_INT},
+    {"lalt",        0 , GRUB_ARG_OPTION_OPTIONAL, "block left    alt key (2=no, 0=unpress, 1=press)", "2", ARG_TYPE_INT},
+    {"ralt",       'a', GRUB_ARG_OPTION_OPTIONAL, "block right   alt key (2=no, 0=unpress, 1=press)", "2", ARG_TYPE_INT},
+    {"lctrl",       0 , GRUB_ARG_OPTION_OPTIONAL, "block left  ctrl  key (2=no, 0=unpress, 1=press)", "2", ARG_TYPE_INT},
+    {"rctrl",       0 , GRUB_ARG_OPTION_OPTIONAL, "block right ctrl  key (2=no, 0=unpress, 1=press)", "2", ARG_TYPE_INT},
+    {"sysreq",      0 , GRUB_ARG_OPTION_OPTIONAL, "block sys req     key (2=no, 0=unpress, 1=press)", "2", ARG_TYPE_INT},
+    {"numkey",      0 , GRUB_ARG_OPTION_OPTIONAL, "block numlock     key (2=no, 0=unpress, 1=press)", "2", ARG_TYPE_INT},
+    {"capskey",     0 , GRUB_ARG_OPTION_OPTIONAL, "block capslock    key (2=no, 0=unpress, 1=press)", "2", ARG_TYPE_INT},
+    {"scrkey",      0 , GRUB_ARG_OPTION_OPTIONAL, "block scrolllock  key (2=no, 0=unpress, 1=press)", "2", ARG_TYPE_INT},
+    {"inskey",      0 , GRUB_ARG_OPTION_OPTIONAL, "block insert      key (2=no, 0=unpress, 1=press)", "2", ARG_TYPE_INT},
+    {"noled",       0 , 0, "Don't try to set LEDs. Try if blocks.", 0, 0},
+    {0, 0, 0, 0, 0, 0}
+  };
+
+GRUB_MOD_INIT
+{
+  (void)mod;			/* To stop warning. */
+  grub_register_command ("keystroke", grub_cmd_keystroke, GRUB_COMMAND_FLAG_BOTH,
+			 "keystroke [options] [KEY1 [KEY2 ...[KEY16]...]]", 
+"Send a keystroke to OS. Set keyboard mode and block some keys as pressed/unpressed. Keys are unblocked on next press.", options);
+  grub_preboot_add (grub_keystroke_preboot);
+}
+
+GRUB_MOD_FINI
+{
+  grub_unregister_command ("keystroke");
+  grub_preboot_remove (grub_keystroke_preboot);
+}