// SPDX-License-Identifier: GPL-2.0

/*

* Copyright © 2016 Intel Corporation

*

* Authors:

* Scott Bauer <scott.bauer@intel.com>

* Rafael Antognolli <rafael.antognolli@intel.com>

*/

#define pr_fmt(fmt) KBUILD_MODNAME ":OPAL: " fmt

#include <linux/delay.h>

#include <linux/device.h>

#include <linux/kernel.h>

#include <linux/list.h>

#include <linux/blkdev.h>

#include <linux/slab.h>

#include <linux/uaccess.h>

#include <uapi/linux/sed-opal.h>

#include <linux/sed-opal.h>

#include <linux/sed-opal-key.h>

#include <linux/string.h>

#include <linux/kdev_t.h>

#include <linux/key.h>

#include <linux/key-type.h>

#include <keys/user-type.h>

#include "opal_proto.h"

#define IO_BUFFER_LENGTH 2048

#define MAX_TOKS 64

/* Number of bytes needed by cmd_finalize. */

#define CMD_FINALIZE_BYTES_NEEDED 7

static struct key *sed_opal_keyring;

struct opal_step {

int (*fn)(struct opal_dev *dev, void *data);

void *data;

};

typedef int (cont_fn)(struct opal_dev *dev);

enum opal_atom_width {

OPAL_WIDTH_TINY,

OPAL_WIDTH_SHORT,

OPAL_WIDTH_MEDIUM,

OPAL_WIDTH_LONG,

OPAL_WIDTH_TOKEN

};

/*

* On the parsed response, we don't store again the toks that are already

* stored in the response buffer. Instead, for each token, we just store a

* pointer to the position in the buffer where the token starts, and the size

* of the token in bytes.

*/

struct opal_resp_tok {

const u8 *pos;

size_t len;

enum opal_response_token type;

enum opal_atom_width width;

union {

u64 u;

s64 s;

} stored;

};

/*

* From the response header it's not possible to know how many tokens there are

* on the payload. So we hardcode that the maximum will be MAX_TOKS, and later

* if we start dealing with messages that have more than that, we can increase

* this number. This is done to avoid having to make two passes through the

* response, the first one counting how many tokens we have and the second one

* actually storing the positions.

*/

struct parsed_resp {

int num;

struct opal_resp_tok toks[MAX_TOKS];

};

struct opal_dev {

u32 flags;

void *data;

sec_send_recv *send_recv;

struct mutex dev_lock;

u16 comid;

u32 hsn;

u32 tsn;

u64 align; /* alignment granularity */

u64 lowest_lba;

u32 logical_block_size;

u8 align_required; /* ALIGN: 0 or 1 */

size_t pos;

u8 *cmd;

u8 *resp;

struct parsed_resp parsed;

size_t prev_d_len;

void *prev_data;

struct list_head unlk_lst;

};

static const u8 opaluid[][OPAL_UID_LENGTH] = {

/* users */

[OPAL_SMUID_UID] =

{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff },

[OPAL_THISSP_UID] =

{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01 },

[OPAL_ADMINSP_UID] =

{ 0x00, 0x00, 0x02, 0x05, 0x00, 0x00, 0x00, 0x01 },

[OPAL_LOCKINGSP_UID] =

{ 0x00, 0x00, 0x02, 0x05, 0x00, 0x00, 0x00, 0x02 },

[OPAL_ENTERPRISE_LOCKINGSP_UID] =

{ 0x00, 0x00, 0x02, 0x05, 0x00, 0x01, 0x00, 0x01 },

[OPAL_ANYBODY_UID] =

{ 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x01 },

[OPAL_SID_UID] =

{ 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x06 },

[OPAL_ADMIN1_UID] =

{ 0x00, 0x00, 0x00, 0x09, 0x00, 0x01, 0x00, 0x01 },

[OPAL_USER1_UID] =

{ 0x00, 0x00, 0x00, 0x09, 0x00, 0x03, 0x00, 0x01 },

[OPAL_USER2_UID] =

{ 0x00, 0x00, 0x00, 0x09, 0x00, 0x03, 0x00, 0x02 },

[OPAL_PSID_UID] =

{ 0x00, 0x00, 0x00, 0x09, 0x00, 0x01, 0xff, 0x01 },

[OPAL_ENTERPRISE_BANDMASTER0_UID] =

{ 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x80, 0x01 },

[OPAL_ENTERPRISE_ERASEMASTER_UID] =

{ 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x84, 0x01 },

/* tables */

[OPAL_TABLE_TABLE] =

{ 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01 },

[OPAL_LOCKINGRANGE_GLOBAL] =

{ 0x00, 0x00, 0x08, 0x02, 0x00, 0x00, 0x00, 0x01 },

[OPAL_LOCKINGRANGE_ACE_START_TO_KEY] =

{ 0x00, 0x00, 0x00, 0x08, 0x00, 0x03, 0xD0, 0x01 },

[OPAL_LOCKINGRANGE_ACE_RDLOCKED] =

{ 0x00, 0x00, 0x00, 0x08, 0x00, 0x03, 0xE0, 0x01 },

[OPAL_LOCKINGRANGE_ACE_WRLOCKED] =

{ 0x00, 0x00, 0x00, 0x08, 0x00, 0x03, 0xE8, 0x01 },

[OPAL_MBRCONTROL] =

{ 0x00, 0x00, 0x08, 0x03, 0x00, 0x00, 0x00, 0x01 },

[OPAL_MBR] =

{ 0x00, 0x00, 0x08, 0x04, 0x00, 0x00, 0x00, 0x00 },

[OPAL_AUTHORITY_TABLE] =

{ 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x00},

[OPAL_C_PIN_TABLE] =

{ 0x00, 0x00, 0x00, 0x0B, 0x00, 0x00, 0x00, 0x00},

[OPAL_LOCKING_INFO_TABLE] =

{ 0x00, 0x00, 0x08, 0x01, 0x00, 0x00, 0x00, 0x01 },

[OPAL_ENTERPRISE_LOCKING_INFO_TABLE] =

{ 0x00, 0x00, 0x08, 0x01, 0x00, 0x00, 0x00, 0x00 },

[OPAL_DATASTORE] =

{ 0x00, 0x00, 0x10, 0x01, 0x00, 0x00, 0x00, 0x00 },

[OPAL_LOCKING_TABLE] =

{ 0x00, 0x00, 0x08, 0x02, 0x00, 0x00, 0x00, 0x00 },

/* C_PIN_TABLE object ID's */

[OPAL_C_PIN_MSID] =

{ 0x00, 0x00, 0x00, 0x0B, 0x00, 0x00, 0x84, 0x02},

[OPAL_C_PIN_SID] =

{ 0x00, 0x00, 0x00, 0x0B, 0x00, 0x00, 0x00, 0x01},

[OPAL_C_PIN_ADMIN1] =

{ 0x00, 0x00, 0x00, 0x0B, 0x00, 0x01, 0x00, 0x01},

/* half UID's (only first 4 bytes used) */

[OPAL_HALF_UID_AUTHORITY_OBJ_REF] =

{ 0x00, 0x00, 0x0C, 0x05, 0xff, 0xff, 0xff, 0xff },

[OPAL_HALF_UID_BOOLEAN_ACE] =

{ 0x00, 0x00, 0x04, 0x0E, 0xff, 0xff, 0xff, 0xff },

/* special value for omitted optional parameter */

[OPAL_UID_HEXFF] =

{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff},

};

/*

* TCG Storage SSC Methods.

* Derived from: TCG_Storage_Architecture_Core_Spec_v2.01_r1.00

* Section: 6.3 Assigned UIDs

*/

static const u8 opalmethod[][OPAL_METHOD_LENGTH] = {

[OPAL_PROPERTIES] =

{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x01 },

[OPAL_STARTSESSION] =

{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x02 },

[OPAL_REVERT] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x02, 0x02 },

[OPAL_ACTIVATE] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x02, 0x03 },

[OPAL_EGET] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x06 },

[OPAL_ESET] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x07 },

[OPAL_NEXT] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x08 },

[OPAL_EAUTHENTICATE] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x0c },

[OPAL_GETACL] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x0d },

[OPAL_GENKEY] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x10 },

[OPAL_REVERTSP] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x11 },

[OPAL_GET] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x16 },

[OPAL_SET] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x17 },

[OPAL_AUTHENTICATE] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x1c },

[OPAL_RANDOM] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x06, 0x01 },

[OPAL_ERASE] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x08, 0x03 },

[OPAL_REACTIVATE] =

{ 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x08, 0x01 },

};

static int end_opal_session_error(struct opal_dev *dev);

static int opal_discovery0_step(struct opal_dev *dev);

struct opal_suspend_data {

struct opal_lock_unlock unlk;

u8 lr;

struct list_head node;

};

/*

* Derived from:

* TCG_Storage_Architecture_Core_Spec_v2.01_r1.00

* Section: 5.1.5 Method Status Codes

*/

static const char * const opal_errors[] = {

"Success",

"Not Authorized",

"Unknown Error",

"SP Busy",

"SP Failed",

"SP Disabled",

"SP Frozen",

"No Sessions Available",

"Uniqueness Conflict",

"Insufficient Space",

"Insufficient Rows",

"Invalid Function",

"Invalid Parameter",

"Invalid Reference",

"Unknown Error",

"TPER Malfunction",

"Transaction Failure",

"Response Overflow",

"Authority Locked Out",

};

static const char *opal_error_to_human(int error)

{

if (error == 0x3f)

return "Failed";

if (error >= ARRAY_SIZE(opal_errors) || error < 0)

return "Unknown Error";

return opal_errors[error];

}

static void print_buffer(const u8 *ptr, u32 length)

{

#ifdef DEBUG

print_hex_dump_bytes("OPAL: ", DUMP_PREFIX_OFFSET, ptr, length);

pr_debug("\n");

#endif

}

/*

* Allocate/update a SED Opal key and add it to the SED Opal keyring.

*/

static int update_sed_opal_key(const char *desc, u_char *key_data, int keylen)

{

key_ref_t kr;

if (!sed_opal_keyring)

return -ENOKEY;

kr = key_create_or_update(make_key_ref(sed_opal_keyring, true), "user",

desc, (const void *)key_data, keylen,

KEY_USR_VIEW | KEY_USR_SEARCH | KEY_USR_WRITE,

KEY_ALLOC_NOT_IN_QUOTA | KEY_ALLOC_BUILT_IN |

KEY_ALLOC_BYPASS_RESTRICTION);

if (IS_ERR(kr)) {

pr_err("Error adding SED key (%ld)\n", PTR_ERR(kr));

return PTR_ERR(kr);

}

return 0;

}

/*

* Read a SED Opal key from the SED Opal keyring.

*/

static int read_sed_opal_key(const char *key_name, u_char *buffer, int buflen)

{

int ret;

key_ref_t kref;

struct key *key;

if (!sed_opal_keyring)

return -ENOKEY;

kref = keyring_search(make_key_ref(sed_opal_keyring, true),

&key_type_user, key_name, true);

if (IS_ERR(kref))

return PTR_ERR(kref);

key = key_ref_to_ptr(kref);

down_read(&key->sem);

ret = key_validate(key);

if (ret == 0) {

if (buflen > key->datalen)

buflen = key->datalen;

ret = key->type->read(key, (char *)buffer, buflen);

}

up_read(&key->sem);

key_ref_put(kref);

return ret;

}

static int opal_get_key(struct opal_dev *dev, struct opal_key *key)

{

int ret = 0;

switch (key->key_type) {

case OPAL_INCLUDED:

/* the key is ready to use */

break;

case OPAL_KEYRING:

/* the key is in the keyring */

ret = read_sed_opal_key(OPAL_AUTH_KEY, key->key, OPAL_KEY_MAX);

if (ret > 0) {

if (ret > U8_MAX) {

ret = -ENOSPC;

goto error;

}

key->key_len = ret;

key->key_type = OPAL_INCLUDED;

}

break;

default:

ret = -EINVAL;

break;

}

if (ret < 0)

goto error;

/* must have a PEK by now or it's an error */

if (key->key_type != OPAL_INCLUDED || key->key_len == 0) {

ret = -EINVAL;

goto error;

}

return 0;

error:

pr_debug("Error getting password: %d\n", ret);

return ret;

}

static bool check_tper(const void *data)

{

const struct d0_tper_features *tper = data;

u8 flags = tper->supported_features;

if (!(flags & TPER_SYNC_SUPPORTED)) {

pr_debug("TPer sync not supported. flags = %d\n",

tper->supported_features);

return false;

}

return true;

}

static bool check_lcksuppt(const void *data)

{

const struct d0_locking_features *lfeat = data;

u8 sup_feat = lfeat->supported_features;

return !!(sup_feat & LOCKING_SUPPORTED_MASK);

}

static bool check_lckenabled(const void *data)

{

const struct d0_locking_features *lfeat = data;

u8 sup_feat = lfeat->supported_features;

return !!(sup_feat & LOCKING_ENABLED_MASK);

}

static bool check_locked(const void *data)

{

const struct d0_locking_features *lfeat = data;

u8 sup_feat = lfeat->supported_features;

return !!(sup_feat & LOCKED_MASK);

}

static bool check_mbrenabled(const void *data)

{

const struct d0_locking_features *lfeat = data;

u8 sup_feat = lfeat->supported_features;

return !!(sup_feat & MBR_ENABLED_MASK);

}

static bool check_mbrdone(const void *data)

{

const struct d0_locking_features *lfeat = data;

u8 sup_feat = lfeat->supported_features;

return !!(sup_feat & MBR_DONE_MASK);

}

static bool check_sum(const void *data)

{

const struct d0_single_user_mode *sum = data;

u32 nlo = be32_to_cpu(sum->num_locking_objects);

if (nlo == 0) {

pr_debug("Need at least one locking object.\n");

return false;

}

pr_debug("Number of locking objects: %d\n", nlo);

return true;

}

static u16 get_comid_v100(const void *data)

{

const struct d0_opal_v100 *v100 = data;

return be16_to_cpu(v100->baseComID);

}

static u16 get_comid_v200(const void *data)

{

const struct d0_opal_v200 *v200 = data;

return be16_to_cpu(v200->baseComID);

}

static int opal_send_cmd(struct opal_dev *dev)

{

return dev->send_recv(dev->data, dev->comid, TCG_SECP_01,

dev->cmd, IO_BUFFER_LENGTH,

true);

}

static int opal_recv_cmd(struct opal_dev *dev)

{

return dev->send_recv(dev->data, dev->comid, TCG_SECP_01,

dev->resp, IO_BUFFER_LENGTH,

false);

}

static int opal_recv_check(struct opal_dev *dev)

{

size_t buflen = IO_BUFFER_LENGTH;

void *buffer = dev->resp;

struct opal_header *hdr = buffer;

int ret;

do {

pr_debug("Sent OPAL command: outstanding=%d, minTransfer=%d\n",

hdr->cp.outstandingData,

hdr->cp.minTransfer);

if (hdr->cp.outstandingData == 0 ||

hdr->cp.minTransfer != 0)

return 0;

memset(buffer, 0, buflen);

ret = opal_recv_cmd(dev);

} while (!ret);

return ret;

}

static int opal_send_recv(struct opal_dev *dev, cont_fn *cont)

{

int ret;

ret = opal_send_cmd(dev);

if (ret)

return ret;

ret = opal_recv_cmd(dev);

if (ret)

return ret;

ret = opal_recv_check(dev);

if (ret)

return ret;

return cont(dev);

}

static void check_geometry(struct opal_dev *dev, const void *data)

{

const struct d0_geometry_features *geo = data;

dev->align = be64_to_cpu(geo->alignment_granularity);

dev->lowest_lba = be64_to_cpu(geo->lowest_aligned_lba);

dev->logical_block_size = be32_to_cpu(geo->logical_block_size);

dev->align_required = geo->reserved01 & 1;

}

static int execute_step(struct opal_dev *dev,

const struct opal_step *step, size_t stepIndex)

{

int error = step->fn(dev, step->data);

if (error) {

pr_debug("Step %zu (%pS) failed with error %d: %s\n",

stepIndex, step->fn, error,

opal_error_to_human(error));

}

return error;

}

static int execute_steps(struct opal_dev *dev,

const struct opal_step *steps, size_t n_steps)

{

size_t state = 0;

int error;

/* first do a discovery0 */

error = opal_discovery0_step(dev);

if (error)

return error;

for (state = 0; state < n_steps; state++) {

error = execute_step(dev, &steps[state], state);

if (error)

goto out_error;

}

return 0;

out_error:

/*

* For each OPAL command the first step in steps starts some sort of

* session. If an error occurred in the initial discovery0 or if an

* error occurred in the first step (and thus stopping the loop with

* state == 0) then there was an error before or during the attempt to

* start a session. Therefore we shouldn't attempt to terminate a

* session, as one has not yet been created.

*/

if (state > 0)

end_opal_session_error(dev);

return error;

}

static int opal_discovery0_end(struct opal_dev *dev, void *data)

{

struct opal_discovery *discv_out = data; /* may be NULL */

u8 __user *buf_out;

u64 len_out;

bool found_com_id = false, supported = true, single_user = false;

const struct d0_header *hdr = (struct d0_header *)dev->resp;

const u8 *epos = dev->resp, *cpos = dev->resp;

u16 comid = 0;

u32 hlen = be32_to_cpu(hdr->length);

print_buffer(dev->resp, hlen);

dev->flags &= OPAL_FL_SUPPORTED;

if (hlen > IO_BUFFER_LENGTH - sizeof(*hdr)) {

pr_debug("Discovery length overflows buffer (%zu+%u)/%u\n",

sizeof(*hdr), hlen, IO_BUFFER_LENGTH);

return -EFAULT;

}

if (discv_out) {

buf_out = (u8 __user *)(uintptr_t)discv_out->data;

len_out = min_t(u64, discv_out->size, hlen);

if (buf_out && copy_to_user(buf_out, dev->resp, len_out))

return -EFAULT;

discv_out->size = hlen; /* actual size of data */

}

epos += hlen; /* end of buffer */

cpos += sizeof(*hdr); /* current position on buffer */

while (cpos < epos && supported) {

const struct d0_features *body =

(const struct d0_features *)cpos;

switch (be16_to_cpu(body->code)) {

case FC_TPER:

supported = check_tper(body->features);

break;

case FC_SINGLEUSER:

single_user = check_sum(body->features);

if (single_user)

dev->flags |= OPAL_FL_SUM_SUPPORTED;

break;

case FC_GEOMETRY:

check_geometry(dev, body);

break;

case FC_LOCKING:

if (check_lcksuppt(body->features))

dev->flags |= OPAL_FL_LOCKING_SUPPORTED;

if (check_lckenabled(body->features))

dev->flags |= OPAL_FL_LOCKING_ENABLED;

if (check_locked(body->features))

dev->flags |= OPAL_FL_LOCKED;

if (check_mbrenabled(body->features))

dev->flags |= OPAL_FL_MBR_ENABLED;

if (check_mbrdone(body->features))

dev->flags |= OPAL_FL_MBR_DONE;

break;

case FC_ENTERPRISE:

case FC_DATASTORE:

/* some ignored properties */

pr_debug("Found OPAL feature description: %d\n",

be16_to_cpu(body->code));

break;

case FC_OPALV100:

comid = get_comid_v100(body->features);

found_com_id = true;

break;

case FC_OPALV200:

comid = get_comid_v200(body->features);

found_com_id = true;

break;

case 0xbfff ... 0xffff:

/* vendor specific, just ignore */

break;

default:

pr_debug("OPAL Unknown feature: %d\n",

be16_to_cpu(body->code));

}

cpos += body->length + 4;

}

if (!supported) {

pr_debug("This device is not Opal enabled. Not Supported!\n");

return -EOPNOTSUPP;

}

if (!single_user)

pr_debug("Device doesn't support single user mode\n");

if (!found_com_id) {

pr_debug("Could not find OPAL comid for device. Returning early\n");

return -EOPNOTSUPP;

}

dev->comid = comid;

return 0;

}

static int opal_discovery0(struct opal_dev *dev, void *data)

{

int ret;

memset(dev->resp, 0, IO_BUFFER_LENGTH);

dev->comid = OPAL_DISCOVERY_COMID;

ret = opal_recv_cmd(dev);

if (ret)

return ret;

return opal_discovery0_end(dev, data);

}

static int opal_discovery0_step(struct opal_dev *dev)

{

const struct opal_step discovery0_step = {

opal_discovery0, NULL

};

return execute_step(dev, &discovery0_step, 0);

}

static size_t remaining_size(struct opal_dev *cmd)

{

return IO_BUFFER_LENGTH - cmd->pos;

}

static bool can_add(int *err, struct opal_dev *cmd, size_t len)

{

if (*err)

return false;

if (remaining_size(cmd) < len) {

pr_debug("Error adding %zu bytes: end of buffer.\n", len);

*err = -ERANGE;

return false;

}

return true;

}

static void add_token_u8(int *err, struct opal_dev *cmd, u8 tok)

{

if (!can_add(err, cmd, 1))

return;

cmd->cmd[cmd->pos++] = tok;

}

static void add_short_atom_header(struct opal_dev *cmd, bool bytestring,

bool has_sign, int len)

{

u8 atom;

int err = 0;

atom = SHORT_ATOM_ID;

atom |= bytestring ? SHORT_ATOM_BYTESTRING : 0;

atom |= has_sign ? SHORT_ATOM_SIGNED : 0;

atom |= len & SHORT_ATOM_LEN_MASK;

add_token_u8(&err, cmd, atom);

}

static void add_medium_atom_header(struct opal_dev *cmd, bool bytestring,

bool has_sign, int len)

{

u8 header0;

header0 = MEDIUM_ATOM_ID;

header0 |= bytestring ? MEDIUM_ATOM_BYTESTRING : 0;

header0 |= has_sign ? MEDIUM_ATOM_SIGNED : 0;

header0 |= (len >> 8) & MEDIUM_ATOM_LEN_MASK;

cmd->cmd[cmd->pos++] = header0;

cmd->cmd[cmd->pos++] = len;

}

static void add_token_u64(int *err, struct opal_dev *cmd, u64 number)

{

size_t len;

int msb;

if (!(number & ~TINY_ATOM_DATA_MASK)) {

add_token_u8(err, cmd, number);

return;

}

msb = fls64(number);

len = DIV_ROUND_UP(msb, 8);

if (!can_add(err, cmd, len + 1)) {

pr_debug("Error adding u64: end of buffer.\n");

return;

}

add_short_atom_header(cmd, false, false, len);

while (len--)

add_token_u8(err, cmd, number >> (len * 8));

}

static u8 *add_bytestring_header(int *err, struct opal_dev *cmd, size_t len)

{

size_t header_len = 1;

bool is_short_atom = true;

if (len & ~SHORT_ATOM_LEN_MASK) {

header_len = 2;

is_short_atom = false;

}

if (!can_add(err, cmd, header_len + len)) {

pr_debug("Error adding bytestring: end of buffer.\n");

return NULL;

}

if (is_short_atom)

add_short_atom_header(cmd, true, false, len);

else

add_medium_atom_header(cmd, true, false, len);

return &cmd->cmd[cmd->pos];

}

static void add_token_bytestring(int *err, struct opal_dev *cmd,

const u8 *bytestring, size_t len)

{

u8 *start;

start = add_bytestring_header(err, cmd, len);

if (!start)

return;

memcpy(start, bytestring, len);

cmd->pos += len;

}

static int build_locking_range(u8 *buffer, size_t length, u8 lr)

{

if (length > OPAL_UID_LENGTH) {

pr_debug("Can't build locking range. Length OOB\n");

return -ERANGE;

}

memcpy(buffer, opaluid[OPAL_LOCKINGRANGE_GLOBAL], OPAL_UID_LENGTH);

if (lr == 0)

return 0;

buffer[5] = LOCKING_RANGE_NON_GLOBAL;

buffer[7] = lr;

return 0;

}

static int build_locking_user(u8 *buffer, size_t length, u8 lr)

{

if (length > OPAL_UID_LENGTH) {

pr_debug("Can't build locking range user. Length OOB\n");

return -ERANGE;

}

memcpy(buffer, opaluid[OPAL_USER1_UID], OPAL_UID_LENGTH);

buffer[7] = lr + 1;

return 0;

}

static void set_comid(struct opal_dev *cmd, u16 comid)

{

struct opal_header *hdr = (struct opal_header *)cmd->cmd;

hdr->cp.extendedComID[0] = comid >> 8;

hdr->cp.extendedComID[1] = comid;

hdr->cp.extendedComID[2] = 0;

hdr->cp.extendedComID[3] = 0;

}

static int cmd_finalize(struct opal_dev *cmd, u32 hsn, u32 tsn)

{

struct opal_header *hdr;

int err = 0;

/*

* Close the parameter list opened from cmd_start.

* The number of bytes added must be equal to

* CMD_FINALIZE_BYTES_NEEDED.

*/

add_token_u8(&err, cmd, OPAL_ENDLIST);

add_token_u8(&err, cmd, OPAL_ENDOFDATA);

add_token_u8(&err, cmd, OPAL_STARTLIST);

add_token_u8(&err, cmd, 0);

add_token_u8(&err, cmd, 0);

add_token_u8(&err, cmd, 0);

add_token_u8(&err, cmd, OPAL_ENDLIST);

if (err) {

pr_debug("Error finalizing command.\n");

return -EFAULT;

}

hdr = (struct opal_header *) cmd->cmd;

hdr->pkt.tsn = cpu_to_be32(tsn);

hdr->pkt.hsn = cpu_to_be32(hsn);

hdr->subpkt.length = cpu_to_be32(cmd->pos - sizeof(*hdr));

while (cmd->pos % 4) {

if (cmd->pos >= IO_BUFFER_LENGTH) {

pr_debug("Error: Buffer overrun\n");

return -ERANGE;

}

cmd->cmd[cmd->pos++] = 0;

}

hdr->pkt.length = cpu_to_be32(cmd->pos - sizeof(hdr->cp) -

sizeof(hdr->pkt));

hdr->cp.length = cpu_to_be32(cmd->pos - sizeof(hdr->cp));

return 0;

}

static const struct opal_resp_tok *response_get_token(

const struct parsed_resp *resp,

int n)

{

const struct opal_resp_tok *tok;

if (!resp) {

pr_debug("Response is NULL\n");

return ERR_PTR(-EINVAL);

}

if (n >= resp->num) {

pr_debug("Token number doesn't exist: %d, resp: %d\n",

n, resp->num);

return ERR_PTR(-EINVAL);

}

tok = &resp->toks[n];

if (tok->len == 0) {

pr_debug("Token length must be non-zero\n");

return ERR_PTR(-EINVAL);

}

return tok;

}

static ssize_t response_parse_tiny(struct opal_resp_tok *tok,

const u8 *pos)

{

tok->pos = pos;

tok->len = 1;

tok->width = OPAL_WIDTH_TINY;

if (pos[0] & TINY_ATOM_SIGNED) {

tok->type = OPAL_DTA_TOKENID_SINT;

} else {

tok->type = OPAL_DTA_TOKENID_UINT;

tok->stored.u = pos[0] & 0x3f;

}

return tok->len;

}

static ssize_t response_parse_short(struct opal_resp_tok *tok,

const u8 *pos)

{

tok->pos = pos;

tok->len = (pos[0] & SHORT_ATOM_LEN_MASK) + 1;

tok->width = OPAL_WIDTH_SHORT;

if (pos[0] & SHORT_ATOM_BYTESTRING) {

tok->type = OPAL_DTA_TOKENID_BYTESTRING;

} else if (pos[0] & SHORT_ATOM_SIGNED) {

tok->type = OPAL_DTA_TOKENID_SINT;

} else {

u64 u_integer = 0;

ssize_t i, b = 0;

tok->type = OPAL_DTA_TOKENID_UINT;

if (tok->len > 9) {

pr_debug("uint64 with more than 8 bytes\n");

return -EINVAL;

}

for (i = tok->len - 1; i > 0; i--) {

u_integer |= ((u64)pos[i] << (8 * b));

b++;

}

tok->stored.u = u_integer;

}

return tok->len;

}

static ssize_t response_parse_medium(struct opal_resp_tok *tok,

const u8 *pos)

{

tok->pos = pos;

tok->len = (((pos[0] & MEDIUM_ATOM_LEN_MASK) << 8) | pos[1]) + 2;

tok->width = OPAL_WIDTH_MEDIUM;

if (pos[0] & MEDIUM_ATOM_BYTESTRING)

tok->type = OPAL_DTA_TOKENID_BYTESTRING;

else if (pos[0] & MEDIUM_ATOM_SIGNED)

tok->type = OPAL_DTA_TOKENID_SINT;

else

tok->type = OPAL_DTA_TOKENID_UINT;

return tok->len;

}

static ssize_t response_parse_long(struct opal_resp_tok *tok,

const u8 *pos)

{

tok->pos = pos;

tok->len = ((pos[1] << 16) | (pos[2] << 8) | pos[3]) + 4;

tok->width = OPAL_WIDTH_LONG;

if (pos[0] & LONG_ATOM_BYTESTRING)

tok->type = OPAL_DTA_TOKENID_BYTESTRING;

else if (pos[0] & LONG_ATOM_SIGNED)

tok->type = OPAL_DTA_TOKENID_SINT;

else

tok->type = OPAL_DTA_TOKENID_UINT;

return tok->len;

}