diff --git a/commands/menu.c b/commands/menu.c
index f3bd78d..f734db3 100644
--- a/commands/menu.c
+++ b/commands/menu.c
@@ -26,6 +26,7 @@
 #include <menu.h>
 #include <getopt.h>
 #include <errno.h>
+#include <linux/err.h>
 
 typedef enum {
 #if defined(CONFIG_CMD_MENU_MANAGEMENT)
@@ -66,8 +67,7 @@
 static int do_menu_entry_add(struct cmd_menu *cm)
 {
 	struct menu_entry *me;
-	struct menu *m, *sm;
-	int ret = -ENOMEM;
+	struct menu *m;
 
 	if (!cm->menu || (!cm->command && !cm->submenu) || !cm->description)
 		return -EINVAL;
@@ -79,50 +79,16 @@
 		return -EINVAL;
 	}
 
-	me = menu_entry_alloc();
+	if (cm->submenu)
+		me = menu_add_submenu(m, cm->submenu, cm->description);
+	else
+		me = menu_add_command_entry(m, cm->description, cm->command);
 	if (!me)
-		return -ENOMEM;
-
-	if (cm->submenu) {
-		me->action = menu_action_show;
-
-		sm = menu_get_by_name(cm->submenu);
-
-		if (!sm) {
-			eprintf("SubMenu '%s' not found\n", cm->menu);
-			goto free;
-		}
-
-		me->priv = sm;
-	} else {
-		me->action = menu_action_run;
-
-		me->priv = strdup(cm->command);
-		if (!me->priv)
-			goto free;
-	}
-
-	me->display = strdup(cm->description);
-	if (!me->display)
-		goto free;
-
-	ret = menu_add_entry(m, me);
-
-	if (ret)
-		goto free;
+		return PTR_ERR(me);
 
 	me->non_re_ent = !cm->re_entrant;
 
 	return 0;
-
-free:
-	eputs("Entry add fail\n");
-
-	free(me->priv);
-
-	menu_entry_free(me);
-
-	return ret;
 }
 
 /*
diff --git a/common/menu.c b/common/menu.c
index 6e6637a..7620d9e 100644
--- a/common/menu.c
+++ b/common/menu.c
@@ -29,6 +29,7 @@
 #include <xfuncs.h>
 #include <errno.h>
 #include <readkey.h>
+#include <linux/err.h>
 
 static LIST_HEAD(menus);
 
@@ -143,8 +144,7 @@
 	if (!me)
 		return;
 
-	free(me->display);
-	free(me);
+	me->free(me);
 }
 
 static void print_menu_entry(struct menu *m, struct menu_entry *me, int reverse)
@@ -273,14 +273,74 @@
 
 void menu_action_exit(struct menu *m, struct menu_entry *me) {}
 
-void menu_action_run(struct menu *m, struct menu_entry *me)
+struct submenu {
+	char *submenu;
+	struct menu_entry entry;
+};
+
+static void menu_action_show(struct menu *m, struct menu_entry *me)
 {
+	struct submenu *s = container_of(me, struct submenu, entry);
+	struct menu *sm;
+
+	sm = menu_get_by_name(s->submenu);
+	if (sm)
+		menu_show(sm);
+	else
+		eprintf("no such menu: %s\n", s->submenu);
+}
+
+static void submenu_free(struct menu_entry *me)
+{
+	struct submenu *s = container_of(me, struct submenu, entry);
+
+	free(s->entry.display);
+	free(s->submenu);
+	free(s);
+}
+
+struct menu_entry *menu_add_submenu(struct menu *parent, char *submenu, char *display)
+{
+	struct submenu *s = calloc(1, sizeof(*s));
 	int ret;
-	const char *s = getenv((const char*)me->priv);
+
+	if (!s)
+		return ERR_PTR(-ENOMEM);
+
+	s->submenu = strdup(submenu);
+	s->entry.action = menu_action_show;
+	s->entry.free = submenu_free;
+	s->entry.display = strdup(display);
+	if (!s->entry.display || !s->submenu) {
+		ret = -ENOMEM;
+		goto err_free;
+	}
+
+	ret = menu_add_entry(parent, &s->entry);
+	if (ret)
+		goto err_free;
+
+	return &s->entry;
+
+err_free:
+	submenu_free(&s->entry);
+	return ERR_PTR(ret);
+}
+
+struct action_entry {
+	char *command;
+	struct menu_entry entry;
+};
+
+static void menu_action_command(struct menu *m, struct menu_entry *me)
+{
+	struct action_entry *e = container_of(me, struct action_entry, entry);
+	int ret;
+	const char *s = getenv(e->command);
 
 	/* can be a command as boot */
 	if (!s)
-		s = me->priv;
+		s = e->command;
 
 	ret = run_command (s, 0);
 
@@ -288,9 +348,41 @@
 		udelay(1000000);
 }
 
-void menu_action_show(struct menu *m, struct menu_entry *me)
+static void menu_command_free(struct menu_entry *me)
 {
-	struct menu *sm = me->priv;
+	struct action_entry *e = container_of(me, struct action_entry, entry);
 
-	menu_show(sm);
+	free(e->entry.display);
+	free(e->command);
+
+	free(e);
 }
+
+struct menu_entry *menu_add_command_entry(struct menu *m, char *display, char *command)
+{
+	struct action_entry *e = calloc(1, sizeof(*e));
+	int ret;
+
+	if (!e)
+		return ERR_PTR(-ENOMEM);
+
+	e->command = strdup(command);
+	e->entry.action = menu_action_command;
+	e->entry.free = menu_command_free;
+	e->entry.display = strdup(display);
+
+	if (!e->entry.display || !e->command) {
+		ret = -ENOMEM;
+		goto err_free;
+	}
+
+	ret = menu_add_entry(m, &e->entry);
+	if (ret)
+		goto err_free;
+
+	return &e->entry;
+err_free:
+	menu_command_free(&e->entry);
+	return ERR_PTR(ret);
+}
+
diff --git a/include/menu.h b/include/menu.h
index 4f85ed6..22bfc23 100644
--- a/include/menu.h
+++ b/include/menu.h
@@ -32,10 +32,10 @@
 	int num;
 	char *display;
 	void (*action)(struct menu *m, struct menu_entry *me);
+	void (*free)(struct menu_entry *me);
 	int non_re_ent;
 
 	struct list_head list;
-	void *priv;
 };
 
 struct menu {
@@ -65,6 +65,8 @@
 	}
 	return m;
 }
+struct menu_entry *menu_add_submenu(struct menu *parent, char *submenu, char *display);
+struct menu_entry *menu_add_command_entry(struct menu *m, char *display, char *command);
 void menu_free(struct menu *m);
 int menu_add(struct menu* m);
 void menu_remove(struct menu *m);
@@ -89,8 +91,6 @@
 /*
  * menu entry action functions
  */
-void menu_action_run(struct menu *m, struct menu_entry *me);
-void menu_action_show(struct menu *m, struct menu_entry *me);
 void menu_action_exit(struct menu *m, struct menu_entry *me);
 
 #endif /* __MENU_H__ */