This is a direct copy of the existing generic CPU code. Signed-off-by: Samuel Holland <samuel@sholland.org>