This is used several times in this file. Put it in a function to avoid code duplication. Also add a test for this function. There are no cros_ec tests at present, so it is time to update the code. Signed-off-by: Simon Glass <sjg@chromium.org>