Been working towards undergrad degree on and off, now finally found my passion and want to go for a PhD. What should I do?
What matters most to be accepted into and succeed in a PhD program is an advisor who values you and your work and will vouch for you.  Grades only matter if you're applying through the front door in a pile of 1000 applications, which you want to avoid.

During your remaining years in undergrad, look up and seek out the Mathematics professors at the college or university you want to attend.  Read their research, journal publications, conference presentation, research agenda, etc. and figure out how you can contribute to that and build upon it.  Then reach out to a select couple of them and have coffee with them, discuss your plans, volunteer to join one of their research projects as an undergraduate research assistant (sometimes it's even paid), and so on.  If you demonstrate aptitude, particularly when it comes to novel research and deep understanding of the subject material AND you have found the right advisor who wants to work with you, that person will help you with the application process and take you on as their graduate student.

Good luck!