The Check Endstop logic must be:

if (current_block->steps_x != current_block->steps_y || (TEST(out_bits,
X_AXIS) == TEST(out_bits, Y_AXIS)))
if (TEST(out_bits, X_HEAD))

not

if (TEST(out_bits, X_HEAD) && (current_block->steps_x !=
current_block->steps_y || (TEST(out_bits, X_AXIS) == TEST(out_bits,
Y_AXIS))))

Same applies for Y axis.