За PIC16 мисля, че ще е най добре да си го напишеш с побитови манипулации, не знам твоя компилатор как точно работи побитово затова нещо като псевдокод ще напиша колкото да фанеш идеяtа:
Това коре долу ще е 20-на инструкции за ремапване на 8 бита. C компилаторите за PIC не се оправят добре с << и >> на 16 битови променливи и най вероятно твоята идея ще отиде към 40-50 инструкции ... трябва да оптимизираш според архитектура/компилатор + дизасемблиране и стъкмистика.