Add lcd_test tool, a big mess for lcd code