求教！！！矩阵键盘及LCD显示

我有一段程序实现了部分功能，还需要加6位密码检测功能，初始密码为012345，当密码检测正确时则LED绿灯亮，共3次检测。
请教如何修改程序。
以下程序能实验LCD温度显示及对应按键输入
#include<reg51.h>
#define uchar unsigned char
#define uint unsigned int
sbit DQ=P2^6;//ds18b20与单片机连接口
sbit RS=P2^5;
sbit RW=P2^4;
sbit EN=P2^3;
sbit LED1=P2^0;
sbit LED2=P2^1;

unsigned char code str1[]={"temp: "};
uchar data disdata[5];
uint tvalue;//温度值
uchar tflag;//温度正负标志
uchar key_led;

uchar code table1[]={0x10,0x06,0x09,0x08,0x08,0x09,0x06,0x00};//字符℃
unsigned char code disp_code[]={"0123456789abcdef"};
unsigned char code key_code[]={
0xe7,0xeb,0xed,0xee,0xd7,0xdb,0xdd,0xde,
0xb7,0xbb,0xbd,0xbe,0x77,0x7b,0x7d,0x7e };//矩阵按键

/*******************************************************************/
/* 延时子程序 */
/*******************************************************************/
void delay1ms(unsigned int ms)//延时1毫秒
{
unsigned int i,j;
for(i=0;i<ms;i++)
for(j=0;j<100;j++);
}

void wr_com(unsigned char com)//写指令
{
delay1ms(1);
RS=0;//输入指令
RW=0;//向LCD写入指令或数据
EN=0;//使能端
P0=com;
delay1ms(1);
EN=1;//读取信息
delay1ms(1);
EN=0;//下降沿执行指令
}
void wr_dat(unsigned char dat)//写数据
{
delay1ms(1);
RS=1;//输入数据
RW=0;
EN=0;
P0=dat;
delay1ms(1);
EN=1;
delay1ms(1);
EN=0;//执行指令
}
void addr_x_y(unsigned char x,bit y) //写坐标，定位置

{
unsigned char temp=0x80;
if(y)
{
temp|=0x40;// 第二行显示
}
temp|=x;
wr_com(temp);
}
void display_char(unsigned char x,bit y,unsigned char p)
//在指定位置显示一个字符。
{
addr_x_y(x,y);
wr_dat(p);
}


void lcd_init(void) //初始化设置
{
delay1ms(15);
wr_com(0x38);
delay1ms(5);
wr_com(0x08);
delay1ms(5);
wr_com(0x01);
delay1ms(5);
wr_com(0x06);
delay1ms(5);
wr_com(0x0c);
delay1ms(5);
}

/**********************************************************
键盘扫描子函数
**********************************************************/
uchar keyscan()
{
uchar scan1,scan2,keycode,j;
P1=0xf0;
scan1=P1;
if((scan1&0xf0)!=0xf0) //判键是否按下
{
delay1ms(30); //延时30ms 消抖
scan1=P1;
if((scan1&0xf0)!=0xf0) //二次判键是否按下
{
P1=0x0f;
scan2=P1;
keycode=scan1|scan2; //组合成键编码
for(j=0;j<=15;j++)
{
if(keycode== key_code[j]) //查表得键值
{
delay1ms(30);
key_led=j;
}
}
}
}

else
key_led=16;

return (key_led);
}
/**********************************************************
判键是否按下子函数

/******************************新增**********************/
void displaycc()
{
static uchar x = 0;
if(key_led!=16)
{ display_char(x,1,disp_code[key_led]);
x = x+1;
x = x%16;
}
}
/**********************************************************/
void keydown_andplay()
{
keyscan();
displaycc();
while(keyscan()!=16);

}


void display(unsigned char *p)//显示
{
while(*p!='\0')
{
wr_dat(*p);
p++;
delay1ms(1);
}
}

void init_play()//初始化显示
{
lcd_init();
wr_com(0x80);//第一行
display(str1);
}


/*************ds1820程序*************/
void delay_18B20(unsigned int i)//延时1微秒
{
while(i--);
}
void ds1820rst()/*ds1820复位*/
{
unsigned char x=0;
DQ = 1;//DQ复位
delay_18B20(4); //延时
DQ = 0; //DQ拉低
delay_18B20(100); //精确延时大于480us
DQ = 1;//拉高
delay_18B20(40);
}

uchar ds1820rd()/*读数据*/
{
unsigned char i=0;
unsigned char dat = 0;
for (i=8;i>0;i--)
{
DQ = 0; //给脉冲信号
dat>>=1;
DQ = 1; //给脉冲信号
if(DQ)
dat|=0x80;
delay_18B20(10);
}
return(dat);
}


void ds1820wr(uchar wdata)/*写数据*/
{
unsigned char i=0;
for (i=8; i>0; i--)
{
DQ = 0;
DQ = wdata&0x01;
delay_18B20(10);
DQ = 1;
wdata>>=1;
}
}


read_temp()/*读取温度值并转换*/
{
uchar a,b;
ds1820rst();
ds1820wr(0xcc);//*跳过读序列号*/
ds1820wr(0x44);//*启动温度转换*/
ds1820rst();
ds1820wr(0xcc);//*跳过读序列号*/
ds1820wr(0xbe);//*读取温度*/
a=ds1820rd();
b=ds1820rd();
tvalue=b;
tvalue<<=8;
tvalue=tvalue|a;
if(tvalue<0x0fff)
tflag=0;
else
{
tvalue=~tvalue+1;
tflag=1;
}
tvalue=tvalue*(0.625);//温度值扩大10倍，精确到1位小数
return(tvalue);
}

/**********************************************/
void ds1820disp()//温度值显示
{
uchar flagdat;
disdata[0]=tvalue/1000+0x30;//百位数
disdata[1]=tvalue%1000/100+0x30;//十位数
disdata[2]=tvalue%100/10+0x30;//个位数
disdata[3]=tvalue%10+0x30;//小数位
if(tflag==0)
flagdat=0x20;//正温度不显示符号
else
flagdat=0x2d;//负温度显示负号:-
if(disdata[0]==0x30)
{
disdata[0]=0x20;//如果百位为0，不显示
if(disdata[1]==0x30)
{
disdata[1]=0x20;//如果百位为0，十位为0也不显示
}
}
wr_com(0x85);
wr_dat(flagdat);//显示符号位
wr_com(0x86);
wr_dat(disdata[0]);//显示百位
wr_com(0x87);
wr_dat(disdata[1]);//显示十位
wr_com(0x88);
wr_dat(disdata[2]);//显示个位
wr_com(0x89);
wr_dat(0x2e);//显示小数点
wr_com(0x8a);
wr_dat(disdata[3]);//显示小数位
}

/******************************************************/
void wendu()
{ int m;
read_temp();//读取温度
wr_com(0x40); //设定CGRAM地址
for(m=0;m<8;m++) //
{
wr_dat(table1[m]);
}
wr_com(0x8b); //设定上排的显示位置 ,显示℃
wr_dat(0);
ds1820disp();//显示
}

/*************主程序**************/
void main()
{
init_play();//初始化显示
while(1)
{
wendu();
keydown_andplay();
}
}