In order to make it easier to add SG support,
let's split the big loop out to its own function.
Signed-off-by: Felipe Balbi <balbi@ti.com>