Reworked based on Atom Kernel Source

Change-Id: Ibcefc6dc6ee4924c23f8d2c103d114c95aa4d78e
Started optimising to reduce code size

Change-Id: I35bba965064f455e24f6648611b3b0ecbd826720
More optimisation

Change-Id: Ic780437c0f882014b220609640106ec6f401f60f