فرق الطور بين موجتين بالاسمبلي ارجو مساعدتي؟

السلام عليكم اخواني محتاج مساعدتكم في كتابة برنامج بلغة الاسمبلي والبرنامج هو يقوم بادخال موجتين على المايكروكونترولر 8051 وهذا البرنامج يقوم بقياس فرق الطور بين الموجتين واخراج القياس على 7 seg في الحقيقة انا عندي البرنامج لكن يوجد خطأ بالكود فأرجو مساعدتي لأكمال هذا البرنامج

 org0
jmp main
org 3h
jmp sub1
org 13h
jmp sub2
org 30h
main:mov ie,#b4h
setb it0
setb it1
jmp $
sub1:mov tmod,#2h
 mov th0,#00h
mov tl0,#00h
setb tro
RETi
sub2:clr TRo
mov p2,TL0
RETi