Need Help setting up 401k Formula for safe harbor benefit

I need help setting up the formula for our 401k Pretax & 401k Roth plan. See detail below.

Company match for 401k is 100% up to 3%, then 50% match for next 2%

= If Employee is 3%, company match is 3%

= If Employee is 4%, company match is 3.5%

= If Employee is 5% or more, company match is 4%

Thanks,

Deb Loiselle