Doing a function call in C/C++ is not expensive. I wish people would stop saying this, it's just not true, it's one of the most annoying myths in programming. A C function call is on the order of a tiny number of nanoseconds. Here's a benchmark to compare [1]. It's an infinitesimal amount, and it is absolutely swamped by something like a cache miss. If you eliminate one cache miss but add 50 function calls, that's probably a net benefit.
The only sense in which function calls in C are "expensive" is during compilation: if a compiler can inline a function call, it can potentially do tons of new optimizations that it couldn't do before, and that can yield a huge improvment in performance. But the function call ITSELF is almost never the problem.