메뉴 구조 관련 프로그래밍에 대해 조언좀 구합니다.

안녕하세요.
이제 갓 입사해서 임베디드 시스템쪽으로 공부하고 있는 햇병아리입니다.
첫 과제가 테스트 보드에 각 디바이스를 모니터링 하는 프로그램을 짜는 것입니다.
가령
1) 보드에 LED가 있으면 제대로 동작하는지에 관한 프로그램 작성
2) RS-232C 관련 동작 확인
3) KEY 관련
4) Display
5) USB 등등 입니다.

솔직히 위의 다섯가지만 있으면 무한 while문에 case문으로 해결하면 되지만
예로 3번 하위 메뉴로 5개의 메뉴가 있고 그 다섯개의 메뉴중의 어떤 것은 또 하위로 4개의 메뉴를 가지는 이러한 형태입니다.

결국은 처음에 제일 상위 메뉴를 등록하고 up, down 키를 누르면 다음 단계의 메뉴나 전 단계의 메뉴로 이동합니다.
그러다가 특정 메뉴에서 set 키를 누르면 메뉴를 실행하는데 이때 하위 메뉴가 있으면 하위 메뉴의 첫번째 메뉴를 우선 display 해주는 형태입니다.
여기서 원하는 메뉴를 선택하고 set하면 그 메뉴를 실행하고 상위 메뉴로 가는 형태입니다.

설명이 좀 잘 안됐는데 이해를 해주시고요.....
처음에는 리스트 구조를 이용하려고 했는데.... 임베디드다 보니 malloc 이런게 안 먹히는 군요...
좋은 방법이 없을까요?
정말 2주 고민했는데 아직 좋은 구조를 찾지 못했습니다.
혹 좋은 방법이나 소스를 아시고 계시는 분들 도움좀 주시면 정말 고맙겠습니다.