Does this need to happen every time, because you already cll it at the init.
Btw take a look into static inline functions, that will save some stack space ;)