diff options
Diffstat (limited to 'block/sed-opal.c')
-rw-r--r-- | block/sed-opal.c | 37 |
1 files changed, 27 insertions, 10 deletions
diff --git a/block/sed-opal.c b/block/sed-opal.c index e4929eec547f..945f4b8610e0 100644 --- a/block/sed-opal.c +++ b/block/sed-opal.c @@ -554,15 +554,14 @@ static void add_token_u64(int *err, struct opal_dev *cmd, u64 number) size_t len; int msb; - u8 n; if (!(number & ~TINY_ATOM_DATA_MASK)) { add_token_u8(err, cmd, number); return; } - msb = fls(number); - len = DIV_ROUND_UP(msb, 4); + msb = fls64(number); + len = DIV_ROUND_UP(msb, 8); if (cmd->pos >= IO_BUFFER_LENGTH - len - 1) { pr_debug("Error adding u64: end of buffer.\n"); @@ -570,10 +569,8 @@ static void add_token_u64(int *err, struct opal_dev *cmd, u64 number) return; } add_short_atom_header(cmd, false, false, len); - while (len--) { - n = number >> (len * 8); - add_token_u8(err, cmd, n); - } + while (len--) + add_token_u8(err, cmd, number >> (len * 8)); } static void add_token_bytestring(int *err, struct opal_dev *cmd, @@ -871,6 +868,9 @@ static int response_parse(const u8 *buf, size_t length, static size_t response_get_string(const struct parsed_resp *resp, int n, const char **store) { + u8 skip; + const struct opal_resp_tok *token; + *store = NULL; if (!resp) { pr_debug("Response is NULL\n"); @@ -883,13 +883,30 @@ static size_t response_get_string(const struct parsed_resp *resp, int n, return 0; } - if (resp->toks[n].type != OPAL_DTA_TOKENID_BYTESTRING) { + token = &resp->toks[n]; + if (token->type != OPAL_DTA_TOKENID_BYTESTRING) { pr_debug("Token is not a byte string!\n"); return 0; } - *store = resp->toks[n].pos + 1; - return resp->toks[n].len - 1; + switch (token->width) { + case OPAL_WIDTH_TINY: + case OPAL_WIDTH_SHORT: + skip = 1; + break; + case OPAL_WIDTH_MEDIUM: + skip = 2; + break; + case OPAL_WIDTH_LONG: + skip = 4; + break; + default: + pr_debug("Token has invalid width!\n"); + return 0; + } + + *store = token->pos + skip; + return token->len - skip; } static u64 response_get_u64(const struct parsed_resp *resp, int n) |