Add diagonal subroutine