bootctrl: Update MediaTek boot control HAL to 1.2 and import libmtk_bsg
* libmtk_bsg is under the 3-Clause BSD License which allow us to redistribute the code without any kind of DMCA from MediaTek.
Change-Id: Ie06236cb9c04fded696319703803a69ff67c7cb5
diff --git a/bootctrl/Android.bp b/bootctrl/Android.bp
index 18224fb..a84200e 100644
--- a/bootctrl/Android.bp
+++ b/bootctrl/Android.bp
@@ -15,8 +15,8 @@
//
cc_library_shared {
- name: "android.hardware.boot@1.1-mtkimpl",
- stem: "android.hardware.boot@1.0-impl-1.1-mtkimpl",
+ name: "android.hardware.boot@1.2-mtkimpl",
+ stem: "android.hardware.boot@1.0-impl-1.2-mtkimpl",
defaults: [
"hidl_defaults",
"libboot_control_defaults",
@@ -25,7 +25,8 @@
vendor: true,
recovery_available: true,
srcs: ["BootControl.cpp", "boot_region_control.cpp"],
-
+ cppflags: ["-Wno-unused-variable"],
+ cflags: ["-Wno-unused-variable"],
header_libs: ["device_kernel_headers"],
shared_libs: [
@@ -35,6 +36,8 @@
"libutils",
"android.hardware.boot@1.0",
"android.hardware.boot@1.1",
+ "android.hardware.boot@1.2",
+ "libmtk_bsg"
],
static_libs: [
"libboot_control",
diff --git a/bootctrl/BootControl.cpp b/bootctrl/BootControl.cpp
index 2bf19a7..34dc52a 100644
--- a/bootctrl/BootControl.cpp
+++ b/bootctrl/BootControl.cpp
@@ -13,7 +13,7 @@
* See the License for the specific language governing permissions and
* limitations under the License.
*/
-#define LOG_TAG "android.hardware.boot@1.1-mtkimpl"
+#define LOG_TAG "android.hardware.boot@1.2-mtkimpl"
#include <memory>
@@ -28,7 +28,7 @@
namespace android {
namespace hardware {
namespace boot {
-namespace V1_1 {
+namespace V1_2 {
namespace implementation {
using ::android::hardware::boot::V1_0::CommandResult;
@@ -142,6 +142,10 @@
return impl_.GetSnapshotMergeStatus();
}
+Return<uint32_t> BootControl::getActiveBootSlot() {
+ return impl_.GetActiveBootSlot();
+}
+
IBootControl* HIDL_FETCH_IBootControl(const char* /* hal */) {
auto module = std::make_unique<BootControl>();
if (!module->Init()) {
@@ -155,4 +159,4 @@
} // namespace V1_1
} // namespace boot
} // namespace hardware
-} // namespace android
+} // namespace android
\ No newline at end of file
diff --git a/bootctrl/BootControl.h b/bootctrl/BootControl.h
index d63b244..a215fcb 100644
--- a/bootctrl/BootControl.h
+++ b/bootctrl/BootControl.h
@@ -16,7 +16,7 @@
#pragma once
-#include <android/hardware/boot/1.1/IBootControl.h>
+#include <android/hardware/boot/1.2/IBootControl.h>
#include <hidl/MQDescriptor.h>
#include <hidl/Status.h>
#include <libboot_control/libboot_control.h>
@@ -25,13 +25,13 @@
namespace android {
namespace hardware {
namespace boot {
-namespace V1_1 {
+namespace V1_2 {
namespace implementation {
using ::android::hardware::Return;
using ::android::hardware::Void;
using ::android::hardware::boot::V1_0::BoolResult;
-using ::android::hardware::boot::V1_1::IBootControl;
+using ::android::hardware::boot::V1_2::IBootControl;
using ::android::hardware::boot::V1_1::MergeStatus;
class BootControl : public IBootControl {
@@ -52,6 +52,9 @@
Return<bool> setSnapshotMergeStatus(MergeStatus status) override;
Return<MergeStatus> getSnapshotMergeStatus() override;
+ // Methods from ::android::hardware::boot::V1_2::IBootControl.
+ Return<uint32_t> getActiveBootSlot() override;
+
private:
android::bootable::BootControl impl_;
android::bootable::BootControlExt implext_;
@@ -64,4 +67,4 @@
} // namespace V1_1
} // namespace boot
} // namespace hardware
-} // namespace android
+} // namespace android
\ No newline at end of file
diff --git a/bootctrl/boot_control_definition.h b/bootctrl/boot_control_definition.h
index 5d36a98..887f829 100644
--- a/bootctrl/boot_control_definition.h
+++ b/bootctrl/boot_control_definition.h
@@ -115,4 +115,4 @@
bool LoadBootloaderControl(const std::string& misc_device, bootloader_control* buffer);
uint32_t BootloaderControlLECRC(const bootloader_control* boot_ctrl);
}
-}
+}
\ No newline at end of file
diff --git a/bootctrl/boot_region_control.cpp b/bootctrl/boot_region_control.cpp
index bd528a3..e9e8834 100644
--- a/bootctrl/boot_region_control.cpp
+++ b/bootctrl/boot_region_control.cpp
@@ -11,6 +11,9 @@
#endif
#include "boot_region_control_private.h"
+extern "C"{
+ #include "mtk_ioctl.h"
+}
namespace android {
namespace bootable {
@@ -107,9 +110,10 @@
idata.buf_byte = 1;
if (ioctl(fd, UFS_IOCTL_QUERY, &idata) < 0) {
- LOG(ERROR) << "ufs_set boot_part fail";
- ret = false;
+ LOG(ERROR) << "ufs_set boot_part old fail";
+ ret = ioctrl_w_attr("/dev/ufs-bsg0", QUERY_ATTR_IDN_BOOT_LUN_EN, 0, 0, boot);
}
+
close(fd);
return ret;
}
@@ -142,4 +146,4 @@
}
#endif //#if !defined(ARCH_X86)
}
-}
+}
\ No newline at end of file
diff --git a/libmtk_bsg/Android.bp b/libmtk_bsg/Android.bp
new file mode 100644
index 0000000..8665c5b
--- /dev/null
+++ b/libmtk_bsg/Android.bp
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2021, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+cc_library_shared {
+ name: "libmtk_bsg",
+ vendor: true,
+ recovery_available: true,
+ srcs: ["*.c"],
+
+ header_libs: ["device_kernel_headers"],
+ export_include_dirs: ["include"],
+}
diff --git a/libmtk_bsg/Makefile.disable b/libmtk_bsg/Makefile.disable
new file mode 100644
index 0000000..8942054
--- /dev/null
+++ b/libmtk_bsg/Makefile.disable
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+export CC := $(CROSS_COMPILE)gcc
+AM_CFLAGS = -D_FILE_OFFSET_BITS=64 -D_FORTIFY_SOURCE=2
+CFLAGS ?= -g -O2 -static
+
+ifneq ($(CROSS_COMPILE),)
+ LDFLAGS += -static
+endif
+
+#CXXFLAGS = -DDEBUG
+
+objects = \
+ ufs.o \
+ ufs_cmds.o \
+ options.o \
+ scsi_bsg_util.o \
+ ufs_err_hist.o \
+ unipro.o \
+ ufs_ffu.o \
+ ufs_vendor.o\
+ hmac_sha2.o \
+ sha2.o \
+ ufs_rpmb.o \
+ ufs_hmr.o
+
+CHECKFLAGS = -Wall -Wundef -Wno-missing-braces
+
+DEPFLAGS = -Wp,-MMD,$(@D)/.$(@F).d,-MT,$@
+override CFLAGS := $(CHECKFLAGS) $(AM_CFLAGS) $(CFLAGS) $(INC_DIR) $(CXXFLAGS)
+progs = ufs-utils
+ifdef C
+ check = sparse $(CHECKFLAGS)
+endif
+
+.c.o:
+ifdef C
+ $(check) $<
+endif
+ $(CC) $(CPPFLAGS) $(CFLAGS) $(DEPFLAGS) -c $< -o $@
+
+ufs-utils:$(objects)
+ $(CC) $(CFLAGS) -o $@ $(objects) $(LDFLAGS) $(LIBS)
+
+help:
+ @echo "\033[31m==============Build Instructions==============\033[0m"
+ @echo "\033[92mTo build ufs_utils follow the following steps\033[0m"
+ @echo "\033[92m1 Set CROSS_COMPILE variable\033[0m"
+ @echo "\033[92m2 Build the tool using \"make\"\033[0m"
+ @echo "\033[92m3 Clean the tool using \"make clean\"\033[0m"
+
+clean:
+ @rm -f $(progs) $(objects) .*.o.d
+.PHONY: all clean
diff --git a/libmtk_bsg/README.md b/libmtk_bsg/README.md
new file mode 100644
index 0000000..58eb6e1
--- /dev/null
+++ b/libmtk_bsg/README.md
@@ -0,0 +1,87 @@
+# UFS Tool ver 1.9 #
+
+## Description: ##
+a) Read/Write device flags, attributes & descriptors by
+using the BSG infrastructure in linux kernel (applied to 5.1 rc1)
+Due to the issues in UFS BSG driver, the following patch have to be
+applied:
+ https://lore.kernel.org/patchwork/patch/1076796/
+ https://patchwork.kernel.org/patch/11011891/
+b) Get/Set UNIPRO attributes
+c) HMR - Host Manual Refresh functionality
+The following options may work with the SCSI BSG device
+using sg v4 structure (e.g. /dev/0:0:0:0)
+or via a SCSI Generic interface using sg v3 struct (e.g. /dev/block/sda)
+d) Error History
+e) FFU - Field Firmware Update
+f) Send Vendor commands based on SCSI WRITE/READ Buffer commands
+g) RPMB functionality
+
+The tool is aligned to the UFS 3.1 spec.
+
+## Build: ##
+### Set CROSS\_COMPILE variable(e.g.): ###
+export CROSS\_COMPILE=/XXX/aarch64-linux-gnu-
+
+### Build: ###
+"make"
+
+### Clean: ###
+ "make clean"
+
+## Usage ##
+Copy the tool into a directory on the device (e.g.
+/data/local/tmp).
+Run the tool without arguments or with -h/--help
+ options in order to list the supported features:
+E.g. Run:
+./ufs-utils --help
+Output:
+ ufs-utils help|--help|-h Show the help.
+
+ ufs-utils -v
+ Show the version.
+
+ ufs-utils <desc | attr | fl | err_hist | uic | ffu | vendor | rpmb | hmr> --help|-h
+ Show detailed help for a command
+
+ Run the tool's help for the ufs configuration features in order to
+ get full information related to the feature, all options and the
+ examples. E.g.: getting help for ufs flags Run: ./ufs-utils fl --help
+ Output: Flags command usage:
+
+ ufs-utils fl [-t] <flag idn> [-a|-r|-o|-e] [-p] <device_path>
+
+ -t Flags type idn
+ Available flags and its access, based on UFS ver 3.0 :
+ 0 : Reserved
+ 1 : fDeviceInit | Read | SetOnly
+ 2 : fPermanentWPEn | Read | WriteOnce
+ 3 : fPowerOnWPEn | Read | ResetOnPower
+ 4 : fBackgroundOpsEn | Read | Volatile
+ 5 : fDeviceLifeSpanModeEn | Read | Volatile
+ 6 : fPurgeEnable | WriteOnly | Volatile
+ 7 : fRefreshEnable | WriteOnly | Volatile
+ 8 : fPhyResourceRemoval | Read | Persistent
+ 9 : fBusyRTC | ReadOnly
+ 10 : Reserved
+ 11 : fPermanentlyDisableFw | Read | WriteOnce
+
+ -a read and print all readable flags for the device
+
+ -r read operation (default), for readable flag(s)
+
+ -e set flag operation
+
+ -c clear/reset flag operation
+
+ -o toggle flag operation
+
+ -p path to ufs bsg device
+
+ Example - Read the bkops operation flag
+ ufs-utils fl -t 4 -p /dev/ufs-bsg
+
+## Authors ##
+signed-off-by:Arthur Simchaev (arthur.simchaev@wdc.com)
+signed-off-by:Avri Altman (avri.altman@wdc.com)
diff --git a/libmtk_bsg/hmac_sha2.c b/libmtk_bsg/hmac_sha2.c
new file mode 100644
index 0000000..1130ebf
--- /dev/null
+++ b/libmtk_bsg/hmac_sha2.c
@@ -0,0 +1,544 @@
+/*
+ * HMAC-SHA-224/256/384/512 implementation
+ * Last update: 06/15/2005
+ * Issue date: 06/15/2005
+ *
+ * Copyright (C) 2005 Olivier Gay <olivier.gay@a3.epfl.ch>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the project nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <string.h>
+
+#include "hmac_sha2.h"
+
+/* HMAC-SHA-224 functions */
+
+void hmac_sha224_init(hmac_sha224_ctx *ctx, const unsigned char *key,
+ unsigned int key_size)
+{
+ unsigned int fill;
+ unsigned int num;
+
+ const unsigned char *key_used;
+ unsigned char key_temp[SHA224_DIGEST_SIZE];
+ int i;
+
+ if (key_size == SHA224_BLOCK_SIZE) {
+ key_used = key;
+ num = SHA224_BLOCK_SIZE;
+ } else {
+ if (key_size > SHA224_BLOCK_SIZE){
+ num = SHA224_DIGEST_SIZE;
+ sha224(key, key_size, key_temp);
+ key_used = key_temp;
+ } else { /* key_size > SHA224_BLOCK_SIZE */
+ key_used = key;
+ num = key_size;
+ }
+ fill = SHA224_BLOCK_SIZE - num;
+
+ memset(ctx->block_ipad + num, 0x36, fill);
+ memset(ctx->block_opad + num, 0x5c, fill);
+ }
+
+ for (i = 0; i < (int) num; i++) {
+ ctx->block_ipad[i] = key_used[i] ^ 0x36;
+ ctx->block_opad[i] = key_used[i] ^ 0x5c;
+ }
+
+ sha224_init(&ctx->ctx_inside);
+ sha224_update(&ctx->ctx_inside, ctx->block_ipad, SHA224_BLOCK_SIZE);
+
+ sha224_init(&ctx->ctx_outside);
+ sha224_update(&ctx->ctx_outside, ctx->block_opad,
+ SHA224_BLOCK_SIZE);
+
+ /* for hmac_reinit */
+ memcpy(&ctx->ctx_inside_reinit, &ctx->ctx_inside,
+ sizeof(sha224_ctx));
+ memcpy(&ctx->ctx_outside_reinit, &ctx->ctx_outside,
+ sizeof(sha224_ctx));
+}
+
+void hmac_sha224_reinit(hmac_sha224_ctx *ctx)
+{
+ memcpy(&ctx->ctx_inside, &ctx->ctx_inside_reinit,
+ sizeof(sha224_ctx));
+ memcpy(&ctx->ctx_outside, &ctx->ctx_outside_reinit,
+ sizeof(sha224_ctx));
+}
+
+void hmac_sha224_update(hmac_sha224_ctx *ctx, const unsigned char *message,
+ unsigned int message_len)
+{
+ sha224_update(&ctx->ctx_inside, message, message_len);
+}
+
+void hmac_sha224_final(hmac_sha224_ctx *ctx, unsigned char *mac,
+ unsigned int mac_size)
+{
+ unsigned char digest_inside[SHA224_DIGEST_SIZE];
+ unsigned char mac_temp[SHA224_DIGEST_SIZE];
+
+ sha224_final(&ctx->ctx_inside, digest_inside);
+ sha224_update(&ctx->ctx_outside, digest_inside, SHA224_DIGEST_SIZE);
+ sha224_final(&ctx->ctx_outside, mac_temp);
+ memcpy(mac, mac_temp, mac_size);
+}
+
+void hmac_sha224(const unsigned char *key, unsigned int key_size,
+ const unsigned char *message, unsigned int message_len,
+ unsigned char *mac, unsigned mac_size)
+{
+ hmac_sha224_ctx ctx;
+
+ hmac_sha224_init(&ctx, key, key_size);
+ hmac_sha224_update(&ctx, message, message_len);
+ hmac_sha224_final(&ctx, mac, mac_size);
+}
+
+/* HMAC-SHA-256 functions */
+
+void hmac_sha256_init(hmac_sha256_ctx *ctx, const unsigned char *key,
+ unsigned int key_size)
+{
+ unsigned int fill;
+ unsigned int num;
+
+ const unsigned char *key_used;
+ unsigned char key_temp[SHA256_DIGEST_SIZE];
+ int i;
+
+ if (key_size == SHA256_BLOCK_SIZE) {
+ key_used = key;
+ num = SHA256_BLOCK_SIZE;
+ } else {
+ if (key_size > SHA256_BLOCK_SIZE){
+ num = SHA256_DIGEST_SIZE;
+ sha256(key, key_size, key_temp);
+ key_used = key_temp;
+ } else { /* key_size > SHA256_BLOCK_SIZE */
+ key_used = key;
+ num = key_size;
+ }
+ fill = SHA256_BLOCK_SIZE - num;
+
+ memset(ctx->block_ipad + num, 0x36, fill);
+ memset(ctx->block_opad + num, 0x5c, fill);
+ }
+
+ for (i = 0; i < (int) num; i++) {
+ ctx->block_ipad[i] = key_used[i] ^ 0x36;
+ ctx->block_opad[i] = key_used[i] ^ 0x5c;
+ }
+
+ sha256_init(&ctx->ctx_inside);
+ sha256_update(&ctx->ctx_inside, ctx->block_ipad, SHA256_BLOCK_SIZE);
+
+ sha256_init(&ctx->ctx_outside);
+ sha256_update(&ctx->ctx_outside, ctx->block_opad,
+ SHA256_BLOCK_SIZE);
+
+ /* for hmac_reinit */
+ memcpy(&ctx->ctx_inside_reinit, &ctx->ctx_inside,
+ sizeof(sha256_ctx));
+ memcpy(&ctx->ctx_outside_reinit, &ctx->ctx_outside,
+ sizeof(sha256_ctx));
+}
+
+void hmac_sha256_reinit(hmac_sha256_ctx *ctx)
+{
+ memcpy(&ctx->ctx_inside, &ctx->ctx_inside_reinit,
+ sizeof(sha256_ctx));
+ memcpy(&ctx->ctx_outside, &ctx->ctx_outside_reinit,
+ sizeof(sha256_ctx));
+}
+
+void hmac_sha256_update(hmac_sha256_ctx *ctx, const unsigned char *message,
+ unsigned int message_len)
+{
+ sha256_update(&ctx->ctx_inside, message, message_len);
+}
+
+void hmac_sha256_final(hmac_sha256_ctx *ctx, unsigned char *mac,
+ unsigned int mac_size)
+{
+ unsigned char digest_inside[SHA256_DIGEST_SIZE];
+ unsigned char mac_temp[SHA256_DIGEST_SIZE];
+
+ sha256_final(&ctx->ctx_inside, digest_inside);
+ sha256_update(&ctx->ctx_outside, digest_inside, SHA256_DIGEST_SIZE);
+ sha256_final(&ctx->ctx_outside, mac_temp);
+ memcpy(mac, mac_temp, mac_size);
+}
+
+void hmac_sha256(const unsigned char *key, unsigned int key_size,
+ const unsigned char *message, unsigned int message_len,
+ unsigned char *mac, unsigned mac_size)
+{
+ hmac_sha256_ctx ctx;
+
+ hmac_sha256_init(&ctx, key, key_size);
+ hmac_sha256_update(&ctx, message, message_len);
+ hmac_sha256_final(&ctx, mac, mac_size);
+}
+
+/* HMAC-SHA-384 functions */
+
+void hmac_sha384_init(hmac_sha384_ctx *ctx, const unsigned char *key,
+ unsigned int key_size)
+{
+ unsigned int fill;
+ unsigned int num;
+
+ const unsigned char *key_used;
+ unsigned char key_temp[SHA384_DIGEST_SIZE];
+ int i;
+
+ if (key_size == SHA384_BLOCK_SIZE) {
+ key_used = key;
+ num = SHA384_BLOCK_SIZE;
+ } else {
+ if (key_size > SHA384_BLOCK_SIZE){
+ num = SHA384_DIGEST_SIZE;
+ sha384(key, key_size, key_temp);
+ key_used = key_temp;
+ } else { /* key_size > SHA384_BLOCK_SIZE */
+ key_used = key;
+ num = key_size;
+ }
+ fill = SHA384_BLOCK_SIZE - num;
+
+ memset(ctx->block_ipad + num, 0x36, fill);
+ memset(ctx->block_opad + num, 0x5c, fill);
+ }
+
+ for (i = 0; i < (int) num; i++) {
+ ctx->block_ipad[i] = key_used[i] ^ 0x36;
+ ctx->block_opad[i] = key_used[i] ^ 0x5c;
+ }
+
+ sha384_init(&ctx->ctx_inside);
+ sha384_update(&ctx->ctx_inside, ctx->block_ipad, SHA384_BLOCK_SIZE);
+
+ sha384_init(&ctx->ctx_outside);
+ sha384_update(&ctx->ctx_outside, ctx->block_opad,
+ SHA384_BLOCK_SIZE);
+
+ /* for hmac_reinit */
+ memcpy(&ctx->ctx_inside_reinit, &ctx->ctx_inside,
+ sizeof(sha384_ctx));
+ memcpy(&ctx->ctx_outside_reinit, &ctx->ctx_outside,
+ sizeof(sha384_ctx));
+}
+
+void hmac_sha384_reinit(hmac_sha384_ctx *ctx)
+{
+ memcpy(&ctx->ctx_inside, &ctx->ctx_inside_reinit,
+ sizeof(sha384_ctx));
+ memcpy(&ctx->ctx_outside, &ctx->ctx_outside_reinit,
+ sizeof(sha384_ctx));
+}
+
+void hmac_sha384_update(hmac_sha384_ctx *ctx, const unsigned char *message,
+ unsigned int message_len)
+{
+ sha384_update(&ctx->ctx_inside, message, message_len);
+}
+
+void hmac_sha384_final(hmac_sha384_ctx *ctx, unsigned char *mac,
+ unsigned int mac_size)
+{
+ unsigned char digest_inside[SHA384_DIGEST_SIZE];
+ unsigned char mac_temp[SHA384_DIGEST_SIZE];
+
+ sha384_final(&ctx->ctx_inside, digest_inside);
+ sha384_update(&ctx->ctx_outside, digest_inside, SHA384_DIGEST_SIZE);
+ sha384_final(&ctx->ctx_outside, mac_temp);
+ memcpy(mac, mac_temp, mac_size);
+}
+
+void hmac_sha384(const unsigned char *key, unsigned int key_size,
+ const unsigned char *message, unsigned int message_len,
+ unsigned char *mac, unsigned mac_size)
+{
+ hmac_sha384_ctx ctx;
+
+ hmac_sha384_init(&ctx, key, key_size);
+ hmac_sha384_update(&ctx, message, message_len);
+ hmac_sha384_final(&ctx, mac, mac_size);
+}
+
+/* HMAC-SHA-512 functions */
+
+void hmac_sha512_init(hmac_sha512_ctx *ctx, const unsigned char *key,
+ unsigned int key_size)
+{
+ unsigned int fill;
+ unsigned int num;
+
+ const unsigned char *key_used;
+ unsigned char key_temp[SHA512_DIGEST_SIZE];
+ int i;
+
+ if (key_size == SHA512_BLOCK_SIZE) {
+ key_used = key;
+ num = SHA512_BLOCK_SIZE;
+ } else {
+ if (key_size > SHA512_BLOCK_SIZE){
+ num = SHA512_DIGEST_SIZE;
+ sha512(key, key_size, key_temp);
+ key_used = key_temp;
+ } else { /* key_size > SHA512_BLOCK_SIZE */
+ key_used = key;
+ num = key_size;
+ }
+ fill = SHA512_BLOCK_SIZE - num;
+
+ memset(ctx->block_ipad + num, 0x36, fill);
+ memset(ctx->block_opad + num, 0x5c, fill);
+ }
+
+ for (i = 0; i < (int) num; i++) {
+ ctx->block_ipad[i] = key_used[i] ^ 0x36;
+ ctx->block_opad[i] = key_used[i] ^ 0x5c;
+ }
+
+ sha512_init(&ctx->ctx_inside);
+ sha512_update(&ctx->ctx_inside, ctx->block_ipad, SHA512_BLOCK_SIZE);
+
+ sha512_init(&ctx->ctx_outside);
+ sha512_update(&ctx->ctx_outside, ctx->block_opad,
+ SHA512_BLOCK_SIZE);
+
+ /* for hmac_reinit */
+ memcpy(&ctx->ctx_inside_reinit, &ctx->ctx_inside,
+ sizeof(sha512_ctx));
+ memcpy(&ctx->ctx_outside_reinit, &ctx->ctx_outside,
+ sizeof(sha512_ctx));
+}
+
+void hmac_sha512_reinit(hmac_sha512_ctx *ctx)
+{
+ memcpy(&ctx->ctx_inside, &ctx->ctx_inside_reinit,
+ sizeof(sha512_ctx));
+ memcpy(&ctx->ctx_outside, &ctx->ctx_outside_reinit,
+ sizeof(sha512_ctx));
+}
+
+void hmac_sha512_update(hmac_sha512_ctx *ctx, const unsigned char *message,
+ unsigned int message_len)
+{
+ sha512_update(&ctx->ctx_inside, message, message_len);
+}
+
+void hmac_sha512_final(hmac_sha512_ctx *ctx, unsigned char *mac,
+ unsigned int mac_size)
+{
+ unsigned char digest_inside[SHA512_DIGEST_SIZE];
+ unsigned char mac_temp[SHA512_DIGEST_SIZE];
+
+ sha512_final(&ctx->ctx_inside, digest_inside);
+ sha512_update(&ctx->ctx_outside, digest_inside, SHA512_DIGEST_SIZE);
+ sha512_final(&ctx->ctx_outside, mac_temp);
+ memcpy(mac, mac_temp, mac_size);
+}
+
+void hmac_sha512(const unsigned char *key, unsigned int key_size,
+ const unsigned char *message, unsigned int message_len,
+ unsigned char *mac, unsigned mac_size)
+{
+ hmac_sha512_ctx ctx;
+
+ hmac_sha512_init(&ctx, key, key_size);
+ hmac_sha512_update(&ctx, message, message_len);
+ hmac_sha512_final(&ctx, mac, mac_size);
+}
+
+#ifdef TEST_VECTORS
+
+/* IETF Validation tests */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+void test(const char *vector, unsigned char *digest,
+ unsigned int digest_size)
+{
+ char output[2 * SHA512_DIGEST_SIZE + 1];
+ int i;
+
+ output[2 * digest_size] = '\0';
+
+ for (i = 0; i < (int) digest_size ; i++) {
+ sprintf(output + 2*i, "%02x", digest[i]);
+ }
+
+ printf("H: %s\n", output);
+ if (strcmp(vector, output)) {
+ fprintf(stderr, "Test failed.\n");
+ exit(1);
+ }
+}
+
+int main(void)
+{
+ static const char *vectors[] =
+ {
+ /* HMAC-SHA-224 */
+ "896fb1128abbdf196832107cd49df33f47b4b1169912ba4f53684b22",
+ "a30e01098bc6dbbf45690f3a7e9e6d0f8bbea2a39e6148008fd05e44",
+ "7fb3cb3588c6c1f6ffa9694d7d6ad2649365b0c1f65d69d1ec8333ea",
+ "6c11506874013cac6a2abc1bb382627cec6a90d86efc012de7afec5a",
+ "0e2aea68a90c8d37c988bcdb9fca6fa8",
+ "95e9a0db962095adaebe9b2d6f0dbce2d499f112f2d2b7273fa6870e",
+ "3a854166ac5d9f023f54d517d0b39dbd946770db9c2b95c9f6f565d1",
+ /* HMAC-SHA-256 */
+ "b0344c61d8db38535ca8afceaf0bf12b881dc200c9833da726e9376c2e32cff7",
+ "5bdcc146bf60754e6a042426089575c75a003f089d2739839dec58b964ec3843",
+ "773ea91e36800e46854db8ebd09181a72959098b3ef8c122d9635514ced565fe",
+ "82558a389a443c0ea4cc819899f2083a85f0faa3e578f8077a2e3ff46729665b",
+ "a3b6167473100ee06e0c796c2955552b",
+ "60e431591ee0b67f0d8a26aacbf5b77f8e0bc6213728c5140546040f0ee37f54",
+ "9b09ffa71b942fcb27635fbcd5b0e944bfdc63644f0713938a7f51535c3a35e2",
+ /* HMAC-SHA-384 */
+ "afd03944d84895626b0825f4ab46907f15f9dadbe4101ec682aa034c7cebc59c"
+ "faea9ea9076ede7f4af152e8b2fa9cb6",
+ "af45d2e376484031617f78d2b58a6b1b9c7ef464f5a01b47e42ec3736322445e"
+ "8e2240ca5e69e2c78b3239ecfab21649",
+ "88062608d3e6ad8a0aa2ace014c8a86f0aa635d947ac9febe83ef4e55966144b"
+ "2a5ab39dc13814b94e3ab6e101a34f27",
+ "3e8a69b7783c25851933ab6290af6ca77a9981480850009cc5577c6e1f573b4e"
+ "6801dd23c4a7d679ccf8a386c674cffb",
+ "3abf34c3503b2a23a46efc619baef897",
+ "4ece084485813e9088d2c63a041bc5b44f9ef1012a2b588f3cd11f05033ac4c6"
+ "0c2ef6ab4030fe8296248df163f44952",
+ "6617178e941f020d351e2f254e8fd32c602420feb0b8fb9adccebb82461e99c5"
+ "a678cc31e799176d3860e6110c46523e",
+ /* HMAC-SHA-512 */
+ "87aa7cdea5ef619d4ff0b4241a1d6cb02379f4e2ce4ec2787ad0b30545e17cde"
+ "daa833b7d6b8a702038b274eaea3f4e4be9d914eeb61f1702e696c203a126854",
+ "164b7a7bfcf819e2e395fbe73b56e0a387bd64222e831fd610270cd7ea250554"
+ "9758bf75c05a994a6d034f65f8f0e6fdcaeab1a34d4a6b4b636e070a38bce737",
+ "fa73b0089d56a284efb0f0756c890be9b1b5dbdd8ee81a3655f83e33b2279d39"
+ "bf3e848279a722c806b485a47e67c807b946a337bee8942674278859e13292fb",
+ "b0ba465637458c6990e5a8c5f61d4af7e576d97ff94b872de76f8050361ee3db"
+ "a91ca5c11aa25eb4d679275cc5788063a5f19741120c4f2de2adebeb10a298dd",
+ "415fad6271580a531d4179bc891d87a6",
+ "80b24263c7c1a3ebb71493c1dd7be8b49b46d1f41b4aeec1121b013783f8f352"
+ "6b56d037e05f2598bd0fd2215d6a1e5295e64f73f63f0aec8b915a985d786598",
+ "e37b6a775dc87dbaa4dfa9f96e5e3ffddebd71f8867289865df5a32d20cdc944"
+ "b6022cac3c4982b10d5eeb55c3e4de15134676fb6de0446065c97440fa8c6a58"
+ };
+
+ static char *messages[] =
+ {
+ "Hi There",
+ "what do ya want for nothing?",
+ NULL,
+ NULL,
+ "Test With Truncation",
+ "Test Using Larger Than Block-Size Key - Hash Key First",
+ "This is a test using a larger than block-size key "
+ "and a larger than block-size data. The key needs"
+ " to be hashed before being used by the HMAC algorithm."
+ };
+
+ unsigned char mac[SHA512_DIGEST_SIZE];
+ unsigned char *keys[7];
+ unsigned int keys_len[7] = {20, 4, 20, 25, 20, 131, 131};
+ unsigned int messages2and3_len = 50;
+ unsigned int mac_224_size, mac_256_size, mac_384_size, mac_512_size;
+ int i;
+
+ for (i = 0; i < 7; i++) {
+ keys[i] = malloc(keys_len[i]);
+ if (keys[i] == NULL) {
+ fprintf(stderr, "Can't allocate memory\n");
+ return 1;
+ }
+ }
+
+ memset(keys[0], 0x0b, keys_len[0]);
+ strcpy((char *) keys[1], "Jefe");
+ memset(keys[2], 0xaa, keys_len[2]);
+ for (i = 0; i < (int) keys_len[3]; i++)
+ keys[3][i] = (unsigned char) i + 1;
+ memset(keys[4], 0x0c, keys_len[4]);
+ memset(keys[5], 0xaa, keys_len[5]);
+ memset(keys[6], 0xaa, keys_len[6]);
+
+ messages[2] = malloc(messages2and3_len + 1);
+ messages[3] = malloc(messages2and3_len + 1);
+
+ if (messages[2] == NULL || messages[3] == NULL) {
+ fprintf(stderr, "Can't allocate memory\n");
+ return 1;
+ }
+
+ messages[2][messages2and3_len] = '\0';
+ messages[3][messages2and3_len] = '\0';
+
+ memset(messages[2], 0xdd, messages2and3_len);
+ memset(messages[3], 0xcd, messages2and3_len);
+
+ printf("HMAC-SHA-2 IETF Validation tests\n\n");
+
+ for (i = 0; i < 7; i++) {
+ if (i != 4) {
+ mac_224_size = SHA224_DIGEST_SIZE;
+ mac_256_size = SHA256_DIGEST_SIZE;
+ mac_384_size = SHA384_DIGEST_SIZE;
+ mac_512_size = SHA512_DIGEST_SIZE;
+ } else {
+ mac_224_size = 128 / 8; mac_256_size = 128 / 8;
+ mac_384_size = 128 / 8; mac_512_size = 128 / 8;
+ }
+
+ printf("Test %d:\n", i + 1);
+
+ hmac_sha224(keys[i], keys_len[i], (unsigned char *) messages[i],
+ strlen(messages[i]), mac, mac_224_size);
+ test(vectors[i], mac, mac_224_size);
+ hmac_sha256(keys[i], keys_len[i], (unsigned char *) messages[i],
+ strlen(messages[i]), mac, mac_256_size);
+ test(vectors[7 + i], mac, mac_256_size);
+ hmac_sha384(keys[i], keys_len[i], (unsigned char *) messages[i],
+ strlen(messages[i]), mac, mac_384_size);
+ test(vectors[14 + i], mac, mac_384_size);
+ hmac_sha512(keys[i], keys_len[i], (unsigned char *) messages[i],
+ strlen(messages[i]), mac, mac_512_size);
+ test(vectors[21 + i], mac, mac_512_size);
+ }
+
+ printf("All tests passed.\n");
+
+ return 0;
+}
+
+#endif /* TEST_VECTORS */
+
diff --git a/libmtk_bsg/hmac_sha2.h b/libmtk_bsg/hmac_sha2.h
new file mode 100644
index 0000000..64b0202
--- /dev/null
+++ b/libmtk_bsg/hmac_sha2.h
@@ -0,0 +1,140 @@
+/*
+ * HMAC-SHA-224/256/384/512 implementation
+ * Last update: 06/15/2005
+ * Issue date: 06/15/2005
+ *
+ * Copyright (C) 2005 Olivier Gay <olivier.gay@a3.epfl.ch>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the project nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#ifndef HMAC_SHA2_H
+#define HMAC_SHA2_H
+
+#include "sha2.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+ sha224_ctx ctx_inside;
+ sha224_ctx ctx_outside;
+
+ /* for hmac_reinit */
+ sha224_ctx ctx_inside_reinit;
+ sha224_ctx ctx_outside_reinit;
+
+ unsigned char block_ipad[SHA224_BLOCK_SIZE];
+ unsigned char block_opad[SHA224_BLOCK_SIZE];
+} hmac_sha224_ctx;
+
+typedef struct {
+ sha256_ctx ctx_inside;
+ sha256_ctx ctx_outside;
+
+ /* for hmac_reinit */
+ sha256_ctx ctx_inside_reinit;
+ sha256_ctx ctx_outside_reinit;
+
+ unsigned char block_ipad[SHA256_BLOCK_SIZE];
+ unsigned char block_opad[SHA256_BLOCK_SIZE];
+} hmac_sha256_ctx;
+
+typedef struct {
+ sha384_ctx ctx_inside;
+ sha384_ctx ctx_outside;
+
+ /* for hmac_reinit */
+ sha384_ctx ctx_inside_reinit;
+ sha384_ctx ctx_outside_reinit;
+
+ unsigned char block_ipad[SHA384_BLOCK_SIZE];
+ unsigned char block_opad[SHA384_BLOCK_SIZE];
+} hmac_sha384_ctx;
+
+typedef struct {
+ sha512_ctx ctx_inside;
+ sha512_ctx ctx_outside;
+
+ /* for hmac_reinit */
+ sha512_ctx ctx_inside_reinit;
+ sha512_ctx ctx_outside_reinit;
+
+ unsigned char block_ipad[SHA512_BLOCK_SIZE];
+ unsigned char block_opad[SHA512_BLOCK_SIZE];
+} hmac_sha512_ctx;
+
+void hmac_sha224_init(hmac_sha224_ctx *ctx, const unsigned char *key,
+ unsigned int key_size);
+void hmac_sha224_reinit(hmac_sha224_ctx *ctx);
+void hmac_sha224_update(hmac_sha224_ctx *ctx, const unsigned char *message,
+ unsigned int message_len);
+void hmac_sha224_final(hmac_sha224_ctx *ctx, unsigned char *mac,
+ unsigned int mac_size);
+void hmac_sha224(const unsigned char *key, unsigned int key_size,
+ const unsigned char *message, unsigned int message_len,
+ unsigned char *mac, unsigned mac_size);
+
+void hmac_sha256_init(hmac_sha256_ctx *ctx, const unsigned char *key,
+ unsigned int key_size);
+void hmac_sha256_reinit(hmac_sha256_ctx *ctx);
+void hmac_sha256_update(hmac_sha256_ctx *ctx, const unsigned char *message,
+ unsigned int message_len);
+void hmac_sha256_final(hmac_sha256_ctx *ctx, unsigned char *mac,
+ unsigned int mac_size);
+void hmac_sha256(const unsigned char *key, unsigned int key_size,
+ const unsigned char *message, unsigned int message_len,
+ unsigned char *mac, unsigned mac_size);
+
+void hmac_sha384_init(hmac_sha384_ctx *ctx, const unsigned char *key,
+ unsigned int key_size);
+void hmac_sha384_reinit(hmac_sha384_ctx *ctx);
+void hmac_sha384_update(hmac_sha384_ctx *ctx, const unsigned char *message,
+ unsigned int message_len);
+void hmac_sha384_final(hmac_sha384_ctx *ctx, unsigned char *mac,
+ unsigned int mac_size);
+void hmac_sha384(const unsigned char *key, unsigned int key_size,
+ const unsigned char *message, unsigned int message_len,
+ unsigned char *mac, unsigned mac_size);
+
+void hmac_sha512_init(hmac_sha512_ctx *ctx, const unsigned char *key,
+ unsigned int key_size);
+void hmac_sha512_reinit(hmac_sha512_ctx *ctx);
+void hmac_sha512_update(hmac_sha512_ctx *ctx, const unsigned char *message,
+ unsigned int message_len);
+void hmac_sha512_final(hmac_sha512_ctx *ctx, unsigned char *mac,
+ unsigned int mac_size);
+void hmac_sha512(const unsigned char *key, unsigned int key_size,
+ const unsigned char *message, unsigned int message_len,
+ unsigned char *mac, unsigned mac_size);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* !HMAC_SHA2_H */
+
diff --git a/libmtk_bsg/include/mtk_iotctl.h b/libmtk_bsg/include/mtk_iotctl.h
new file mode 100644
index 0000000..786c141
--- /dev/null
+++ b/libmtk_bsg/include/mtk_iotctl.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_IOCTL_H_
+#define MTK_IOCTL_H_
+
+
+int ioctrl_w_attr(const char path[], uint8_t idn, uint8_t index, uint8_t selector, uint32_t value);
+// int ioctrl_r_attr(int fd, uint8_t idn, uint8_t index, uint8_t selector, uint32_t *value);
+
+
+#endif
diff --git a/libmtk_bsg/ioctl.h b/libmtk_bsg/ioctl.h
new file mode 100644
index 0000000..45f2197
--- /dev/null
+++ b/libmtk_bsg/ioctl.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef UAPI_SCSI_IOCTL_H_
+#define UAPI_SCSI_IOCTL_H_
+
+#ifndef SG_IO
+/* synchronous SCSI command ioctl, (only in version 3 interface) */
+#define SG_IO 0x2285 /* similar effect as write() followed by read() */
+#endif
+
+#ifndef _UAPIBSG_H
+#include <linux/types.h>
+
+#define DEF_TIMEOUT_MSEC (60000)
+#define BSG_PROTOCOL_SCSI 0
+
+#define BSG_SUB_PROTOCOL_SCSI_CMD 0
+#define BSG_SUB_PROTOCOL_SCSI_TMF 1
+#define BSG_SUB_PROTOCOL_SCSI_TRANSPORT 2
+
+/*
+ * For flag constants below:
+ * sg.h sg_io_hdr also has bits defined for it's flags member. These
+ * two flag values (0x10 and 0x20) have the same meaning in sg.h . For
+ * bsg the BSG_FLAG_Q_AT_HEAD flag is ignored since it is the deafult.
+ */
+#define BSG_FLAG_Q_AT_TAIL 0x10 /* default is Q_AT_HEAD */
+#define BSG_FLAG_Q_AT_HEAD 0x20
+
+struct sg_io_v4 {
+ __s32 guard; /* [i] 'Q' to differentiate from v3 */
+ __u32 protocol; /* [i] 0 -> SCSI , .... */
+ __u32 subprotocol; /* [i] 0 -> SCSI command, 1 -> SCSI task
+ management function, .... */
+
+ __u32 request_len; /* [i] in bytes */
+ __u64 request; /* [i], [*i] {SCSI: cdb} */
+ __u64 request_tag; /* [i] {SCSI: task tag (only if flagged)} */
+ __u32 request_attr; /* [i] {SCSI: task attribute} */
+ __u32 request_priority; /* [i] {SCSI: task priority} */
+ __u32 request_extra; /* [i] {spare, for padding} */
+ __u32 max_response_len; /* [i] in bytes */
+ __u64 response; /* [i], [*o] {SCSI: (auto)sense data} */
+
+ /* "dout_": data out (to device); "din_": data in (from device) */
+ __u32 dout_iovec_count; /* [i] 0 -> "flat" dout transfer else
+ dout_xfer points to array of iovec */
+ __u32 dout_xfer_len; /* [i] bytes to be transferred to device */
+ __u32 din_iovec_count; /* [i] 0 -> "flat" din transfer */
+ __u32 din_xfer_len; /* [i] bytes to be transferred from device */
+ __u64 dout_xferp; /* [i], [*i] */
+ __u64 din_xferp; /* [i], [*o] */
+
+ __u32 timeout; /* [i] units: millisecond */
+ __u32 flags; /* [i] bit mask */
+ __u64 usr_ptr; /* [i->o] unused internally */
+ __u32 spare_in; /* [i] */
+
+ __u32 driver_status; /* [o] 0 -> ok */
+ __u32 transport_status; /* [o] 0 -> ok */
+ __u32 device_status; /* [o] {SCSI: command completion status} */
+ __u32 retry_delay; /* [o] {SCSI: status auxiliary information} */
+ __u32 info; /* [o] additional information */
+ __u32 duration; /* [o] time to complete, in milliseconds */
+ __u32 response_len; /* [o] bytes of response actually written */
+ __s32 din_resid; /* [o] din_xfer_len - actual_din_xfer_len */
+ __s32 dout_resid; /* [o] dout_xfer_len - actual_dout_xfer_len */
+ __u64 generated_tag; /* [o] {SCSI: transport generated task tag} */
+ __u32 spare_out; /* [o] */
+
+ __u32 padding;
+};
+
+/*
+ * SCSI Generic v3 struct copied from include/scsi/sg.h
+ */
+typedef struct sg_io_hdr {
+ int interface_id; /* [i] 'S' for SCSI generic (required) */
+ int dxfer_direction; /* [i] data transfer direction */
+ unsigned char cmd_len; /* [i] SCSI command length ( <= 16 bytes) */
+ unsigned char mx_sb_len; /* [i] max length to write to sbp */
+ unsigned short int iovec_count; /* [i] 0 implies no scatter gather */
+ unsigned int dxfer_len; /* [i] byte count of data transfer */
+ void *dxferp; /* [i], [*io] points to data transfer memory
+ or scatter gather list */
+ unsigned char *cmdp; /* [i], [*i] points to command to perform */
+ unsigned char *sbp; /* [i], [*o] points to sense_buffer memory */
+ unsigned int timeout; /* [i] MAX_UINT->no timeout (unit: millisec) */
+ unsigned int flags; /* [i] 0 -> default, see SG_FLAG... */
+ int pack_id; /* [i->o] unused internally (normally) */
+ void *usr_ptr; /* [i->o] unused internally */
+ unsigned char status; /* [o] scsi status */
+ unsigned char masked_status;/* [o] shifted, masked scsi status */
+ unsigned char msg_status; /* [o] messaging level data (optional) */
+ unsigned char sb_len_wr; /* [o] byte count actually written to sbp */
+ unsigned short int host_status; /* [o] errors from host adapter */
+ unsigned short int driver_status;/* [o] errors from software driver */
+ int resid; /* [o] dxfer_len - actual_transferred */
+ unsigned int duration; /* [o] time taken by cmd (unit: millisec) */
+ unsigned int info; /* [o] auxiliary information */
+} sg_io_hdr_t;
+
+#endif /* _UAPIBSG_H */
+#endif /* UAPI_SCSI_IOCTL_H_ */
diff --git a/libmtk_bsg/mtk_ioctl.c b/libmtk_bsg/mtk_ioctl.c
new file mode 100644
index 0000000..dea729b
--- /dev/null
+++ b/libmtk_bsg/mtk_ioctl.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* Wrapping the ioctrl of linux kernel*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <stdint.h>
+#include <endian.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include "ufs_cmds.h"
+#include "options.h"
+#include "ufs.h"
+
+
+
+static void initialized_options(struct tool_options *options)
+{
+ memset(options, INVALID, sizeof(*options));
+ options->path[0] = '\0';
+ options->keypath[0] = '\0';
+ options->data = NULL;
+ options->sg_type = SG4_TYPE;
+}
+
+
+/**
+ * @brief write attribute to bsg device
+ *
+ * @param path device path e.g /dev/block/sdc
+ * @param idn idn of attribute
+ * @param index index of attribute
+ * @param selector selector of attribute
+ * @param value the value to write to attribute. If the descriptor accepts only size less than DWORD,
+ * other bytes are ignored.
+ * @return int return OK on success write.
+ */
+int ioctrl_w_attr(const char path[], uint8_t idn, uint8_t index, uint8_t selector, uint32_t value){
+
+ struct tool_options options;
+ initialized_options(&options);
+
+ options.config_type_inx = ATTR_TYPE;
+ options.opr = WRITE; // write
+
+ // TODO: check reseult
+ // alloc buffer
+ options.data = (uint32_t *)calloc(1, sizeof(uint32_t));
+
+ // verify_write(&options);
+
+ options.idn = idn;
+ options.index = index;
+ options.selector = selector;
+ strcpy(options.path, path);// TODO: check copy error
+ *((uint32_t *)options.data) = value;
+
+ verify_arg_and_set_default(&options);
+
+ if (do_attributes(&options)){
+ print_error("attribute write failed");
+ return ERROR;
+ }
+
+ return OK;
+}
+
+
diff --git a/libmtk_bsg/options.c b/libmtk_bsg/options.c
new file mode 100644
index 0000000..a619cb3
--- /dev/null
+++ b/libmtk_bsg/options.c
@@ -0,0 +1,842 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+#include <errno.h>
+
+#include "options.h"
+#include "ufs.h"
+#include "unipro.h"
+#include "ufs_ffu.h"
+#include "ufs_rpmb.h"
+#include "ufs_hmr.h"
+
+static int verify_and_set_idn(struct tool_options *options);
+static int verify_read(struct tool_options *options);
+static int verify_write(struct tool_options *options);
+static int verify_and_set_flag_operation(int opr_type,
+ struct tool_options *options);
+static int verify_and_set_device_path(struct tool_options *options);
+static int verify_and_set_index(struct tool_options *options);
+static int verify_and_set_selector(struct tool_options *options);
+static int verify_target(struct tool_options *options, int target);
+static int verify_and_set_ffu_chunk_size(struct tool_options *options);
+static int verify_length(struct tool_options *options);
+static int verify_offset(struct tool_options *options);
+static int verify_and_set_start_addr(struct tool_options *options);
+static int verify_and_set_num_block(struct tool_options *options);
+static int verify_lun(struct tool_options *options);
+static int verify_and_set_key_path(struct tool_options *options);
+static int verify_region(struct tool_options *options);
+static int verify_and_set_hmr_method(struct tool_options *options);
+static int verify_and_set_hmr_unit(struct tool_options *options);
+static int verify_sg_struct(struct tool_options *options);
+
+#define MAX_ADDRESS 0xFFFF
+
+
+int init_options(int opt_cnt, char *opt_arr[], struct tool_options *options)
+{
+ int rc = -EINVAL;
+ int curr_opt = 0;
+ int opt = 0;
+
+ static struct option long_opts[] = {
+ {"peer", no_argument, NULL, 'u'}, /* UFS device */
+ {"local", no_argument, NULL, 'l'}, /* UFS host*/
+ {NULL, 0, NULL, 0}
+ };
+ static char *short_opts = "t:p:w:i:s:O:L:n:k:m:d:x:y:g:rocea";
+
+ while (-1 !=
+ (curr_opt = getopt_long(opt_cnt, opt_arr, short_opts,
+ long_opts, &opt))) {
+ switch (curr_opt) {
+ case 'a':
+ rc = verify_read(options);
+ if (!rc)
+ options->opr = READ_ALL;
+ break;
+ case 't':
+ rc = verify_and_set_idn(options);
+ break;
+ case 'r':
+ rc = verify_read(options);
+ if (!rc)
+ options->opr = READ;
+ break;
+ case 'w':
+ rc = verify_write(options);
+ if (!rc)
+ options->opr = WRITE;
+ break;
+ case 'c':
+ rc = verify_and_set_flag_operation(CLEAR_FLAG,
+ options);
+ break;
+ case 'e':
+ rc = verify_and_set_flag_operation(SET_FLAG, options);
+ break;
+ case 'o':
+ rc = verify_and_set_flag_operation(TOGGLE_FLAG,
+ options);
+ break;
+ case 'p':
+ rc = verify_and_set_device_path(options);
+ break;
+ case 'i':
+ rc = verify_and_set_index(options);
+ break;
+ case 's':
+ if (options->config_type_inx == FFU_TYPE)
+ rc = verify_and_set_ffu_chunk_size(options);
+ else if (options->config_type_inx == RPMB_CMD_TYPE)
+ rc = verify_and_set_start_addr(options);
+ else
+ rc = verify_and_set_selector(options);
+ break;
+ case 'u':
+ rc = verify_target(options, DME_PEER);
+ break;
+ case 'l':
+ rc = verify_target(options, DME_LOCAL);
+ break;
+ case 'd':
+ rc = verify_lun(options);
+ break;
+ case 'L':
+ rc = verify_length(options);
+ break;
+ case 'O':
+ rc = verify_offset(options);
+ break;
+ case 'n':
+ rc = verify_and_set_num_block(options);
+ break;
+ case 'k':
+ rc = verify_and_set_key_path(options);
+ break;
+ case 'm':
+ rc = verify_region(options);
+ break;
+ case 'x':
+ rc = verify_and_set_hmr_method(options);
+ break;
+ case 'y':
+ rc = verify_and_set_hmr_unit(options);
+ break;
+ case 'g':
+ rc = verify_sg_struct(options);
+ break;
+ default:
+ rc = -EINVAL;
+ break;
+ }
+ if (rc)
+ break;
+ }
+
+ if (!rc)
+ rc = verify_arg_and_set_default(options);
+
+ return rc;
+}
+
+static int verify_target(struct tool_options *options, int target)
+{
+ if (options->target != INVALID) {
+ print_error("duplicated operate target.");
+ goto out;
+ }
+
+ options->target = target;
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int verify_and_set_index(struct tool_options *options)
+{
+ int index = INVALID;
+
+ if (options->index != INVALID) {
+ print_error("duplicated index");
+ goto out;
+ }
+
+ /* In case atoi returned 0 . Check that is real 0 and not error
+ * arguments . Also check that the value is in correct range
+ */
+ if (strstr(optarg, "0x") || strstr(optarg, "0X"))
+ index = (int)strtol(optarg, NULL, 0);
+ else
+ index = atoi(optarg);
+
+ if ((index == 0 && strcmp(optarg, "0")) || index < 0) {
+ print_error("Invalid argument for index");
+ goto out;
+ }
+
+ options->index = index;
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int verify_and_set_ffu_chunk_size(struct tool_options *options)
+{
+ int chunk_size_kb = atoi(optarg);
+
+ if (!chunk_size_kb) {
+ print_error("Invalid chunk_size %d ", chunk_size_kb);
+ goto out;
+ }
+ options->size = chunk_size_kb * 1024;
+ if ((options->size > MAX_IOCTL_BUF_SIZE) ||
+ (options->size % ALIGNMENT_CHUNK_SIZE)) {
+ print_error("The chunk should be multiple value of 4k, between 4k and %dk",
+ MAX_IOCTL_BUF_SIZE / 1024);
+ goto out;
+ }
+ return OK;
+out:
+ return ERROR;
+}
+
+static int verify_and_set_selector(struct tool_options *options)
+{
+ int selector = INVALID;
+
+ if (options->selector != INVALID) {
+ print_error("duplicated selector");
+ goto out;
+ }
+
+ /* In case atoi returned 0 . Check that is real 0 and not error
+ * arguments . Also check that the value is in correct range
+ */
+ selector = atoi(optarg);
+ if ((selector == 0 && strcmp(optarg, "0")) || selector < 0) {
+ print_error("Invalid argument for selector");
+ goto out;
+ }
+
+ options->selector = selector;
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int verify_and_set_idn(struct tool_options *options)
+{
+ int idn = INVALID;
+
+ if (options->idn != INVALID) {
+ print_error("duplicated type option");
+ goto out;
+ }
+
+ /* In case atoi returned 0. Check that is real 0 and not error
+ * arguments. Also check that the value is in correct range
+ */
+ idn = atoi(optarg);
+ if ((idn == 0 && strcmp(optarg, "0")) || idn < 0) {
+ print_error("Invalid argument for idn");
+ goto out;
+ }
+
+ switch (options->config_type_inx) {
+ case DESC_TYPE:
+ if (idn > QUERY_DESC_IDN_MAX) {
+ print_error("Invalid descriptor idn %d", idn);
+ goto out;
+ }
+ break;
+ case ATTR_TYPE:
+ if (idn >= QUERY_ATTR_IDN_MAX) {
+ print_error("Invalid attr idn %d", idn);
+ goto out;
+ }
+ break;
+ case FLAG_TYPE:
+ if (idn > QUERY_FLAG_IDN_MAX) {
+ print_error("Invalid flag idn %d", idn);
+ goto out;
+ }
+ break;
+ case UIC_TYPE:
+ if (idn >= MAX_UNIPRO_IDN) {
+ print_error("Invalid UIC idn %d", idn);
+ goto out;
+ }
+ break;
+ case FFU_TYPE:
+ if (idn >= UFS_FFU_MAX) {
+ print_error("Invalid ffu cmd %d", idn);
+ goto out;
+ }
+ break;
+ case RPMB_CMD_TYPE:
+ if (idn >= RPMB_CMD_MAX) {
+ print_error("Invalid rpmb cmd %d", idn);
+ goto out;
+ }
+ break;
+ default:
+ print_error("Invalid UFS configuration type %d", idn);
+ goto out;
+ }
+
+ options->idn = idn;
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int verify_length(struct tool_options *options)
+{
+ int len = INVALID;
+
+ if (options->len != INVALID) {
+ print_error("duplicated length option");
+ goto out;
+ }
+
+ /* In case atoi returned 0. Check that is real 0 and not error
+ * arguments. Also check that the value is in correct range
+ */
+ len = atoi(optarg);
+ if (len == 0 || len < 0 || len > BLOCK_SIZE) {
+ print_error("Invalid argument for length. The value should be between 1 to %dB",
+ BLOCK_SIZE);
+ goto out;
+ }
+
+ options->len = len;
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int verify_offset(struct tool_options *options)
+{
+ int offset = INVALID;
+
+ if (options->offset != INVALID) {
+ print_error("duplicated offset option");
+ goto out;
+ }
+ if (strstr(optarg, "0x") || strstr(optarg, "0X"))
+ offset = (int)strtol(optarg, NULL, 0);
+ else
+ offset = atoi(optarg);
+ if ((offset == 0 && strcmp(optarg, "0")) || offset < 0) {
+ print_error("Invalid argument for offset");
+ goto out;
+ }
+
+ options->offset = offset;
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int verify_and_set_start_addr(struct tool_options *options)
+{
+ int start_block = 0;
+
+ if (options->config_type_inx != RPMB_CMD_TYPE) {
+ print_error("start block address using only for rpmb cmd");
+ goto out;
+ }
+
+ start_block = atoi(optarg);
+ if ((start_block == 0 && strcmp(optarg, "0")) ||
+ start_block < 0 || start_block > MAX_ADDRESS) {
+ print_error("Invalid start address");
+ goto out;
+ } else
+ options->start_block = start_block;
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int verify_and_set_num_block(struct tool_options *options)
+{
+ int num_block = 0;
+
+ if (options->config_type_inx != RPMB_CMD_TYPE) {
+ print_error("num_block using only for rpmb cmd");
+ goto out;
+ }
+
+ num_block = atoi(optarg);
+ if ((num_block == 0 && strcmp(optarg, "0")) || num_block < 0) {
+ print_error("Invalid numbers of block");
+ goto out;
+ } else
+ options->num_block = num_block;
+ return OK;
+
+out:
+ return ERROR;
+
+}
+
+static int verify_and_set_key_path(struct tool_options *options)
+{
+ if (options->config_type_inx != RPMB_CMD_TYPE) {
+ print_error("key path using only for rpmb cmd");
+ goto out;
+ }
+
+ if (options->keypath[0] != '\0') {
+ print_error("Duplicate Key path");
+ goto out;
+ }
+
+ if ((optarg == NULL) || (optarg[0] == 0)) {
+ print_error("Key path missed");
+ goto out;
+ }
+
+ if (strlen(optarg) >= PATH_MAX) {
+ print_error("Key path is too long");
+ goto out;
+ }
+
+ strcpy(options->keypath, optarg);
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int verify_sg_struct(struct tool_options *options)
+{
+ int8_t sg_type;
+
+ if ((0 == (sg_type = atoi(optarg)) && 0 != (strcmp(optarg, "0"))) ||
+ ((sg_type != SG3_TYPE) && (sg_type != SG4_TYPE))) {
+ print_error("Invalid SG struct");
+ return ERROR;
+ } else
+ options->sg_type = sg_type;
+ return OK;
+}
+
+static int verify_lun(struct tool_options *options)
+{
+ int8_t lun = 0;
+
+ lun = atoi(optarg);
+ if ((lun == 0 && strcmp(optarg, "0")) || lun < 0) {
+ print_error("Invalid lun");
+ return ERROR;
+ }
+ options->lun = lun;
+ return OK;
+}
+
+static int verify_region(struct tool_options *options)
+{
+ int8_t region = 0;
+
+ region = atoi(optarg);
+ if ((region == 0 && strcmp(optarg, "0")) || region < 0
+ || region > 3) {
+ print_error("Invalid RPMB region");
+ return ERROR;
+ }
+ options->region = region;
+ return OK;
+}
+
+static int verify_rpmb_arg(struct tool_options *options)
+{
+ int ret = OK;
+
+ if (options->region == INVALID)
+ options->region = 0;
+
+ switch (options->idn) {
+ case AUTHENTICATION_KEY:
+ if (options->keypath[0] == 0) {
+ print_error("Key path is missed");
+ ret = ERROR;
+ }
+ break;
+ case READ_RPMB:
+ if (!options->data) {
+ print_error("Output data file missed");
+ ret = ERROR;
+ }
+ break;
+ case READ_SEC_RPMB_CONF_BLOCK:
+ if (!options->data) {
+ print_error("Output data file missed");
+ ret = ERROR;
+ }
+ if (options->lun == INVALID) {
+ print_error("LUN parameter is missed");
+ ret = ERROR;
+ }
+
+ if (options->region > 0) {
+ print_error("Config block exist only in region 0");
+ ret = ERROR;
+ }
+ break;
+ case WRITE_RPMB:
+ if (!options->data) {
+ print_error("Input data file missed");
+ ret = ERROR;
+ }
+ if (options->keypath[0] == 0) {
+ print_error("Key path is missed");
+ ret = ERROR;
+ }
+ if (options->num_block == INVALID)
+ options->num_block = 1;
+ if (options->start_block == INVALID)
+ options->start_block = 0;
+ break;
+ case WRITE_SEC_RPMB_CONF_BLOCK:
+ if (!options->data) {
+ print_error("Input data file missed");
+ ret = ERROR;
+ }
+ if (options->keypath[0] == 0) {
+ print_error("Key path is missed");
+ ret = ERROR;
+ }
+ if (options->region > 0) {
+ print_error("Config block exist only in region 0");
+ ret = ERROR;
+ }
+ break;
+ case READ_WRITE_COUNTER:
+ break;
+ default:
+ print_error("Unsupported RPMB cmd %d",
+ options->idn);
+ break;
+ }
+ return ret;
+}
+
+static int verify_and_set_hmr_method(struct tool_options *options)
+{
+ int ret = ERROR;
+ long result;
+
+ if (options->hmr_method != INVALID) {
+ print_error("Duplicated hmr method option");
+ goto out;
+ }
+
+ ret = str_to_long(optarg, 10, &result);
+ if (ret == ERROR) {
+ print_error("Invalid argument for hmr method: not convertable");
+ goto out;
+ }
+
+ if (result < HMR_METHOD_FORCE || result >= HMR_METHOD_MAX) {
+ print_error("Invalid argument for hmr method: out of range");
+ ret = ERROR;
+ goto out;
+ }
+
+ options->hmr_method = result;
+
+out:
+ return ret;
+}
+
+static int verify_and_set_hmr_unit(struct tool_options *options)
+{
+ int ret = ERROR;
+ long result;
+
+ if (options->hmr_unit != INVALID) {
+ print_error("Duplicated hmr unit option");
+ goto out;
+ }
+
+ ret = str_to_long(optarg, 10, &result);
+ if (ret == ERROR) {
+ print_error("Invalid argument for hmr unit: not convertable");
+ goto out;
+ }
+
+ if (result < HMR_UNIT_MIN || result >= HMR_UNIT_MAX) {
+ print_error("Invalid argument for hmr unit: out of range");
+ ret = ERROR;
+ goto out;
+ }
+
+ options->hmr_unit = result;
+
+out:
+ return ret;
+}
+
+
+// TODO: remove arg verification!
+int verify_arg_and_set_default(struct tool_options *options)
+{
+ if (options->path[0] == '\0') {
+ print_error("Missing device path type");
+ goto out;
+ }
+
+ if (options->opr == INVALID)
+ options->opr = READ;
+
+ if (options->opr == WRITE && !options->data) {
+ print_error("Data missed for the write operation");
+ goto out;
+ }
+ if (options->config_type_inx != ERR_HIST_TYPE &&
+ options->config_type_inx != VENDOR_BUFFER_TYPE &&
+ options->config_type_inx != HMR_TYPE &&
+ options->opr != READ_ALL &&
+ options->idn == INVALID) {
+ print_error("The type idn is missed");
+ goto out;
+ }
+
+ if (options->config_type_inx == DESC_TYPE &&
+ options->idn == QUERY_DESC_IDN_STRING &&
+ options->index == INVALID) {
+ print_error("The index is missed");
+ goto out;
+ }
+
+ if (options->config_type_inx == UIC_TYPE) {
+ if (options->idn == INVALID) {
+ /*
+ * As for the Unipro attributes access, should always
+ * specify idn.
+ */
+ print_error("idn of Unipro attributes is missed");
+ goto out;
+ }
+
+ if (options->opr == WRITE && options->target != DME_PEER &&
+ options->target != DME_LOCAL) {
+ /*
+ * As for Unipro attributes write, should
+ * specify accessing target.
+ */
+ print_error("accessing target is missed");
+ goto out;
+ }
+
+ if (options->index == INVALID &&
+ (options->opr == READ || options->opr == WRITE)) {
+ print_error("ID of Unipro attributes is missed");
+ goto out;
+ }
+ }
+
+ if (options->index == INVALID)
+ options->index = 0;
+
+ if (options->selector == INVALID)
+ options->selector = 0;
+
+ if (options->config_type_inx == FFU_TYPE) {
+ if (options->size == INVALID)
+ options->size = MAX_IOCTL_BUF_SIZE;
+ if (options->idn == INVALID)
+ /*Default operation*/
+ options->idn = UFS_FFU;
+ if ((options->idn != UFS_CHECK_FFU_STATUS) &&
+ (options->data == NULL)) {
+ print_error("The FW file name is missing");
+ goto out;
+ }
+ }
+
+ if ((options->config_type_inx == VENDOR_BUFFER_TYPE) &&
+ (options->len == INVALID))
+ options->len = BLOCK_SIZE;
+
+ if (options->config_type_inx == RPMB_CMD_TYPE) {
+ if (verify_rpmb_arg(options))
+ goto out;
+ }
+
+ if (options->config_type_inx == HMR_TYPE) {
+ if (options->hmr_method == INVALID)
+ options->hmr_method = HMR_METHOD_SELECTIVE;
+
+ if (options->hmr_unit == INVALID)
+ options->hmr_unit = HMR_UNIT_MIN;
+ }
+
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int verify_and_set_device_path(struct tool_options *options)
+{
+ if (options->path[0] != '\0') {
+ print_error("Duplicate Device path %d", options->path[0]);
+ goto out;
+ }
+
+ if (optarg[0] == 0) {
+ print_error("Device path missed");
+ goto out;
+ }
+
+ if (strlen(optarg) >= PATH_MAX) {
+ print_error("Device path is too long");
+ goto out;
+ }
+
+ // strcpy(options->path, optarg);
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int verify_read(struct tool_options *options)
+{
+ if (options->opr != INVALID) {
+ print_error("duplicated operation option(read)");
+ goto out;
+ }
+
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int verify_write(struct tool_options *options)
+{
+ errno = 0;
+
+ if (options->opr != INVALID) {
+ print_error("duplicated operation option(write)");
+ goto out;
+ }
+
+ if (optarg[0] == 0) {
+ print_error("Data is missed");
+ goto out;
+ }
+
+ if (options->config_type_inx == DESC_TYPE) {
+ int arg_len = strlen(optarg);
+ int str_desc_max_len = QUERY_DESC_STRING_MAX_SIZE/2 - 2;
+
+ if (options->idn != QUERY_DESC_IDN_CONFIGURAION &&
+ options->idn != QUERY_DESC_IDN_STRING) {
+ print_error("write unavailable for descriptor = %d",
+ options->idn);
+ goto out;
+ }
+
+ if (arg_len > str_desc_max_len) {
+ print_error("Input data is too big");
+ goto out;
+ }
+
+ options->data = (char *)malloc(QUERY_DESC_STRING_MAX_SIZE);
+ if (!options->data) {
+ print_error("Memory Allocation problem");
+ goto out;
+ }
+
+ strcpy(options->data, optarg);
+ }
+
+ if (options->config_type_inx == FLAG_TYPE) {
+ print_error("Please use 'c', 'e', or 'o' for flag operations");
+ goto out;
+ }
+
+ if (options->config_type_inx == ATTR_TYPE ||
+ options->config_type_inx == UIC_TYPE) {
+ options->data = (__u32 *)calloc(1, sizeof(__u32));
+ if (!options->data) {
+ print_error("Memory Allocation problem");
+ goto out;
+ }
+
+ // *(__u32 *)options->data = strtol(optarg, &endptr, 16);
+
+ if (errno != 0) {
+ print_error("Wrong data");
+ goto out;
+ }
+ }
+ if (options->config_type_inx == FFU_TYPE ||
+ options->config_type_inx == VENDOR_BUFFER_TYPE ||
+ options->config_type_inx == RPMB_CMD_TYPE) {
+ int len = strlen(optarg) + 1;
+
+ if (len >= PATH_MAX) {
+ print_error("Input file path is too long");
+ goto out;
+ }
+ options->data = (char *)calloc(1, len);
+ if (options->data == NULL) {
+ print_error("Memory Allocation problem");
+ goto out;
+ } else
+ strcpy(options->data, optarg);
+ }
+
+ return OK;
+
+out:
+ return ERROR;
+}
+
+static int
+verify_and_set_flag_operation(int opr_type, struct tool_options *options)
+{
+ if (options->opr != INVALID) {
+ print_error("duplicated operation option");
+ goto out;
+ }
+
+ if (options->config_type_inx != FLAG_TYPE) {
+ print_error("-c | -o | -e operation only for the flag type");
+ goto out;
+ }
+
+ if (opr_type < CLEAR_FLAG || opr_type > TOGGLE_FLAG) {
+ print_error("Incorrect operation for the flag type");
+ goto out;
+ }
+
+ options->opr = opr_type;
+ return OK;
+
+out:
+ return ERROR;
+}
+
diff --git a/libmtk_bsg/options.h b/libmtk_bsg/options.h
new file mode 100644
index 0000000..6b7c48b
--- /dev/null
+++ b/libmtk_bsg/options.h
@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef OPTIONS_H_
+#define OPTIONS_H_
+#include <stdint.h>
+
+#define OK 0
+#define ERROR -1
+#define INVALID -1
+
+#ifndef _UAPI_LINUX_LIMITS_H
+#define PATH_MAX 4096
+#endif
+
+#define READ 0
+#define WRITE 1
+#define CLEAR_FLAG 2
+#define SET_FLAG 3
+#define TOGGLE_FLAG 4
+#define READ_ALL 5
+
+
+#define ALIGNMENT_CHUNK_SIZE 4096
+
+struct tool_options {
+ /* one of @ufs_cong_type */
+ int config_type_inx;
+ /* opt: -t, type - one of @flag_idn / @attr_idn / @desc_idn
+ * or @unipro attribute idn
+ */
+ int idn;
+ /* opt: -r/w/o/c/a, type of the operation read/write/toggle/clear */
+ int opr;
+ int index;
+ int selector;
+ /* data for writing */
+ void *data;
+ /* @DME_LOCAL or @DME_PEER */
+ int target;
+ int size;
+ int offset;
+ int len;
+ /* HMR related */
+ int hmr_method;
+ int hmr_unit;
+ /*start block address for rpmb cmd */
+ int start_block;
+ int num_block;
+ int8_t lun;
+ /*RPMB region*/
+ int8_t region;
+ int sg_type;
+ char keypath[PATH_MAX];
+ char path[PATH_MAX];
+ // int fd;
+};
+int verify_arg_and_set_default(struct tool_options *options);
+int init_options(int opt_cnt, char **opt_arr, struct tool_options *options);
+#endif /* OPTIONS_H_ */
diff --git a/libmtk_bsg/scsi_bsg_util.c b/libmtk_bsg/scsi_bsg_util.c
new file mode 100644
index 0000000..4d6381b
--- /dev/null
+++ b/libmtk_bsg/scsi_bsg_util.c
@@ -0,0 +1,373 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#pragma GCC diagnostic ignored "-Wall"
+#pragma GCC diagnostic ignored "-Wextra"
+#pragma GCC diagnostic ignored "-Wsign-compare"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <stdint.h>
+#include <endian.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include "ioctl.h"
+#include "ufs.h"
+#include "scsi_bsg_util.h"
+#include "ufs_rpmb.h"
+
+#define UPIU_HEADER_DWORD(byte3, byte2, byte1, byte0)\
+ htobe32((byte3 << 24) | (byte2 << 16) |\
+ (byte1 << 8) | (byte0))
+#define SEC_SPEC_OFFSET 2
+#define SEC_TRANS_LEN_OFFSET 6
+/* description of the sense key values */
+static const char *const snstext[] = {
+ "No Sense", /* 0: There is no sense information */
+ "Recovered Error", /* 1: The last command completed successfully
+ but used error correction */
+ "Not Ready", /* 2: The addressed target is not ready */
+ "Medium Error", /* 3: Data error detected on the medium */
+ "Hardware Error", /* 4: Controller or device failure */
+ "Illegal Request", /* 5: Error in request */
+ "Unit Attention", /* 6: Removable medium was changed, or
+ the target has been reset, or ... */
+ "Data Protect", /* 7: Access to the data is blocked */
+ "Blank Check", /* 8: Reached unexpected written or unwritten
+ region of the medium */
+ "Vendor Specific",
+ "Copy Aborted", /* A: COPY or COMPARE was aborted */
+ "Aborted Command", /* B: The target aborted the command */
+ "Equal", /* C: A SEARCH DATA command found data equal */
+ "Volume Overflow", /* D: Medium full with still data to be written */
+ "Miscompare", /* E: Source data and data on the medium
+ do not agree */
+};
+
+static int send_scsi_cmd(int fd, const __u8 *cdb, void *buf,
+ __u8 cmd_len, __u32 byte_cnt, int dir, __u8 sg_type);
+
+/* Get sense key string or NULL if not available */
+static const char *sense_key_string(__u8 key)
+{
+ if (key <= 0xE)
+ return snstext[key];
+
+ return NULL;
+}
+
+static inline void put_unaligned_be24(__u32 val, void *p)
+{
+ ((__u8 *)p)[0] = (val >> 16) & 0xff;
+ ((__u8 *)p)[1] = (val >> 8) & 0xff;
+ ((__u8 *)p)[2] = val & 0xff;
+}
+
+static int write_file_with_counter(const char *pattern, const void *buffer,
+ int length)
+{
+#ifdef DEBUG
+ static int counter = 1;
+ char filename[1024] = {0};
+ sprintf(filename, pattern, counter++);
+ return write_file(filename, buffer, length);
+#else
+ return 0;
+#endif
+}
+
+int write_buffer(int fd, __u8 *buf, __u8 mode, __u8 buf_id, __u32 buf_offset,
+ int byte_count, __u8 sg_type)
+{
+ int ret;
+ unsigned char write_buf_cmd [WRITE_BUF_CMDLEN] = {
+ WRITE_BUFFER_CMD, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+ if (fd < 0 || buf == NULL || byte_count <= 0) {
+ perror("scsi write cmd: wrong parameters");
+ return -EINVAL;
+ }
+
+ write_buf_cmd[1] = mode;
+ write_buf_cmd[2] = buf_id;
+ put_unaligned_be24((uint32_t)buf_offset, write_buf_cmd + 3);
+ put_unaligned_be24(byte_count, write_buf_cmd + 6);
+ WRITE_LOG("Start : %s mode %d , buf_id %d", __func__, mode, buf_id);
+ ret = send_scsi_cmd(fd, write_buf_cmd, buf,
+ WRITE_BUF_CMDLEN, byte_count,
+ SG_DXFER_TO_DEV, sg_type);
+ if (ret < 0) {
+ print_error("SG_IO WRITE BUFFER data error ret %d", ret);
+ }
+ return ret;
+}
+
+int read_buffer(int fd, __u8 *buf, __u8 mode, __u8 buf_id,
+ __u32 buf_offset, int byte_count, __u8 sg_type)
+{
+
+ int ret;
+ unsigned char read_buf_cmd[READ_BUF_CMDLEN] = {READ_BUFFER_CMD,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+ if (fd < 0 || buf == NULL || byte_count <= 0) {
+ print_error("scsi read cmd: wrong parameters");
+ return -EINVAL;
+ }
+
+ read_buf_cmd[1] = mode;
+ read_buf_cmd[2] = buf_id;
+ put_unaligned_be24((__u32)buf_offset, read_buf_cmd + 3);
+ put_unaligned_be24((__u32)byte_count, read_buf_cmd + 6);
+ WRITE_LOG("Start : %s\n", __func__);
+ ret = send_scsi_cmd(fd, read_buf_cmd, buf,
+ READ_BUF_CMDLEN, byte_count,
+ SG_DXFER_FROM_DEV, sg_type);
+
+ if (ret < 0) {
+ print_error("SG_IO READ BUFFER data error ret %d", ret);
+ }
+
+ return ret;
+}
+
+int scsi_security_in(int fd, struct rpmb_frame *frame, int cnt, __u8 region,
+ __u8 sg_type)
+{
+ int ret;
+ __u32 trans_len = cnt * sizeof(struct rpmb_frame);
+ __u16 sec_spec = (region << 8) | SEC_SPECIFIC_UFS_RPMB;
+ unsigned char sec_in_cmd[SEC_PROTOCOL_CMD_SIZE] = {
+ SECURITY_PROTOCOL_IN, SEC_PROTOCOL_UFS,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+ WRITE_LOG("Start : %s\n", __func__);
+ if (fd < 0 || frame == NULL || cnt <= 0) {
+ print_error("scsi sec_in cmd: wrong parameters");
+ return ERROR;
+ }
+
+ *(__u16 *)(sec_in_cmd + SEC_SPEC_OFFSET) = htobe16(sec_spec);
+ *(__u32 *)(sec_in_cmd + SEC_TRANS_LEN_OFFSET) = htobe32(trans_len);
+
+ ret = send_scsi_cmd(fd, sec_in_cmd, frame, SEC_PROTOCOL_CMD_SIZE,
+ trans_len, SG_DXFER_FROM_DEV, sg_type);
+
+ return ret;
+}
+
+int scsi_security_out(int fd, struct rpmb_frame *frame_in,
+ unsigned int cnt, __u8 region, __u8 sg_type)
+{
+ int ret;
+ __u32 trans_len = cnt * sizeof(struct rpmb_frame);
+ __u16 sec_spec = (region << 8) | SEC_SPECIFIC_UFS_RPMB;
+ unsigned char sec_out_cmd[SEC_PROTOCOL_CMD_SIZE] = {
+ SECURITY_PROTOCOL_OUT, SEC_PROTOCOL_UFS,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+ if (fd < 0 || frame_in == NULL || cnt <= 0) {
+ print_error("scsi sec_out cmd: wrong parameters");
+ return ERROR;
+ }
+ *(__u16 *)(sec_out_cmd + SEC_SPEC_OFFSET) = htobe16(sec_spec);
+ *(__u32 *)(sec_out_cmd + SEC_TRANS_LEN_OFFSET) = htobe32(trans_len);
+ ret = send_scsi_cmd(fd, sec_out_cmd, frame_in,
+ SEC_PROTOCOL_CMD_SIZE, trans_len,
+ SG_DXFER_TO_DEV, sg_type);
+
+ return ret;
+}
+
+void prepare_upiu(struct ufs_bsg_request *bsg_req,
+ __u8 query_req_func, __u16 data_len,
+ __u8 opcode, __u8 idn, __u8 index, __u8 sel)
+{
+ bsg_req->msgcode = UPIU_TRANSACTION_QUERY_REQ;
+
+ /* Fill UPIU header */
+ bsg_req->upiu_req.header.dword_0 =
+ UPIU_HEADER_DWORD(UPIU_TRANSACTION_QUERY_REQ, 0, 0, 0);
+ bsg_req->upiu_req.header.dword_1 =
+ UPIU_HEADER_DWORD(0, query_req_func, 0, 0);
+ bsg_req->upiu_req.header.dword_2 =
+ UPIU_HEADER_DWORD(0, 0, data_len >> 8, (__u8)data_len);
+
+ /* Fill Transaction Specific Fields */
+ bsg_req->upiu_req.qr.opcode = opcode;
+ bsg_req->upiu_req.qr.idn = idn;
+ bsg_req->upiu_req.qr.index = index;
+ bsg_req->upiu_req.qr.selector = sel;
+ bsg_req->upiu_req.qr.length = htobe16(data_len);
+}
+
+/**
+ * send_scsi_cmd - Utility function for SCSI command sending
+ * @fd: bsg driver file descriptor
+ * @cdb: pointer to SCSI cmd cdb buffer
+ * @buf: pointer to the SCSI cmd data buffer
+ * @cmd_len: SCSI command length
+ * @byte_cnt: SCSI data length
+ * @dir: The cmd direction
+ *
+ **/
+static int send_scsi_cmd(int fd, const __u8 *cdb, void *buf, __u8 cmd_len,
+ __u32 byte_cnt, int dir, __u8 sg_type)
+{
+ int ret;
+ void *sg_struct;
+ struct sg_io_v4 io_hdr_v4 = { 0 };
+ struct sg_io_hdr io_hdr_v3 = { 0 };
+ __u8 sense_buffer[SENSE_BUFF_LEN] = { 0 };
+
+ if ((byte_cnt && buf == NULL) || cdb == NULL) {
+ print_error("send_scsi_cmd: wrong parameters");
+ return -EINVAL;
+ }
+
+ if (sg_type == SG4_TYPE) {
+ io_hdr_v4.guard = 'Q';
+ io_hdr_v4.protocol = BSG_PROTOCOL_SCSI;
+ io_hdr_v4.subprotocol = BSG_SUB_PROTOCOL_SCSI_CMD;
+ io_hdr_v4.response = (__u64)sense_buffer;
+ io_hdr_v4.max_response_len = SENSE_BUFF_LEN;
+ io_hdr_v4.request_len = cmd_len;
+ if (dir == SG_DXFER_FROM_DEV) {
+ io_hdr_v4.din_xfer_len = (__u32)byte_cnt;
+ io_hdr_v4.din_xferp = (__u64)buf;
+ } else {
+ io_hdr_v4.dout_xfer_len = (__u32)byte_cnt;
+ io_hdr_v4.dout_xferp = (__u64)buf;
+ }
+ io_hdr_v4.request = (__u64)cdb;
+ sg_struct = &io_hdr_v4;
+ }
+ else {
+ io_hdr_v3.interface_id = 'S';
+ io_hdr_v3.cmd_len = cmd_len;
+ io_hdr_v3.mx_sb_len = SENSE_BUFF_LEN;
+ io_hdr_v3.dxfer_direction = dir;
+ io_hdr_v3.dxfer_len = byte_cnt;
+ io_hdr_v3.dxferp = buf;
+ /* pointer to command buf (rbufCmdBlk) */
+ io_hdr_v3.cmdp = (unsigned char *)cdb;
+ io_hdr_v3.sbp = sense_buffer;
+ io_hdr_v3.timeout = DEF_TIMEOUT_MSEC;
+ sg_struct = &io_hdr_v3;
+ }
+ WRITE_LOG("Start : %s cmd = %x len %d sg_type %d\n", __func__, cdb[0],
+ byte_cnt, sg_type);
+
+ write_file_with_counter("scsi_cmd_cdb_%d.bin",
+ cdb, cmd_len);
+
+ while (((ret = ioctl(fd, SG_IO, sg_struct)) < 0) &&
+ ((errno == EINTR) || (errno == EAGAIN)));
+ if (sg_type == SG4_TYPE) {
+ if (io_hdr_v4.info != 0) {
+ print_error("Command fail with status %x , senseKey %s, asc 0x%02x, ascq 0x%02x",
+ io_hdr_v4.info,
+ sense_key_string(sense_buffer[2]),
+ sense_buffer[12],
+ sense_buffer[13]);
+ ret = -EINVAL;
+ }
+ }
+ else {
+ if (io_hdr_v3.status) {
+ print_error("Command fail with status %x , senseKey %s, asc 0x%02x, ascq 0x%02x",
+ io_hdr_v3.status,
+ sense_key_string(sense_buffer[2]),
+ sense_buffer[12],
+ sense_buffer[13]);
+ ret = -EINVAL;
+ }
+
+ }
+
+ return ret;
+}
+
+/**
+ * send_bsg_scsi_trs - Utility function for SCSI transport cmd sending
+ * @fd: ufs bsg driver file descriptor
+ * @request_buff: pointer to the Query Request
+ * @reply_buff: pointer to the Query Response
+ * @req_buf_len: Query Request data length
+ * @reply_buf_len: Query Response data length
+ * @data_buf: pointer to the data buffer
+ *
+ * The function using ufs bsg infrastructure in linux kernel (/dev/ufs-bsg)
+ * in order to send Query request command
+ **/
+int send_bsg_scsi_trs(int fd, struct ufs_bsg_request *request_buff,
+ struct ufs_bsg_reply *reply_buff, __u32 req_buf_len,
+ __u32 reply_buf_len, __u8 *data_buf)
+{
+ int ret;
+ struct sg_io_v4 io_hdr_v4 = { 0 };
+
+ if (request_buff == NULL || reply_buff == NULL) {
+ print_error("%s: wrong parameters", __func__);
+ return -EINVAL;
+ }
+
+ if (req_buf_len != 0 || reply_buf_len != 0) {
+ if (data_buf == NULL) {
+ print_error("%s: data_buf is NULL", __func__);
+ return -EINVAL;
+ }
+ }
+
+ io_hdr_v4.guard = 'Q';
+ io_hdr_v4.protocol = BSG_PROTOCOL_SCSI;
+ io_hdr_v4.subprotocol = BSG_SUB_PROTOCOL_SCSI_TRANSPORT;
+ io_hdr_v4.response = (__u64)reply_buff;
+ io_hdr_v4.max_response_len = BSG_REPLY_SZ;
+ io_hdr_v4.request_len = BSG_REQUEST_SZ;
+ io_hdr_v4.request = (__u64)request_buff;
+
+ if (req_buf_len > 0) {
+ /* write descriptor */
+ io_hdr_v4.dout_xferp = (__u64)(data_buf);
+ io_hdr_v4.dout_xfer_len = req_buf_len;
+ } else if (reply_buf_len > 0) {
+ /* read descriptor */
+ io_hdr_v4.din_xferp = (__u64)(data_buf);
+ io_hdr_v4.din_xfer_len = reply_buf_len;
+ }
+
+ WRITE_LOG("%s cmd = %x req_len %d , res_len %d\n", __func__,
+ request_buff->upiu_req.qr.idn, req_buf_len,
+ reply_buf_len);
+
+ write_file_with_counter("bsg_reg_%d.bin",
+ &request_buff->upiu_req,
+ sizeof(struct utp_upiu_req));
+
+
+ while (((ret = ioctl(fd, SG_IO, &io_hdr_v4)) < 0) &&
+ ((errno == EINTR) || (errno == EAGAIN)))
+ ;
+
+ if (io_hdr_v4.info != 0) {
+ print_error("Command fail with status %x ",
+ io_hdr_v4.info);
+
+ ret = -EINVAL;
+ }
+
+ write_file_with_counter("bsg_rsp_%d.bin", reply_buff,
+ BSG_REPLY_SZ);
+
+ WRITE_LOG("%s res_len %d\n", __func__,
+ reply_buff->reply_payload_rcv_len);
+ return ret;
+}
diff --git a/libmtk_bsg/scsi_bsg_util.h b/libmtk_bsg/scsi_bsg_util.h
new file mode 100644
index 0000000..fb48a33
--- /dev/null
+++ b/libmtk_bsg/scsi_bsg_util.h
@@ -0,0 +1,155 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef BSG_UTIL_H_
+#define BSG_UTIL_H_
+
+/* In case include/uapi/scsi/scsi_bsg_ufs.h is not included*/
+#ifndef SCSI_BSG_UFS_H
+/*
+ * This file intended to be included by both kernel and user space
+ */
+
+#define UFS_CDB_SIZE 16
+
+#define BUFFER_VENDOR_MODE 0x01
+#define BUFFER_DATA_MODE 0x02
+#define BUFFER_FFU_MODE 0x0E
+#define BUFFER_EHS_MODE 0x1C
+
+#define SG_DXFER_NONE -1 /* e.g. a SCSI Test Unit Ready command */
+#define SG_DXFER_TO_DEV -2 /* e.g. a SCSI WRITE command */
+#define SG_DXFER_FROM_DEV -3 /* e.g. a SCSI READ command */
+
+#define SENSE_BUFF_LEN (18)
+#define WRITE_BUF_CMDLEN 10
+#define READ_BUF_CMDLEN 10
+#define SEC_PROTOCOL_TIMEOUT_MSEC (1000)
+#define SEC_PROTOCOL_CMD_SIZE (12)
+#define SEC_PROTOCOL_UFS (0xEC)
+#define SEC_SPECIFIC_UFS_RPMB (0x0001)
+#define WRITE_BUFFER_CMD 0x3B
+#define READ_BUFFER_CMD 0x3c
+#define SECURITY_PROTOCOL_IN 0xa2
+#define SECURITY_PROTOCOL_OUT 0xb5
+
+/**
+ * struct utp_upiu_header - UPIU header structure
+ * @dword_0: UPIU header DW-0
+ * @dword_1: UPIU header DW-1
+ * @dword_2: UPIU header DW-2
+ */
+struct utp_upiu_header {
+ __be32 dword_0;
+ __be32 dword_1;
+ __be32 dword_2;
+};
+
+/**
+ * struct utp_upiu_query - upiu request buffer structure for
+ * query request.
+ * @opcode: command to perform B-0
+ * @idn: a value that indicates the particular type of data B-1
+ * @index: Index to further identify data B-2
+ * @selector: Index to further identify data B-3
+ * @reserved_osf: spec reserved field B-4,5
+ * @length: number of descriptor bytes to read/write B-6,7
+ * @value: Attribute value to be written DW-5
+ * @reserved: spec reserved DW-6,7
+ */
+struct utp_upiu_query {
+ __u8 opcode;
+ __u8 idn;
+ __u8 index;
+ __u8 selector;
+ __be16 reserved_osf;
+ __be16 length;
+ __be32 value;
+ __be32 reserved[2];
+};
+
+/**
+ * struct utp_upiu_cmd - Command UPIU structure
+ * @data_transfer_len: Data Transfer Length DW-3
+ * @cdb: Command Descriptor Block CDB DW-4 to DW-7
+ */
+struct utp_upiu_cmd {
+ __be32 exp_data_transfer_len;
+ __u8 cdb[UFS_CDB_SIZE];
+};
+
+/**
+ * struct utp_upiu_req - general upiu request structure
+ * @header:UPIU header structure DW-0 to DW-2
+ * @sc: fields structure for scsi command DW-3 to DW-7
+ * @qr: fields structure for query request DW-3 to DW-7
+ */
+struct utp_upiu_req {
+ struct utp_upiu_header header;
+ union {
+ struct utp_upiu_cmd sc;
+ struct utp_upiu_query qr;
+ struct utp_upiu_query tr;
+ /* use utp_upiu_query to host the 4 dwords of uic command */
+ struct utp_upiu_query uc;
+ };
+};
+
+/* request (CDB) structure of the sg_io_v4 */
+struct ufs_bsg_request {
+ __u32 msgcode;
+ struct utp_upiu_req upiu_req;
+};
+
+/* response (request sense data) structure of the sg_io_v4 */
+struct ufs_bsg_reply {
+ /*
+ * The completion result. Result exists in two forms:
+ * if negative, it is an -Exxx system errno value. There will
+ * be no further reply information supplied.
+ * else, it's the 4-byte scsi error result, with driver, host,
+ * msg and status fields. The per-msgcode reply structure
+ * will contain valid data.
+ */
+ __u32 result;
+
+ /* If there was reply_payload, how much was received? */
+ __u32 reply_payload_rcv_len;
+
+ struct utp_upiu_req upiu_rsp;
+};
+#endif /* SCSI_BSG_UFS_H.*/
+
+struct rpmb_frame {
+ __u8 stuff[196];
+ __u8 key_mac[32];
+ __u8 data[256];
+ __u8 nonce[16];
+ __u32 write_counter;
+ __u16 addr;
+ __u16 block_count;
+ __u16 result;
+ __u16 req_resp;
+};
+
+#define BSG_REPLY_SZ (sizeof(struct ufs_bsg_reply))
+#define BSG_REQUEST_SZ (sizeof(struct ufs_bsg_request))
+
+int send_bsg_scsi_trs(int fd, struct ufs_bsg_request *request_buff,
+ struct ufs_bsg_reply *reply_buff, __u32 request_len,
+ __u32 reply_len, __u8 *data_buf);
+void prepare_upiu(struct ufs_bsg_request *bsg_req, __u8 query_req_func,
+ __u16 data_len, __u8 opcode, __u8 idn, __u8 index, __u8 sel);
+int read_buffer(int fd, __u8 *buf, uint8_t mode, __u8 buf_id,
+ __u32 buf_offset, int byte_count, __u8 sg_type);
+int write_buffer(int fd, __u8 *buf, __u8 mode, __u8 buf_id, __u32 buf_offset,
+ int byte_count, __u8 sg_type);
+int scsi_security_out(int fd, struct rpmb_frame *frame_in,
+ unsigned int cnt, __u8 region, __u8 sg_type);
+int scsi_security_in(int fd, struct rpmb_frame *frame, int cnt,
+ __u8 region, __u8 sg_type);
+#endif /* BSG_UTIL_H_ */
+
diff --git a/libmtk_bsg/sha2.c b/libmtk_bsg/sha2.c
new file mode 100644
index 0000000..0fe4c37
--- /dev/null
+++ b/libmtk_bsg/sha2.c
@@ -0,0 +1,949 @@
+/*
+ * FIPS 180-2 SHA-224/256/384/512 implementation
+ * Last update: 02/02/2007
+ * Issue date: 04/30/2005
+ *
+ * Copyright (C) 2005, 2007 Olivier Gay <olivier.gay@a3.epfl.ch>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the project nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#if 0
+#define UNROLL_LOOPS /* Enable loops unrolling */
+#endif
+
+#include <string.h>
+
+#include "sha2.h"
+
+#define SHFR(x, n) (x >> n)
+#define ROTR(x, n) ((x >> n) | (x << ((sizeof(x) << 3) - n)))
+#define ROTL(x, n) ((x << n) | (x >> ((sizeof(x) << 3) - n)))
+#define CH(x, y, z) ((x & y) ^ (~x & z))
+#define MAJ(x, y, z) ((x & y) ^ (x & z) ^ (y & z))
+
+#define SHA256_F1(x) (ROTR(x, 2) ^ ROTR(x, 13) ^ ROTR(x, 22))
+#define SHA256_F2(x) (ROTR(x, 6) ^ ROTR(x, 11) ^ ROTR(x, 25))
+#define SHA256_F3(x) (ROTR(x, 7) ^ ROTR(x, 18) ^ SHFR(x, 3))
+#define SHA256_F4(x) (ROTR(x, 17) ^ ROTR(x, 19) ^ SHFR(x, 10))
+
+#define SHA512_F1(x) (ROTR(x, 28) ^ ROTR(x, 34) ^ ROTR(x, 39))
+#define SHA512_F2(x) (ROTR(x, 14) ^ ROTR(x, 18) ^ ROTR(x, 41))
+#define SHA512_F3(x) (ROTR(x, 1) ^ ROTR(x, 8) ^ SHFR(x, 7))
+#define SHA512_F4(x) (ROTR(x, 19) ^ ROTR(x, 61) ^ SHFR(x, 6))
+
+#define UNPACK32(x, str) \
+{ \
+ *((str) + 3) = (uint8) ((x) ); \
+ *((str) + 2) = (uint8) ((x) >> 8); \
+ *((str) + 1) = (uint8) ((x) >> 16); \
+ *((str) + 0) = (uint8) ((x) >> 24); \
+}
+
+#define PACK32(str, x) \
+{ \
+ *(x) = ((uint32) *((str) + 3) ) \
+ | ((uint32) *((str) + 2) << 8) \
+ | ((uint32) *((str) + 1) << 16) \
+ | ((uint32) *((str) + 0) << 24); \
+}
+
+#define UNPACK64(x, str) \
+{ \
+ *((str) + 7) = (uint8) ((x) ); \
+ *((str) + 6) = (uint8) ((x) >> 8); \
+ *((str) + 5) = (uint8) ((x) >> 16); \
+ *((str) + 4) = (uint8) ((x) >> 24); \
+ *((str) + 3) = (uint8) ((x) >> 32); \
+ *((str) + 2) = (uint8) ((x) >> 40); \
+ *((str) + 1) = (uint8) ((x) >> 48); \
+ *((str) + 0) = (uint8) ((x) >> 56); \
+}
+
+#define PACK64(str, x) \
+{ \
+ *(x) = ((uint64) *((str) + 7) ) \
+ | ((uint64) *((str) + 6) << 8) \
+ | ((uint64) *((str) + 5) << 16) \
+ | ((uint64) *((str) + 4) << 24) \
+ | ((uint64) *((str) + 3) << 32) \
+ | ((uint64) *((str) + 2) << 40) \
+ | ((uint64) *((str) + 1) << 48) \
+ | ((uint64) *((str) + 0) << 56); \
+}
+
+/* Macros used for loops unrolling */
+
+#define SHA256_SCR(i) \
+{ \
+ w[i] = SHA256_F4(w[i - 2]) + w[i - 7] \
+ + SHA256_F3(w[i - 15]) + w[i - 16]; \
+}
+
+#define SHA512_SCR(i) \
+{ \
+ w[i] = SHA512_F4(w[i - 2]) + w[i - 7] \
+ + SHA512_F3(w[i - 15]) + w[i - 16]; \
+}
+
+#define SHA256_EXP(a, b, c, d, e, f, g, h, j) \
+{ \
+ t1 = wv[h] + SHA256_F2(wv[e]) + CH(wv[e], wv[f], wv[g]) \
+ + sha256_k[j] + w[j]; \
+ t2 = SHA256_F1(wv[a]) + MAJ(wv[a], wv[b], wv[c]); \
+ wv[d] += t1; \
+ wv[h] = t1 + t2; \
+}
+
+#define SHA512_EXP(a, b, c, d, e, f, g ,h, j) \
+{ \
+ t1 = wv[h] + SHA512_F2(wv[e]) + CH(wv[e], wv[f], wv[g]) \
+ + sha512_k[j] + w[j]; \
+ t2 = SHA512_F1(wv[a]) + MAJ(wv[a], wv[b], wv[c]); \
+ wv[d] += t1; \
+ wv[h] = t1 + t2; \
+}
+
+uint32 sha224_h0[8] =
+ {0xc1059ed8, 0x367cd507, 0x3070dd17, 0xf70e5939,
+ 0xffc00b31, 0x68581511, 0x64f98fa7, 0xbefa4fa4};
+
+uint32 sha256_h0[8] =
+ {0x6a09e667, 0xbb67ae85, 0x3c6ef372, 0xa54ff53a,
+ 0x510e527f, 0x9b05688c, 0x1f83d9ab, 0x5be0cd19};
+
+uint64 sha384_h0[8] =
+ {0xcbbb9d5dc1059ed8ULL, 0x629a292a367cd507ULL,
+ 0x9159015a3070dd17ULL, 0x152fecd8f70e5939ULL,
+ 0x67332667ffc00b31ULL, 0x8eb44a8768581511ULL,
+ 0xdb0c2e0d64f98fa7ULL, 0x47b5481dbefa4fa4ULL};
+
+uint64 sha512_h0[8] =
+ {0x6a09e667f3bcc908ULL, 0xbb67ae8584caa73bULL,
+ 0x3c6ef372fe94f82bULL, 0xa54ff53a5f1d36f1ULL,
+ 0x510e527fade682d1ULL, 0x9b05688c2b3e6c1fULL,
+ 0x1f83d9abfb41bd6bULL, 0x5be0cd19137e2179ULL};
+
+uint32 sha256_k[64] =
+ {0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5,
+ 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5,
+ 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3,
+ 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174,
+ 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc,
+ 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da,
+ 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7,
+ 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967,
+ 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13,
+ 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85,
+ 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3,
+ 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070,
+ 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5,
+ 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3,
+ 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208,
+ 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2};
+
+uint64 sha512_k[80] =
+ {0x428a2f98d728ae22ULL, 0x7137449123ef65cdULL,
+ 0xb5c0fbcfec4d3b2fULL, 0xe9b5dba58189dbbcULL,
+ 0x3956c25bf348b538ULL, 0x59f111f1b605d019ULL,
+ 0x923f82a4af194f9bULL, 0xab1c5ed5da6d8118ULL,
+ 0xd807aa98a3030242ULL, 0x12835b0145706fbeULL,
+ 0x243185be4ee4b28cULL, 0x550c7dc3d5ffb4e2ULL,
+ 0x72be5d74f27b896fULL, 0x80deb1fe3b1696b1ULL,
+ 0x9bdc06a725c71235ULL, 0xc19bf174cf692694ULL,
+ 0xe49b69c19ef14ad2ULL, 0xefbe4786384f25e3ULL,
+ 0x0fc19dc68b8cd5b5ULL, 0x240ca1cc77ac9c65ULL,
+ 0x2de92c6f592b0275ULL, 0x4a7484aa6ea6e483ULL,
+ 0x5cb0a9dcbd41fbd4ULL, 0x76f988da831153b5ULL,
+ 0x983e5152ee66dfabULL, 0xa831c66d2db43210ULL,
+ 0xb00327c898fb213fULL, 0xbf597fc7beef0ee4ULL,
+ 0xc6e00bf33da88fc2ULL, 0xd5a79147930aa725ULL,
+ 0x06ca6351e003826fULL, 0x142929670a0e6e70ULL,
+ 0x27b70a8546d22ffcULL, 0x2e1b21385c26c926ULL,
+ 0x4d2c6dfc5ac42aedULL, 0x53380d139d95b3dfULL,
+ 0x650a73548baf63deULL, 0x766a0abb3c77b2a8ULL,
+ 0x81c2c92e47edaee6ULL, 0x92722c851482353bULL,
+ 0xa2bfe8a14cf10364ULL, 0xa81a664bbc423001ULL,
+ 0xc24b8b70d0f89791ULL, 0xc76c51a30654be30ULL,
+ 0xd192e819d6ef5218ULL, 0xd69906245565a910ULL,
+ 0xf40e35855771202aULL, 0x106aa07032bbd1b8ULL,
+ 0x19a4c116b8d2d0c8ULL, 0x1e376c085141ab53ULL,
+ 0x2748774cdf8eeb99ULL, 0x34b0bcb5e19b48a8ULL,
+ 0x391c0cb3c5c95a63ULL, 0x4ed8aa4ae3418acbULL,
+ 0x5b9cca4f7763e373ULL, 0x682e6ff3d6b2b8a3ULL,
+ 0x748f82ee5defb2fcULL, 0x78a5636f43172f60ULL,
+ 0x84c87814a1f0ab72ULL, 0x8cc702081a6439ecULL,
+ 0x90befffa23631e28ULL, 0xa4506cebde82bde9ULL,
+ 0xbef9a3f7b2c67915ULL, 0xc67178f2e372532bULL,
+ 0xca273eceea26619cULL, 0xd186b8c721c0c207ULL,
+ 0xeada7dd6cde0eb1eULL, 0xf57d4f7fee6ed178ULL,
+ 0x06f067aa72176fbaULL, 0x0a637dc5a2c898a6ULL,
+ 0x113f9804bef90daeULL, 0x1b710b35131c471bULL,
+ 0x28db77f523047d84ULL, 0x32caab7b40c72493ULL,
+ 0x3c9ebe0a15c9bebcULL, 0x431d67c49c100d4cULL,
+ 0x4cc5d4becb3e42b6ULL, 0x597f299cfc657e2aULL,
+ 0x5fcb6fab3ad6faecULL, 0x6c44198c4a475817ULL};
+
+/* SHA-256 functions */
+
+void sha256_transf(sha256_ctx *ctx, const unsigned char *message,
+ unsigned int block_nb)
+{
+ uint32 w[64];
+ uint32 wv[8];
+ uint32 t1, t2;
+ const unsigned char *sub_block;
+ int i;
+
+#ifndef UNROLL_LOOPS
+ int j;
+#endif
+
+ for (i = 0; i < (int) block_nb; i++) {
+ sub_block = message + (i << 6);
+
+#ifndef UNROLL_LOOPS
+ for (j = 0; j < 16; j++) {
+ PACK32(&sub_block[j << 2], &w[j]);
+ }
+
+ for (j = 16; j < 64; j++) {
+ SHA256_SCR(j);
+ }
+
+ for (j = 0; j < 8; j++) {
+ wv[j] = ctx->h[j];
+ }
+
+ for (j = 0; j < 64; j++) {
+ t1 = wv[7] + SHA256_F2(wv[4]) + CH(wv[4], wv[5], wv[6])
+ + sha256_k[j] + w[j];
+ t2 = SHA256_F1(wv[0]) + MAJ(wv[0], wv[1], wv[2]);
+ wv[7] = wv[6];
+ wv[6] = wv[5];
+ wv[5] = wv[4];
+ wv[4] = wv[3] + t1;
+ wv[3] = wv[2];
+ wv[2] = wv[1];
+ wv[1] = wv[0];
+ wv[0] = t1 + t2;
+ }
+
+ for (j = 0; j < 8; j++) {
+ ctx->h[j] += wv[j];
+ }
+#else
+ PACK32(&sub_block[ 0], &w[ 0]); PACK32(&sub_block[ 4], &w[ 1]);
+ PACK32(&sub_block[ 8], &w[ 2]); PACK32(&sub_block[12], &w[ 3]);
+ PACK32(&sub_block[16], &w[ 4]); PACK32(&sub_block[20], &w[ 5]);
+ PACK32(&sub_block[24], &w[ 6]); PACK32(&sub_block[28], &w[ 7]);
+ PACK32(&sub_block[32], &w[ 8]); PACK32(&sub_block[36], &w[ 9]);
+ PACK32(&sub_block[40], &w[10]); PACK32(&sub_block[44], &w[11]);
+ PACK32(&sub_block[48], &w[12]); PACK32(&sub_block[52], &w[13]);
+ PACK32(&sub_block[56], &w[14]); PACK32(&sub_block[60], &w[15]);
+
+ SHA256_SCR(16); SHA256_SCR(17); SHA256_SCR(18); SHA256_SCR(19);
+ SHA256_SCR(20); SHA256_SCR(21); SHA256_SCR(22); SHA256_SCR(23);
+ SHA256_SCR(24); SHA256_SCR(25); SHA256_SCR(26); SHA256_SCR(27);
+ SHA256_SCR(28); SHA256_SCR(29); SHA256_SCR(30); SHA256_SCR(31);
+ SHA256_SCR(32); SHA256_SCR(33); SHA256_SCR(34); SHA256_SCR(35);
+ SHA256_SCR(36); SHA256_SCR(37); SHA256_SCR(38); SHA256_SCR(39);
+ SHA256_SCR(40); SHA256_SCR(41); SHA256_SCR(42); SHA256_SCR(43);
+ SHA256_SCR(44); SHA256_SCR(45); SHA256_SCR(46); SHA256_SCR(47);
+ SHA256_SCR(48); SHA256_SCR(49); SHA256_SCR(50); SHA256_SCR(51);
+ SHA256_SCR(52); SHA256_SCR(53); SHA256_SCR(54); SHA256_SCR(55);
+ SHA256_SCR(56); SHA256_SCR(57); SHA256_SCR(58); SHA256_SCR(59);
+ SHA256_SCR(60); SHA256_SCR(61); SHA256_SCR(62); SHA256_SCR(63);
+
+ wv[0] = ctx->h[0]; wv[1] = ctx->h[1];
+ wv[2] = ctx->h[2]; wv[3] = ctx->h[3];
+ wv[4] = ctx->h[4]; wv[5] = ctx->h[5];
+ wv[6] = ctx->h[6]; wv[7] = ctx->h[7];
+
+ SHA256_EXP(0,1,2,3,4,5,6,7, 0); SHA256_EXP(7,0,1,2,3,4,5,6, 1);
+ SHA256_EXP(6,7,0,1,2,3,4,5, 2); SHA256_EXP(5,6,7,0,1,2,3,4, 3);
+ SHA256_EXP(4,5,6,7,0,1,2,3, 4); SHA256_EXP(3,4,5,6,7,0,1,2, 5);
+ SHA256_EXP(2,3,4,5,6,7,0,1, 6); SHA256_EXP(1,2,3,4,5,6,7,0, 7);
+ SHA256_EXP(0,1,2,3,4,5,6,7, 8); SHA256_EXP(7,0,1,2,3,4,5,6, 9);
+ SHA256_EXP(6,7,0,1,2,3,4,5,10); SHA256_EXP(5,6,7,0,1,2,3,4,11);
+ SHA256_EXP(4,5,6,7,0,1,2,3,12); SHA256_EXP(3,4,5,6,7,0,1,2,13);
+ SHA256_EXP(2,3,4,5,6,7,0,1,14); SHA256_EXP(1,2,3,4,5,6,7,0,15);
+ SHA256_EXP(0,1,2,3,4,5,6,7,16); SHA256_EXP(7,0,1,2,3,4,5,6,17);
+ SHA256_EXP(6,7,0,1,2,3,4,5,18); SHA256_EXP(5,6,7,0,1,2,3,4,19);
+ SHA256_EXP(4,5,6,7,0,1,2,3,20); SHA256_EXP(3,4,5,6,7,0,1,2,21);
+ SHA256_EXP(2,3,4,5,6,7,0,1,22); SHA256_EXP(1,2,3,4,5,6,7,0,23);
+ SHA256_EXP(0,1,2,3,4,5,6,7,24); SHA256_EXP(7,0,1,2,3,4,5,6,25);
+ SHA256_EXP(6,7,0,1,2,3,4,5,26); SHA256_EXP(5,6,7,0,1,2,3,4,27);
+ SHA256_EXP(4,5,6,7,0,1,2,3,28); SHA256_EXP(3,4,5,6,7,0,1,2,29);
+ SHA256_EXP(2,3,4,5,6,7,0,1,30); SHA256_EXP(1,2,3,4,5,6,7,0,31);
+ SHA256_EXP(0,1,2,3,4,5,6,7,32); SHA256_EXP(7,0,1,2,3,4,5,6,33);
+ SHA256_EXP(6,7,0,1,2,3,4,5,34); SHA256_EXP(5,6,7,0,1,2,3,4,35);
+ SHA256_EXP(4,5,6,7,0,1,2,3,36); SHA256_EXP(3,4,5,6,7,0,1,2,37);
+ SHA256_EXP(2,3,4,5,6,7,0,1,38); SHA256_EXP(1,2,3,4,5,6,7,0,39);
+ SHA256_EXP(0,1,2,3,4,5,6,7,40); SHA256_EXP(7,0,1,2,3,4,5,6,41);
+ SHA256_EXP(6,7,0,1,2,3,4,5,42); SHA256_EXP(5,6,7,0,1,2,3,4,43);
+ SHA256_EXP(4,5,6,7,0,1,2,3,44); SHA256_EXP(3,4,5,6,7,0,1,2,45);
+ SHA256_EXP(2,3,4,5,6,7,0,1,46); SHA256_EXP(1,2,3,4,5,6,7,0,47);
+ SHA256_EXP(0,1,2,3,4,5,6,7,48); SHA256_EXP(7,0,1,2,3,4,5,6,49);
+ SHA256_EXP(6,7,0,1,2,3,4,5,50); SHA256_EXP(5,6,7,0,1,2,3,4,51);
+ SHA256_EXP(4,5,6,7,0,1,2,3,52); SHA256_EXP(3,4,5,6,7,0,1,2,53);
+ SHA256_EXP(2,3,4,5,6,7,0,1,54); SHA256_EXP(1,2,3,4,5,6,7,0,55);
+ SHA256_EXP(0,1,2,3,4,5,6,7,56); SHA256_EXP(7,0,1,2,3,4,5,6,57);
+ SHA256_EXP(6,7,0,1,2,3,4,5,58); SHA256_EXP(5,6,7,0,1,2,3,4,59);
+ SHA256_EXP(4,5,6,7,0,1,2,3,60); SHA256_EXP(3,4,5,6,7,0,1,2,61);
+ SHA256_EXP(2,3,4,5,6,7,0,1,62); SHA256_EXP(1,2,3,4,5,6,7,0,63);
+
+ ctx->h[0] += wv[0]; ctx->h[1] += wv[1];
+ ctx->h[2] += wv[2]; ctx->h[3] += wv[3];
+ ctx->h[4] += wv[4]; ctx->h[5] += wv[5];
+ ctx->h[6] += wv[6]; ctx->h[7] += wv[7];
+#endif /* !UNROLL_LOOPS */
+ }
+}
+
+void sha256(const unsigned char *message, unsigned int len, unsigned char *digest)
+{
+ sha256_ctx ctx;
+
+ sha256_init(&ctx);
+ sha256_update(&ctx, message, len);
+ sha256_final(&ctx, digest);
+}
+
+void sha256_init(sha256_ctx *ctx)
+{
+#ifndef UNROLL_LOOPS
+ int i;
+ for (i = 0; i < 8; i++) {
+ ctx->h[i] = sha256_h0[i];
+ }
+#else
+ ctx->h[0] = sha256_h0[0]; ctx->h[1] = sha256_h0[1];
+ ctx->h[2] = sha256_h0[2]; ctx->h[3] = sha256_h0[3];
+ ctx->h[4] = sha256_h0[4]; ctx->h[5] = sha256_h0[5];
+ ctx->h[6] = sha256_h0[6]; ctx->h[7] = sha256_h0[7];
+#endif /* !UNROLL_LOOPS */
+
+ ctx->len = 0;
+ ctx->tot_len = 0;
+}
+
+void sha256_update(sha256_ctx *ctx, const unsigned char *message,
+ unsigned int len)
+{
+ unsigned int block_nb;
+ unsigned int new_len, rem_len, tmp_len;
+ const unsigned char *shifted_message;
+
+ tmp_len = SHA256_BLOCK_SIZE - ctx->len;
+ rem_len = len < tmp_len ? len : tmp_len;
+
+ memcpy(&ctx->block[ctx->len], message, rem_len);
+
+ if (ctx->len + len < SHA256_BLOCK_SIZE) {
+ ctx->len += len;
+ return;
+ }
+
+ new_len = len - rem_len;
+ block_nb = new_len / SHA256_BLOCK_SIZE;
+
+ shifted_message = message + rem_len;
+
+ sha256_transf(ctx, ctx->block, 1);
+ sha256_transf(ctx, shifted_message, block_nb);
+
+ rem_len = new_len % SHA256_BLOCK_SIZE;
+
+ memcpy(ctx->block, &shifted_message[block_nb << 6],
+ rem_len);
+
+ ctx->len = rem_len;
+ ctx->tot_len += (block_nb + 1) << 6;
+}
+
+void sha256_final(sha256_ctx *ctx, unsigned char *digest)
+{
+ unsigned int block_nb;
+ unsigned int pm_len;
+ unsigned int len_b;
+
+#ifndef UNROLL_LOOPS
+ int i;
+#endif
+
+ block_nb = (1 + ((SHA256_BLOCK_SIZE - 9)
+ < (ctx->len % SHA256_BLOCK_SIZE)));
+
+ len_b = (ctx->tot_len + ctx->len) << 3;
+ pm_len = block_nb << 6;
+
+ memset(ctx->block + ctx->len, 0, pm_len - ctx->len);
+ ctx->block[ctx->len] = 0x80;
+ UNPACK32(len_b, ctx->block + pm_len - 4);
+
+ sha256_transf(ctx, ctx->block, block_nb);
+
+#ifndef UNROLL_LOOPS
+ for (i = 0 ; i < 8; i++) {
+ UNPACK32(ctx->h[i], &digest[i << 2]);
+ }
+#else
+ UNPACK32(ctx->h[0], &digest[ 0]);
+ UNPACK32(ctx->h[1], &digest[ 4]);
+ UNPACK32(ctx->h[2], &digest[ 8]);
+ UNPACK32(ctx->h[3], &digest[12]);
+ UNPACK32(ctx->h[4], &digest[16]);
+ UNPACK32(ctx->h[5], &digest[20]);
+ UNPACK32(ctx->h[6], &digest[24]);
+ UNPACK32(ctx->h[7], &digest[28]);
+#endif /* !UNROLL_LOOPS */
+}
+
+/* SHA-512 functions */
+
+void sha512_transf(sha512_ctx *ctx, const unsigned char *message,
+ unsigned int block_nb)
+{
+ uint64 w[80];
+ uint64 wv[8];
+ uint64 t1, t2;
+ const unsigned char *sub_block;
+ int i, j;
+
+ for (i = 0; i < (int) block_nb; i++) {
+ sub_block = message + (i << 7);
+
+#ifndef UNROLL_LOOPS
+ for (j = 0; j < 16; j++) {
+ PACK64(&sub_block[j << 3], &w[j]);
+ }
+
+ for (j = 16; j < 80; j++) {
+ SHA512_SCR(j);
+ }
+
+ for (j = 0; j < 8; j++) {
+ wv[j] = ctx->h[j];
+ }
+
+ for (j = 0; j < 80; j++) {
+ t1 = wv[7] + SHA512_F2(wv[4]) + CH(wv[4], wv[5], wv[6])
+ + sha512_k[j] + w[j];
+ t2 = SHA512_F1(wv[0]) + MAJ(wv[0], wv[1], wv[2]);
+ wv[7] = wv[6];
+ wv[6] = wv[5];
+ wv[5] = wv[4];
+ wv[4] = wv[3] + t1;
+ wv[3] = wv[2];
+ wv[2] = wv[1];
+ wv[1] = wv[0];
+ wv[0] = t1 + t2;
+ }
+
+ for (j = 0; j < 8; j++) {
+ ctx->h[j] += wv[j];
+ }
+#else
+ PACK64(&sub_block[ 0], &w[ 0]); PACK64(&sub_block[ 8], &w[ 1]);
+ PACK64(&sub_block[ 16], &w[ 2]); PACK64(&sub_block[ 24], &w[ 3]);
+ PACK64(&sub_block[ 32], &w[ 4]); PACK64(&sub_block[ 40], &w[ 5]);
+ PACK64(&sub_block[ 48], &w[ 6]); PACK64(&sub_block[ 56], &w[ 7]);
+ PACK64(&sub_block[ 64], &w[ 8]); PACK64(&sub_block[ 72], &w[ 9]);
+ PACK64(&sub_block[ 80], &w[10]); PACK64(&sub_block[ 88], &w[11]);
+ PACK64(&sub_block[ 96], &w[12]); PACK64(&sub_block[104], &w[13]);
+ PACK64(&sub_block[112], &w[14]); PACK64(&sub_block[120], &w[15]);
+
+ SHA512_SCR(16); SHA512_SCR(17); SHA512_SCR(18); SHA512_SCR(19);
+ SHA512_SCR(20); SHA512_SCR(21); SHA512_SCR(22); SHA512_SCR(23);
+ SHA512_SCR(24); SHA512_SCR(25); SHA512_SCR(26); SHA512_SCR(27);
+ SHA512_SCR(28); SHA512_SCR(29); SHA512_SCR(30); SHA512_SCR(31);
+ SHA512_SCR(32); SHA512_SCR(33); SHA512_SCR(34); SHA512_SCR(35);
+ SHA512_SCR(36); SHA512_SCR(37); SHA512_SCR(38); SHA512_SCR(39);
+ SHA512_SCR(40); SHA512_SCR(41); SHA512_SCR(42); SHA512_SCR(43);
+ SHA512_SCR(44); SHA512_SCR(45); SHA512_SCR(46); SHA512_SCR(47);
+ SHA512_SCR(48); SHA512_SCR(49); SHA512_SCR(50); SHA512_SCR(51);
+ SHA512_SCR(52); SHA512_SCR(53); SHA512_SCR(54); SHA512_SCR(55);
+ SHA512_SCR(56); SHA512_SCR(57); SHA512_SCR(58); SHA512_SCR(59);
+ SHA512_SCR(60); SHA512_SCR(61); SHA512_SCR(62); SHA512_SCR(63);
+ SHA512_SCR(64); SHA512_SCR(65); SHA512_SCR(66); SHA512_SCR(67);
+ SHA512_SCR(68); SHA512_SCR(69); SHA512_SCR(70); SHA512_SCR(71);
+ SHA512_SCR(72); SHA512_SCR(73); SHA512_SCR(74); SHA512_SCR(75);
+ SHA512_SCR(76); SHA512_SCR(77); SHA512_SCR(78); SHA512_SCR(79);
+
+ wv[0] = ctx->h[0]; wv[1] = ctx->h[1];
+ wv[2] = ctx->h[2]; wv[3] = ctx->h[3];
+ wv[4] = ctx->h[4]; wv[5] = ctx->h[5];
+ wv[6] = ctx->h[6]; wv[7] = ctx->h[7];
+
+ j = 0;
+
+ do {
+ SHA512_EXP(0,1,2,3,4,5,6,7,j); j++;
+ SHA512_EXP(7,0,1,2,3,4,5,6,j); j++;
+ SHA512_EXP(6,7,0,1,2,3,4,5,j); j++;
+ SHA512_EXP(5,6,7,0,1,2,3,4,j); j++;
+ SHA512_EXP(4,5,6,7,0,1,2,3,j); j++;
+ SHA512_EXP(3,4,5,6,7,0,1,2,j); j++;
+ SHA512_EXP(2,3,4,5,6,7,0,1,j); j++;
+ SHA512_EXP(1,2,3,4,5,6,7,0,j); j++;
+ } while (j < 80);
+
+ ctx->h[0] += wv[0]; ctx->h[1] += wv[1];
+ ctx->h[2] += wv[2]; ctx->h[3] += wv[3];
+ ctx->h[4] += wv[4]; ctx->h[5] += wv[5];
+ ctx->h[6] += wv[6]; ctx->h[7] += wv[7];
+#endif /* !UNROLL_LOOPS */
+ }
+}
+
+void sha512(const unsigned char *message, unsigned int len,
+ unsigned char *digest)
+{
+ sha512_ctx ctx;
+
+ sha512_init(&ctx);
+ sha512_update(&ctx, message, len);
+ sha512_final(&ctx, digest);
+}
+
+void sha512_init(sha512_ctx *ctx)
+{
+#ifndef UNROLL_LOOPS
+ int i;
+ for (i = 0; i < 8; i++) {
+ ctx->h[i] = sha512_h0[i];
+ }
+#else
+ ctx->h[0] = sha512_h0[0]; ctx->h[1] = sha512_h0[1];
+ ctx->h[2] = sha512_h0[2]; ctx->h[3] = sha512_h0[3];
+ ctx->h[4] = sha512_h0[4]; ctx->h[5] = sha512_h0[5];
+ ctx->h[6] = sha512_h0[6]; ctx->h[7] = sha512_h0[7];
+#endif /* !UNROLL_LOOPS */
+
+ ctx->len = 0;
+ ctx->tot_len = 0;
+}
+
+void sha512_update(sha512_ctx *ctx, const unsigned char *message,
+ unsigned int len)
+{
+ unsigned int block_nb;
+ unsigned int new_len, rem_len, tmp_len;
+ const unsigned char *shifted_message;
+
+ tmp_len = SHA512_BLOCK_SIZE - ctx->len;
+ rem_len = len < tmp_len ? len : tmp_len;
+
+ memcpy(&ctx->block[ctx->len], message, rem_len);
+
+ if (ctx->len + len < SHA512_BLOCK_SIZE) {
+ ctx->len += len;
+ return;
+ }
+
+ new_len = len - rem_len;
+ block_nb = new_len / SHA512_BLOCK_SIZE;
+
+ shifted_message = message + rem_len;
+
+ sha512_transf(ctx, ctx->block, 1);
+ sha512_transf(ctx, shifted_message, block_nb);
+
+ rem_len = new_len % SHA512_BLOCK_SIZE;
+
+ memcpy(ctx->block, &shifted_message[block_nb << 7],
+ rem_len);
+
+ ctx->len = rem_len;
+ ctx->tot_len += (block_nb + 1) << 7;
+}
+
+void sha512_final(sha512_ctx *ctx, unsigned char *digest)
+{
+ unsigned int block_nb;
+ unsigned int pm_len;
+ unsigned int len_b;
+
+#ifndef UNROLL_LOOPS
+ int i;
+#endif
+
+ block_nb = 1 + ((SHA512_BLOCK_SIZE - 17)
+ < (ctx->len % SHA512_BLOCK_SIZE));
+
+ len_b = (ctx->tot_len + ctx->len) << 3;
+ pm_len = block_nb << 7;
+
+ memset(ctx->block + ctx->len, 0, pm_len - ctx->len);
+ ctx->block[ctx->len] = 0x80;
+ UNPACK32(len_b, ctx->block + pm_len - 4);
+
+ sha512_transf(ctx, ctx->block, block_nb);
+
+#ifndef UNROLL_LOOPS
+ for (i = 0 ; i < 8; i++) {
+ UNPACK64(ctx->h[i], &digest[i << 3]);
+ }
+#else
+ UNPACK64(ctx->h[0], &digest[ 0]);
+ UNPACK64(ctx->h[1], &digest[ 8]);
+ UNPACK64(ctx->h[2], &digest[16]);
+ UNPACK64(ctx->h[3], &digest[24]);
+ UNPACK64(ctx->h[4], &digest[32]);
+ UNPACK64(ctx->h[5], &digest[40]);
+ UNPACK64(ctx->h[6], &digest[48]);
+ UNPACK64(ctx->h[7], &digest[56]);
+#endif /* !UNROLL_LOOPS */
+}
+
+/* SHA-384 functions */
+
+void sha384(const unsigned char *message, unsigned int len,
+ unsigned char *digest)
+{
+ sha384_ctx ctx;
+
+ sha384_init(&ctx);
+ sha384_update(&ctx, message, len);
+ sha384_final(&ctx, digest);
+}
+
+void sha384_init(sha384_ctx *ctx)
+{
+#ifndef UNROLL_LOOPS
+ int i;
+ for (i = 0; i < 8; i++) {
+ ctx->h[i] = sha384_h0[i];
+ }
+#else
+ ctx->h[0] = sha384_h0[0]; ctx->h[1] = sha384_h0[1];
+ ctx->h[2] = sha384_h0[2]; ctx->h[3] = sha384_h0[3];
+ ctx->h[4] = sha384_h0[4]; ctx->h[5] = sha384_h0[5];
+ ctx->h[6] = sha384_h0[6]; ctx->h[7] = sha384_h0[7];
+#endif /* !UNROLL_LOOPS */
+
+ ctx->len = 0;
+ ctx->tot_len = 0;
+}
+
+void sha384_update(sha384_ctx *ctx, const unsigned char *message,
+ unsigned int len)
+{
+ unsigned int block_nb;
+ unsigned int new_len, rem_len, tmp_len;
+ const unsigned char *shifted_message;
+
+ tmp_len = SHA384_BLOCK_SIZE - ctx->len;
+ rem_len = len < tmp_len ? len : tmp_len;
+
+ memcpy(&ctx->block[ctx->len], message, rem_len);
+
+ if (ctx->len + len < SHA384_BLOCK_SIZE) {
+ ctx->len += len;
+ return;
+ }
+
+ new_len = len - rem_len;
+ block_nb = new_len / SHA384_BLOCK_SIZE;
+
+ shifted_message = message + rem_len;
+
+ sha512_transf(ctx, ctx->block, 1);
+ sha512_transf(ctx, shifted_message, block_nb);
+
+ rem_len = new_len % SHA384_BLOCK_SIZE;
+
+ memcpy(ctx->block, &shifted_message[block_nb << 7],
+ rem_len);
+
+ ctx->len = rem_len;
+ ctx->tot_len += (block_nb + 1) << 7;
+}
+
+void sha384_final(sha384_ctx *ctx, unsigned char *digest)
+{
+ unsigned int block_nb;
+ unsigned int pm_len;
+ unsigned int len_b;
+
+#ifndef UNROLL_LOOPS
+ int i;
+#endif
+
+ block_nb = (1 + ((SHA384_BLOCK_SIZE - 17)
+ < (ctx->len % SHA384_BLOCK_SIZE)));
+
+ len_b = (ctx->tot_len + ctx->len) << 3;
+ pm_len = block_nb << 7;
+
+ memset(ctx->block + ctx->len, 0, pm_len - ctx->len);
+ ctx->block[ctx->len] = 0x80;
+ UNPACK32(len_b, ctx->block + pm_len - 4);
+
+ sha512_transf(ctx, ctx->block, block_nb);
+
+#ifndef UNROLL_LOOPS
+ for (i = 0 ; i < 6; i++) {
+ UNPACK64(ctx->h[i], &digest[i << 3]);
+ }
+#else
+ UNPACK64(ctx->h[0], &digest[ 0]);
+ UNPACK64(ctx->h[1], &digest[ 8]);
+ UNPACK64(ctx->h[2], &digest[16]);
+ UNPACK64(ctx->h[3], &digest[24]);
+ UNPACK64(ctx->h[4], &digest[32]);
+ UNPACK64(ctx->h[5], &digest[40]);
+#endif /* !UNROLL_LOOPS */
+}
+
+/* SHA-224 functions */
+
+void sha224(const unsigned char *message, unsigned int len,
+ unsigned char *digest)
+{
+ sha224_ctx ctx;
+
+ sha224_init(&ctx);
+ sha224_update(&ctx, message, len);
+ sha224_final(&ctx, digest);
+}
+
+void sha224_init(sha224_ctx *ctx)
+{
+#ifndef UNROLL_LOOPS
+ int i;
+ for (i = 0; i < 8; i++) {
+ ctx->h[i] = sha224_h0[i];
+ }
+#else
+ ctx->h[0] = sha224_h0[0]; ctx->h[1] = sha224_h0[1];
+ ctx->h[2] = sha224_h0[2]; ctx->h[3] = sha224_h0[3];
+ ctx->h[4] = sha224_h0[4]; ctx->h[5] = sha224_h0[5];
+ ctx->h[6] = sha224_h0[6]; ctx->h[7] = sha224_h0[7];
+#endif /* !UNROLL_LOOPS */
+
+ ctx->len = 0;
+ ctx->tot_len = 0;
+}
+
+void sha224_update(sha224_ctx *ctx, const unsigned char *message,
+ unsigned int len)
+{
+ unsigned int block_nb;
+ unsigned int new_len, rem_len, tmp_len;
+ const unsigned char *shifted_message;
+
+ tmp_len = SHA224_BLOCK_SIZE - ctx->len;
+ rem_len = len < tmp_len ? len : tmp_len;
+
+ memcpy(&ctx->block[ctx->len], message, rem_len);
+
+ if (ctx->len + len < SHA224_BLOCK_SIZE) {
+ ctx->len += len;
+ return;
+ }
+
+ new_len = len - rem_len;
+ block_nb = new_len / SHA224_BLOCK_SIZE;
+
+ shifted_message = message + rem_len;
+
+ sha256_transf(ctx, ctx->block, 1);
+ sha256_transf(ctx, shifted_message, block_nb);
+
+ rem_len = new_len % SHA224_BLOCK_SIZE;
+
+ memcpy(ctx->block, &shifted_message[block_nb << 6],
+ rem_len);
+
+ ctx->len = rem_len;
+ ctx->tot_len += (block_nb + 1) << 6;
+}
+
+void sha224_final(sha224_ctx *ctx, unsigned char *digest)
+{
+ unsigned int block_nb;
+ unsigned int pm_len;
+ unsigned int len_b;
+
+#ifndef UNROLL_LOOPS
+ int i;
+#endif
+
+ block_nb = (1 + ((SHA224_BLOCK_SIZE - 9)
+ < (ctx->len % SHA224_BLOCK_SIZE)));
+
+ len_b = (ctx->tot_len + ctx->len) << 3;
+ pm_len = block_nb << 6;
+
+ memset(ctx->block + ctx->len, 0, pm_len - ctx->len);
+ ctx->block[ctx->len] = 0x80;
+ UNPACK32(len_b, ctx->block + pm_len - 4);
+
+ sha256_transf(ctx, ctx->block, block_nb);
+
+#ifndef UNROLL_LOOPS
+ for (i = 0 ; i < 7; i++) {
+ UNPACK32(ctx->h[i], &digest[i << 2]);
+ }
+#else
+ UNPACK32(ctx->h[0], &digest[ 0]);
+ UNPACK32(ctx->h[1], &digest[ 4]);
+ UNPACK32(ctx->h[2], &digest[ 8]);
+ UNPACK32(ctx->h[3], &digest[12]);
+ UNPACK32(ctx->h[4], &digest[16]);
+ UNPACK32(ctx->h[5], &digest[20]);
+ UNPACK32(ctx->h[6], &digest[24]);
+#endif /* !UNROLL_LOOPS */
+}
+
+#ifdef TEST_VECTORS
+
+/* FIPS 180-2 Validation tests */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+void test(const char *vector, unsigned char *digest,
+ unsigned int digest_size)
+{
+ char output[2 * SHA512_DIGEST_SIZE + 1];
+ int i;
+
+ output[2 * digest_size] = '\0';
+
+ for (i = 0; i < (int) digest_size ; i++) {
+ sprintf(output + 2 * i, "%02x", digest[i]);
+ }
+
+ printf("H: %s\n", output);
+ if (strcmp(vector, output)) {
+ fprintf(stderr, "Test failed.\n");
+ exit(EXIT_FAILURE);
+ }
+}
+
+int main(void)
+{
+ static const char *vectors[4][3] =
+ { /* SHA-224 */
+ {
+ "23097d223405d8228642a477bda255b32aadbce4bda0b3f7e36c9da7",
+ "75388b16512776cc5dba5da1fd890150b0c6455cb4f58b1952522525",
+ "20794655980c91d8bbb4c1ea97618a4bf03f42581948b2ee4ee7ad67",
+ },
+ /* SHA-256 */
+ {
+ "ba7816bf8f01cfea414140de5dae2223b00361a396177a9cb410ff61f20015ad",
+ "248d6a61d20638b8e5c026930c3e6039a33ce45964ff2167f6ecedd419db06c1",
+ "cdc76e5c9914fb9281a1c7e284d73e67f1809a48a497200e046d39ccc7112cd0",
+ },
+ /* SHA-384 */
+ {
+ "cb00753f45a35e8bb5a03d699ac65007272c32ab0eded1631a8b605a43ff5bed"
+ "8086072ba1e7cc2358baeca134c825a7",
+ "09330c33f71147e83d192fc782cd1b4753111b173b3b05d22fa08086e3b0f712"
+ "fcc7c71a557e2db966c3e9fa91746039",
+ "9d0e1809716474cb086e834e310a4a1ced149e9c00f248527972cec5704c2a5b"
+ "07b8b3dc38ecc4ebae97ddd87f3d8985",
+ },
+ /* SHA-512 */
+ {
+ "ddaf35a193617abacc417349ae20413112e6fa4e89a97ea20a9eeee64b55d39a"
+ "2192992a274fc1a836ba3c23a3feebbd454d4423643ce80e2a9ac94fa54ca49f",
+ "8e959b75dae313da8cf4f72814fc143f8f7779c6eb9f7fa17299aeadb6889018"
+ "501d289e4900f7e4331b99dec4b5433ac7d329eeb6dd26545e96e55b874be909",
+ "e718483d0ce769644e2e42c7bc15b4638e1f98b13b2044285632a803afa973eb"
+ "de0ff244877ea60a4cb0432ce577c31beb009c5c2c49aa2e4eadb217ad8cc09b"
+ }
+ };
+
+ static const char message1[] = "abc";
+ static const char message2a[] = "abcdbcdecdefdefgefghfghighijhi"
+ "jkijkljklmklmnlmnomnopnopq";
+ static const char message2b[] = "abcdefghbcdefghicdefghijdefghijkefghij"
+ "klfghijklmghijklmnhijklmnoijklmnopjklm"
+ "nopqklmnopqrlmnopqrsmnopqrstnopqrstu";
+ unsigned char *message3;
+ unsigned int message3_len = 1000000;
+ unsigned char digest[SHA512_DIGEST_SIZE];
+
+ message3 = malloc(message3_len);
+ if (message3 == NULL) {
+ fprintf(stderr, "Can't allocate memory\n");
+ return -1;
+ }
+ memset(message3, 'a', message3_len);
+
+ printf("SHA-2 FIPS 180-2 Validation tests\n\n");
+ printf("SHA-224 Test vectors\n");
+
+ sha224((const unsigned char *) message1, strlen(message1), digest);
+ test(vectors[0][0], digest, SHA224_DIGEST_SIZE);
+ sha224((const unsigned char *) message2a, strlen(message2a), digest);
+ test(vectors[0][1], digest, SHA224_DIGEST_SIZE);
+ sha224(message3, message3_len, digest);
+ test(vectors[0][2], digest, SHA224_DIGEST_SIZE);
+ printf("\n");
+
+ printf("SHA-256 Test vectors\n");
+
+ sha256((const unsigned char *) message1, strlen(message1), digest);
+ test(vectors[1][0], digest, SHA256_DIGEST_SIZE);
+ sha256((const unsigned char *) message2a, strlen(message2a), digest);
+ test(vectors[1][1], digest, SHA256_DIGEST_SIZE);
+ sha256(message3, message3_len, digest);
+ test(vectors[1][2], digest, SHA256_DIGEST_SIZE);
+ printf("\n");
+
+ printf("SHA-384 Test vectors\n");
+
+ sha384((const unsigned char *) message1, strlen(message1), digest);
+ test(vectors[2][0], digest, SHA384_DIGEST_SIZE);
+ sha384((const unsigned char *)message2b, strlen(message2b), digest);
+ test(vectors[2][1], digest, SHA384_DIGEST_SIZE);
+ sha384(message3, message3_len, digest);
+ test(vectors[2][2], digest, SHA384_DIGEST_SIZE);
+ printf("\n");
+
+ printf("SHA-512 Test vectors\n");
+
+ sha512((const unsigned char *) message1, strlen(message1), digest);
+ test(vectors[3][0], digest, SHA512_DIGEST_SIZE);
+ sha512((const unsigned char *) message2b, strlen(message2b), digest);
+ test(vectors[3][1], digest, SHA512_DIGEST_SIZE);
+ sha512(message3, message3_len, digest);
+ test(vectors[3][2], digest, SHA512_DIGEST_SIZE);
+ printf("\n");
+
+ printf("All tests passed.\n");
+
+ return 0;
+}
+
+#endif /* TEST_VECTORS */
+
diff --git a/libmtk_bsg/sha2.h b/libmtk_bsg/sha2.h
new file mode 100644
index 0000000..4ac5524
--- /dev/null
+++ b/libmtk_bsg/sha2.h
@@ -0,0 +1,108 @@
+/*
+ * HMAC-SHA-224/256/384/512 implementation
+ * Last update: 06/15/2005
+ * Issue date: 06/15/2005
+ *
+ * Copyright (C) 2005 Olivier Gay <olivier.gay@a3.epfl.ch>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the project nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#ifndef SHA2_H
+#define SHA2_H
+
+#define SHA224_DIGEST_SIZE ( 224 / 8)
+#define SHA256_DIGEST_SIZE ( 256 / 8)
+#define SHA384_DIGEST_SIZE ( 384 / 8)
+#define SHA512_DIGEST_SIZE ( 512 / 8)
+
+#define SHA256_BLOCK_SIZE ( 512 / 8)
+#define SHA512_BLOCK_SIZE (1024 / 8)
+#define SHA384_BLOCK_SIZE SHA512_BLOCK_SIZE
+#define SHA224_BLOCK_SIZE SHA256_BLOCK_SIZE
+
+#ifndef SHA2_TYPES
+#define SHA2_TYPES
+typedef unsigned char uint8;
+typedef unsigned int uint32;
+typedef unsigned long long uint64;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+ unsigned int tot_len;
+ unsigned int len;
+ unsigned char block[2 * SHA256_BLOCK_SIZE];
+ uint32 h[8];
+} sha256_ctx;
+
+typedef struct {
+ unsigned int tot_len;
+ unsigned int len;
+ unsigned char block[2 * SHA512_BLOCK_SIZE];
+ uint64 h[8];
+} sha512_ctx;
+
+typedef sha512_ctx sha384_ctx;
+typedef sha256_ctx sha224_ctx;
+
+void sha224_init(sha224_ctx *ctx);
+void sha224_update(sha224_ctx *ctx, const unsigned char *message,
+ unsigned int len);
+void sha224_final(sha224_ctx *ctx, unsigned char *digest);
+void sha224(const unsigned char *message, unsigned int len,
+ unsigned char *digest);
+
+void sha256_init(sha256_ctx * ctx);
+void sha256_update(sha256_ctx *ctx, const unsigned char *message,
+ unsigned int len);
+void sha256_final(sha256_ctx *ctx, unsigned char *digest);
+void sha256(const unsigned char *message, unsigned int len,
+ unsigned char *digest);
+
+void sha384_init(sha384_ctx *ctx);
+void sha384_update(sha384_ctx *ctx, const unsigned char *message,
+ unsigned int len);
+void sha384_final(sha384_ctx *ctx, unsigned char *digest);
+void sha384(const unsigned char *message, unsigned int len,
+ unsigned char *digest);
+
+void sha512_init(sha512_ctx *ctx);
+void sha512_update(sha512_ctx *ctx, const unsigned char *message,
+ unsigned int len);
+void sha512_final(sha512_ctx *ctx, unsigned char *digest);
+void sha512(const unsigned char *message, unsigned int len,
+ unsigned char *digest);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* !SHA2_H */
+
diff --git a/libmtk_bsg/ufs.c b/libmtk_bsg/ufs.c
new file mode 100644
index 0000000..6ed37e4
--- /dev/null
+++ b/libmtk_bsg/ufs.c
@@ -0,0 +1,271 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#pragma GCC diagnostic ignored "-Wall"
+#pragma GCC diagnostic ignored "-Wextra"
+#pragma GCC diagnostic ignored "-Wsign-compare"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdarg.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <errno.h>
+#include <limits.h>
+
+#include "ufs_cmds.h"
+#include "options.h"
+#include "ufs.h"
+#include "ufs_err_hist.h"
+#include "unipro.h"
+#include "ufs_ffu.h"
+#include "ufs_vendor.h"
+#include "ufs_rpmb.h"
+#include "ufs_hmr.h"
+
+#define UFS_BSG_UTIL_VERSION "1.9"
+typedef int (*command_function)(struct tool_options *opt);
+
+struct tool_command {
+ command_function func; /* function which implements the command */
+ char *conf_type; /* one of: descriptor/attributes/flags */
+ int conf_type_ind; /* confiruration type index */
+};
+
+static struct tool_command commands[] = {
+ /*
+ * avoid short commands different for the case only
+ */
+ { do_desc, "desc", DESC_TYPE},
+ { do_attributes, "attr", ATTR_TYPE},
+ { do_flags, "fl", FLAG_TYPE},
+ { do_err_hist, "err_hist", ERR_HIST_TYPE},
+ { do_uic, "uic", UIC_TYPE},
+ { do_ffu, "ffu", FFU_TYPE},
+ { do_vendor, "vendor", VENDOR_BUFFER_TYPE},
+ { do_rpmb, "rpmb", RPMB_CMD_TYPE},
+ { do_hmr, "hmr", HMR_TYPE},
+ { 0, 0, 0}
+};
+
+static char *get_prgname(char *programname)
+{
+ char *np;
+
+ np = strrchr(programname, '/');
+ if (!np)
+ np = programname;
+ else
+ np++;
+
+ return np;
+}
+
+static void help(char *np)
+{
+ char help_str[256] = {0};
+
+ strcat(help_str, "<desc | attr | fl | err_hist | uic | ffu | vendor | "
+ "rpmb | hmr>");
+ printf("\n Usage:\n");
+ printf("\n\t%s help|--help|-h\n\t\tShow the help.\n", np);
+ printf("\n\t%s -v\n\t\tShow the version.\n", np);
+ printf("\n\t%s %s%s", np, help_str,
+ " --help|-h\n\t\tShow detailed help for a command\n");
+}
+
+static void initialized_options(struct tool_options *options)
+{
+ memset(options, INVALID, sizeof(*options));
+ options->path[0] = '\0';
+ options->keypath[0] = '\0';
+ options->data = NULL;
+ options->sg_type = SG4_TYPE;
+}
+
+static int parse_args(int argc, char **argv, command_function *func,
+ struct tool_options *options)
+{
+ int rc = OK;
+ struct tool_command *cp;
+ char *prgname = get_prgname(argv[0]);
+
+ if (argc == 2 && !strcmp(argv[1], "-v")) {
+ printf("\n\t %s ver: %s\n", prgname, UFS_BSG_UTIL_VERSION);
+ goto out;
+ } else if (argc <= 2) {
+ help(prgname);
+ goto out;
+ }
+
+ for (cp = commands; cp->conf_type; cp++) {
+ if (!strcmp(argv[1], cp->conf_type)) {
+ options->config_type_inx = cp->conf_type_ind;
+ *func = cp->func;
+ break;
+ }
+ }
+
+ if (options->config_type_inx == INVALID) {
+ print_error("Please enter the correct config type");
+ help(prgname);
+ rc = -EINVAL;
+ goto out;
+ }
+
+ if (argc == 3 &&
+ (!strcmp(argv[2], "-h") || !strcmp(argv[2], "--help"))) {
+ print_command_help(prgname, options->config_type_inx);
+ *func = 0;
+ goto out;
+ }
+
+ rc = init_options(argc, argv, options);
+
+out:
+ return rc;
+}
+
+int write_file(const char *name, const void *buffer, int length)
+{
+ int fd;
+ int rc = 0;
+ size_t ret;
+
+ WRITE_LOG("writing file %s length=%d\n", name, length);
+ fd = open(name, O_RDWR | O_CREAT | O_TRUNC | O_SYNC, 0600);
+ if (fd == -1) {
+ WRITE_LOG("%s: failed in open errno=%d", __func__, errno);
+ return -ENOENT;
+ }
+
+ ret = write(fd, buffer, length);
+ if (length != ret) {
+ WRITE_LOG( "%s: failed in write errno=%d", __func__, errno);
+ rc = -EIO;
+ }
+
+ close(fd);
+ return rc;
+}
+
+void print_error(const char *msg, ...)
+{
+ va_list args;
+
+ printf("\n Err: ");
+ va_start(args, msg);
+ vprintf(msg, args);
+ va_end(args);
+ printf("\n");
+}
+
+void print_warn(const char *msg, ...)
+{
+ va_list args;
+
+ va_start(args, msg);
+ fprintf(stderr, "\nWARN: ");
+ vfprintf(stderr, msg, args);
+ va_end(args);
+ printf("\n");
+}
+
+void print_command_help(char *prgname, int config_type)
+{
+ switch (config_type) {
+ case DESC_TYPE:
+ desc_help(prgname);
+ break;
+ case ATTR_TYPE:
+ attribute_help(prgname);
+ break;
+ case FLAG_TYPE:
+ flag_help(prgname);
+ break;
+ case ERR_HIST_TYPE:
+ err_hist_help(prgname);
+ break;
+ case FFU_TYPE:
+ ffu_help(prgname);
+ break;
+ case UIC_TYPE:
+ unipro_help(prgname);
+ break;
+ case VENDOR_BUFFER_TYPE:
+ vendor_help(prgname);
+ break;
+ case RPMB_CMD_TYPE:
+ rpmb_help(prgname);
+ break;
+ case HMR_TYPE:
+ hmr_help(prgname);
+ break;
+ default:
+ print_error("Unsupported cmd type");
+ break;
+ }
+}
+
+/*
+ * Wrapper for strtol() function.
+ *
+ * strtol() has advantages over atoi():
+ * - has error handling
+ * - handles not only decimal, but acts accordingly to the 'base' argument
+ * - accepts strings with "0x" prefix
+ * - stores address of the first invalid character
+ */
+long str_to_long(char *nptr, int base, long *result)
+{
+ char *endptr;
+
+ if (!nptr || !result)
+ return ERROR;
+
+ /*
+ * From man:
+ * Since strtol() can legitimately return 0, LONG_MAX, or LONG_MIN
+ * on both success and failure, the calling program should set errno to 0
+ * before the call...
+ */
+ errno = 0;
+
+ *result = strtol(optarg, &endptr, base);
+
+ if (endptr == nptr || /* no conversion performed */
+ *endptr != '\0' || /* some chars not converted */
+ *result == LONG_MIN || /* underflow occured */
+ *result == LONG_MAX || /* overflow occured */
+ errno != 0) /* any other error */
+ return ERROR;
+
+ return OK;
+}
+
+int main(int ac, char **av)
+{
+ int rc;
+ command_function func = NULL;
+ struct tool_options options;
+
+ initialized_options(&options);
+
+ rc = parse_args(ac, av, &func, &options);
+ if (rc)
+ goto out;
+
+ if (func)
+ rc = func(&options);
+
+out:
+ if (options.data)
+ free(options.data);
+ return rc ? EXIT_FAILURE : EXIT_SUCCESS;
+}
+
diff --git a/libmtk_bsg/ufs.h b/libmtk_bsg/ufs.h
new file mode 100644
index 0000000..f382aa0
--- /dev/null
+++ b/libmtk_bsg/ufs.h
@@ -0,0 +1,190 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+
+#ifndef UFS_H_
+#define UFS_H_
+#include "ioctl.h"
+#include "scsi_bsg_util.h"
+
+#define BLOCK_SIZE 512
+
+/*
+ * Generally the max HW max chunk is 512KB,
+ * but in order to be in safe side tool using 256KB as max chunk size
+ * between user and kernel space
+*/
+#define MAX_IOCTL_BUF_SIZE (256L * 1024)
+
+enum sg_struct_type {
+ SG4_TYPE = 0,
+ SG3_TYPE
+};
+
+/* Flag idn for Query Requests*/
+enum flag_idn {
+ QUERY_FLAG_IDN_RESERVED1 = 0x00,
+ QUERY_FLAG_IDN_FDEVICEINIT = 0x01,
+ QUERY_FLAG_IDN_PERMANENT_WPE = 0x02,
+ QUERY_FLAG_IDN_PWR_ON_WPE = 0x03,
+ QUERY_FLAG_IDN_BKOPS_EN = 0x04,
+ QUERY_FLAG_IDN_DEVICE_LIFE_SPAN_MODE_EN = 0x05,
+ QUERY_FLAG_IDN_PURGE_ENABLE = 0x06,
+ QUERY_FLAG_IDN_REFRESH_ENABLE = 0x07,
+ QUERY_FLAG_IDN_FPHYRESOURCEREMOVAL = 0x08,
+ QUERY_FLAG_IDN_BUSY_RTC = 0x09,
+ QUERY_FLAG_IDN_RESERVED4 = 0x0A,
+ QUERY_FLAG_IDN_PERMANENTLYDISABLEFW = 0x0B,
+ QUERY_FLAG_IDN_WB_EN = 0x0E,
+ QUERY_FLAG_IDN_WB_BUF_FLUSH_EN = 0x0F,
+ QUERY_FLAG_IDN_WB_BUF_FLUSH_H8 = 0x10,
+ QUERY_FLAG_IDN_HPB_RESET = 0x11,
+ QUERY_FLAG_IDN_HPB_EN = 0x12,
+ QUERY_FLAG_IDN_MAX
+};
+
+/* Attribute idn for Query requests */
+enum attr_idn {
+ QUERY_ATTR_IDN_BOOT_LU_EN = 0x00,
+ QUERY_ATTR_IDN_RESERVED1 = 0x01,
+ QUERY_ATTR_IDN_POWER_MODE = 0x02,
+ QUERY_ATTR_IDN_ACTIVE_ICC_LVL = 0x03,
+ QUERY_ATTR_IDN_OOO_DATA_EN = 0x04,
+ QUERY_ATTR_IDN_BKOPS_STATUS = 0x05,
+ QUERY_ATTR_IDN_PURGE_STATUS = 0x06,
+ QUERY_ATTR_IDN_MAX_DATA_IN = 0x07,
+ QUERY_ATTR_IDN_MAX_DATA_OUT = 0x08,
+ QUERY_ATTR_IDN_DYN_CAP_NEEDED = 0x09,
+ QUERY_ATTR_IDN_REF_CLK_FREQ = 0x0A,
+ QUERY_ATTR_IDN_CONF_DESC_LOCK = 0x0B,
+ QUERY_ATTR_IDN_MAX_NUM_OF_RTT = 0x0C,
+ QUERY_ATTR_IDN_EE_CONTROL = 0x0D,
+ QUERY_ATTR_IDN_EE_STATUS = 0x0E,
+ QUERY_ATTR_IDN_SECONDS_PASSED = 0x0F,
+ QUERY_ATTR_IDN_CNTX_CONF = 0x10,
+ QUERY_ATTR_IDN_OBSOLETE = 0x11,
+ QUERY_ATTR_IDN_RESERVED2 = 0x12,
+ QUERY_ATTR_IDN_RESERVED3 = 0x13,
+ QUERY_ATTR_IDN_DEVICE_FFU_STATUS = 0x14,
+ QUERY_ATTR_IDN_DEVICE_PSA_STATE = 0x15,
+ QUERY_ATTR_IDN_DEVICE_PSA_DATA_SIZE = 0x16,
+ QUERY_ATTR_IDN_CASE_REF_CLK_GATING_WAIT_TIME = 0x17,
+ QUERY_ATTR_IDN_CASE_ROUGH_TEMPERAURE = 0x18,
+ QUERY_ATTR_IDN_TOO_HIGH_TEMP_BOUNDARY = 0x19,
+ QUERY_ATTR_IDN_TOO_LOW_TEMP_BOUNDARY = 0x1A,
+ QUERY_ATTR_IDN_THROTTLING_STAT = 0x1B,
+ QUERY_ATTR_IDN_WB_FLUSH_STAT = 0x1C,
+ QUERY_ATTR_IDN_WB_BUF_SIZE = 0x1D,
+ QUERY_ATTR_IDN_WB_LIFE_TIME_EST = 0x1E,
+ QUERY_ATTR_IDN_WB_CUR_BUF_SIZE = 0x1F,
+ QUERY_ATTR_IDN_REFRESH_STATUS = 0x2C,
+ QUERY_ATTR_IDN_REFRESH_FREQ = 0x2D,
+ QUERY_ATTR_IDN_REFRESH_UNIT = 0x2E,
+ QUERY_ATTR_IDN_REFRESH_METHOD = 0x2F,
+ QUERY_ATTR_IDN_MAX
+};
+
+/* Descriptor idn for Query requests */
+enum desc_idn {
+ QUERY_DESC_IDN_DEVICE = 0x0,
+ QUERY_DESC_IDN_CONFIGURAION = 0x1,
+ QUERY_DESC_IDN_UNIT = 0x2,
+ QUERY_DESC_IDN_RFU_0 = 0x3,
+ QUERY_DESC_IDN_INTERCONNECT = 0x4,
+ QUERY_DESC_IDN_STRING = 0x5,
+ QUERY_DESC_IDN_RFU_1 = 0x6,
+ QUERY_DESC_IDN_GEOMETRY = 0x7,
+ QUERY_DESC_IDN_POWER = 0x8,
+ QUERY_DESC_IDN_HEALTH = 0x9,
+ QUERY_DESC_IDN_RFU_3 = 0xA,
+ QUERY_DESC_IDN_MAX = 0xFF,
+};
+
+/* UTP QUERY Transaction Specific Fields OpCode */
+enum query_opcode {
+ UPIU_QUERY_OPCODE_NOP = 0x0,
+ UPIU_QUERY_OPCODE_READ_DESC = 0x1,
+ UPIU_QUERY_OPCODE_WRITE_DESC = 0x2,
+ UPIU_QUERY_OPCODE_READ_ATTR = 0x3,
+ UPIU_QUERY_OPCODE_WRITE_ATTR = 0x4,
+ UPIU_QUERY_OPCODE_READ_FLAG = 0x5,
+ UPIU_QUERY_OPCODE_SET_FLAG = 0x6,
+ UPIU_QUERY_OPCODE_CLEAR_FLAG = 0x7,
+ UPIU_QUERY_OPCODE_TOGGLE_FLAG = 0x8,
+ UPIU_QUERY_OPCODE_MAX,
+};
+
+enum ufs_desc_max_size {
+ /* Max descriptors size from 2.1 to 3.0 UFS spec */
+ QUERY_DESC_DEVICE_MAX_SIZE_3_0 = 0x40,
+ QUERY_DESC_CONFIGURAION_MAX_SIZE_3_0 = 0x90,
+ QUERY_DESC_UNIT_MAX_SIZE_3_0 = 0x23,
+ QUERY_DESC_GEOMETRY_MAX_SIZE_3_0 = 0x48,
+ QUERY_DESC_HEALTH_MAX_SIZE_2_1 = 0x25,
+
+ /* Max descriptors size for 3.1 UFS spec */
+ QUERY_DESC_DEVICE_MAX_SIZE = 0x59,
+ QUERY_DESC_CONFIGURAION_MAX_SIZE = 0xe6,
+ QUERY_DESC_GEOMETRY_MAX_SIZE = 0x57,
+ QUERY_DESC_UNIT_MAX_SIZE = 0x2d,
+
+ /*
+ * Max. 126 UNICODE characters (2 bytes per character) plus 2 bytes
+ * of descriptor header.
+ */
+ QUERY_DESC_STRING_MAX_SIZE = 0xFE,
+ QUERY_DESC_INTERCONNECT_MAX_SIZE = 0x06,
+ QUERY_DESC_POWER_MAX_SIZE = 0x62,
+ QUERY_DESC_HEALTH_MAX_SIZE = 0x2d
+};
+
+/* UPIU Read/Write flags */
+enum {
+ UPIU_CMD_FLAGS_NONE = 0x00,
+ UPIU_CMD_FLAGS_WRITE = 0x20,
+ UPIU_CMD_FLAGS_READ = 0x40,
+};
+
+/* UPIU Query request function */
+enum {
+ UPIU_QUERY_FUNC_STANDARD_READ_REQUEST = 0x01,
+ UPIU_QUERY_FUNC_STANDARD_WRITE_REQUEST = 0x81,
+};
+
+enum ufs_cong_type {
+ DESC_TYPE = 0,
+ ATTR_TYPE,
+ FLAG_TYPE,
+ ERR_HIST_TYPE,
+ UIC_TYPE,
+ FFU_TYPE,
+ VENDOR_BUFFER_TYPE,
+ RPMB_CMD_TYPE,
+ HMR_TYPE
+};
+
+/* UTP UPIU Transaction Codes Initiator to Target */
+enum {
+ UPIU_TRANSACTION_NOP_OUT = 0x00,
+ UPIU_TRANSACTION_COMMAND = 0x01,
+ UPIU_TRANSACTION_DATA_OUT = 0x02,
+ UPIU_TRANSACTION_TASK_REQ = 0x04,
+ UPIU_TRANSACTION_QUERY_REQ = 0x16,
+};
+
+int write_file(const char *name, const void *buffer, int length);
+void print_error(const char *msg, ...);
+void print_warn(const char *msg, ...);
+long str_to_long(char *nptr, int base, long *result);
+#ifdef DEBUG
+ #define WRITE_LOG(format, ...) fprintf(stderr, format"\n", __VA_ARGS__)
+ #define WRITE_LOG0(text) fprintf(stderr, "%s\n", text)
+#else
+ #define WRITE_LOG(...)
+ #define WRITE_LOG0(...)
+#endif
+
+#endif /* UFS_H_ */
diff --git a/libmtk_bsg/ufs_cmds.c b/libmtk_bsg/ufs_cmds.c
new file mode 100644
index 0000000..2da620f
--- /dev/null
+++ b/libmtk_bsg/ufs_cmds.c
@@ -0,0 +1,1302 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#pragma GCC diagnostic ignored "-Wall"
+#pragma GCC diagnostic ignored "-Wextra"
+#pragma GCC diagnostic ignored "-Wsign-compare"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <ctype.h>
+#include <endian.h>
+#include <errno.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "ufs.h"
+#include "ufs_cmds.h"
+#include "options.h"
+
+#define STR_BUF_LEN 33
+#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
+#define ATTR_RSRV() "Reserved", BYTE, ACC_INVALID, MODE_INVALID, LEVEL_INVALID
+
+#define CONFIG_HEADER_OFFSET 0x16
+#define CONFIG_LUN_OFFSET 0x1A
+
+/* Config desc. offsets for UFS 2.0 - 3.0 spec */
+#define CONFIG_HEADER_OFFSET_3_0 0x10
+#define CONFIG_LUN_OFFSET_3_0 0x10
+
+struct desc_field_offset device_desc_field_name[] = {
+ {"bLength", 0x00, BYTE},
+ {"bDescriptorType", 0x01, BYTE},
+ {"bDevice", 0x02, BYTE},
+ {"bDeviceClass", 0x03, BYTE},
+ {"bDeviceSubClass", 0x04, BYTE},
+ {"bProtocol", 0x05, BYTE},
+ {"bNumberLU", 0x06, BYTE},
+ {"bNumberWLU", 0x07, BYTE},
+ {"bBootEnable", 0x08, BYTE},
+ {"bDescrAccessEn", 0x09, BYTE},
+ {"bInitPowerMode", 0x0A, BYTE},
+ {"bHighPriorityLUN", 0x0B, BYTE},
+ {"bSecureRemovalType", 0x0C, BYTE},
+ {"bSecurityLU", 0x0D, BYTE},
+ {"bBackgroundOpsTermLat", 0x0E, BYTE},
+ {"bInitActiveICCLevel", 0x0F, BYTE},
+ {"wSpecVersion", 0x10, WORD},
+ {"wManufactureDate", 0x12, WORD},
+ {"iManufactureName", 0x14, BYTE},
+ {"iProductName", 0x15, BYTE},
+ {"iSerialNumber", 0x16, BYTE},
+ {"iOemID", 0x17, BYTE},
+ {"ManufacturerID", 0x18, WORD},
+ {"bUD0BaseOffset", 0x1A, BYTE},
+ {"bUDConfigPLength", 0x1B, BYTE},
+ {"bDeviceRTTCap", 0x1C, BYTE},
+ {"wPeriodicRTCUpdate", 0x1D, WORD},
+ {"bUFSFeaturesSupport", 0x1F, BYTE},
+ {"bFFUTimeout", 0x20, BYTE},
+ {"bQueueDepth", 0x21, BYTE},
+ {"wDeviceVersion", 0x22, WORD},
+ {"bNumSecureWPArea", 0x24, BYTE},
+ {"dPSAMaxDataSize", 0x25, DWORD},
+ {"bPSAStateTimeout", 0x29, BYTE},
+ {"iProductRevisionLevel", 0x2A, BYTE},
+ {"Reserved1", 0x2B, BYTE},
+ {"Reserved2", 0x2C, DWORD},
+ {"Reserved3", 0x30, DWORD},
+ {"Reserved4", 0x34, DWORD},
+ {"Reserved5", 0x38, DWORD},
+ {"Reserved6", 0x3c, DWORD},
+ {"wHPBVersion", 0x40, WORD},
+ {"bHPBControl", 0x42, BYTE},
+ {"Reserved8", 0x43, DWORD},
+ {"Reserved9", 0x47, DDWORD},
+ {"dExtendedUFSFeaturesSupport", 0x4F, DWORD},
+ {"bWriteBoosterBufferPreserveUserSpaceEn", 0x53, BYTE},
+ {"bWriteBoosterBufferType", 0x54, BYTE},
+ {"dNumSharedWriteBoosterBufferAllocUnits", 0x55, DWORD}
+};
+
+struct desc_field_offset device_config_desc_field_name[] = {
+ {"bLength", 0x00, BYTE},
+ {"bDescriptorType", 0x01, BYTE},
+ {"bConfDescContinue", 0x02, BYTE},
+ {"bBootEnable", 0x03, BYTE},
+ {"bDescrAccessEn", 0x04, BYTE},
+ {"bInitPowerMode", 0x05, BYTE},
+ {"bHighPriorityLUN", 0x06, BYTE},
+ {"bSecureRemovalType", 0x07, BYTE},
+ {"bInitActiveICCLevel", 0x08, BYTE},
+ {"wPeriodicRTCUpdate", 0x09, WORD},
+ {"bHPBControl", 0x0B, BYTE},
+ {"bRPMBRegionEnable", 0x0C, BYTE},
+ {"bRPMBRegion1Size", 0x0D, BYTE},
+ {"bRPMBRegion2Size", 0x0E, BYTE},
+ {"bRPMBRegion3Size", 0x0F, BYTE},
+ {"bWriteBoosterBufferPreserveUserSpaceEn", 0x10, BYTE},
+ {"bWriteBoosterBufferType", 0x11, BYTE},
+ {"dNumSharedWriteBoosterBufferAllocUnits", 0x12, DWORD}
+};
+
+struct desc_field_offset device_config_unit_desc_field_name[] = {
+ {"bLUEnable", 0x00, BYTE},
+ {"bBootLunID", 0x01, BYTE},
+ {"bLUWriteProtect", 0x02, BYTE},
+ {"bMemoryType", 0x03, BYTE},
+ {"dNumAllocUnits", 0x04, DWORD},
+ {"bDataReliability", 0x08, BYTE},
+ {"bLogicalBlockSize", 0x09, BYTE},
+ {"bProvisioningType", 0x0A, BYTE},
+ {"wContextCapabilities", 0x0B, WORD},
+ {"wLUMaxActiveHPBRegions", 0x10, WORD},
+ {"wHPBPinnedRegionStartIdx", 0x12, WORD},
+ {"wNumHPBPinnedRegions", 0x14, WORD},
+ {"dLUNumWriteBoosterBufferAllocUnits", 0x16, DWORD}
+};
+
+struct desc_field_offset device_geo_desc_conf_field_name[] = {
+ {"bLength", 0x00, BYTE},
+ {"bDescriptorType ", 0x01, BYTE},
+ {"bMediaTechnology", 0x02, BYTE},
+ {"qTotalRawDeviceCapacity", 0x04, DDWORD},
+ {"bMaxNumberLU", 0x0C, BYTE},
+ {"dSegmentSize", 0x0D, DWORD},
+ {"bAllocationUnitSize", 0x11, BYTE},
+ {"bMinAddrBlockSize", 0x12, BYTE},
+ {"bOptimalReadBlockSize", 0x13, BYTE},
+ {"bOptimalWriteBlockSize", 0x14, BYTE},
+ {"bMaxInBufferSize", 0x15, BYTE},
+ {"bMaxOutBufferSize", 0x16, BYTE},
+ {"bRPMB_ReadWriteSize", 0x17, BYTE},
+ {"bDynamicCapacityResourcePolicy", 0x18, BYTE},
+ {"bDataOrdering", 0x19, BYTE},
+ {"bMaxContexIDNumber", 0x1A, BYTE},
+ {"bSysDataTagUnitSize", 0x1B, BYTE},
+ {"bSysDataTagResSize", 0x1C, BYTE},
+ {"bSupportedSecRTypes", 0x1D, BYTE},
+ {"wSupportedMemoryTypes", 0x1E, WORD},
+ {"dSystemCodeMaxNAllocU", 0x20, DWORD},
+ {"wSystemCodeCapAdjFac", 0x24, DWORD},
+ {"dNonPersistMaxNAllocU", 0x26, DWORD},
+ {"wNonPersistCapAdjFac", 0x2A, WORD},
+ {"dEnhanced1MaxNAllocU", 0x2C, DWORD},
+ {"wEnhanced1CapAdjFac", 0x30, WORD},
+ {"dEnhanced2MaxNAllocU", 0x32, DWORD},
+ {"wEnhanced2CapAdjFac", 0x36, WORD},
+ {"dEnhanced3MaxNAllocU", 0x38, DWORD},
+ {"wEnhanced3CapAdjFac", 0x3C, WORD},
+ {"dEnhanced4MaxNAllocU", 0x3E, DWORD},
+ {"wEnhanced4CapAdjFac", 0X42, WORD},
+ {"dOptimalLogicalBlockSize", 0X44, DWORD},
+ {"bHPBRegionSize", 0X48, BYTE},
+ {"bHPBNumberLU", 0X49, BYTE},
+ {"bHPBSubRegionSize", 0X4a, BYTE},
+ {"wDeviceMaxActiveHPBRegions", 0X4b, WORD},
+ {"Reserved", 0X4d, WORD},
+ {"dWriteBoosterBufferMaxNAllocUnits", 0X4f, DWORD},
+ {"bDeviceMaxWriteBoosterLUs", 0X53, BYTE},
+ {"bWriteBoosterBufferCapAdjFac", 0X54, BYTE},
+ {"bSupportedWriteBoosterBufferUserSpaceReductionTypes", 0X55, BYTE},
+ {"bSupportedWriteBoosterBufferTypes", 0X56, BYTE}
+};
+
+struct desc_field_offset device_interconnect_desc_conf_field_name[] = {
+ {"bLength", 0x00, BYTE},
+ {"bDescriptorType", 0x01, BYTE},
+ {"bcdUniproVersion", 0x02, WORD},
+ {"bcdMphyVersion ", 0x04, WORD},
+};
+
+struct desc_field_offset device_unit_desc_field_name[] = {
+ {"bLength", 0x00, BYTE},
+ {"bDescriptorType", 0x01, BYTE},
+ {"bUnitIndex", 0x02, BYTE},
+ {"bLUEnable", 0x03, BYTE},
+ {"bBootLunID", 0x04, BYTE},
+ {"bLUWriteProtect", 0x05, BYTE},
+ {"bLUQueueDepth", 0x06, BYTE},
+ {"bPSASensitive", 0x07, BYTE},
+ {"bMemoryType", 0x08, BYTE},
+ {"bDataReliability", 0x09, BYTE},
+ {"bLogicalBlockSize", 0x0A, BYTE},
+ {"qLogicalBlockCount", 0x0B, DDWORD},
+ {"dEraseBlockSize", 0x13, DWORD},
+ {"bProvisioningType", 0x17, BYTE},
+ {"qPhyMemResourceCount", 0x18, DDWORD},
+ {"wContextCapabilities", 0x20, WORD},
+ {"bLargeUnitGranularity_M1", 0x22, BYTE},
+ {"wLUMaxActiveHPBRegions", 0x23, WORD},
+ {"wHPBPinnedRegionStartIdx", 0x25, WORD},
+ {"wNumHPBPinnedRegions", 0x27, WORD},
+ {"dLUNumWriteBoosterBufferAllocUnits", 0x29, DWORD}
+};
+
+struct desc_field_offset device_unit_rpmb_desc_field_name[] = {
+ {"bLength", 0x00, BYTE},
+ {"bDescriptorType", 0x01, BYTE},
+ {"bUnitIndex", 0x02, BYTE},
+ {"bLUEnable", 0x03, BYTE},
+ {"bBootLunID", 0x04, BYTE},
+ {"bLUWriteProtect", 0x05, BYTE},
+ {"bLUQueueDepth", 0x06, BYTE},
+ {"bPSASensitive", 0x07, BYTE},
+ {"bMemoryType", 0x08, BYTE},
+ {"bRPMBRegionEnable", 0x09, BYTE},
+ {"bLogicalBlockSize", 0x0A, BYTE},
+ {"qLogicalBlockCount", 0x0B, DDWORD},
+ {"bRPMBRegion0Size", 0x13, BYTE},
+ {"bRPMBRegion1Size", 0x14, BYTE},
+ {"bRPMBRegion2Size", 0x15, BYTE},
+ {"bRPMBRegion3Size", 0x16, BYTE},
+ {"bProvisioningType", 0x17, BYTE},
+ {"qPhyMemResourceCount", 0x18, DDWORD},
+ {"wContextCapabilities", 0x20, WORD},
+ {"wContextCapabilities", 0x22, BYTE},
+};
+
+struct desc_field_offset device_power_desc_conf_field_name[] = {
+ {"bLength", 0x00, BYTE},
+ {"bDescriptorType", 0x01, BYTE},
+ {"wActiveICCLevelsVCC", 0x02, 32},
+ {"wActiveICCLevelsVCCQ", 0x22, 32},
+ {"wActiveICCLevelsVCCQ2", 0x42, 32}
+};
+
+struct desc_field_offset device_health_desc_conf_field_name[] = {
+ {"bLength", 0x00, BYTE},
+ {"bDescriptorType", 0x01, BYTE},
+ {"bPreEOLInfo", 0x02, BYTE},
+ {"bDeviceLifeTimeEstA", 0x03, BYTE},
+ {"bDeviceLifeTimeEstB", 0x04, BYTE},
+ {"VendorPropInfo", 0x05, 32},
+ {"dRefreshTotalCount", 0x25, DWORD},
+ {"dRefreshProgress", 0x29, DWORD},
+};
+
+struct query_err_res {
+ char *name;
+ __u8 opcode;
+};
+
+struct attr_fields ufs_attrs[] = {
+ {"bBootLunEn", BYTE, (URD|UWRT), (READ_ONLY|WRITE_PRSIST), DEV},
+ {"bMAX_DATA_SIZE_FOR_HPB_SINGLE_CMD", BYTE, URD, READ_ONLY, DEV},
+ {"bCurrentPowerMode", BYTE, URD, READ_ONLY, DEV},
+ {"bActiveICCLevel", BYTE, (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV},
+ {"bOutOfOrderDataEn", BYTE, (URD|UWRT), (READ_NRML|WRITE_ONCE), DEV},
+ {"bBackgroundOpStatus", BYTE, URD, READ_ONLY, DEV},
+ {"bPurgeStatus", BYTE, URD, READ_ONLY, DEV},
+ {"bMaxDataInSize", BYTE, (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV},
+ {"bMaxDataOutSize", BYTE, (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV},
+ {"dDynCapNeeded", WORD, URD, READ_ONLY, ARRAY},
+ {"bRefClkFreq", BYTE, (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV},
+ {"bConfigDescrLock", BYTE, (URD|UWRT), (READ_NRML|WRITE_ONCE), DEV},
+ {"bMaxNumOfRTT", BYTE, (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV},
+ {"wExceptionEventControl", WORD, URD, READ_NRML, DEV},
+ {"wExceptionEventStatus", WORD, URD, READ_ONLY, DEV},
+ {"dSecondsPassed", DWORD, UWRT, WRITE_ONLY, DEV},
+ {"wContextConf", WORD, (URD|UWRT), (READ_NRML|WRITE_VLT), ARRAY},
+ {"Reserved", BYTE, ACC_INVALID, MODE_INVALID, LEVEL_INVALID},
+ {"Reserved", BYTE, ACC_INVALID, MODE_INVALID, LEVEL_INVALID},
+ {"Reserved", BYTE, ACC_INVALID, MODE_INVALID, LEVEL_INVALID},
+ {"bDeviceFFUStatus", BYTE, URD, READ_ONLY, DEV},
+ {"bPSAState", BYTE, (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV},
+ {"dPSADataSize", DWORD, (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV},
+ {"bRefClkGatingWaitTime", BYTE, URD, READ_ONLY, DEV},
+ {"bDeviceCaseRoughTemperaure", BYTE, URD, READ_ONLY, DEV},
+ {"bDeviceTooHighTempBoundary", BYTE, URD, READ_ONLY, DEV},
+/*1A*/ {"bDeviceTooLowTempBoundary", BYTE, URD, READ_ONLY, DEV},
+/*1B*/ {"bThrottlingStatus", BYTE, URD, READ_ONLY, DEV},
+/*1C*/ {"bWBBufFlushStatus", BYTE, URD, READ_ONLY, DEV | ARRAY},
+/*1D*/ {"bAvailableWBBufSize", BYTE, URD, READ_ONLY, DEV | ARRAY},
+/*1E*/ {"bWBBufLifeTimeEst", BYTE, URD, READ_ONLY, DEV | ARRAY},
+/*1F*/ {"bCurrentWBBufSize", DWORD, URD, READ_ONLY, DEV | ARRAY},
+ {ATTR_RSRV()},
+ {ATTR_RSRV()},
+ {ATTR_RSRV()},
+ {ATTR_RSRV()},
+ {ATTR_RSRV()},
+ {ATTR_RSRV()},
+ {ATTR_RSRV()},
+ {ATTR_RSRV()},
+ {ATTR_RSRV()},
+ {ATTR_RSRV()},
+ {ATTR_RSRV()},
+ {ATTR_RSRV()},
+/*2C*/ {"bRefreshStatus", BYTE, URD, READ_ONLY, DEV},
+ {"bRefreshFreq", BYTE, (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV},
+ {"bRefreshUnit", BYTE, (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV},
+ {"bRefreshMethod", BYTE, (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV}
+};
+
+struct flag_fields ufs_flags[] = {
+ {"Reserved", ACC_INVALID, MODE_INVALID, LEVEL_INVALID},
+ {"fDeviceInit", (URD|UWRT), (READ_NRML|SET_ONLY), DEV},
+ {"fPermanentWPEn", (URD|UWRT), (READ_NRML|WRITE_ONCE), DEV},
+ {"fPowerOnWPEn", (URD|UWRT), (READ_NRML|WRITE_PWR), DEV},
+ {"fBackgroundOpsEn", (URD|UWRT), (READ_NRML|WRITE_VLT), DEV},
+ {"fDeviceLifeSpanModeEn", (URD|UWRT), (READ_NRML|WRITE_VLT), DEV},
+ {"fPurgeEnable", UWRT, (WRITE_ONLY|WRITE_VLT), DEV},
+ {"fRefreshEnable", UWRT, (WRITE_ONLY|WRITE_VLT), DEV},
+ {"fPhyResourceRemoval", (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV},
+ {"fBusyRTC", URD, READ_ONLY, DEV},
+ {"Reserved", ACC_INVALID, MODE_INVALID, LEVEL_INVALID},
+ {"fPermanentlyDisableFw", (URD|UWRT), (READ_NRML|WRITE_ONCE), DEV},
+ {"Reserved", ACC_INVALID, MODE_INVALID, LEVEL_INVALID},
+/*D*/ {"Reserved", ACC_INVALID, MODE_INVALID, LEVEL_INVALID},
+/*E*/ {"fWriteBoosterEn", (URD|UWRT), (READ_NRML|WRITE_VLT), DEV | ARRAY},
+/*F*/ {"fWBFlushEn", (URD|UWRT), (READ_NRML|WRITE_VLT), DEV | ARRAY},
+/*10h*/ {"fWBFlushDuringHibernate", (URD|UWRT), (READ_NRML|WRITE_VLT),
+ DEV | ARRAY},
+/*11h*/ {"fHPBReset", (URD|UWRT), (READ_NRML|SET_ONLY), DEV},
+/*12h*/ {"fHPBEn", (URD|UWRT), (READ_NRML|WRITE_PRSIST), DEV},
+};
+
+static struct query_err_res query_err_status[] = {
+ {"Success", 0xF0},
+ {"Reserved1", 0xF1},
+ {"Reserved2", 0xF2},
+ {"Reserved3", 0xF3},
+ {"Reserved4", 0xF4},
+ {"Reserved5", 0xF5},
+ {"Parameter not readable", 0xF6},
+ {"Parameter not written", 0xF7},
+ {"Parameter already written", 0xF8},
+ {"Invalid LENGTH", 0xF9},
+ {"Invalid value", 0xFA},
+ {"Invalid SELECTOR", 0xFB},
+ {"Invalid INDEX", 0xFC},
+ {"Invalid IDN", 0xFD},
+ {"Invalid OPCODE", 0xFE},
+ {"General failure", 0xFF}
+};
+
+static const char *const desc_text[] = {
+ "Device",
+ "Config",
+ "Unit",
+ "RFU0",
+ "Interconnect",
+ "String",
+ "RFU1",
+ "Geometry",
+ "Power",
+ "Health"
+};
+
+int do_read_desc(int fd, struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp, __u8 idn, __u8 index,
+ __u16 desc_buf_len, __u8 *data_buf);
+static int do_unit_desc(int fd, __u8 lun);
+static int do_power_desc(int fd);
+static int do_conf_desc(int fd, __u8 opt, __u8 index, char *data_file);
+static int do_string_desc(int fd, char *str_data, __u8 idn, __u8 opr,
+ __u8 index);
+int do_query_rq(int fd, struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp, __u8 query_req_func,
+ __u8 opcode, __u8 idn, __u8 index, __u8 sel,
+ __u16 req_buf_len, __u16 res_buf_len, __u8 *data_buf);
+static int do_write_desc(int fd, struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp, __u8 idn, __u8 index,
+ __u16 desc_buf_len, __u8 *data_buf);
+static void query_response_error(__u8 opcode, __u8 idn);
+
+static void print_power_desc_icc(__u8 *desc_buf, int vccIndex)
+{
+ int i, offset = 0;
+ struct desc_field_offset *tmp;
+
+ if (vccIndex < 2 ||
+ vccIndex > ARRAY_SIZE(device_power_desc_conf_field_name) - 1) {
+ print_error("Illegal power desc index %d", vccIndex);
+ return;
+ }
+
+ tmp = &device_power_desc_conf_field_name[vccIndex];
+ offset = tmp->offset;
+ printf("\nPower Descriptor %s", tmp->name);
+ for (i = offset; i < offset + 32 ; i += 2) {
+ printf("\nLevel %2d value : 0x%x", (i - offset)/2,
+ be16toh((__u16)desc_buf[i]));
+ }
+ printf("\n");
+}
+
+static void print_vendor_info(__u8 *desc_buf, int len)
+{
+ int i;
+
+ if (!desc_buf)
+ return;
+
+ for (i = 0 ; i < len; i++) {
+ if (!(i%16))
+ printf("\t\n");
+ printf("0x%02x ", desc_buf[i]);
+ }
+ printf("\n");
+}
+
+void print_descriptors(char *desc_str, __u8 *desc_buf,
+ struct desc_field_offset *desc_array, int arr_size)
+{
+ int i;
+ struct desc_field_offset *tmp;
+ char str_buf[STR_BUF_LEN];
+ int offset = 0;
+
+ for (i = 0; offset < arr_size; ++i) {
+ tmp = &desc_array[i];
+ offset = tmp->offset + tmp->width_in_bytes;
+
+ if (tmp->width_in_bytes == BYTE) {
+ printf("%s [Byte offset 0x%x]: %s = 0x%x\n", desc_str,
+ tmp->offset, tmp->name, desc_buf[tmp->offset]);
+ } else if (tmp->width_in_bytes == WORD) {
+ printf("%s [Byte offset 0x%x]: %s = 0x%x\n", desc_str,
+ tmp->offset, tmp->name,
+ be16toh(*(__u16 *)&desc_buf[tmp->offset]));
+ } else if (tmp->width_in_bytes == DWORD) {
+ printf("%s [Byte offset 0x%x]: %s = 0x%x\n", desc_str,
+ tmp->offset, tmp->name,
+ be32toh(*(__u32 *)&desc_buf[tmp->offset]));
+ } else if (tmp->width_in_bytes == DDWORD) {
+ printf("%s [Byte offset 0x%x]: %s = 0x%lx\n",
+ desc_str, tmp->offset, tmp->name,
+ be64toh(*(__u64 *)&desc_buf[tmp->offset]));
+ } else if ((tmp->width_in_bytes > DDWORD) &&
+ tmp->width_in_bytes < STR_BUF_LEN) {
+ if (!strcmp(tmp->name, "VendorPropInfo")) {
+ printf("%s [Byte offset 0x%x]: %s =\n",
+ desc_str,
+ tmp->offset,
+ tmp->name);
+ print_vendor_info(&desc_buf[tmp->offset],
+ tmp->width_in_bytes);
+ } else {
+ memset(str_buf, 0, STR_BUF_LEN);
+ memcpy(str_buf, &desc_buf[tmp->offset],
+ tmp->width_in_bytes);
+ printf("%s [Byte offset 0x%x]: %s = %s\n",
+ desc_str,
+ tmp->offset, tmp->name, str_buf);
+ }
+ } else {
+ printf("%s [Byte offset 0x%x]: %s Wrong Width = %d",
+ desc_str, tmp->offset, tmp->name,
+ tmp->width_in_bytes);
+ }
+ }
+}
+
+static char *access_type_string(__u8 current_att, __u8 config_type,
+ char *access_string)
+{
+ enum acc_mode mode;
+
+ switch (config_type) {
+ case ATTR_TYPE:
+ if (current_att >= QUERY_ATTR_IDN_MAX)
+ return NULL;
+ mode = ufs_attrs[current_att].acc_mode;
+ break;
+ case FLAG_TYPE:
+ if (current_att >= QUERY_FLAG_IDN_MAX)
+ return NULL;
+ mode = ufs_flags[current_att].acc_mode;
+ break;
+ default:
+ return NULL;
+ }
+
+ if (mode & READ_NRML)
+ strcat(access_string, " | Read");
+ if (mode & READ_ONLY)
+ strcat(access_string, " | ReadOnly");
+ if (mode & WRITE_ONLY)
+ strcat(access_string, " | WriteOnly");
+ if (mode & WRITE_ONCE)
+ strcat(access_string, " | WriteOnce");
+ if (mode & WRITE_PRSIST)
+ strcat(access_string, " | Persistent");
+ if (mode & WRITE_VLT)
+ strcat(access_string, " | Volatile");
+ if (mode & SET_ONLY)
+ strcat(access_string, " | SetOnly");
+ if (mode & WRITE_PWR)
+ strcat(access_string, " | ResetOnPower");
+
+ return access_string;
+}
+
+static void query_response_error(__u8 opcode, __u8 idn)
+{
+ __u8 query_response_inx = opcode & 0x0F;
+
+ printf("\n %s, for idn 0x%02x\n",
+ query_err_status[query_response_inx].name, idn);
+}
+
+void desc_help(char *tool_name)
+{
+ printf("\n Descriptor command usage:\n");
+ printf("\n\t%s desc [-t] <descriptor idn> [-a|-r|-w] <data> [-p] "
+ "<device_path> \n", tool_name);
+ printf("\n\t-t\t description type idn\n"
+ "\t\t Available description types based on UFS ver 3.1 :\n"
+ "\t\t\t0:\tDevice\n"
+ "\t\t\t1:\tConfiguration\n"
+ "\t\t\t2:\tUnit\n"
+ "\t\t\t3:\tRFU\n"
+ "\t\t\t4:\tInterconnect\n"
+ "\t\t\t5:\tString\n"
+ "\t\t\t6:\tRFU\n"
+ "\t\t\t7:\tGeometry\n"
+ "\t\t\t8:\tPower\n"
+ "\t\t\t9:\tDevice Health\n"
+ "\t\t\t10..255: RFU\n");
+ printf("\n\t-r\t read operation (default) for readable descriptors\n");
+ printf("\n\t-w\t write operation , for writable descriptors\n");
+ printf("\t\t Set the input configuration file after -w opt\n");
+ printf("\t\t for Configuration descriptor\n");
+ printf("\t\t Set the input string after -w opt\n");
+ printf("\t\t for String descriptor\n");
+ printf("\n\t-i\t Set index parameter(default = 0)\n");
+ printf("\n\t-s\t Set selector parameter(default = 0)\n");
+ printf("\n\t-p\t path to ufs bsg device\n");
+}
+
+void attribute_help(char *tool_name)
+{
+ __u8 current_att = 0;
+ char access_string[100] = {0};
+
+ printf("\n Attributes command usage:\n");
+ printf("\n\t%s attr [-t] <attr_idn> [-a|-r|-w] <data_hex> [-p]"
+ " <device_path> \n", tool_name);
+ printf("\n\t-t\t Attributes type idn\n"
+ "\t\t Available attributes and its access based on"
+ " UFS ver 3.1 :\n");
+
+ while (current_att < ARRAY_SIZE(ufs_attrs)) {
+ printf("\t\t\t %-3d: %-25s %s\n",
+ current_att,
+ ufs_attrs[current_att].name,
+ access_type_string(current_att, ATTR_TYPE,
+ access_string));
+ current_att++;
+ memset(access_string, 0, 100);
+ }
+
+ printf("\n\t-a\tread and print all readable attributes"
+ " for the device\n");
+ printf("\n\t-r\tread operation (default), for readable attribute(s)\n");
+ printf("\n\t-w\twrite operation (with hex data),"
+ " for writable attribute\n");
+ printf("\n\t-i\t Set index parameter(default = 0)\n");
+ printf("\n\t-s\t Set selector parameter(default = 0)\n");
+ printf("\n\t-p\tpath to ufs bsg device\n");
+ printf("\n\tExample - Read bBootLunEn\n"
+ "\t\t%s attr -t 0 -p /dev/ufs-bsg\n", tool_name);
+}
+
+void flag_help(char *tool_name)
+{
+ __u8 current_flag = 0;
+ char access_string[100] = {0};
+
+ printf("\n Flags command usage:\n");
+ printf("\n\t%s fl [-t] <flag idn> [-a|-r|-o|-e] [-p]"
+ " <device_path>\n", tool_name);
+ printf("\n\t-t\t Flags type idn\n"
+ "\t\t Available flags and its access, based on UFS ver 3.1 :\n");
+
+ while (current_flag < QUERY_FLAG_IDN_MAX) {
+ printf("\t\t\t %-3d: %-25s %s\n", current_flag,
+ ufs_flags[current_flag].name,
+ access_type_string(current_flag, FLAG_TYPE,
+ access_string));
+ current_flag++;
+ memset(access_string, 0, 100);
+ }
+ printf("\n\t-a\t read and print all readable flags for the device\n");
+ printf("\n\t-r\t read operation (default), for readable flag(s)\n");
+ printf("\n\t-e\t set flag operation\n");
+ printf("\n\t-c\t clear/reset flag operation\n");
+ printf("\n\t-o\t toggle flag operation\n");
+ printf("\n\t-i\t Set index parameter(default = 0)\n");
+ printf("\n\t-s\t Set selector parameter(default = 0)\n");
+ printf("\n\t-p\t path to ufs bsg device\n");
+ printf("\n\tExample - Read the bkops operation flag\n"
+ "\t\t%s fl -t 4 -p /dev/ufs-bsg\n", tool_name);
+}
+
+int do_device_desc(int fd, __u8 *desc_buff)
+{
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+ __u8 data_buf[QUERY_DESC_DEVICE_MAX_SIZE] = {0};
+ int rc = 0;
+
+ rc = do_read_desc(fd, &bsg_req, &bsg_rsp,
+ QUERY_DESC_IDN_DEVICE, 0,
+ QUERY_DESC_DEVICE_MAX_SIZE, data_buf);
+ if (rc) {
+ print_error("Could not read device descriptor , error %d", rc);
+ goto out;
+ }
+ if (!desc_buff)
+ print_descriptors("Device Descriptor", data_buf,
+ device_desc_field_name, data_buf[0]);
+ else
+ memcpy(desc_buff, data_buf, data_buf[0]);
+
+out:
+ return rc;
+}
+
+static int do_unit_desc(int fd, __u8 lun)
+{
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+ __u8 data_buf[QUERY_DESC_UNIT_MAX_SIZE] = {0};
+ int ret = 0;
+
+ ret = do_read_desc(fd, &bsg_req, &bsg_rsp, QUERY_DESC_IDN_UNIT, lun,
+ QUERY_DESC_UNIT_MAX_SIZE, data_buf);
+ if (ret) {
+ print_error("Could not read unit descriptor error", ret);
+ goto out;
+ }
+
+ if (lun == 0xc4)
+ print_descriptors("RPMB LUN Descriptor", data_buf,
+ device_unit_rpmb_desc_field_name, data_buf[0]);
+ else
+ print_descriptors("LUN Descriptor", data_buf,
+ device_unit_desc_field_name, data_buf[0]);
+
+
+out:
+ return ret;
+}
+
+static int do_interconnect_desc(int fd)
+{
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+ __u8 data_buf[QUERY_DESC_INTERCONNECT_MAX_SIZE] = {0};
+ int ret = 0;
+
+ ret = do_read_desc(fd, &bsg_req, &bsg_rsp, QUERY_DESC_IDN_INTERCONNECT,
+ 0, QUERY_DESC_INTERCONNECT_MAX_SIZE, data_buf);
+ if (ret) {
+ print_error("Could not read interconnect descriptor error %d",
+ ret);
+ goto out;
+ }
+
+ print_descriptors("Interconnect Descriptor", data_buf,
+ device_interconnect_desc_conf_field_name, data_buf[0]);
+
+out:
+ return ret;
+}
+
+static int do_geo_desc(int fd)
+{
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+ __u8 data_buf[QUERY_DESC_GEOMETRY_MAX_SIZE] = {0};
+ int ret = 0;
+
+ ret = do_read_desc(fd, &bsg_req, &bsg_rsp, QUERY_DESC_IDN_GEOMETRY, 0,
+ QUERY_DESC_GEOMETRY_MAX_SIZE, data_buf);
+ if (ret) {
+ print_error("Could not read geometry descriptor , error %d",
+ ret);
+ goto out;
+ }
+
+ print_descriptors("Geometry Descriptor", data_buf,
+ device_geo_desc_conf_field_name, data_buf[0]);
+
+out:
+ return ret;
+}
+
+static int do_power_desc(int fd)
+{
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+ __u8 data_buf[QUERY_DESC_POWER_MAX_SIZE] = {0};
+ int ret = 0;
+
+ ret = do_read_desc(fd, &bsg_req, &bsg_rsp,
+ QUERY_DESC_IDN_POWER, 0, QUERY_DESC_POWER_MAX_SIZE,
+ data_buf);
+ if (ret) {
+ print_error("Could not read power descriptor , error %d", ret);
+ goto out;
+ }
+
+ printf("Power Descriptor[Byte offset 0x%x]: %s = 0x%x\n",
+ device_power_desc_conf_field_name[0].offset,
+ device_power_desc_conf_field_name[0].name, data_buf[0]);
+
+ printf("Power Descriptor[Byte offset 0x%x]: %s = 0x%x\n",
+ device_power_desc_conf_field_name[1].offset,
+ device_power_desc_conf_field_name[1].name, data_buf[1]);
+
+ print_power_desc_icc(data_buf, 2);
+ print_power_desc_icc(data_buf, 3);
+ print_power_desc_icc(data_buf, 4);
+
+out:
+ return ret;
+}
+
+static int do_health_desc(int fd)
+{
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+ __u8 data_buf[QUERY_DESC_HEALTH_MAX_SIZE] = {0};
+ int ret = 0;
+
+ ret = do_read_desc(fd, &bsg_req, &bsg_rsp, QUERY_DESC_IDN_HEALTH, 0,
+ QUERY_DESC_HEALTH_MAX_SIZE, data_buf);
+ if (ret) {
+ print_error("Could not read device health descriptor error %d",
+ ret);
+ goto out;
+ }
+
+ print_descriptors("Device Health Descriptor:", data_buf,
+ device_health_desc_conf_field_name, data_buf[0]);
+
+out:
+ return ret;
+}
+
+static void create_str_desc_data(__u8 *dest_buf, const char *str, __u8 len)
+{
+ int j = 3;
+ int i;
+
+ dest_buf[0] = len * 2 + 2;
+ dest_buf[1] = QUERY_DESC_IDN_STRING;
+ for (i = 0; i < len ; i++) {
+ dest_buf[j] = *(str++);
+ j = j + 2;
+ }
+}
+
+
+
+
+
+static int do_string_desc(int fd, char *str_data, __u8 idn, __u8 opr,
+ __u8 index)
+{
+ int rc = 0;
+ __u8 data_buf[QUERY_DESC_STRING_MAX_SIZE] = {0};
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+ int len, i;
+
+ if (opr == WRITE) {
+ len = strlen(str_data);
+ create_str_desc_data(data_buf, str_data, len);
+ rc = do_write_desc(fd, &bsg_req, &bsg_rsp,
+ QUERY_DESC_IDN_STRING, index,
+ len * 2 + 2, data_buf);
+ if (rc == OK)
+ printf("\nString Descriptor was written\n");
+ } else {
+ rc = do_read_desc(fd, &bsg_req, &bsg_rsp, QUERY_DESC_IDN_STRING,
+ index, QUERY_DESC_STRING_MAX_SIZE, data_buf);
+ if (!rc) {
+ printf("\nString Desc(Row data):\n");
+ for (i = 0; i < bsg_rsp.reply_payload_rcv_len; i++)
+ printf("0x%02x ", data_buf[i]);
+ printf("\n");
+ }
+ }
+ return rc;
+}
+
+static int do_conf_desc(int fd, __u8 opt, __u8 index, char *data_file)
+{
+ int rc = OK;
+ int file_size;
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+ __u8 conf_desc_buf[QUERY_DESC_CONFIGURAION_MAX_SIZE] = {0};
+ int offset, i;
+ int data_fd = INVALID;
+ char *filename_header = "config_desc_data_ind_%d";
+ char output_file[30] = {0};
+
+ if (opt == WRITE) {
+ data_fd = open(data_file, O_RDONLY);
+ if (data_fd < 0) {
+ perror("can't open input file");
+ return ERROR;
+ }
+
+ file_size = lseek(data_fd, 0, SEEK_END);
+ if (file_size <= 0) {
+ print_error("Wrong config file");
+ rc = ERROR;
+ goto out;
+ }
+ lseek(data_fd, 0, SEEK_SET);
+
+ rc = read(data_fd, conf_desc_buf, file_size);
+ if (rc <= 0) {
+ print_error("Cannot config file");
+ rc = ERROR;
+ goto out;
+ }
+
+ rc = do_write_desc(fd, &bsg_req, &bsg_rsp,
+ QUERY_DESC_IDN_CONFIGURAION, index,
+ file_size,
+ conf_desc_buf);
+ if (!rc)
+ printf("Config Descriptor was written to device\n");
+ } else {
+ __u8 head_off = CONFIG_HEADER_OFFSET;
+ __u8 lun_off = CONFIG_LUN_OFFSET;
+
+ rc = do_read_desc(fd, &bsg_req, &bsg_rsp,
+ QUERY_DESC_IDN_CONFIGURAION,
+ index, QUERY_DESC_CONFIGURAION_MAX_SIZE,
+ conf_desc_buf);
+ if (rc) {
+ print_error("Coudn't read config descriptor error %d",
+ rc);
+
+ goto out;
+ }
+
+ if (conf_desc_buf[0] == QUERY_DESC_CONFIGURAION_MAX_SIZE_3_0) {
+ head_off = CONFIG_HEADER_OFFSET_3_0;
+ lun_off = CONFIG_LUN_OFFSET_3_0;
+ }
+
+ print_descriptors("Config Device Descriptor:",
+ conf_desc_buf,
+ device_config_desc_field_name,
+ head_off);
+
+ offset = head_off;
+ for (i = 0 ; i < 8; i++) {
+ printf("Config %d Unit Descriptor:\n", i);
+ print_descriptors("Config Descriptor:",
+ conf_desc_buf + offset,
+ device_config_unit_desc_field_name,
+ lun_off);
+ offset = offset + lun_off;
+ }
+ sprintf(output_file, filename_header, index);
+ data_fd = open(output_file, O_WRONLY | O_CREAT | O_TRUNC,
+ S_IRUSR | S_IWUSR);
+ if (data_fd < 0) {
+ perror("can't open output file");
+ return ERROR;
+ }
+
+ rc = write(data_fd, conf_desc_buf, conf_desc_buf[0]);
+ if (rc <= 0) {
+ print_error("Could not write config data into %s file",
+ output_file);
+ rc = ERROR;
+ goto out;
+ }
+
+ printf("Config Descriptor was written into %s file\n",
+ output_file);
+ }
+out:
+ if (data_fd != INVALID)
+ close(data_fd);
+ return rc;
+}
+
+int do_desc(struct tool_options *opt)
+{
+ int fd;
+ int rc = OK;
+ int oflag = O_RDWR;
+
+ if (opt->opr == READ_ALL || opt->opr == READ)
+ oflag = O_RDONLY;
+
+ fd = open(opt->path, oflag);
+ if (fd < 0) {
+ print_error("open");
+ return ERROR;
+ }
+
+ if (opt->opr == READ_ALL) {
+ if (do_device_desc(fd, NULL) || do_unit_desc(fd, 0) ||
+ do_interconnect_desc(fd) || do_geo_desc(fd) ||
+ do_power_desc(fd) ||
+ do_health_desc(fd) ||
+ do_conf_desc(fd, READ, 0, NULL))
+ rc = ERROR;
+ goto out;
+ }
+
+ switch (opt->idn) {
+ case QUERY_DESC_IDN_DEVICE:
+ rc = do_device_desc(fd, NULL);
+ break;
+ case QUERY_DESC_IDN_CONFIGURAION:
+ if (opt->opr == READ)
+ rc = do_conf_desc(fd, opt->opr, opt->index, NULL);
+ else
+ rc = do_conf_desc(fd, opt->opr, opt->index,
+ (char *)opt->data);
+ break;
+ case QUERY_DESC_IDN_UNIT:
+ rc = do_unit_desc(fd, opt->index);
+ break;
+ case QUERY_DESC_IDN_GEOMETRY:
+ rc = do_geo_desc(fd);
+ break;
+ case QUERY_DESC_IDN_POWER:
+ rc = do_power_desc(fd);
+ break;
+ case QUERY_DESC_IDN_STRING:
+ rc = do_string_desc(fd, (char *)opt->data, opt->idn, opt->opr,
+ opt->index);
+ break;
+ case QUERY_DESC_IDN_HEALTH:
+ rc = do_health_desc(fd);
+ break;
+ case QUERY_DESC_IDN_INTERCONNECT:
+ rc = do_interconnect_desc(fd);
+ break;
+ default:
+ print_error("Unsupported Descriptor type %d", opt->idn);
+ rc = -EINVAL;
+ break;
+ }
+
+out:
+ close(fd);
+ return rc;
+}
+
+void print_attribute(struct attr_fields *attr, __u8 *attr_buffer)
+{
+ if (attr->width_in_bytes == BYTE)
+ printf("%-26s := 0x%02x\n", attr->name, attr_buffer[0]);
+ else if (attr->width_in_bytes == WORD)
+ printf("%-26s := 0x%04x\n", attr->name, *(__u16 *)attr_buffer);
+ else if (attr->width_in_bytes == DWORD)
+ printf("%-26s := 0x%08x\n", attr->name,
+ be32toh(*(__u32 *)attr_buffer));
+}
+
+int do_query_rq(int fd, struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp, __u8 query_req_func,
+ __u8 opcode, __u8 idn, __u8 index, __u8 sel,
+ __u16 req_buf_len, __u16 res_buf_len, __u8 *data_buf)
+{
+ int rc = OK;
+ __u8 res_code;
+ __u16 len = res_buf_len;
+
+ if (req_buf_len > 0)
+ len = req_buf_len;
+
+ prepare_upiu(bsg_req, query_req_func, len, opcode, idn,
+ index, sel);
+
+ rc = send_bsg_scsi_trs(fd, bsg_req, bsg_rsp, req_buf_len, res_buf_len,
+ data_buf);
+
+ if (rc) {
+ print_error("%s: query failed, status %d idn: %d, i: %d, s: %d",
+ __func__, rc, idn, index, sel);
+ rc = ERROR;
+ goto out;
+ }
+
+ res_code = (be32toh(bsg_rsp->upiu_rsp.header.dword_1) >> 8) & 0xff;
+ if (res_code) {
+ query_response_error(res_code, idn);
+ rc = ERROR;
+ }
+out:
+ return rc;
+}
+
+static int do_write_desc(int fd, struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp, __u8 idn, __u8 index,
+ __u16 desc_buf_len, __u8 *data_buf)
+{
+ return do_query_rq(fd, bsg_req, bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_WRITE_REQUEST,
+ UPIU_QUERY_OPCODE_WRITE_DESC, idn, index,
+ 0, desc_buf_len, 0, data_buf);
+}
+
+static int check_read_desc_size(__u8 idn, __u8 *data_buf)
+{
+ bool unoff = false;
+ int rc = OK;
+
+ switch (idn) {
+ case QUERY_DESC_IDN_DEVICE:
+ if ((data_buf[0] != QUERY_DESC_DEVICE_MAX_SIZE) &&
+ (data_buf[0] != QUERY_DESC_DEVICE_MAX_SIZE_3_0))
+ unoff = true;
+ break;
+ case QUERY_DESC_IDN_CONFIGURAION:
+ if ((data_buf[0] != QUERY_DESC_CONFIGURAION_MAX_SIZE) &&
+ (data_buf[0] != QUERY_DESC_CONFIGURAION_MAX_SIZE_3_0))
+ unoff = true;
+ break;
+ case QUERY_DESC_IDN_UNIT:
+ if ((data_buf[0] != QUERY_DESC_UNIT_MAX_SIZE) &&
+ (data_buf[0] != QUERY_DESC_UNIT_MAX_SIZE_3_0))
+ unoff = true;
+ break;
+ case QUERY_DESC_IDN_INTERCONNECT:
+ if (data_buf[0] != QUERY_DESC_INTERCONNECT_MAX_SIZE)
+ unoff = true;
+ break;
+ case QUERY_DESC_IDN_GEOMETRY:
+ if ((data_buf[0] != QUERY_DESC_GEOMETRY_MAX_SIZE) &&
+ (data_buf[0] != QUERY_DESC_GEOMETRY_MAX_SIZE_3_0))
+ unoff = true;
+ break;
+ case QUERY_DESC_IDN_POWER:
+ if (data_buf[0] != QUERY_DESC_POWER_MAX_SIZE)
+ unoff = true;
+ break;
+ case QUERY_DESC_IDN_HEALTH:
+ if ((data_buf[0] != QUERY_DESC_HEALTH_MAX_SIZE) &&
+ (data_buf[0] != QUERY_DESC_HEALTH_MAX_SIZE_2_1))
+ unoff = true;
+ break;
+ }
+
+ if (unoff) {
+ int file_status;
+
+ rc = ERROR;
+ print_error("Unofficial %s desc size, len = 0x%x",
+ (char *)desc_text[idn], data_buf[0]);
+ file_status = write_file("unofficial.dat", data_buf,
+ data_buf[0]);
+ if (!file_status)
+ printf("\nunofficial.dat raw data file was created\n");
+ }
+
+ return rc;
+}
+
+int do_read_desc(int fd, struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp, __u8 idn, __u8 index,
+ __u16 desc_buf_len, __u8 *data_buf)
+{
+ int rc;
+
+ rc = do_query_rq(fd, bsg_req, bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_READ_REQUEST,
+ UPIU_QUERY_OPCODE_READ_DESC, idn, index, 0,
+ 0, desc_buf_len, data_buf);
+ if (!rc)
+ rc = check_read_desc_size(idn, data_buf);
+
+ return rc;
+}
+
+int do_attributes(struct tool_options *opt)
+{
+ int fd;
+ int rc = OK;
+ struct attr_fields *tmp = NULL;
+ int oflag = O_RDWR;
+ __u8 att_idn;
+ __u32 attr_value;
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+
+ if (opt->opr == READ_ALL || opt->opr == READ)
+ oflag = O_RDONLY;
+
+ fd = open(opt->path, oflag);
+ if (fd < 0) {
+ print_error("open");
+ return ERROR;
+ }
+ tmp = &ufs_attrs[opt->idn];
+
+ if (opt->opr == READ_ALL) {
+ att_idn = QUERY_ATTR_IDN_BOOT_LU_EN;
+
+ while (att_idn < ARRAY_SIZE(ufs_attrs)) {
+ tmp = &ufs_attrs[att_idn];
+ if (tmp->acc_type == ACC_INVALID ||
+ tmp->acc_mode == WRITE_ONLY) {
+ att_idn++;
+ continue;
+ }
+
+ rc = do_query_rq(fd, &bsg_req, &bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_READ_REQUEST,
+ UPIU_QUERY_OPCODE_READ_ATTR, att_idn,
+ opt->index, opt->selector, 0, 0, 0);
+ if (rc == OK) {
+ attr_value = be32toh(bsg_rsp.upiu_rsp.qr.value);
+ print_attribute(tmp, (__u8 *)&attr_value);
+ }
+
+ memset(&bsg_rsp, 0, BSG_REPLY_SZ);
+ att_idn++;
+ }
+ } else if (opt->opr == WRITE) {
+ if (tmp->acc_type == ACC_INVALID ||
+ tmp->acc_mode == READ_ONLY) {
+ print_error("%s Attribute is not writable", tmp->name);
+ rc = ERROR;
+ goto out;
+ }
+
+ attr_value = *(__u32 *)opt->data;
+ switch (tmp->width_in_bytes) {
+ case BYTE:
+ if (attr_value > 0xFF) {
+ print_error("Wrong write data for %s attr\n",
+ tmp->name);
+ rc = ERROR;
+ goto out;
+ }
+ break;
+ case WORD:
+ if (attr_value > 0xFFFF) {
+ print_error("Wrong write data for %s attr\n",
+ tmp->name);
+ rc = ERROR;
+ goto out;
+ }
+ break;
+ case DWORD:
+ /* avoid -switch warning - no need to check value */
+ break;
+ default:
+ print_error("Unsupported width %d",
+ tmp->width_in_bytes);
+ rc = ERROR;
+ goto out;
+ }
+
+ bsg_req.upiu_req.qr.value = htobe32(attr_value);
+ rc = do_query_rq(fd, &bsg_req, &bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_WRITE_REQUEST,
+ UPIU_QUERY_OPCODE_WRITE_ATTR, opt->idn,
+ opt->index, opt->selector, 0, 0, 0);
+ } else if (opt->opr == READ) {
+ if (tmp->acc_type == ACC_INVALID ||
+ tmp->acc_mode == WRITE_ONLY) {
+ print_error("%s attribute is not readable", tmp->name);
+ rc = ERROR;
+ goto out;
+ }
+
+ rc = do_query_rq(fd, &bsg_req, &bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_READ_REQUEST,
+ UPIU_QUERY_OPCODE_READ_ATTR, opt->idn,
+ opt->index, opt->selector, 0, 0, 0);
+ if (rc == OK) {
+ attr_value = be32toh(bsg_rsp.upiu_rsp.qr.value);
+ print_attribute(tmp, (__u8 *)&attr_value);
+ }
+ }
+out:
+ close(fd);
+ return rc;
+}
+
+int do_flags(struct tool_options *opt)
+{
+ int fd;
+ int rc = OK;
+ __u8 opcode, flag_idn;
+ struct flag_fields *tmp;
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+ int oflag = O_RDWR;
+
+ if (opt->opr == READ_ALL || opt->opr == READ)
+ oflag = O_RDONLY;
+
+ fd = open(opt->path, oflag);
+ if (fd < 0) {
+ print_error("open");
+ return ERROR;
+ }
+
+ tmp = &ufs_flags[opt->idn];
+
+ switch (opt->opr) {
+ case READ_ALL:
+ flag_idn = QUERY_FLAG_IDN_FDEVICEINIT;
+ printf("UFS Device Flags:\n");
+ while (flag_idn < QUERY_FLAG_IDN_MAX) {
+ tmp = &ufs_flags[flag_idn];
+ if (tmp->acc_type == ACC_INVALID ||
+ tmp->acc_type == UWRT) {
+ flag_idn++;
+ continue;
+ }
+
+ rc = do_query_rq(fd, &bsg_req, &bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_READ_REQUEST,
+ UPIU_QUERY_OPCODE_READ_FLAG, flag_idn,
+ opt->index, opt->selector, 0, 0, 0);
+ if (rc == OK) {
+ printf("%-26s := 0x%01x\n", tmp->name,
+ be32toh(bsg_rsp.upiu_rsp.qr.value) &
+ 0xff);
+ } else {
+ /* on failuire make note and keep going */
+ print_error("Read for flag %s failed",
+ tmp->name);
+ }
+
+ memset(&bsg_rsp, 0, BSG_REPLY_SZ);
+ flag_idn++;
+ }
+ break;
+ case CLEAR_FLAG:
+ case TOGGLE_FLAG:
+ case SET_FLAG:
+ if (tmp->acc_type == ACC_INVALID ||
+ tmp->acc_mode == READ_ONLY) {
+ print_error("%s flag is not writable", tmp->name);
+ rc = ERROR;
+ } else if ((tmp->acc_mode & SET_ONLY) &&
+ opt->opr != SET_FLAG) {
+ print_error("Only set operation supported for %s flag",
+ tmp->name);
+ rc = ERROR;
+ } else {
+ if (opt->opr == CLEAR_FLAG)
+ opcode = UPIU_QUERY_OPCODE_CLEAR_FLAG;
+ else if (opt->opr == SET_FLAG)
+ opcode = UPIU_QUERY_OPCODE_SET_FLAG;
+ else if (opt->opr == TOGGLE_FLAG)
+ opcode = UPIU_QUERY_OPCODE_TOGGLE_FLAG;
+ rc = do_query_rq(fd, &bsg_req, &bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_WRITE_REQUEST,
+ opcode, opt->idn, opt->index,
+ opt->selector, 0, 0, 0);
+ if (rc)
+ print_error("The operation for flag %s failed",
+ tmp->name);
+ }
+ break;
+ case READ:/*Read operation */
+ if (tmp->acc_type == ACC_INVALID || tmp->acc_type == UWRT) {
+ print_error("%s flag is not readable", tmp->name);
+ rc = ERROR;
+ } else {
+ rc = do_query_rq(fd, &bsg_req, &bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_READ_REQUEST,
+ UPIU_QUERY_OPCODE_READ_FLAG, opt->idn,
+ opt->index, opt->selector, 0, 0, 0);
+ if (rc == OK)
+ printf("%-26s := 0x%01x\n", tmp->name,
+ be32toh(bsg_rsp.upiu_rsp.qr.value) &
+ 0xff);
+ else
+ print_error("Read for flag %s failed", tmp->name);
+ }
+ break;
+ default:
+ print_error("Unsupported operation for %s flag", tmp->name);
+ rc = ERROR;
+ break;
+ }
+
+ close(fd);
+ return rc;
+}
diff --git a/libmtk_bsg/ufs_cmds.h b/libmtk_bsg/ufs_cmds.h
new file mode 100644
index 0000000..abd68fb
--- /dev/null
+++ b/libmtk_bsg/ufs_cmds.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef UFS_CMNDS_H_
+#define UFS_CMNDS_H_
+
+#include "options.h"
+#include <asm/types.h>
+
+
+enum field_width {
+ BYTE = (1 << 0),
+ WORD = (1 << 1),
+ DWORD = (1 << 2),
+ DDWORD = (1 << 3)
+};
+
+struct desc_field_offset {
+ char *name;
+ int offset;
+ enum field_width width_in_bytes;
+};
+
+enum acc_mode {
+ READ_NRML = (1 << 0),
+ READ_ONLY = (1 << 1),
+ WRITE_ONLY = (1 << 2),
+ WRITE_ONCE = (1 << 3),
+ WRITE_PRSIST = (1 << 4),
+ WRITE_VLT = (1 << 5),
+ SET_ONLY = (1 << 6),
+ WRITE_PWR = (1 << 7),
+ MODE_INVALID = (1 << 8)
+};
+
+enum attr_level {
+ DEV = (1 << 0),
+ ARRAY = (1 << 1),
+ LEVEL_INVALID = (1 << 2)
+};
+
+enum access_type {
+ URD = (1 << 0),
+ UWRT = (1 << 1),
+ ACC_INVALID = (1 << 2)
+};
+
+struct attr_fields {
+ char *name;
+ enum field_width width_in_bytes;
+ enum access_type acc_type;
+ enum acc_mode acc_mode;
+ enum attr_level device_level;
+};
+
+struct flag_fields {
+ char *name;
+ enum access_type acc_type;
+ enum acc_mode acc_mode;
+ enum attr_level device_level;
+};
+
+int do_desc(struct tool_options *opt);
+int do_attributes(struct tool_options *opt);
+int do_flags(struct tool_options *opt);
+void print_command_help (char *prgname, int config_type);
+int do_device_desc(int fd, __u8 *desc_buff);
+void desc_help(char *tool_name);
+void attribute_help(char *tool_name);
+void flag_help(char *tool_name);
+#endif /* UFS_CMNDS_H_ */
diff --git a/libmtk_bsg/ufs_err_hist.c b/libmtk_bsg/ufs_err_hist.c
new file mode 100644
index 0000000..bf7e15d
--- /dev/null
+++ b/libmtk_bsg/ufs_err_hist.c
@@ -0,0 +1,259 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * UFS3.0(UFSv3.0 JESD220D spec.) allows to retrieve the error history by using
+ * the READ BUFFER command.
+ */
+
+#include "ufs_err_hist.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <endian.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <errno.h>
+
+#include "ufs.h"
+#include "ufs_cmds.h"
+#include "options.h"
+#include "ioctl.h"
+
+#define MIN(a, b) (((a) < (b))?(a):(b))
+#define MAX(a, b) (((a) > (b))?(a):(b))
+
+/*
+ * The spec actualy says: "and ALLOCATION LENGTH set to at least 2088
+ * (i.e., large enough to transfer the complete error history directory)."
+ * This is apparently an error because it doesn't adds up to the entries
+ * count and sizes: The error history header is 32bytes, and there can be
+ * up to (0xEF – 0x10 + 1) = 224 entries. Each entry is 8bytes, so the
+ * directory should weight 224*8 + 32 = 1824bytes, and not 2088.
+ */
+#define EHS_DIR_ALLOC_LEN 1824
+/* According to the spec, BUF ID can be between 0x10 0xEF range */
+#define EHS_MIN_BUF_ID 0x10
+#define EHS_MAX_BUF_ID 0xEF
+#define EHS_MAX_ENTRIES (EHS_MAX_BUF_ID - EHS_MIN_BUF_ID + 1)
+/* 3 bytes for Allocation Length field + 3 bytes for Buffer offset field*/
+#define READ_BUF_MAX_AVAIL_LEN (0xFFFFFF + 0xFFFFFF)
+#define BLOCKS_IN_FAD_BLOCK (MAX_IOCTL_BUF_SIZE / BLOCK_SIZE)
+
+struct ehs_directory_entry {
+ u_int8_t buffer_id;
+ u_int8_t reserved[3];
+ u_int32_t length;
+};
+
+struct ehs_directory_header {
+ u_int8_t vendor_id[8];
+ u_int8_t version;
+ u_int8_t reserved1;
+ u_int8_t reserved2[20];
+ u_int16_t length;
+};
+
+struct ehs_directory_buffer {
+ struct ehs_directory_header hdr;
+ struct ehs_directory_entry entries[EHS_MAX_ENTRIES];
+};
+
+static inline int write_single_fad(const int file, const void *buffer, int sz)
+{
+ if (write(file, buffer, sz) != sz)
+ return -EIO;
+ else
+ return 0;
+}
+
+static int log_ehs_buffer(int fd, int file, __u8 *buf, __u8 buf_id,
+ __u32 len, __u8 sg_type)
+{
+ int rc = -EINVAL;
+ __u32 sent = 0;
+ int i = 0;
+
+ printf("\nPlease wait for error history extraction\n");
+ while (sent < len) {
+ __u32 ofst = i * MAX_IOCTL_BUF_SIZE;
+ __u32 sz =
+ (len - sent >= MAX_IOCTL_BUF_SIZE) ? MAX_IOCTL_BUF_SIZE :
+ (len - sent);
+
+ rc = read_buffer(fd, buf, BUFFER_EHS_MODE, buf_id, ofst, sz,
+ sg_type);
+ if (rc) {
+ print_error("read_buffer buff_id 0x%x fad %d",
+ buf_id, i);
+ goto out;
+ }
+
+ rc = write_single_fad(file, buf, sz);
+ if (rc) {
+ print_error("write buf_id 0x%x fad %d", buf_id, i);
+ goto out;
+ }
+
+ sent += sz;
+ i++;
+ memset(buf, 0x0, MAX_IOCTL_BUF_SIZE);
+ }
+
+ rc = 0;
+out:
+ return rc;
+}
+
+static int log_error_history(int fd, struct ehs_directory_entry *entries,
+ __u8 buffers_cnt, __u8 sg_type)
+{
+ int file;
+ __u8 *buf = NULL;
+ int rc = -EINVAL;
+ int i;
+
+ file = open("error_history.dat", O_RDWR | O_CREAT | O_TRUNC | O_SYNC,
+ S_IWUSR | S_IRUSR);
+ if (file == -1) {
+ perror("open");
+ goto out;
+ }
+
+ /* IO size is limited by max_sectors_kb which is usually 512k - set a
+ * slightly smaller chunk - 256k.
+ */
+ buf = calloc(1, MAX_IOCTL_BUF_SIZE);
+ if (!buf) {
+ rc = -ENOMEM;
+ goto out;
+ }
+
+ for (i = 0; i < buffers_cnt; i++) {
+ struct ehs_directory_entry *entry = entries + i;
+ u_int8_t buf_id = entry->buffer_id;
+ u_int32_t len = be32toh(entry->length);
+
+ if (buf_id < EHS_MIN_BUF_ID || buf_id > EHS_MAX_BUF_ID) {
+ print_error("illegal buffer id 0x%x entry %d",
+ buf_id, i);
+ goto out;
+ }
+
+ if (!len || len > READ_BUF_MAX_AVAIL_LEN) {
+ print_error("illegal len 0x%x entry %d", len, i);
+ goto out;
+ }
+
+ rc = log_ehs_buffer(fd, file, buf, buf_id, len, sg_type);
+ if (rc) {
+ print_error("log_ehs_buffer buffer id 0x%x", buf_id);
+ goto out;
+ }
+ }
+
+ rc = 0;
+out:
+ if (buf)
+ free(buf);
+
+ if (file != -1)
+ close(file);
+ return rc;
+}
+
+static int decode_ehs_directory(struct ehs_directory_buffer *ehs_dir,
+ __u8 *buffers_cnt)
+{
+ int rc = -EINVAL;
+ u_int16_t length = 0;
+
+ if (!ehs_dir)
+ goto out;
+
+ length = be16toh(ehs_dir->hdr.length);
+ if (!length || length % sizeof(struct ehs_directory_entry)) {
+ print_error("Illegal directory length 0x%x", length);
+ goto out;
+ }
+
+ *buffers_cnt = length / sizeof(struct ehs_directory_entry);
+ if (*buffers_cnt > EHS_MAX_ENTRIES) {
+ print_error("Illegal buffers count %d", *buffers_cnt);
+ goto out;
+ }
+
+ rc = 0;
+out:
+ return rc;
+}
+
+int do_err_hist(struct tool_options *opt)
+{
+ int rc = INVALID;
+ int fd;
+ __u8 *ehs_buf = NULL;
+ struct ehs_directory_buffer *ehs_dir = NULL;
+ __u8 ehs_buffer_cnt = 0;
+
+ fd = open(opt->path, O_RDWR | O_SYNC);
+ if (fd < 0) {
+ perror("open");
+ return ERROR;
+ }
+
+ WRITE_LOG("Start : %s cmd type %d", __func__, opt->idn);
+ ehs_buf = calloc(1, EHS_DIR_ALLOC_LEN);
+ if (!ehs_buf) {
+ rc = -ENOMEM;
+ goto out;
+ }
+
+ rc = read_buffer(fd, ehs_buf, BUFFER_EHS_MODE, 0, 0,
+ EHS_DIR_ALLOC_LEN, opt->sg_type);
+ if (rc)
+ goto out;
+
+ rc = write_file("error_history_directory.dat", ehs_buf,
+ EHS_DIR_ALLOC_LEN);
+ if (rc)
+ goto out;
+
+ printf("error_history_directory.dat is created\n");
+
+ ehs_dir = (struct ehs_directory_buffer *)ehs_buf;
+ rc = decode_ehs_directory(ehs_dir, &ehs_buffer_cnt);
+ if (rc)
+ goto out;
+
+ printf("retrieving error history, this may take a while\n\n");
+ rc = log_error_history(fd, ehs_dir->entries, ehs_buffer_cnt,
+ opt->sg_type);
+ if (rc)
+ goto out;
+
+ printf("\nerror_history.dat is created\n");
+
+out:
+ if (ehs_buf)
+ free(ehs_buf);
+ close(fd);
+
+ return rc;
+}
+
+void err_hist_help(char *tool_name)
+{
+ printf("\n Error history command usage:\n");
+ printf("\n\t%s err_hist [-p] <path to device> \n", tool_name);
+ printf("\n\t-g\tsg struct ver - 0: SG_IO_VER4 (default), 1: SG_IO_VER3\n");
+ printf("\n\t-p\tPath to the bsg device\n");
+}
diff --git a/libmtk_bsg/ufs_err_hist.h b/libmtk_bsg/ufs_err_hist.h
new file mode 100644
index 0000000..bb83fe9
--- /dev/null
+++ b/libmtk_bsg/ufs_err_hist.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef UFS_ERR_HIST_H_
+#define UFS_ERR_HIST_H_
+#include "options.h"
+
+void err_hist_help(char *tool_name);
+int do_err_hist(struct tool_options *opt);
+#endif /*UFS_ERR_HIST_H_*/
diff --git a/libmtk_bsg/ufs_ffu.c b/libmtk_bsg/ufs_ffu.c
new file mode 100644
index 0000000..956d82b
--- /dev/null
+++ b/libmtk_bsg/ufs_ffu.c
@@ -0,0 +1,219 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#pragma GCC diagnostic ignored "-Wall"
+#pragma GCC diagnostic ignored "-Wextra"
+#pragma GCC diagnostic ignored "-Wsign-compare"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <endian.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <errno.h>
+
+#include "ufs.h"
+#include "ufs_cmds.h"
+#include "options.h"
+#include "ioctl.h"
+#include "ufs_ffu.h"
+#include "scsi_bsg_util.h"
+
+#define DEVICE_VERSION_OFFSET 0x1E
+#define FFU_STATUS_ATTR 0x14
+
+enum ffu_status_type {
+ NO_INFORMATION,
+ SUCCESSFUL_MICROCODE_UPDATE,
+ MICROCODE_CORRUPTION_ERROR,
+ INTERNAL_ERROR,
+ MICROCODE_VERSION_MISMATCH,
+ GENERAL_ERROR = 0xFF
+};
+
+
+extern int do_query_rq(int fd, struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp, __u8 query_req_func,
+ __u8 opcode, __u8 idn, __u8 index, __u8 sel,
+ __u16 req_buf_len, __u16 res_buf_len, __u8 *data_buf);
+extern struct desc_field_offset device_desc_field_name[];
+
+/* Get sense key string or NULL if not available */
+static const char *
+ffu_status_string(enum ffu_status_type status)
+{
+ switch (status) {
+ case NO_INFORMATION:
+ return "NO INFORMATION";
+ break;
+ case SUCCESSFUL_MICROCODE_UPDATE:
+ return "SUCCESSFUL MICROCODE UPDATE";
+ break;
+ case INTERNAL_ERROR:
+ return "INTERNAL ERROR";
+ break;
+ case MICROCODE_CORRUPTION_ERROR:
+ return "MICROCODE CORRUPTION ERROR";
+ break;
+ case GENERAL_ERROR:
+ return "GENERAL ERROR";
+ break;
+ default:
+ return "UNSUPPORTED STATUS";
+ break;
+ }
+ return 0;
+}
+
+static int flash_ffu(int fd, struct tool_options *opt)
+{
+ int rc = INVALID;
+ int input_fd = INVALID;
+ off_t file_size;
+ __u8 *p_data = NULL;
+ uint32_t chunk_size = opt->size;
+ uint32_t buf_offset = 0;
+ uint32_t write_buf_count;
+
+ input_fd = open(opt->data, O_RDONLY | O_SYNC);
+ if (input_fd < 0) {
+ perror("Input file open");
+ goto out;
+ }
+
+ file_size = lseek(input_fd, 0, SEEK_END);
+ /* The FFU file shall be aligned to 4k */
+ if ((file_size <= 0) || (file_size % ALIGNMENT_CHUNK_SIZE)) {
+ print_error("Wrong FFU file");
+ goto out;
+ }
+ lseek(input_fd, 0, SEEK_SET);
+ p_data = calloc(file_size, sizeof(__u8));
+ if (!p_data) {
+ print_error("Cannot allocate FFU size %d", file_size);
+ goto out;
+ }
+ if (read(input_fd, (char *)p_data, file_size) !=
+ file_size) {
+ print_error("Read FFU is failed");
+ goto out;
+ }
+
+ while (file_size > 0) {
+ if (file_size > chunk_size)
+ write_buf_count = chunk_size;
+ else
+ write_buf_count = file_size;
+ rc = write_buffer(fd, p_data + buf_offset, BUFFER_FFU_MODE, 0,
+ buf_offset, write_buf_count, opt->sg_type);
+ if (rc) {
+ print_error("Write error %d:", rc);
+ goto out;
+ }
+ buf_offset = buf_offset + write_buf_count;
+ file_size = file_size - write_buf_count;
+ }
+
+ sync();
+ printf("\nFFU was written to the device, reboot and check status\n");
+
+out:
+ if (input_fd != INVALID)
+ close(input_fd);
+ if (p_data)
+ free(p_data);
+ return rc;
+
+}
+
+static int check_ffu_status(int fd, struct tool_options *opt)
+{
+ int rc = ERROR;
+ __u8 dev_desc[QUERY_DESC_DEVICE_MAX_SIZE] = {0};
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+ __u32 attr_value;
+ __u16 *ufs_feature_support;
+ struct desc_field_offset *tmp = &device_desc_field_name[DEVICE_VERSION_OFFSET];
+
+ rc = do_query_rq(fd, &bsg_req, &bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_READ_REQUEST,
+ UPIU_QUERY_OPCODE_READ_ATTR, FFU_STATUS_ATTR,
+ 0, 0, 0, 0, 0);
+ if (rc) {
+ print_warn("cannot read bDeviceFFUStatus attribute status");
+ goto out;
+ }
+
+ else {
+ attr_value = be32toh(bsg_rsp.upiu_rsp.qr.value);
+ printf("%-20s := 0x%02x (%s)\n", "bDeviceFFUStatus",
+ attr_value,
+ ffu_status_string((enum ffu_status_type)attr_value));
+ }
+
+ rc = do_device_desc(fd, (__u8 *)&dev_desc);
+ if (rc != OK)
+ print_error("Could not read device descriptor in order to "
+ "read device version\n");
+ else {
+ ufs_feature_support = (__u16 *)&dev_desc[tmp->offset];
+ printf("%s = 0x%x\n", tmp->name, *ufs_feature_support);
+ }
+out:
+ return rc;
+}
+
+int do_ffu(struct tool_options *opt)
+{
+ int rc = INVALID;
+ int fd = INVALID;
+
+ fd = open(opt->path, O_RDWR | O_SYNC);
+ if (fd < 0) {
+ perror("open");
+ exit(1);
+ }
+
+ switch (opt->idn) {
+ case UFS_FFU:
+ rc = flash_ffu(fd, opt);
+ break;
+ case UFS_CHECK_FFU_STATUS:
+ rc = check_ffu_status(fd, opt);
+ break;
+ default:
+ print_error("Unsupported FFU type operation");
+ break;
+ }
+
+ close(fd);
+ return rc;
+}
+
+void ffu_help(char *tool_name)
+{
+ printf("\n FFU command usage:\n");
+ printf("\n\t%s ffu [-t] <ffu cmd idn> [-p] <path to device> \n",
+ tool_name);
+ printf("\n\t-t\t FFU cmd idn\n");
+ printf("\t\t\t %-3d: %-25s\n",
+ UFS_FFU,
+ "FFU, flash FFU");
+ printf("\t\t\t %-3d: %-25s\n",
+ UFS_CHECK_FFU_STATUS,
+ "Check FFU status (check FFU status attribute and display FW version)");
+ printf("\n\t-s\t Max chunk size in KB alignment to 4KB, "
+ "which FFU file will be split (optional)\n");
+ printf("\n\t-w\t path to FFU file\n");
+ printf("\n\t-g\t sg struct ver - 0: SG_IO_VER4 (default), 1: SG_IO_VER3\n");
+ printf("\n\t-p\t bsg device path for FFU, ufs-bsg for Check FFU status\n");
+}
diff --git a/libmtk_bsg/ufs_ffu.h b/libmtk_bsg/ufs_ffu.h
new file mode 100644
index 0000000..beefd07
--- /dev/null
+++ b/libmtk_bsg/ufs_ffu.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef UFS_FFU_H_
+#define UFS_FFU_H_
+
+
+enum ffu_type {
+ UFS_FFU = 0,
+ UFS_CHECK_FFU_STATUS,
+ UFS_FFU_MAX
+};
+
+void ffu_help(char *tool_name);
+int do_ffu(struct tool_options *opt);
+#endif /*UFS_FFU_H_*/
diff --git a/libmtk_bsg/ufs_hmr.c b/libmtk_bsg/ufs_hmr.c
new file mode 100644
index 0000000..92c3e0c
--- /dev/null
+++ b/libmtk_bsg/ufs_hmr.c
@@ -0,0 +1,1182 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#pragma GCC diagnostic ignored "-Wall"
+#pragma GCC diagnostic ignored "-Wextra"
+#pragma GCC diagnostic ignored "-Wsign-compare"
+#pragma GCC diagnostic ignored "-Wpointer-bool-conversion"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <endian.h>
+#include <fcntl.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "ufs.h"
+#include "ufs_hmr.h"
+#include "ufs_cmds.h"
+
+/*
+ * HMR Quirks: 1 - enable, 0 - disable
+ */
+
+/*
+ * Big-little-endian byte order: some params
+ * may occur in order other than expected.
+ * Change byte order for the following params.
+ */
+#define HMR_QUIRK_REFRESH_TOTCOUNT_BYTE_ORDER 1
+#define HMR_QUIRK_REFRESH_PROGRESS_BYTE_ORDER 1
+
+/*
+ * There is the maximum number of HMR operations in the life
+ * of the device. Having reached this number, the device will
+ * return "General Failure" and set the bRefreshStatus attribute
+ * to 0x05.
+ */
+#define HMR_REFRESH_MAX_TOTCOUNT 200
+
+/*
+ * Output progress every N iterations.
+ */
+#define HMR_PROGRESS_OUTPUT_ITER 1
+#if HMR_PROGRESS_OUTPUT_ITER <= 0
+# error keep HMR_PROGRESS_OUTPUT_ITER > 0
+#endif
+
+/*
+ * The HMR mode type description string size we
+ * support to output. Can be enlarged if required.
+ */
+#define HMR_MODE_TYPE_MAX_SIZE 40
+
+enum hmr_err_codes {
+ EHMR_OK = 0, /* success */
+ EHMR_REFRESH_PROGRESS = 100, /* wrong refresh progress */
+ EHMR_REFRESH_STATUS, /* wrong refresh status */
+ EHMR_REFRESH_TOTCOUNT, /* wrong refresh total count */
+ EHMR_FREEMEM, /* memory wasn't freed */
+ EHMR_NOMEM, /* not enough memory */
+ EHMR_INVAL, /* invalid argument */
+ EHMR_NORETRY, /* cannot be retried */
+ EHMR_REFRESH_METHOD, /* wrong refrresh method */
+ EHMR_REFRESH_UNIT, /* wrong refrresh unit */
+};
+
+enum hmr_refresh_status {
+ HMR_ST_IDLE = 0,
+ HMR_ST_IN_PROGRESS,
+ HMR_ST_ABORTED,
+ HMR_ST_COMPLETED,
+ HMR_ST_BUSY,
+ HMR_ST_GENERAL,
+};
+
+enum hmr_stage_skip {
+ HMR_SKIP_STATUS_CHECK = 1 << 0,
+ HMR_SKIP_METHOD_SET = 1 << 1,
+ HMR_SKIP_UNIT_SET = 1 << 2,
+};
+
+#pragma pack(push, 1)
+struct descriptor_health_layout {
+ __u8 length;
+ __u8 type;
+ __u8 pre_eol_info;
+ __u8 dev_life_time_est_a;
+ __u8 dev_life_time_est_b;
+ __u8 vendor_prop_info[0x20];
+ __u32 refresh_total_count;
+ __u32 refresh_progress;
+};
+#pragma pack(pop)
+
+struct descriptor {
+ enum desc_idn idn;
+ const char *name;
+ size_t size;
+ void *layout;
+};
+
+extern int do_query_rq(int fd,
+ struct ufs_bsg_request *bsg_req, /* request struct of the sg_io_v4 */
+ struct ufs_bsg_reply *bsg_rsp, /* response struct of the sg_io_v4 */
+ __u8 query_req_func, /* read / write */
+ __u8 opcode, /* opcode r/w of desc/attr/fl etc.*/
+ __u8 idn, /* (-t) util option */
+ __u8 index, /* (-i) util option */
+ __u8 sel, /* (-s) util option */
+ __u16 req_buf_len, /* request buffer size */
+ __u16 res_buf_len, /* response buffer size */
+ __u8 *data_buf); /* buffer with/for data */
+
+extern struct desc_field_offset device_health_desc_conf_field_name[];
+extern struct attr_fields ufs_attrs[];
+extern struct flag_fields ufs_flags[];
+
+static struct descriptor_health_layout desc_health_layout;
+
+static struct descriptor desc_health = {
+ QUERY_DESC_IDN_HEALTH,
+ "Health",
+ sizeof desc_health_layout,
+ &desc_health_layout
+};
+
+static inline void hmr_delay_retry(int sec)
+{
+ if (sec > 0)
+ sleep(sec);
+}
+
+static inline void hmr_output_message(const char *msg)
+{
+ /*
+ * |---------------------------------------------------------|
+ * | Message format template: |
+ * |---------------------------------------------------------|
+ * |<<-6->| |<<-29-> |
+ * |HMR: | |variable string |
+ * | |
+ * |---------------------------------------------------------|
+ * |HMR: | |waiting idle status |
+ * |---------------------------------------------------------|
+ */
+ printf("%-6s %-29s\r", "HMR:", msg);
+ fflush(stdout);
+}
+
+static inline void hmr_output_progress(__u32 progress,
+ __u64 iter, int sec, const char *msg)
+{
+ /*
+ * |---------------------------------------------------------|
+ * | Progress format template: |
+ * |---------------------------------------------------------|
+ * |<<-6->| | <-9->>| |<<-35-> |
+ * |HMR: | | hex dig| |variable string |
+ * | |
+ * |---------------------------------------------------------|
+ * | The case of up to 100% progress: |
+ * |---------------------------------------------------------|
+ * |<<-6->| | <-9->>| |<<-35-> |
+ * |HMR: | | f20a| | |
+ * | | | | | |
+ * |---------------------------------------------------------|
+ * | The case of 100% progress: |
+ * |---------------------------------------------------------|
+ * |<<-6->| | <-8->>| |<<-20-> |
+ * |HMR: | | 100|% | |
+ * |---------------------------------------------------------|
+ */
+
+ /*
+ * Completed - print 100%,
+ * otherwise print hex progress indicator every N iterations.
+ */
+ if (!progress)
+ printf("%-6s %8d%% %-20s\r", "HMR:", 100, msg);
+ else if (0 == (iter % HMR_PROGRESS_OUTPUT_ITER))
+ printf("%-6s %9x %-20s\r", "HMR:", progress, msg);
+
+ fflush(stdout);
+ if (sec > 0)
+ sleep(sec);
+}
+
+static inline void hmr_output_header(const char *unit_str, int method)
+{
+ int count;
+ const char *method_str;
+ char mode_str[HMR_MODE_TYPE_MAX_SIZE];
+
+ method_str = method == HMR_METHOD_FORCE ? "force" : "selective";
+
+ count = snprintf(mode_str, sizeof mode_str, "method:%s, unit:%s",
+ method_str, unit_str);
+
+ if (count >= HMR_MODE_TYPE_MAX_SIZE)
+ ; /* the output was truncated, enlarge HMR_MODE_TYPE_MAX_SIZE */
+
+ /*
+ * |---------------------------------------------------------|
+ * | Header format template: |
+ * |---------------------------------------------------------|
+ * |<<-6->| | <-9->>| |<<-35-> |
+ * |HMR: | | status| |mode type |
+ * | |
+ * |---------------------------------------------------------|
+ * | Examples: |
+ * |---------------------------------------------------------|
+ * |HMR: | | started| |method:selective, unit:minimum |
+ * | | | | | |
+ * |---------------------------------------------------------|
+ * |HMR: | | started| |method:force, unit:full |
+ * | | | | | |
+ * |---------------------------------------------------------|
+ */
+ printf("%-6s %9s %-30s\n", "HMR:", "started", mode_str);
+ fflush(stdout);
+}
+
+static inline void hmr_output_footer(int rc)
+{
+ /*
+ * |---------------------------------------------------------|
+ * | Footer format template: |
+ * |---------------------------------------------------------|
+ * |<<-6->| | <-9->>| |<<-35-> |
+ * |HMR: | | status| |error msg / code |
+ * | |
+ * |---------------------------------------------------------|
+ * | Examples: |
+ * |---------------------------------------------------------|
+ * |HMR: | |completed| | OK |
+ * | | | | | |
+ * |---------------------------------------------------------|
+ * |HMR: | | stopped| |-115 |
+ * | | | | | |
+ * |---------------------------------------------------------|
+ */
+
+ /*
+ * Leading LF is due to CR-ending on progress print out.
+ * In case of error LF was already sent with the error msg.
+ */
+ if (0 == rc)
+ printf("\n%-6s %9s %-30s\n", "HMR:", "completed", "OK");
+ else
+ printf("%-6s %9s %-30d\n", "HMR:", "stopped", rc);
+
+ fflush(stdout);
+}
+
+static inline void hmr_query_error(int rc,
+ const char *job_type, /* write/read/set/clear/etc. */
+ const char *subject, /* desc/attr/flag/etc. */
+ int opcode, /* opcode r/w of desc/attr/fl etc. */
+ const char *field_name, /* name of the operated field */
+ int field_idn) /* index of the field */
+{
+ print_error("hmr: query command: %s %s failed: "
+ "opcode 0x%x, field-name %s, field-idn 0x%x, rc %d.\n",
+ job_type, subject, opcode, field_name, field_idn);
+}
+
+static inline int hmr_attr_sanity(enum attr_idn idn)
+{
+ if (idn < 0 || idn >= QUERY_ATTR_IDN_MAX)
+ return -EHMR_INVAL;
+
+ return EHMR_OK;
+}
+
+static inline int hmr_flag_sanity(enum flag_idn idn)
+{
+ if (idn < 0 || idn >= QUERY_FLAG_IDN_MAX)
+ return -EHMR_INVAL;
+
+ return EHMR_OK;
+}
+
+static inline int hmr_desc_sanity(enum desc_idn idn)
+{
+ if (idn < 0 || idn >= QUERY_DESC_IDN_MAX)
+ return -EHMR_INVAL;
+
+ return EHMR_OK;
+}
+
+static int hmr_dev_open(const char* path, int *fd)
+{
+ int rc = 0;
+
+ errno = 0;
+
+ *fd = open(path, O_RDWR);
+ if (*fd < 0) {
+ rc = errno; /* save errno: errno can be changed by the print */
+ print_error("hmr: %s: '%s'", strerror(rc), path);
+ }
+
+ return rc == 0 ? rc : -rc;
+}
+
+static int hmr_dev_close(const char* path, int fd)
+{
+ int rc;
+
+ errno = 0;
+
+ rc = close(fd);
+ if (rc) {
+ rc = errno;
+ print_error("hmr: %s: '%s'", strerror(rc), path);
+ }
+
+ return rc == 0 ? rc : -rc;
+}
+
+static int hmr_attr_read(__u32 *result,
+ int fd,
+ struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp,
+ enum attr_idn idn)
+{
+ int rc;
+ struct attr_fields *field;
+
+ if (hmr_attr_sanity(idn) != EHMR_OK || !result)
+ return -EHMR_INVAL;
+
+ field = &ufs_attrs[idn];
+
+ /* Query to read attribute */
+ rc = do_query_rq(fd,
+ bsg_req,
+ bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_READ_REQUEST,
+ UPIU_QUERY_OPCODE_READ_ATTR,
+ idn,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0);
+
+ if (rc) {
+ hmr_query_error(rc, "read", "attr", UPIU_QUERY_OPCODE_READ_ATTR,
+ field->name, idn);
+ goto out;
+ }
+
+ *result = be32toh(bsg_rsp->upiu_rsp.qr.value);
+
+out:
+ return rc;
+}
+
+static int hmr_attr_write(__u32 value,
+ int fd,
+ struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp,
+ enum attr_idn idn)
+{
+ int rc;
+ struct attr_fields *field;
+
+ if (hmr_attr_sanity(idn) != EHMR_OK)
+ return -EHMR_INVAL;
+
+ field = &ufs_attrs[idn];
+
+ bsg_req->upiu_req.qr.value = htobe32(value);
+
+ /* Query to write attribute */
+ rc = do_query_rq(fd,
+ bsg_req,
+ bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_WRITE_REQUEST,
+ UPIU_QUERY_OPCODE_WRITE_ATTR,
+ idn,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0);
+
+ if (rc)
+ hmr_query_error(rc, "write", "attr", UPIU_QUERY_OPCODE_WRITE_ATTR,
+ field->name, idn);
+
+ return rc;
+}
+
+static int hmr_flag_modify(int fd,
+ struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp,
+ enum query_opcode opcode,
+ enum flag_idn idn)
+{
+ int rc;
+ struct flag_fields *field;
+
+ if (opcode != UPIU_QUERY_OPCODE_SET_FLAG &&
+ opcode != UPIU_QUERY_OPCODE_CLEAR_FLAG &&
+ opcode != UPIU_QUERY_OPCODE_TOGGLE_FLAG)
+ return -EHMR_INVAL;
+
+ if (hmr_flag_sanity(idn) != EHMR_OK)
+ return -EHMR_INVAL;
+
+ field = &ufs_flags[idn];
+
+ /* Query to set/clear/toggle flag */
+ rc = do_query_rq(fd,
+ bsg_req,
+ bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_WRITE_REQUEST,
+ opcode,
+ idn,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0);
+
+ if (rc)
+ hmr_query_error(rc, "modify", "flag", opcode, field->name, idn);
+
+ return rc;
+}
+
+static int hmr_desc_read(struct descriptor *result,
+ int fd,
+ struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp,
+ enum desc_idn idn)
+{
+ int rc;
+
+ if (hmr_desc_sanity(idn) != EHMR_OK ||
+ !result ||
+ !result->layout ||
+ result->size <= 0)
+ return -EHMR_INVAL;
+
+ /* Query to read descriptor */
+ rc = do_query_rq(fd,
+ bsg_req,
+ bsg_rsp,
+ UPIU_QUERY_FUNC_STANDARD_READ_REQUEST,
+ UPIU_QUERY_OPCODE_READ_DESC,
+ idn,
+ 0,
+ 0,
+ 0,
+ result->size,
+ result->layout);
+
+ if (rc)
+ hmr_query_error(rc, "read", "desc", UPIU_QUERY_OPCODE_READ_DESC,
+ result->name, idn);
+
+ return rc;
+}
+
+static int hmr_progress_read(__u32 *result,
+ int fd,
+ struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp)
+{
+ int rc;
+ struct descriptor *desc;
+ struct descriptor_health_layout *layout;
+
+ desc = &desc_health;
+
+ /* Read descriptor Health */
+ rc = hmr_desc_read(desc,
+ fd,
+ bsg_req,
+ bsg_rsp,
+ desc->idn);
+
+ if (rc)
+ goto out;
+
+ layout = desc->layout;
+
+ if (!HMR_QUIRK_REFRESH_PROGRESS_BYTE_ORDER)
+ *result = be32toh(layout->refresh_progress);
+ else
+ *result = layout->refresh_progress;
+
+ return rc;
+
+out:
+ print_error("hmr: read progress: failed.");
+ return rc;
+}
+
+static int inline hmr_method_set(int fd, int method)
+{
+ int rc;
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+
+ /* Set attribute bRefreshMethod - force or selective */
+ rc = hmr_attr_write(method,
+ fd, &bsg_req, &bsg_rsp, QUERY_ATTR_IDN_REFRESH_METHOD);
+
+ return rc;
+}
+
+static inline int hmr_unit_set(int fd, int unit)
+{
+ int rc;
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+
+ /* Set attribute bRefreshUnit - minimum or full */
+ rc = hmr_attr_write(unit,
+ fd, &bsg_req, &bsg_rsp, QUERY_ATTR_IDN_REFRESH_UNIT);
+
+ return rc;
+}
+
+static inline int hmr_precondition_verify_status(int fd,
+ struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp)
+{
+ int rc;
+ int cur = 0;
+ int count = 10;
+ __u32 result;
+
+retry:
+ /* Read refresh status */
+ rc = hmr_attr_read(&result,
+ fd,
+ bsg_req,
+ bsg_rsp,
+ QUERY_ATTR_IDN_REFRESH_STATUS);
+ if (rc)
+ goto out;
+
+ if (result != HMR_ST_IDLE) {
+ /* One time only */
+ if (0 == cur)
+ hmr_output_message("waiting idle status");
+
+ /* Retry */
+ if (++cur <= count) {
+ hmr_delay_retry(1);
+ goto retry;
+ }
+
+ /* Error */
+ print_error("hmr: precondition: "
+ "refresh status != 0x%x (0x%x)", HMR_ST_IDLE, result);
+ rc = -EHMR_REFRESH_STATUS;
+ }
+
+out:
+ return rc;
+}
+
+static inline int hmr_precondition_validate_totcount(__u32 *result,
+ struct descriptor_health_layout *layout)
+{
+ int rc = EHMR_OK;
+ __u32 refresh_totcount;
+
+ if (!result || !layout)
+ return -EHMR_INVAL;
+
+ if (!HMR_QUIRK_REFRESH_TOTCOUNT_BYTE_ORDER)
+ refresh_totcount = be32toh(layout->refresh_total_count);
+ else
+ refresh_totcount = layout->refresh_total_count;
+
+ if (refresh_totcount >= HMR_REFRESH_MAX_TOTCOUNT) {
+ print_error("hmr: precondition: "
+ "refresh total count >= max (0x%x >= 0x%x).",
+ refresh_totcount, HMR_REFRESH_MAX_TOTCOUNT);
+ rc = -EHMR_REFRESH_TOTCOUNT;
+ goto out;
+ }
+
+ *result = refresh_totcount;
+
+out:
+ return rc;
+}
+
+static inline void hmr_output_wrong_run(int method, int unit)
+{
+ print_error("hmr: precondition: device's hmr-progress is not empty. "
+ "Run is possible only with the method: %d, unit: %d",
+ method, unit);
+}
+
+static int hmr_precondition_verify_run(int fd,
+ int req_method, int req_unit, enum hmr_stage_skip *stage_passer)
+{
+ int rc;
+ __u32 dev_method;
+ __u32 dev_unit;
+ __u32 dev_progress;
+
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+
+ if (!stage_passer)
+ return -EHMR_INVAL;
+
+ /*
+ * This code is intended to solve the case when the
+ * utility was run while HMR progress is not empty.
+ * It can happen, for example, when running the utility
+ * in one mode, then exit and run it again in some
+ * other mode, while the underline device's HMR status
+ * is still in progress.
+ *
+ * Let's make the policy simple:
+ * 1. Refresh progress is 0 -> OK.
+ * 2. Requested method equal device method
+ * and
+ * Requested unit equal device unit
+ * 1) Unit is minimum -> OK.
+ * 2) Unit is full -> OK, but have to skip the check for
+ * refresh status to be "idle", since
+ * in this mode it is "in-progress"
+ * until the HMR is completed.
+ * 3. Any other case is considered as violation.
+ */
+
+ /* Read progress */
+ rc = hmr_progress_read(&dev_progress, fd, &bsg_req, &bsg_rsp);
+ if (rc)
+ goto out;
+
+ /* Clean run, may proceed with HMR */
+ if (0 == dev_progress)
+ goto out;
+
+ /* Read method */
+ rc = hmr_attr_read(&dev_method,
+ fd,
+ &bsg_req,
+ &bsg_rsp,
+ QUERY_ATTR_IDN_REFRESH_METHOD);
+ if (rc)
+ goto out;
+
+ /* Read unit */
+ rc = hmr_attr_read(&dev_unit,
+ fd,
+ &bsg_req,
+ &bsg_rsp,
+ QUERY_ATTR_IDN_REFRESH_UNIT);
+ if (rc)
+ goto out;
+
+ /* Method is different, set error */
+ if (req_method != dev_method) {
+ hmr_output_wrong_run(dev_method, dev_unit);
+ rc = -EHMR_REFRESH_UNIT;
+ goto out;
+ }
+
+ /* Unit is different, set error */
+ if (req_unit != dev_unit) {
+ hmr_output_wrong_run(dev_method, dev_unit);
+ rc = -EHMR_REFRESH_METHOD;
+ goto out;
+ }
+
+ /*
+ * The progress is not empty, and method/unit are the same.
+ *
+ * 1. Unit type is full? - skip the refresh status check,
+ * since it will never be completed/idle in this mode until ends.
+ * 2. Skip set method and unit, no need - the values are already set.
+ */
+ if (dev_unit == HMR_UNIT_FULL)
+ *stage_passer |= HMR_SKIP_STATUS_CHECK;
+
+ *stage_passer |= (HMR_SKIP_METHOD_SET | HMR_SKIP_UNIT_SET);
+
+out:
+ return rc;
+}
+
+static int hmr_precondition_verify(int fd,
+ struct tool_options *opt, __u32 *totcount,
+ enum hmr_stage_skip *stage_passer)
+{
+ int rc;
+ struct descriptor *desc;
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+
+ if (!opt || !totcount || !stage_passer)
+ return -EHMR_INVAL;
+
+ /*
+ * 1. Verify the run perspective.
+ *
+ * In case of non-empty progress, make a decision
+ * if the run still can be proceeded.
+ */
+ rc = hmr_precondition_verify_run(fd,
+ opt->hmr_method, opt->hmr_unit, stage_passer);
+ if (rc)
+ goto err;
+
+ /*
+ * 2. Verify refresh total count.
+ *
+ * Note: the Health descriptor was already read in
+ * hmr_precondition_verify_run(), so updated
+ * refresh total count is already available.
+ */
+ desc = &desc_health;
+ rc = hmr_precondition_validate_totcount(totcount, desc->layout);
+ if (rc)
+ goto err;
+
+ /* Skip the refresh status check */
+ if (*stage_passer & HMR_SKIP_STATUS_CHECK)
+ goto success;
+
+ /*
+ * 3. Verify refresh status.
+ *
+ * Read refresh status.
+ * In case the status is not Idle - retry.
+ *
+ * We are expecting here to get the Idle status (0x00),
+ * but in some cases the status may be different:
+ * aborted (0x02), completed (0x03), etc.
+ *
+ * Usually, the statuses of this kind are saved until
+ * the first reading of the attribute is occured, then
+ * the value o the attribute should be change to Idle (0x00),
+ * but let's be generous and give it a sufficient number of
+ * retries.
+ */
+ rc = hmr_precondition_verify_status(fd, &bsg_req, &bsg_rsp);
+ if (rc)
+ goto err;
+
+success:
+ return rc;
+
+err:
+ print_error("hmr: precondition failed.");
+ return rc;
+}
+
+static inline int hmr_postcondition_verify_progress(int fd,
+ struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp)
+{
+ int rc;
+ __u32 result;
+
+ /* Read Health descriptor, and get refresh progress field */
+ rc = hmr_progress_read(&result, fd, bsg_req, bsg_rsp);
+ if (rc)
+ goto out;
+
+ /* Progress should be 0 on HMR completion */
+ if (result != 0) {
+ print_error("hmr: postcondition: "
+ "refresh progress != 0x0 (0x%x).", result);
+ rc = -EHMR_REFRESH_PROGRESS;
+ goto out;
+ }
+
+out:
+ return rc;
+}
+
+static inline int hmr_postcondition_verify_status(int fd,
+ struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp)
+{
+ int rc;
+ __u32 result;
+
+ /* Read refresh status */
+ rc = hmr_attr_read(&result,
+ fd,
+ bsg_req,
+ bsg_rsp,
+ QUERY_ATTR_IDN_REFRESH_STATUS);
+
+ if (rc)
+ goto out;
+
+ /* The accepted statuses are Completed and Idle */
+ if (result != HMR_ST_COMPLETED && result != HMR_ST_IDLE) {
+ print_error("hmr: postcondition: "
+ "refresh status != (0x%x or 0x%x) (0x%x)",
+ HMR_ST_COMPLETED, HMR_ST_IDLE, result);
+ rc = -EHMR_REFRESH_STATUS;
+ goto out;
+ }
+
+out:
+ return rc;
+}
+
+static inline int hmr_postcondition_verify_totcount(__u32 prev_totcount,
+ struct descriptor_health_layout *layout)
+{
+ int rc = EHMR_OK;
+ __u32 result;
+
+ if (!layout)
+ return -EHMR_INVAL;
+
+ if (!HMR_QUIRK_REFRESH_TOTCOUNT_BYTE_ORDER)
+ result = be32toh(layout->refresh_total_count);
+ else
+ result = layout->refresh_total_count;
+
+ if (result <= prev_totcount) {
+ print_error("hmr: postcondition: "
+ "refresh total count 0x%x <= 0x%x.",
+ prev_totcount, result);
+ rc = -EHMR_REFRESH_TOTCOUNT;
+ }
+
+ return rc;
+}
+
+static int hmr_postcondition_verify(int fd, __u32 totcount)
+{
+ int rc;
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+ struct descriptor *desc;
+
+ desc = &desc_health;
+
+ /*
+ * 1. Verify refresh progress.
+ *
+ * Progress equal to 0 indicates refresh was completed.
+ */
+ rc = hmr_postcondition_verify_progress(fd, &bsg_req, &bsg_rsp);
+ if (rc)
+ goto err;
+
+ /*
+ * 2. Check refresh status.
+ *
+ * The expected value is Completed (0x03) in first read,
+ * and Idle (0x00) in any following read (assuming no new
+ * HMR process begins).
+ * Perhaps, depending on the type of test, the first reading
+ * has already taken place. Therefore, we may consider both
+ * values to be valid.
+ */
+ rc = hmr_postcondition_verify_status(fd, &bsg_req, &bsg_rsp);
+ if (rc)
+ goto err;
+
+ /*
+ * 3. Verify refresh total count.
+ *
+ * Refresh total count expected to be increased
+ * upon completion.
+ *
+ * The health descriptor was already read in step 1
+ * doing hmr_progress_read(). Just use its layout.
+ */
+ rc = hmr_postcondition_verify_totcount(totcount, desc->layout);
+ if (rc)
+ goto err;
+
+ /* Success */
+ return rc;
+
+err:
+ print_error("hmr: postcondition failed.");
+ return rc;
+}
+
+static inline int hmr_refresh_initiate_retry(int fd,
+ struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp,
+ int cur,
+ int count)
+{
+ int rc;
+ __u32 result;
+
+ if (cur > count)
+ return -EHMR_NORETRY;
+
+ /*
+ * Read refresh status.
+ * Then retry only in case the status is Busy.
+ */
+ rc = hmr_attr_read(&result,
+ fd,
+ bsg_req,
+ bsg_rsp,
+ QUERY_ATTR_IDN_REFRESH_STATUS);
+
+ if (rc)
+ goto out;
+
+ /* Other than Busy - canot be retried */
+ if (result != HMR_ST_BUSY) {
+ rc = -EHMR_NORETRY;
+ goto out;
+ }
+
+out:
+ /* 0 - can be retried, otherwise cannot */
+ return rc;
+}
+
+static int hmr_refresh_initiate(int fd,
+ struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp)
+{
+ int rc;
+ int err;
+ int cur = 0;
+ int count = 10;
+
+ /*
+ * Initiate refresh.
+ * For that, set flag fRefreshEnable to 1.
+ * In case device command queues are not empty,
+ * this query request may fail. In such a case
+ * the bRefreshStatus attribite shell be set to
+ * 0x04 - HMR_ST_BUSY. This case is handled by
+ * hmr_refresh_initiate_retry().
+ */
+retry:
+ rc = hmr_flag_modify(fd, bsg_req, bsg_rsp,
+ UPIU_QUERY_OPCODE_SET_FLAG,
+ QUERY_FLAG_IDN_REFRESH_ENABLE);
+
+ /* Success */
+ if (!rc)
+ goto out;
+
+ /* Check query can be retried */
+ err = hmr_refresh_initiate_retry(fd, bsg_req, bsg_rsp, ++cur, count);
+ if (!err) {
+ hmr_delay_retry(1);
+ goto retry;
+ }
+
+ print_error("hmr: initiate unit refresh: setting flag "
+ "fRefreshEnable failed after %d retries (%d).", count, rc);
+
+out:
+ return rc;
+}
+
+static int hmr_unit_verify_completed(int fd,
+ struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp)
+{
+ __u32 result;
+ int rc;
+ int cur = 0;
+ int count = 10;
+
+ /*
+ * Read refresh status.
+ *
+ * The expected status is Completed, which changes to Idle
+ * after it was read once. Thus, if a given attribute reads
+ * some other process, then we are in a race state and there
+ * is a possibility that the attribute value will be reset
+ * to Idle by another process. Thus, we will consider both
+ * statuses - Completed and Idle as valid at this stage.
+ */
+retry:
+ rc = hmr_attr_read(&result,
+ fd,
+ bsg_req,
+ bsg_rsp,
+ QUERY_ATTR_IDN_REFRESH_STATUS);
+
+ /* Error or (Completed / Idle) */
+ if (rc ||
+ result == HMR_ST_COMPLETED ||
+ result == HMR_ST_IDLE)
+ goto out;
+
+ /* Retry */
+ if (++cur <= count) {
+ hmr_delay_retry(1);
+ goto retry;
+ }
+
+ /*
+ * Cannot be completed, result holds
+ * other than HMR_ST_COMPLETED or HMR_ST_IDLE status.
+ */
+ rc = -EHMR_REFRESH_STATUS;
+ print_error("hmr: verify unit completed: getting status "
+ "completed failed after %d retries (%d).", count, result);
+
+out:
+ return rc;
+}
+
+static int hmr_full_start(int fd, int method)
+{
+ int rc;
+ __u32 result;
+ __u64 iter = 0;
+
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+
+ hmr_output_header("full", method);
+
+ /* Start full refresh */
+ rc = hmr_refresh_initiate(fd, &bsg_req, &bsg_rsp);
+ if (rc)
+ goto out;
+
+ /* Start progress loop */
+ while (1) {
+ /* Sample progress */
+ rc = hmr_progress_read(&result, fd, &bsg_req, &bsg_rsp);
+ if (rc)
+ goto out;
+
+ hmr_output_progress(result, iter++, 1, "");
+
+ /* Progress is 0 - completed */
+ if (0 == result)
+ break;
+ }
+
+out:
+ hmr_output_footer(rc);
+ return rc;
+}
+
+static int hmr_unit_start(int fd, int method)
+{
+ int rc;
+ __u32 result;
+ __u64 iter = 0;
+
+ struct ufs_bsg_request bsg_req = {0};
+ struct ufs_bsg_reply bsg_rsp = {0};
+
+ hmr_output_header("minimum", method);
+
+ /* Start HMR loop */
+ while (1) {
+ /* Start unit refresh */
+ rc = hmr_refresh_initiate(fd, &bsg_req, &bsg_rsp);
+ if (rc)
+ goto out;
+
+ /* Verify completed */
+ rc = hmr_unit_verify_completed(fd, &bsg_req, &bsg_rsp);
+ if (rc)
+ goto out;
+
+ /* Read progress */
+ rc = hmr_progress_read(&result, fd, &bsg_req, &bsg_rsp);
+ if (rc)
+ goto out;
+
+ hmr_output_progress(result, iter++, 0, "");
+
+ /* Progress is 0 - completed */
+ if (0 == result)
+ break;
+ }
+
+out:
+ hmr_output_footer(rc);
+ return rc;
+}
+
+int do_hmr(struct tool_options *opt)
+{
+ int rc;
+ int fd;
+ int (*hmr_job)(int, int);
+ __u32 refresh_totcount;
+ enum hmr_stage_skip stage_passer = 0;
+
+ if (!opt || !opt->path)
+ return -EHMR_INVAL;
+
+ /* Open dev in subject */
+ rc = hmr_dev_open(opt->path, &fd);
+ if (rc)
+ goto out;
+
+ /* Make verifications prior to HMR */
+ rc = hmr_precondition_verify(fd, opt, &refresh_totcount, &stage_passer);
+ if (rc)
+ goto free;
+
+ /* Set HMR method: force or selective */
+ if (!(stage_passer & HMR_SKIP_METHOD_SET)) {
+ rc = hmr_method_set(fd, opt->hmr_method);
+ if (rc)
+ goto free;
+ }
+
+ /* Set HMR unit: minimum or full */
+ if (!(stage_passer & HMR_SKIP_UNIT_SET)) {
+ rc = hmr_unit_set(fd, opt->hmr_unit);
+ if (rc)
+ goto free;
+ }
+
+ /* Do the HMR job */
+ hmr_job = opt->hmr_unit == HMR_UNIT_MIN ?
+ &hmr_unit_start : &hmr_full_start;
+
+ rc = (*hmr_job)(fd, opt->hmr_method);
+ if (rc)
+ goto free;
+
+ /* Verify variables upon completion */
+ rc = hmr_postcondition_verify(fd, refresh_totcount);
+ if (rc)
+ goto free;
+
+free:
+ rc = hmr_dev_close(opt->path, fd);
+
+out:
+ return rc;
+}
+
+void hmr_help(char *tool_name)
+{
+ /* General use case description */
+ printf("\n HMR command usage:\n");
+ printf("\n\t%s hmr [-p] <path to device> ([-x] <method> [-y] <unit>)\n",
+ tool_name);
+
+ /* -p: mandatory, device path */
+ printf("\n\t-p\t path - mandatory, ufs-bsg device path\n");
+
+ /* -x: optional, HMR method */
+ printf("\n\t-x\t method - optional, the default is %d\n",
+ HMR_METHOD_SELECTIVE);
+ printf("\t\t\t %-3d: %-25s\n",
+ HMR_METHOD_FORCE, "force, refresh all blocks containing data");
+ printf("\t\t\t %-3d: %-25s\n",
+ HMR_METHOD_SELECTIVE, "selective, refresh marked blocks only");
+
+ /* -y: optional, HMR unit */
+ printf("\n\t-y\t unit - optional, the default is %d\n", HMR_UNIT_MIN);
+ printf("\t\t\t %-3d: %-25s\n",
+ HMR_UNIT_MIN, "minimum, perform HMR by minimum refresh units");
+ printf("\t\t\t %-3d: %-25s\n",
+ HMR_UNIT_FULL, "full, perform a full HMR cycle in one command");
+}
diff --git a/libmtk_bsg/ufs_hmr.h b/libmtk_bsg/ufs_hmr.h
new file mode 100644
index 0000000..27de01d
--- /dev/null
+++ b/libmtk_bsg/ufs_hmr.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef UFS_HMR_H_
+#define UFS_HMR_H_
+
+#include "options.h"
+
+enum ufs_hmr_method {
+ HMR_METHOD_FORCE = 1, /* refresh all blocks containing data */
+ HMR_METHOD_SELECTIVE, /* refresh marked blocks only */
+ HMR_METHOD_MAX /* last member indicator */
+};
+
+enum ufs_hmr_unit {
+ HMR_UNIT_MIN = 0, /* perform HMR in small steps (minimum refresh units) */
+ HMR_UNIT_FULL, /* perform full HMR cycle in one command */
+ HMR_UNIT_MAX /* last member indicator */
+};
+
+void hmr_help(char *tool_name);
+int do_hmr(struct tool_options *opt);
+#endif /* UFS_HMR_H_ */
+
diff --git a/libmtk_bsg/ufs_rpmb.c b/libmtk_bsg/ufs_rpmb.c
new file mode 100644
index 0000000..f3d25f8
--- /dev/null
+++ b/libmtk_bsg/ufs_rpmb.c
@@ -0,0 +1,740 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#pragma GCC diagnostic ignored "-Wall"
+#pragma GCC diagnostic ignored "-Wextra"
+#pragma GCC diagnostic ignored "-Wsign-compare"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <dirent.h>
+#include <endian.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <errno.h>
+#include <time.h>
+#include <stdbool.h>
+
+#include "ufs.h"
+#include "ufs_cmds.h"
+#include "options.h"
+#include "ufs_rpmb.h"
+#include "ioctl.h"
+#include "hmac_sha2.h"
+#include "scsi_bsg_util.h"
+
+enum rpmb_op_type {
+ RPMB_WRITE_KEY = 0x01,
+ RPMB_READ_CNT = 0x02,
+ RPMB_WRITE = 0x03,
+ RPMB_READ = 0x04,
+ RPMB_READ_RESP = 0x05,
+ RPMB_SEC_CONF_WRITE = 0x06,
+ RPMB_SEC_CONF_READ = 0x07,
+
+};
+
+/* description of the sense key values */
+static const char *const rpmb_res_txt[] = {
+ "Success",
+ "General failure",
+ "Authentication failure",
+ "Counter failure",
+ "Address failure",
+ "Write failure",
+ "Read failure",
+ "Authentication Key not yet programmed",
+ "Secure Write Protect Configuration Block access failure",
+ "Invalid Secure Write Protect Block Configuration parameter",
+ "Secure Write Protection not applicable"
+};
+
+#define RESP_KEY_PROG 0x100
+#define RESP_COUNTER_READ 0x200
+#define RESP_DATA_WRITE 0x300
+#define RESP_DATA_READ 0x400
+#define RESP_CONF_BLOCK_WRITE 0x600
+#define RESP_CONF_BLOCK_READ 0x700
+
+#define RPMB_KEY_SIZE 32
+#define RPMB_MAC_SIZE 32
+#define RPMB_NONCE_SIZE 16
+#define RPMB_DATA_SIZE 256
+
+#define UFS_BSG_PATH "/dev/ufs-bsg"
+
+#define DEFAULT_RPMB_NUM_BLOCKS 64
+
+#define MAX_ADDRESS 0xFFFF
+#define SECOND_BYTE_MASK 0xFF00
+
+#define MAX_RETRY 3
+
+static unsigned char key[RPMB_KEY_SIZE];
+
+#define CUC(x) ((const unsigned char *)(x))
+
+extern int do_read_desc(int fd, struct ufs_bsg_request *bsg_req,
+ struct ufs_bsg_reply *bsg_rsp, __u8 idn, __u8 index,
+ __u16 desc_buf_len, __u8 *data_buf);
+
+static void hmac_update_frm(hmac_sha256_ctx *ctx, struct rpmb_frame *frm)
+{
+ hmac_sha256_update(ctx, CUC(frm->data), 256);
+ hmac_sha256_update(ctx, CUC(frm->nonce), 16);
+ hmac_sha256_update(ctx, CUC(&frm->write_counter), 4);
+ hmac_sha256_update(ctx, CUC(&frm->addr), 2);
+ hmac_sha256_update(ctx, CUC(&frm->block_count), 2);
+ hmac_sha256_update(ctx, CUC(&frm->result), 2);
+ hmac_sha256_update(ctx, CUC(&frm->req_resp), 2);
+}
+
+static int rpmb_calc_hmac_sha256(struct rpmb_frame *frames, ssize_t blocks_cnt,
+ const unsigned char key[], __u32 key_size,
+ unsigned char mac[], __u32 mac_size)
+{
+ hmac_sha256_ctx ctx;
+ __u32 i;
+
+ hmac_sha256_init(&ctx, key, key_size);
+
+ for (i = 0; i < blocks_cnt; i++)
+ hmac_update_frm(&ctx, (frames + i));
+
+ hmac_sha256_final(&ctx, mac, mac_size);
+
+ return 0;
+}
+
+static void print_operation_error(__u16 result)
+{
+ if (result <= 0xA)
+ printf("\n %s\n", rpmb_res_txt[result]);
+ else
+ printf("\n Unsupported RPMB Operation Error %x\n", result);
+}
+
+static int do_rpmb_op(int fd, struct rpmb_frame *frame_in, __u32 in_cnt,
+ struct rpmb_frame *frame_out, __u32 out_cnt, __u8 region,
+ __u8 sg_type)
+{
+ int ret = -EINVAL;
+ int try_again;
+ __u16 req_resp = 0;
+
+ if (!frame_in || !frame_out || !in_cnt || !out_cnt) {
+ print_error("Wrong rpmb parameters");
+ goto out;
+ }
+ for (try_again = 0; try_again < MAX_RETRY; try_again++) {
+ ret = scsi_security_out(fd, frame_in, in_cnt, region, sg_type);
+ if (!ret)
+ break;
+ if (try_again < MAX_RETRY - 1)
+ WRITE_LOG("SO failed: %d\n", try_again);
+ else
+ print_error("SO 1st RPMB cmd failed");
+ }
+
+ req_resp = be16toh(frame_in->req_resp & SECOND_BYTE_MASK);
+ if ((req_resp == RPMB_WRITE) ||
+ (req_resp == RPMB_WRITE_KEY) ||
+ (req_resp == RPMB_SEC_CONF_WRITE)) {
+ memset(&frame_in[0], 0, sizeof(frame_in[0]));
+ req_resp = (req_resp & SECOND_BYTE_MASK) | RPMB_READ_RESP;
+ frame_in[0].req_resp = htobe16(req_resp);
+ for (try_again = 0; try_again < MAX_RETRY; try_again++) {
+ ret = scsi_security_out(fd, &frame_in[0], 1,
+ region, sg_type);
+ if (!ret)
+ break;
+ if (try_again < MAX_RETRY - 1)
+ WRITE_LOG("SO 2 failed: %d\n", try_again);
+ else
+ print_error("SO 2nd RPMB cmd failed");
+ }
+ }
+ for (try_again = 0; try_again < MAX_RETRY; try_again++) {
+ ret = scsi_security_in(fd, frame_out, out_cnt, region, sg_type);
+ if (!ret) {
+ WRITE_LOG("Result Response addr %d , write count %d\n",
+ be16toh(frame_out->addr),
+ be32toh(frame_out->write_counter));
+ break;
+ }
+ if (try_again < MAX_RETRY - 1)
+ WRITE_LOG("SI failed: %d\n", try_again);
+ else
+ print_error("SI RPMB cmd failed");
+ }
+out:
+ return ret;
+}
+
+static int do_key(int fd, const unsigned char *key, __u8 region, __u8 sg_type)
+{
+ int ret = INVALID;
+ struct rpmb_frame frame_in = { 0 };
+ struct rpmb_frame frame_out = { 0 };
+
+ frame_in.req_resp = htobe16(RPMB_WRITE_KEY);
+
+ if (key == NULL) {
+ WRITE_LOG0("key is NULL");
+ goto out;
+ }
+ memcpy(frame_in.key_mac, key, sizeof(frame_in.key_mac));
+ WRITE_LOG("Start : %s\n", __func__);
+ ret = do_rpmb_op(fd, &frame_in, 1, &frame_out, 1, region, sg_type);
+
+ if (!ret) {
+ if (frame_out.result != 0) {
+ print_operation_error(be16toh(frame_out.result));
+ goto out;
+ } else
+ printf("RPMB key is programmed\n");
+ }
+out:
+ return ret;
+}
+
+static int do_read_counter(int fd, __u32 *cnt, __u8 region, __u8 sg_type,
+ bool prn_cnt)
+{
+ int ret;
+ struct rpmb_frame frame_in = { 0 };
+ struct rpmb_frame frame_out = { 0 };
+
+ WRITE_LOG("Start : %s %d\n", __func__, region);
+ frame_in.req_resp = htobe16(RPMB_READ_CNT);
+ ret = do_rpmb_op(fd, &frame_in, 1, &frame_out, 1, region, sg_type);
+
+ if (!ret) {
+ if (frame_out.result != 0) {
+ print_operation_error(be16toh(frame_out.result));
+ } else {
+ if (prn_cnt)
+ printf("RPMB write counter = %u\n",
+ be32toh(frame_out.write_counter));
+ *cnt = be32toh(frame_out.write_counter);
+ }
+ }
+ return ret;
+}
+
+static int do_read_rpmb(int fd, int out_fd, unsigned char *key,
+ int start_addr, int num_blocks, __u8 region, __u8 sg_type)
+{
+ int ret = ERROR;
+ int i;
+ ssize_t write_size;
+ __u8 max_num_blocks;
+ __u8 num_read_blocks = 0;
+ struct rpmb_frame frame_in = { 0 };
+ struct rpmb_frame *frames_out = NULL;
+ struct rpmb_frame *last_frame;
+ struct ufs_bsg_request bsg_req = { 0 };
+ struct ufs_bsg_reply bsg_rsp = { 0 };
+ int ufs_bsg_fd = INVALID;
+ __u8 data_buf[QUERY_DESC_GEOMETRY_MAX_SIZE] = { 0 };
+
+ WRITE_LOG("Start : %s , address %d , num_blocks %d\n", __func__,
+ start_addr, num_blocks);
+
+ ufs_bsg_fd = open(UFS_BSG_PATH, O_RDWR);
+ if (ufs_bsg_fd != INVALID) {
+ ret = do_read_desc(ufs_bsg_fd, &bsg_req, &bsg_rsp,
+ QUERY_DESC_IDN_GEOMETRY, 0,
+ QUERY_DESC_GEOMETRY_MAX_SIZE, data_buf);
+ }
+
+ if (ret) {
+ /*
+ * Could not read geometry descriptor, max block set
+ * DEFAULT_RPMB_NUM_BLOCKS);
+ */
+ print_warn("Cannot get bRPMB_ReadWriteSize");
+ max_num_blocks = DEFAULT_RPMB_NUM_BLOCKS;
+ } else {
+ max_num_blocks = data_buf[0x17];
+ WRITE_LOG("max_num_blocks : %d\n", max_num_blocks);
+ }
+
+ if (num_blocks > max_num_blocks)
+ num_read_blocks = max_num_blocks;
+ else
+ num_read_blocks = num_blocks;
+ while (num_blocks > 0) {
+ if (start_addr > MAX_ADDRESS) {
+ print_error("Max available address is reached");
+ goto out;
+ }
+
+ frames_out = (struct rpmb_frame *)calloc(num_read_blocks,
+ sizeof(struct rpmb_frame));
+ if (!frames_out) {
+ print_error("Cannot allocate %d RPMB frames",
+ num_blocks);
+ goto out;
+ }
+ frame_in.req_resp = htobe16(RPMB_READ);
+ frame_in.addr = htobe16(start_addr);
+ frame_in.block_count = htobe16(num_read_blocks);
+ ret = do_rpmb_op(fd, &frame_in, 1, frames_out,
+ num_read_blocks, region, sg_type);
+
+ if (ret != 0) {
+ print_error("RPMB operation is failed in addr %d ",
+ start_addr);
+ goto out;
+ }
+ if (frames_out[0].result != 0) {
+ print_operation_error(be16toh(frames_out[0].result));
+ ret = -EINVAL;
+ goto out;
+ }
+ last_frame = &frames_out[num_read_blocks - 1];
+ /* In case an user get the key ,verify the hash */
+ if (key != NULL) {
+ __u8 mac[RPMB_MAC_SIZE];
+
+ rpmb_calc_hmac_sha256(frames_out, num_read_blocks,
+ key, RPMB_KEY_SIZE,
+ mac, RPMB_MAC_SIZE);
+ /*
+ * Compare calculated MAC and MAC from last frame
+ * Note the mac much only in case we read 1 block ,
+ * otherwise the mac field is not much, in all frame ,
+ * include the last one
+ */
+ if (memcmp(mac, last_frame->key_mac, sizeof(mac)))
+ print_warn("RPMB MAC mismatch mac");
+ }
+ for (i = 0; i < num_read_blocks; i++) {
+ write_size = write(out_fd, &(frames_out[i].data),
+ RPMB_DATA_SIZE);
+ if (write_size != RPMB_DATA_SIZE) {
+ WRITE_LOG("%s: failed in write sz=%d errno=%d",
+ __func__, (int)write_size, errno);
+ ret = INVALID;
+ goto out;
+ }
+ }
+ WRITE_LOG("num_blocks : %d start_addr %d num_read_blocks %d\n",
+ num_blocks, start_addr, num_read_blocks);
+ num_blocks = num_blocks - num_read_blocks;
+ start_addr = start_addr + num_read_blocks;
+
+ if (num_blocks > max_num_blocks)
+ num_read_blocks = max_num_blocks;
+ else
+ num_read_blocks = num_blocks;
+
+ if (frames_out) {
+ free(frames_out);
+ frames_out = NULL;
+ }
+ }
+out:
+ if (frames_out)
+ free(frames_out);
+ if (ufs_bsg_fd != INVALID)
+ close(ufs_bsg_fd);
+
+ return ret;
+}
+
+static int do_write_rpmb(int fd, const unsigned char *key, int input_fd,
+ __u32 cnt, __u16 start_addr, __u16 num_blocks,
+ __u8 region, __u8 sg_type)
+{
+ int ret = ERROR;
+ unsigned char mac[RPMB_MAC_SIZE];
+ struct rpmb_frame *frames_in = NULL;
+ struct rpmb_frame frame_out = { 0 };
+ ssize_t read_size = 0;
+ __u8 max_num_blocks;
+ __u8 num_write_blocks = 0;
+ int i = 0;
+ int j = 0;
+ struct ufs_bsg_request bsg_req = { 0 };
+ struct ufs_bsg_reply bsg_rsp = { 0 };
+ int ufs_bsg_fd;
+ __u8 data_buf[QUERY_DESC_GEOMETRY_MAX_SIZE] = { 0 };
+
+ WRITE_LOG("Start : %s\n", __func__);
+
+ ufs_bsg_fd = open(UFS_BSG_PATH, O_RDWR);
+ if (ufs_bsg_fd != INVALID) {
+ ret = do_read_desc(ufs_bsg_fd, &bsg_req, &bsg_rsp,
+ QUERY_DESC_IDN_GEOMETRY, 0,
+ QUERY_DESC_GEOMETRY_MAX_SIZE, data_buf);
+ }
+
+ if (ret) {
+ print_warn("Cannot get bRPMB_ReadWriteSize");
+ max_num_blocks = DEFAULT_RPMB_NUM_BLOCKS;
+ } else {
+ /*bRPMB_ReadWriteSize e.g 0x40 * 256 = 16K*/
+ max_num_blocks = data_buf[0x17];
+ if (max_num_blocks <= 0)
+ max_num_blocks = 1;
+ ret = OK;
+ }
+
+ if (num_blocks > max_num_blocks)
+ num_write_blocks = max_num_blocks;
+ else
+ num_write_blocks = num_blocks;
+ WRITE_LOG("max_num_blocks : %d num_block %d, cnt %d start_addr %d\n",
+ max_num_blocks, num_write_blocks, cnt, start_addr);
+ while (num_blocks > 0) {
+ if (start_addr > MAX_ADDRESS) {
+ print_error("Max available address is reached");
+ goto out;
+ }
+ frames_in = (struct rpmb_frame *)calloc(num_write_blocks,
+ sizeof(struct rpmb_frame));
+ if (!frames_in) {
+ print_error("Cannot allocate %d RPMB frames",
+ num_write_blocks);
+ ret = -ENOMEM;
+ goto out;
+ }
+ for (i = 0; i < num_write_blocks; i++) {
+ frames_in[i].req_resp = htobe16(RPMB_WRITE);
+ frames_in[i].addr = htobe16(start_addr);
+ frames_in[i].block_count = htobe16(num_write_blocks);
+ frames_in[i].write_counter = htobe32(cnt);
+ read_size = read(input_fd, frames_in[i].data,
+ RPMB_DATA_SIZE);
+ if (read_size != RPMB_DATA_SIZE) {
+ WRITE_LOG("%s: failed in read size=%d errno=%d",
+ __func__, (int)read_size, errno);
+ ret = EINVAL;
+ goto out;
+ }
+ }
+
+ rpmb_calc_hmac_sha256(frames_in, num_write_blocks,
+ key, RPMB_KEY_SIZE, mac, RPMB_MAC_SIZE);
+ memcpy(frames_in[num_write_blocks - 1].key_mac,
+ mac, RPMB_MAC_SIZE);
+
+ ret = do_rpmb_op(fd, frames_in, num_write_blocks,
+ &frame_out, 1, region, sg_type);
+ if (ret != 0)
+ goto out;
+
+ /* Check RPMB response */
+ if (frame_out.result != 0) {
+ print_operation_error(be16toh(frame_out.result));
+ ret = -EINVAL;
+ goto out;
+ }
+
+ WRITE_LOG("num_blocks : %d start_addr %d num_write_blocks %d ,"
+ "iter %d,cnt %d\n",
+ num_blocks, start_addr, num_write_blocks, j, cnt);
+ num_blocks = num_blocks - num_write_blocks;
+ start_addr = start_addr + num_write_blocks;
+ if (num_blocks > max_num_blocks)
+ num_write_blocks = max_num_blocks;
+ else
+ num_write_blocks = num_blocks;
+ j++;
+ cnt++;
+ if (frames_in) {
+ free(frames_in);
+ frames_in = NULL;
+ }
+
+ }
+out:
+ if (ufs_bsg_fd != INVALID)
+ close(ufs_bsg_fd);
+ if (frames_in)
+ free(frames_in);
+ return ret;
+}
+
+static int do_read_conf_block(int fd, const unsigned char *key, __u8 lun,
+ int output_fd, __u8 sg_type)
+{
+ int ret = ERROR;
+ ssize_t write_size;
+ struct rpmb_frame frame_in = { 0 };
+ struct rpmb_frame frame_out = { 0 };
+
+ WRITE_LOG("Start : %s\n", __func__);
+
+ frame_in.req_resp = htobe16(RPMB_SEC_CONF_READ);
+ frame_in.data[0] = lun;
+
+ ret = do_rpmb_op(fd, &frame_in, 1, &frame_out, 1, 0, sg_type);
+ if (ret != 0) {
+ print_error("Fail to read Secure Write Config Block");
+ goto out;
+ }
+ if (frame_out.result != 0) {
+ print_operation_error(be16toh(frame_out.result));
+ goto out;
+ }
+
+ if (key != NULL) {
+ __u8 mac[RPMB_MAC_SIZE];
+
+ rpmb_calc_hmac_sha256(&frame_out, 1, key,
+ RPMB_KEY_SIZE, mac, RPMB_MAC_SIZE);
+ /* Compare calculated MAC and MAC from last frame
+ * Note the mac much only in case we read 1 block , otherwise the mac
+ * field is not much, in all frame ,include the last one */
+ if (memcmp(mac, frame_out.key_mac, sizeof(mac)))
+ print_error("RPMB MAC mismatch mac");
+ }
+ write_size = write(output_fd, frame_out.data, RPMB_DATA_SIZE);
+ if (write_size != RPMB_DATA_SIZE) {
+ WRITE_LOG("%s: failed in write size=%d errno=%d",
+ __func__, (int)write_size, errno);
+ ret = ERROR;
+ } else
+ printf("Secure Write Protect Config Block was read\n");
+out:
+ return ret;
+}
+
+static int do_write_conf_block(int fd, const unsigned char *key, int input_fd,
+ __u32 cnt, __u8 sg_type)
+{
+ int ret = INVALID;
+ __u8 mac[RPMB_MAC_SIZE];
+ struct rpmb_frame frame_in = { 0 };
+ struct rpmb_frame frame_out = { 0 };
+ ssize_t read_size = 0;
+
+ WRITE_LOG("Start : %s\n", __func__);
+
+ frame_in.req_resp = htobe16(RPMB_SEC_CONF_WRITE);
+ frame_in.block_count = htobe16(1);
+ frame_in.write_counter = htobe32(cnt);
+ read_size = read(input_fd, frame_in.data, RPMB_DATA_SIZE);
+ if (read_size != RPMB_DATA_SIZE) {
+ WRITE_LOG("%s: failed in read size=%d errno=%d",
+ __func__, (int)read_size, errno);
+ ret = INVALID;
+ goto out;
+ }
+
+ rpmb_calc_hmac_sha256(&frame_in, 1,
+ key, RPMB_KEY_SIZE, mac, RPMB_MAC_SIZE);
+ memcpy(frame_in.key_mac, mac, RPMB_MAC_SIZE);
+
+ ret = do_rpmb_op(fd, &frame_in, 1, &frame_out, 1, 0, sg_type);
+ if (ret != 0) {
+ print_error("Fail to write Secure Write Config Block");
+ goto out;
+ }
+
+ /* Check RPMB response */
+ if (frame_out.result != 0) {
+ print_operation_error(be16toh(frame_out.result));
+ ret = -EINVAL;
+ } else
+ printf("Secure Write Protect Config Block was written\n");
+
+out:
+ return ret;
+}
+
+static unsigned char *get_auth_key(char *key_path)
+{
+ unsigned char *pkey = NULL;
+ int key_fd = INVALID;
+ ssize_t read_size;
+
+ if (key_path == NULL)
+ return NULL;
+
+ key_fd = open(key_path, O_RDONLY);
+ if (key_fd < 0) {
+ perror("Key file open");
+ } else {
+ read_size = read(key_fd, key, RPMB_KEY_SIZE);
+ if (read_size < RPMB_KEY_SIZE) {
+ print_error("Key must be %d bytes length,was read %d",
+ RPMB_KEY_SIZE, read_size);
+ } else
+ pkey = key;
+ }
+
+ if (key_fd != INVALID)
+ close(key_fd);
+ return pkey;
+}
+
+int do_rpmb(struct tool_options *opt)
+{
+ int rc = INVALID;
+ int fd;
+ int output_fd = INVALID;
+ unsigned char *key_ptr = NULL;
+ __u32 cnt = 0;
+ __u8 lun;
+
+ fd = open(opt->path, O_RDWR | O_SYNC);
+ if (fd < 0) {
+ perror("open");
+ return ERROR;
+ }
+
+ switch (opt->idn) {
+ case AUTHENTICATION_KEY:
+ key_ptr = get_auth_key(opt->keypath);
+ if (key_ptr == NULL)
+ goto out;
+ rc = do_key(fd, key_ptr, opt->region, opt->sg_type);
+ break;
+ case READ_WRITE_COUNTER:
+ rc = do_read_counter(fd, &cnt, opt->region, opt->sg_type, true);
+ break;
+ case READ_RPMB:
+ output_fd = open(opt->data, O_WRONLY | O_CREAT | O_SYNC,
+ S_IRUSR | S_IWUSR);
+ if (output_fd < 0) {
+ perror("Output file open");
+ goto out;
+ }
+ if (opt->keypath[0] != 0) {
+ key_ptr = get_auth_key(opt->keypath);
+ if (key_ptr == NULL)
+ goto out;
+ }
+
+ rc = do_read_rpmb(fd, output_fd, key_ptr, opt->start_block,
+ opt->num_block, opt->region, opt->sg_type);
+ if (!rc)
+ printf("Finish to read RPMB data\n");
+ break;
+ case WRITE_RPMB:
+ key_ptr = get_auth_key(opt->keypath);
+ if (key_ptr == NULL)
+ goto out;
+
+ output_fd = open(opt->data, O_RDONLY | O_SYNC);
+ if (output_fd < 0) {
+ perror("Input file open");
+ goto out;
+ }
+ rc = do_read_counter(fd, &cnt, opt->region, opt->sg_type,
+ false);
+ if (rc)
+ goto out;
+ rc = do_write_rpmb(fd, key_ptr, output_fd, cnt,
+ opt->start_block, opt->num_block,
+ opt->region, opt->sg_type);
+ if (!rc)
+ printf("Finish to write RPMB data\n");
+ break;
+ case READ_SEC_RPMB_CONF_BLOCK:
+ lun = opt->lun;
+ if (opt->keypath[0] != 0) {
+ key_ptr = get_auth_key(opt->keypath);
+ if (key_ptr == NULL)
+ goto out;
+ }
+
+ output_fd = open(opt->data, O_WRONLY | O_CREAT | O_SYNC,
+ S_IRUSR | S_IWUSR);
+ if (output_fd < 0) {
+ perror("Output file open");
+ goto out;
+ }
+
+ rc = do_read_conf_block(fd, key_ptr, lun, output_fd,
+ opt->sg_type);
+ break;
+ case WRITE_SEC_RPMB_CONF_BLOCK:
+ key_ptr = get_auth_key(opt->keypath);
+ if (key_ptr == NULL)
+ goto out;
+
+ output_fd = open(opt->data, O_RDONLY | O_SYNC);
+ if (output_fd < 0) {
+ perror("Input file open");
+ goto out;
+ }
+ rc = do_read_counter(fd, &cnt, 0, opt->sg_type, false);
+ if (rc)
+ goto out;
+
+ rc = do_write_conf_block(fd, key_ptr, output_fd, cnt,
+ opt->sg_type);
+ break;
+ default:
+ print_error("Unsupported RPMB cmd %d", opt->idn);
+ break;
+ }
+out:
+ if (output_fd != INVALID)
+ close(output_fd);
+ close(fd);
+ return rc;
+}
+
+void rpmb_help(char *tool_name)
+{
+ printf("\n RPMB command usage:\n");
+ printf("\n\t%s rpmb [-t] <rpmb cmd idn> [-p] <path to device>"
+ " -k <path to device> -l <lun> -d <output/input file.\n",
+ tool_name);
+ printf("\n\t-t\t RPMB cmd type idn\n"
+ "\t\t\t0:\tKey provision\n"
+ "\t\t\t1:\tRead Write counter\n"
+ "\t\t\t2:\tRead RPMB data\n"
+ "\t\t\t3:\tWrite RPMB data\n"
+ "\t\t\t4:\tSecure Write Protect Configuration Block Write\n"
+ "\t\t\t5:\tSecure Write Protect Configuration Block Read\n");
+
+ printf("\n\t-s\t RPMB start address (default value is 0)\n");
+ printf("\n\t-n\t number of RPMB read/write blocks (default value is 1)\n");
+ printf("\n\t-p\t device path (RPMB LUN)\n");
+ printf("\n\t-k\t path to RPMB key, "
+ "the key path must pass to the tool in case of writing to RPMB,\n"
+ "\t\t in case of reading from RPMB ,the key may pass,"
+ " in case we want to validate the MAC\n"
+ "\t\t in case we want to validate the hash value\n");
+ printf("\n\t-l\t lun number(byte) using as parameter "
+ "for Secure Write Config Read\n");
+ printf("\n\t-w\t path to data file\n");
+ printf("\n\t-m\t RPMB region.\n");
+ printf("\n\t-g\t sg struct ver - 0: SG_IO_VER4 (default), 1: SG_IO_VER3\n");
+ printf("\n\tExample - Read 16MB of data from RPMB LUN started "
+ "from address 0 to output file\n"
+ "\t\t %s rpmb -t 2 -p /dev/0:0:0:49476 -s 0 -n 65536 -w output_file\n",
+ tool_name);
+ printf("\n\tExample - Write RPMB key\n"
+ "\t\t %s rpmb -t 0 -p /dev/0:0:0:49476 -k key_file\n",
+ tool_name);
+ printf("\n\tExample - Write RPMB key to region 2\n"
+ "\t\t %s rpmb -t 0 -m 2 -p /dev/0:0:0:49476 -k key_file\n",
+ tool_name);
+ printf("\n\tExample - Write Secure Write Config block\n"
+ "\t\t The input file is Secure Write Config block filled "
+ "according to the spec\n"
+ "\t\t %s rpmb -t 4 -p /dev/0:0:0:49476 -w input_file\n",
+ tool_name);
+ printf("\n\tExample - Read Secure Write Config block\n"
+ "\t\t After the command successfully finished , "
+ "the output file will contains\n"
+ "\t\t Secure Write Config block of lun 1\n"
+ "\t\t %s rpmb -t 5 -p /dev/0:0:0:49476 -l 1 -d output_file\n",
+ tool_name);
+}
diff --git a/libmtk_bsg/ufs_rpmb.h b/libmtk_bsg/ufs_rpmb.h
new file mode 100644
index 0000000..9eaabbe
--- /dev/null
+++ b/libmtk_bsg/ufs_rpmb.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include "options.h"
+
+#ifndef UFS_RPMB_H_
+#define UFS_RPMB_H_
+
+enum rpmb_cmd_type {
+ AUTHENTICATION_KEY = 0,
+ READ_WRITE_COUNTER,
+ READ_RPMB,
+ WRITE_RPMB,
+ WRITE_SEC_RPMB_CONF_BLOCK,
+ READ_SEC_RPMB_CONF_BLOCK,
+ RPMB_CMD_MAX
+};
+
+void rpmb_help(char *tool_name);
+int do_rpmb(struct tool_options *opt);
+
+#endif /* UFS_RPMB_H_ */
diff --git a/libmtk_bsg/ufs_vendor.c b/libmtk_bsg/ufs_vendor.c
new file mode 100644
index 0000000..0e58e34
--- /dev/null
+++ b/libmtk_bsg/ufs_vendor.c
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <errno.h>
+
+#include "ufs.h"
+#include "ufs_cmds.h"
+#include "options.h"
+#include "options.h"
+#include "scsi_bsg_util.h"
+#include "ufs_vendor.h"
+
+static int write_data(struct tool_options *opt, int dev_fd, void *p_data);
+static int read_data(struct tool_options *opt, int dev_fd, void *p_data);
+
+int do_vendor(struct tool_options *opt)
+{
+ int rc = INVALID;
+ int fd;
+ void *p_data;
+
+ WRITE_LOG("Start : %s", __func__);
+ p_data = malloc(opt->len);
+ if (!p_data) {
+ print_error("Cannot allocate %d Bytes", opt->len);
+ return ERROR;
+ }
+
+ fd = open(opt->path, O_RDWR | O_SYNC);
+ if (fd < 0) {
+ perror("Device open");
+ goto out;
+ }
+
+ if (opt->opr == WRITE)
+ rc = write_data(opt, fd, p_data);
+ else
+ rc = read_data(opt, fd, p_data);
+out:
+ free(p_data);
+
+ if (fd != INVALID)
+ close(fd);
+
+ return rc;
+}
+
+static int write_data(struct tool_options *opt, int dev_fd, void *p_data)
+{
+ int input_fd;
+ int rc = INVALID;
+ off_t file_size;
+
+ input_fd = open(opt->data, O_RDONLY | O_SYNC);
+ if (input_fd < 0) {
+ perror("Input file open");
+ return ERROR;
+ }
+
+ file_size = lseek(input_fd, 0, SEEK_END);
+ if ((file_size <= 0) || (file_size < opt->len)) {
+ print_error("Wrong input data file length = %d",
+ file_size);
+ goto out;
+ }
+ lseek(input_fd, 0, SEEK_SET);
+
+ if (read(input_fd, p_data, opt->len) != opt->len) {
+ print_error("Read %d data bytes from input file failed",
+ opt->len);
+ goto out;
+ }
+ rc = write_buffer(dev_fd, p_data, BUFFER_VENDOR_MODE, opt->index,
+ opt->offset, opt->len, opt->sg_type);
+ if (!rc)
+ printf("The vendor buffer was written\n");
+out:
+ close(input_fd);
+ return rc;
+}
+
+static int read_data(struct tool_options *opt, int dev_fd, void *p_data)
+{
+ int rc = INVALID;
+
+ rc = read_buffer(dev_fd, p_data, BUFFER_VENDOR_MODE, opt->index,
+ opt->offset, opt->len, opt->sg_type);
+ if (!rc) {
+ write_file("read_vendor_buffer.dat", p_data, opt->len);
+ printf("read_vendor_buffer.dat created\n");
+ }
+
+ return rc;
+}
+
+void vendor_help(char *tool_name)
+{
+ printf("\n Vendor Write/Read Buffer command usage:\n");
+ printf("\n\t%s vendor [-r ][-w] <path to data file> [-L] <data_len>\n"
+ "\t\t[-O] <buf offset> [-p] <path to device>\n",
+ tool_name);
+ printf("\n\t-r\tRead vendor buffer command[default operation]\n");
+ printf("\n\t-w\tInput file path for write buffer vendor command\n");
+ printf("\n\t-L\tData buffer length, up to 512 Bytes[default value 512B]\n");
+ printf("\n\t-i\tBuffer ID\n");
+ printf("\n\t-g\t sg struct ver - 0: SG_IO_VER4 (default), 1: SG_IO_VER3\n");
+ printf("\n\t-O\tBuffer Offset\n");
+ printf("\n\t-p\tDevice path\n");
+}
diff --git a/libmtk_bsg/ufs_vendor.h b/libmtk_bsg/ufs_vendor.h
new file mode 100644
index 0000000..9f5cd3a
--- /dev/null
+++ b/libmtk_bsg/ufs_vendor.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef UFS_VENDOR_H_
+#define UFS_VENDOR_H_
+
+void vendor_help(char *tool_name);
+int do_vendor(struct tool_options *opt);
+
+#endif /*UFS_VENDOR_H_*/
diff --git a/libmtk_bsg/unipro.c b/libmtk_bsg/unipro.c
new file mode 100644
index 0000000..071f911
--- /dev/null
+++ b/libmtk_bsg/unipro.c
@@ -0,0 +1,573 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <ctype.h>
+#include <errno.h>
+#include <stdint.h>
+
+#include "options.h"
+#include "ufs.h"
+#include "unipro.h"
+
+/*PHY Adapter Layer(L1.5) */
+static struct ufs_uic_attr_fields phy_adapter_attrs[] = {
+ /* PHY Adapter (gettable, settable) Common Attributes */
+ {"PA_ActiveTxDataLanes", 0x1560, (GETTABLE | SETTABLE)},
+ {"PA_TxTrailingClocks", 0x1564, (GETTABLE | SETTABLE)},
+ {"PA_ActiveRxDataLanes", 0x1580, (GETTABLE | SETTABLE)},
+
+ /* PHY Adapter (gettable, static) Common Attributes */
+ {"PA_PHY_Type", 0x1500, (GETTABLE | STATIC)},
+ {"PA_AvailTxDataLanes", 0x1520, (GETTABLE | STATIC)},
+ {"PA_AvailRxDataLanes", 0x1540, (GETTABLE | STATIC)},
+ {"PA_MinRxTrailingClocks", 0x1543, (GETTABLE | STATIC)},
+
+ /* PHY Adapter (gettable, dynamic) Common Attributes */
+ {"PA_TxPWRStatus", 0x1567, (GETTABLE | DYNAMIC)},
+ {"PA_RxPWRStatus", 0x1582, (GETTABLE | DYNAMIC)},
+ {"PA_RemoteVerInfo", 0x15A0, (GETTABLE | DYNAMIC)},
+
+ /* PHY Adapter (gettable, settable) M-PHY-Specific Attributes */
+ {"PA_TxHsG1SyncLength", 0x1552, (GETTABLE | SETTABLE)},
+ {"PA_TxHsG1PrepareLength", 0x1553, (GETTABLE | SETTABLE)},
+ {"PA_TxHsG2SyncLength", 0x1554, (GETTABLE | SETTABLE)},
+ {"PA_TxHsG2PrepareLength", 0x1555, (GETTABLE | SETTABLE)},
+ {"PA_TxHsG3SyncLength", 0x1556, (GETTABLE | SETTABLE)},
+ {"PA_TxHsG3PrepareLength", 0x1557, (GETTABLE | SETTABLE)},
+ {"PA_TxMk2Extension", 0x155A, (GETTABLE | SETTABLE)},
+ {"PA_PeerScrambling", 0x155B, (GETTABLE | SETTABLE)},
+ {"PA_TxSkip", 0x155C, (GETTABLE | SETTABLE)},
+ {"PA_TxSkipPeriod", 0x155D, (GETTABLE | SETTABLE)},
+ {"PA_Local_TX_LCC_Enable", 0x155E, (GETTABLE | SETTABLE)},
+ {"PA_Peer_TX_LCC_Enable", 0x155F, (GETTABLE | SETTABLE)},
+ {"PA_ConnectedTxDataLanes", 0x1561, (GETTABLE | SETTABLE)},
+ {"PA_TxGear", 0x1568, (GETTABLE | SETTABLE)},
+ {"PA_TxTermination", 0x1569, (GETTABLE | SETTABLE)},
+ {"PA_HSSeries", 0x156A, (GETTABLE | SETTABLE)},
+ {"PA_PWRMode", 0x1571, (GETTABLE | SETTABLE)},
+ {"PA_ConnectedRxDataLanes", 0x1581, (GETTABLE | SETTABLE)},
+ {"PA_RxGear", 0x1583, (GETTABLE | SETTABLE)},
+ {"PA_RxTermination", 0x1584, (GETTABLE | SETTABLE)},
+ {"PA_Scrambling", 0x1585, (GETTABLE | SETTABLE)},
+ {"PA_MaxRxPWMGear", 0x1586, (GETTABLE | SETTABLE)},
+ {"PA_MaxRxHSGear", 0x1587, (GETTABLE | SETTABLE)},
+ {"PA_PACPReqTimeout", 0x1590, (GETTABLE | SETTABLE)},
+ {"PA_PACPReqEoBTimeout", 0x1591, (GETTABLE | SETTABLE)},
+ {"PA_LogicalLaneMap", 0x15A1, (GETTABLE | SETTABLE)},
+ {"PA_SleepNoConfigTime", 0x15A2, (GETTABLE | SETTABLE)},
+ {"PA_StallNoConfigTime", 0x15A3, (GETTABLE | SETTABLE)},
+ {"PA_SaveConfigTime", 0x15A4, (GETTABLE | SETTABLE)},
+ {"PA_RxHSUnterminationCapability", 0x15A5, (GETTABLE | SETTABLE)},
+ {"PA_RxLSTerminationCapability", 0x15A6, (GETTABLE | SETTABLE)},
+ {"PA_Hibern8Time", 0x15A7, (GETTABLE | SETTABLE)},
+ {"PA_TActivate", 0x15A8, (GETTABLE | SETTABLE)},
+ {"PA_LocalVerInfo", 0x15A9, (GETTABLE | SETTABLE)},
+ {"PA_Granularity", 0x15AA, (GETTABLE | SETTABLE)},
+ {"PA_MK2ExtensionGuardBand", 0x15AB, (GETTABLE | SETTABLE)},
+ {"PA_PWRModeUserData", 0x15B0, (GETTABLE | SETTABLE)},
+ {"PA_PACPFrameCount", 0x15C0, (GETTABLE | SETTABLE)},
+ {"PA_PACPErrorCount", 0x15C1, (GETTABLE | SETTABLE)},
+ {"PA_PHYTestControl", 0x15C2, (GETTABLE | SETTABLE)},
+ {"PA_TxHsG4SyncLength", 0x15D0, (GETTABLE | SETTABLE)},
+ {"PA_TxHsG4PrepareLength", 0x15D1, (GETTABLE | SETTABLE)},
+ {"PA_PeerRxHsAdaptRefresh", 0x15D2, (GETTABLE | SETTABLE)},
+ {"PA_PeerRxHsAdaptInitial", 0x15D3, (GETTABLE | SETTABLE)},
+ {"PA_TxHsAdaptType", 0x15D4, (GETTABLE | SETTABLE)},
+ {"PA_AdaptAfterLRSTInPA_INIT", 0x15D5, (GETTABLE | SETTABLE)},
+};
+
+/* Unipro QoS Measurement DME Attributes */
+static struct ufs_uic_attr_fields dme_qos_attrs[] = {
+ /* Unipro QoS Measurement DME Attributes */
+ {"DME_TX_DATA_OFL", 0x5100, (GETTABLE | SETTABLE)},
+ {"DME_TX_NAC_RECEIVED", 0x5101, (GETTABLE | SETTABLE)},
+ {"DME_TX_QoS_COUNT", 0x5102, (GETTABLE | SETTABLE)},
+ {"DME_TX_DL_LM_ERROR", 0x5103, (GETTABLE | SETTABLE)},
+ {"DME_RX_DATA_OFL", 0x5110, (GETTABLE | SETTABLE)},
+ {"DME_RX_CRC_ERROR", 0x5111, (GETTABLE | SETTABLE)},
+ {"DME_RX_QoS_COUNT", 0x5112, (GETTABLE | SETTABLE)},
+ {"DME_RX_DL_LM_ERROR", 0x5113, (GETTABLE | SETTABLE)},
+ {"DME_TXRX_DATA_OFL", 0x5120, (GETTABLE | SETTABLE)},
+ {"DME_TXRX_PA_INIT_REQUEST", 0x5121, (GETTABLE | SETTABLE)},
+ {"DME_TXRX_QoS_COUNT", 0x5122, (GETTABLE | SETTABLE)},
+ {"DME_TXRX_DL_LM_ERROR", 0x5123, (GETTABLE | SETTABLE)},
+ {"DME_QoS_ENABLE", 0x5130, (GETTABLE | SETTABLE)},
+ {"DME_QoS_STATUS", 0x5131, (GETTABLE | SETTABLE)},
+};
+
+/* M-TX/M-RX Capability Attributes */
+static struct ufs_uic_attr_fields mipi_mphy_attrs[] = {
+ /* M-PHY TX Capability Attributes */
+ {"TX_HSMODE_Capability", 0x0001, (GETTABLE)},
+ {"TX_HSGEAR_Capability", 0x0002, (GETTABLE)},
+ {"TX_PWMG0_Capability", 0x0003, (GETTABLE)},
+ {"TX_PWMGEAR_Capability", 0x0004, (GETTABLE)},
+ {"TX_Amplitude_Capability", 0x0005, (GETTABLE)},
+ {"TX_ExternalSYNC_Capability", 0x0006, (GETTABLE)},
+ {"TX_HS_Unterminated_LINE_Drive_Capability", 0x0007, (GETTABLE)},
+ {"TX_LS_Terminated_LINE_Drive_Capability", 0x0008, (GETTABLE)},
+ {"TX_Min_SLEEP_NoConfig_Time_Capability", 0x0009, (GETTABLE)},
+ {"TX_Min_STALL_NoConfig_Time_Capability", 0x000A, (GETTABLE)},
+ {"TX_Min_SAVE_Config_Time_Capability", 0x000B, (GETTABLE)},
+ {"TX_REF_CLOCK_SHARED_Capability", 0x000C, (GETTABLE)},
+ {"TX_PHY_MajorMinor_Release_Capability", 0x000D, (GETTABLE)},
+ {"TX_PHY_Editorial_Release_Capability", 0x000E, (GETTABLE)},
+ {"TX_Hibern8Time_Capability", 0x000F, (GETTABLE)},
+ {"TX_Advanced_Granularity_Capability", 0x0010, (GETTABLE)},
+ {"TX_Advanced_Hibern8Time_Capability", 0x0011, (GETTABLE)},
+ {"TX_HS_Equalizer_Setting_Capability", 0x0012, (GETTABLE)},
+
+ /* M-PHY TX Configuration Attributes */
+ {"TX_MODE", 0x0021, (GETTABLE | SETTABLE)},
+ {"TX_HSRATE_Series", 0x0022, (GETTABLE | SETTABLE)},
+ {"TX_HSGEAR", 0x0023, (GETTABLE | SETTABLE)},
+ {"TX_PWMGEAR", 0x0024, (GETTABLE | SETTABLE)},
+ {"TX_Amplitude", 0x0025, (GETTABLE | SETTABLE)},
+ {"TX_HS_SlewRate", 0x0026, (GETTABLE | SETTABLE)},
+ {"TX_SYNC_Source", 0x0027, (GETTABLE | SETTABLE)},
+ {"TX_HS_SYNC_LENGTH", 0x0028, (GETTABLE | SETTABLE)},
+ {"TX_HS_PREPARE_LENGTH", 0x0029, (GETTABLE | SETTABLE)},
+ {"TX_LS_PREPARE_LENGTH", 0x002A, (GETTABLE | SETTABLE)},
+ {"TX_HIBERN8_Control", 0x002B, (GETTABLE | SETTABLE)},
+ {"TX_LCC_Enable", 0x002C, (GETTABLE | SETTABLE)},
+ {"TX_PWM_BURST_Closure_Extension", 0x002D, (GETTABLE | SETTABLE)},
+ {"TX_BYPASS_8B10B_Enable", 0x002E, (GETTABLE | SETTABLE)},
+ {"TX_DRIVER_POLARITY", 0x002F, (GETTABLE | SETTABLE)},
+ {"TX_HS_Unterminated_LINE_Drive_Enable", 0x0030, (GETTABLE | SETTABLE)},
+ {"TX_LS_Terminated_LINE_Drive_Enable", 0x0031, (GETTABLE | SETTABLE)},
+ {"TX_LCC_Sequencer", 0x0032, (GETTABLE | SETTABLE)},
+ {"TX_Min_ActivateTime", 0x0033, (GETTABLE | SETTABLE)},
+ {"TX_PWM_G6_G7_SYNC_LENGTH", 0x0034, (GETTABLE | SETTABLE)},
+ {"TX_Advanced_Granularity_Step", 0x0035, (GETTABLE | SETTABLE)},
+ {"TX_Advanced_Granularity", 0x0036, (GETTABLE | SETTABLE)},
+ {"TX_HS_Equalizer_Setting", 0x0037, (GETTABLE | SETTABLE)},
+ {"TX_Min_SLEEP_NoConfig_Time", 0x0038, (GETTABLE | SETTABLE)},
+ {"TX_Min_STALL_NoConfig_Time", 0x0039, (GETTABLE | SETTABLE)},
+ {"TX_HS_ADAPT_LENGTH", 0x003A, (GETTABLE | SETTABLE)},
+
+ /* M-TX Status Attributes */
+ {"TX_FSM_State", 0x0041, (GETTABLE)},
+
+ /* M-PHY OMC Write-only Attributes */
+ {"MC_Output_Amplitude", 0x0061, (SETTABLE)},
+ {"MC_HS_Unterminated_Enable", 0x0062, (SETTABLE)},
+ {"MC_LS_Terminated_Enable", 0x0063, (SETTABLE)},
+ {"MC_HS_Unterminated_LINE_Drive_Enable", 0x0064, (SETTABLE)},
+ {"MC_LS_Terminated_LINE_Drive_Enable", 0x0065, (SETTABLE)},
+
+ /* M-PHY RX Capability Attributes */
+ {"RX_HSMODE_Capability", 0x0081, (GETTABLE)},
+ {"RX_HSGEAR_Capability", 0x0082, (GETTABLE)},
+ {"RX_PWMG0_Capability", 0x0083, (GETTABLE)},
+ {"RX_PWMGEAR_Capability", 0x0084, (GETTABLE)},
+ {"RX_HS_Unterminated_Capability", 0x0085, (GETTABLE)},
+ {"RX_LS_Terminated_Capability", 0x0086, (GETTABLE)},
+ {"RX_Min_SLEEP_NoConfig_Time_Capability", 0x0087, (GETTABLE)},
+ {"RX_Min_STALL_NoConfig_Time_Capability", 0x0088, (GETTABLE)},
+ {"RX_Min_SAVE_Config_Time_Capability", 0x0089, (GETTABLE)},
+ {"RX_REF_CLOCK_SHARED_Capability", 0x008A, (GETTABLE)},
+ {"RX_HS_G1_SYNC_LENGTH_Capability", 0x008B, (GETTABLE)},
+ {"RX_HS_G1_PREPARE_LENGTH_Capability", 0x008C, (GETTABLE)},
+ {"RX_LS_PREPARE_LENGTH_Capability", 0x008D, (GETTABLE)},
+ {"RX_PWM_Burst_Closure_Length_Capability", 0x008E, (GETTABLE)},
+ {"RX_Min_ActivateTime_Capability", 0x008F, (GETTABLE)},
+ {"RX_PHY_MajorMinor_Release_Capability", 0x0090, (GETTABLE)},
+ {"RX_PHY_Editorial_Release_Capability", 0x0091, (GETTABLE)},
+ {"RX_Hibern8Time_Capability", 0x0092, (GETTABLE)},
+ {"RX_PWM_G6_G7_SYNC_LENGTH_Capability", 0x0093, (GETTABLE)},
+ {"RX_HS_G2_SYNC_LENGTH_Capability", 0x0094, (GETTABLE)},
+ {"RX_HS_G3_SYNC_LENGTH_Capability", 0x0095, (GETTABLE)},
+ {"RX_HS_G2_PREPARE_LENGTH_Capability", 0x0096, (GETTABLE)},
+ {"RX_HS_G3_PREPARE_LENGTH_Capability", 0x0097, (GETTABLE)},
+ {"RX_Advanced_Granularity_Capability", 0x0098, (GETTABLE)},
+ {"RX_Advanced_Hibern8Time_Capability", 0x0099, (GETTABLE)},
+ {"RX_Advanced_Min_ActivateTime_Capability", 0x009A, (GETTABLE)},
+ {"RX_HS_G4_SYNC_LENGTH_Capability", 0x009B, (GETTABLE)},
+ {"RX_HS_G4_PREPARE_LENGTH_Capability", 0x009C, (GETTABLE)},
+ {"RX_HS_Equalizer_Setting_Capability", 0x009D, (GETTABLE)},
+ {"RX_HS_ADAPT_REFRESH_Capability", 0x009E, (GETTABLE)},
+ {"RX_HS_ADAPT_INITIAL_Capability", 0x009F, (GETTABLE)},
+
+ /* M-RX Configuration Attributes */
+ {"RX_MODE", 0x00A1, (GETTABLE | SETTABLE)},
+ {"RX_HSRATE_Series", 0x00A2, (GETTABLE | SETTABLE)},
+ {"RX_HSGEAR", 0x00A3, (GETTABLE | SETTABLE)},
+ {"RX_PWMGEAR", 0x00A4, (GETTABLE | SETTABLE)},
+ {"RX_LS_Terminated_Enable", 0x00A5, (GETTABLE | SETTABLE)},
+ {"RX_HS_Unterminated_Enable", 0x00A6, (GETTABLE | SETTABLE)},
+ {"RX_Enter_HIBERN8", 0x00A7, (GETTABLE | SETTABLE)},
+ {"RX_BYPASS_8B10B_Enable", 0x00A8, (GETTABLE | SETTABLE)},
+ {"RX_Termination_Force_Enable", 0x00A9, (GETTABLE | SETTABLE)},
+ {"RX_ADAPT_Control", 0x00AA, (GETTABLE | SETTABLE)},
+ {"RX_RECEIVER_POLARITY", 0x00AB, (GETTABLE | SETTABLE)},
+ {"RX_HS_ADAPT_LENGTH", 0x00AC, (GETTABLE | SETTABLE)},
+
+ /* M-PHY RX Status Attributes */
+ {"RX_FSM_State", 0x00C1, (GETTABLE)},
+
+ /* M-PHY OMC Status Attributes */
+ {"OMC_TYPE_Capability", 0x00D1, (GETTABLE)},
+ {"MC_HSMODE_Capability", 0x00D2, (GETTABLE)},
+ {"MC_HSGEAR_Capability", 0x00D3, (GETTABLE)},
+ {"MC_HS_START_TIME_Var_Capability", 0x00D4, (GETTABLE)},
+ {"MC_HS_START_TIME_Range_Capability", 0x00D5, (GETTABLE)},
+ {"MC_RX_SA_Capability", 0x00D6, (GETTABLE)},
+ {"MC_HS_LA_Capability", 0x00D7, (GETTABLE)},
+ {"MC_HS_LS_PREPARE_LENGTH", 0x00D8, (GETTABLE)},
+ {"MC_PWMG0_Capability", 0x00D9, (GETTABLE)},
+ {"MC_PWMGEAR_Capability", 0x00DA, (GETTABLE)},
+ {"MC_LS_Terminated_Capability", 0x00DB, (GETTABLE)},
+ {"MC_HS_Unterminated_Capability", 0x00DC, (GETTABLE)},
+ {"MC_LS_Terminated_LINE_Drive_Capability", 0x00DD, (GETTABLE)},
+ {"MC_HS_Unterminated_LINE_Drive_Capabilit", 0x00DE, (GETTABLE)},
+ {"MC_MFG_ID_Part1", 0x00DF, (GETTABLE)},
+ {"MC_MFG_ID_Part2", 0x00E0, (GETTABLE)},
+ {"MC_PHY_MajorMinor_Release_Capability", 0x00E1, (GETTABLE)},
+ {"MC_PHY_Editorial_Release_Capability", 0x00E2, (GETTABLE)},
+ {"MC_Vendor_Info_Part1", 0x00E3, (GETTABLE)},
+ {"MC_Vendor_Info_Part2", 0x00E4, (GETTABLE)},
+ {"MC_Vendor_Info_Part3", 0x00E5, (GETTABLE)},
+ {"MC_Vendor_Info_Part4", 0x00E6, (GETTABLE)},
+};
+
+static struct ufs_unipro_attrs_info uic_attrs_group[MAX_UNIPRO_IDN] = {
+ {
+ "MIPI M-PHY", mipi_mphy_attrs,
+ sizeof(mipi_mphy_attrs) / sizeof(struct ufs_uic_attr_fields)
+ },
+ {
+ "PHY-Adapter", phy_adapter_attrs,
+ sizeof(phy_adapter_attrs) / sizeof(struct ufs_uic_attr_fields)
+ },
+ {
+ "DME Attributes for QoS", dme_qos_attrs,
+ sizeof(dme_qos_attrs) / sizeof(struct ufs_uic_attr_fields)
+ },
+};
+
+static struct uic_cmd_result_code resultcode[] = {
+ {0, "SUCCESS"},
+ {1, "INVALID_MIB_ATTRIBUTE"},
+ {2, "INVALID_MIB_ATTRIBUTE_VALUE"},
+ {3, "READ_ONLY_MIB_ATTRIBUTE"},
+ {4, "WRITE_ONLY_MIB_ATTRIBUTE"},
+ {5, "BAD_INDEX"},
+ {6, "LOCKED_MIB_ATTRIBUTE"},
+ {7, "BAD_TEST_FEATURE_INDEX"},
+ {8, "PEER_COMMUNICATION_FAILURE"},
+ {9, "BUSY"},
+ {10, "DME_FAILURE"},
+};
+
+static int ufshcd_dme_get_attr(int fd, __u32 attr_sel, __u8 peer)
+{
+ struct ufs_bsg_request bsg_req = { 0 };
+ struct ufs_bsg_reply bsg_rsp = { 0 };
+ struct uic_command *uic_cmd =
+ (struct uic_command *)&bsg_req.upiu_req.uc;
+ struct uic_command uic_rsq = { 0 };
+
+ int rt = OK;
+ __u8 res_code;
+
+ uic_cmd->command = peer ? UIC_CMD_DME_PEER_GET : UIC_CMD_DME_GET;
+ uic_cmd->argument1 = attr_sel;
+ bsg_req.msgcode = UPIU_TRANSACTION_UIC_CMD;
+
+ rt = send_bsg_scsi_trs(fd, &bsg_req, &bsg_rsp, 0, 0, 0);
+ if (rt) {
+ print_error("%s: bsg request failed", __func__);
+ rt = ERROR;
+ goto out;
+ }
+
+ memcpy(&uic_rsq, &bsg_rsp.upiu_rsp.uc, UIC_CMD_SIZE);
+ res_code = uic_rsq.argument2 & MASK_UIC_COMMAND_RESULT;
+
+ if (res_code) {
+ __u8 max_code =
+ sizeof(resultcode) /
+ sizeof(struct uic_cmd_result_code);
+
+ if (res_code < (max_code - 1)) {
+ print_error("%s: attr-id 0x%x %s",
+ __func__,
+ UIC_GET_ATTR_ID(attr_sel),
+ resultcode[res_code].def);
+ } else {
+ print_error("%s: ID 0x%x, unknown error code %d",
+ __func__, UIC_GET_ATTR_ID(attr_sel),
+ res_code);
+ }
+
+ rt = ERROR;
+ } else {
+ rt = uic_rsq.argument3;
+ }
+
+out:
+ return rt;
+}
+
+static int ufshcd_dme_set_attr(int fd, __u32 attr_sel, __u8 attr_set,
+ __u32 mib_val, __u8 peer)
+{
+ struct ufs_bsg_request bsg_req = { 0 };
+ struct ufs_bsg_reply bsg_rsp = { 0 };
+ struct uic_command *uic_cmd =
+ (struct uic_command *)&bsg_req.upiu_req.uc;
+ struct uic_command uic_rsq = { 0 };
+
+ int rt = OK;
+ __u8 res_code;
+
+ uic_cmd->command = peer ? UIC_CMD_DME_PEER_SET : UIC_CMD_DME_SET;
+ uic_cmd->argument1 = attr_sel;
+ uic_cmd->argument2 = UIC_ARG_ATTR_TYPE(attr_set);
+ uic_cmd->argument3 = mib_val;
+
+ bsg_req.msgcode = UPIU_TRANSACTION_UIC_CMD;
+
+ rt = send_bsg_scsi_trs(fd, &bsg_req, &bsg_rsp, 0, 0, 0);
+ if (rt) {
+ print_error("%s: bsg request failed", __func__);
+ rt = ERROR;
+ goto out;
+ }
+
+ memcpy(&uic_rsq, &bsg_rsp.upiu_rsp.uc, UIC_CMD_SIZE);
+ res_code = uic_rsq.argument2 & MASK_UIC_COMMAND_RESULT;
+
+ if (res_code) {
+ __u8 max_code = sizeof(resultcode) /
+ sizeof(struct uic_cmd_result_code);
+
+ if (res_code < (max_code - 1)) {
+ print_error("%s: ID 0x%x %s",
+ __func__,
+ UIC_GET_ATTR_ID(attr_sel),
+ resultcode[res_code].def);
+ } else {
+ print_error("%s: ID 0x%x, unkonw error code %d",
+ __func__,
+ UIC_GET_ATTR_ID(attr_sel),
+ res_code);
+ }
+ rt = ERROR;
+ }
+
+out:
+ return rt;
+}
+
+static int check_attr_id(__u32 idn, __u32 id)
+{
+ int index;
+ int qts = uic_attrs_group[idn].items;
+ struct ufs_uic_attr_fields *p = uic_attrs_group[idn].attrs;
+
+ for (index = 0; index < qts; index++) {
+ if (p[index].id == id)
+ return index;
+ }
+ return INVALID;
+}
+
+static void display(int id, const char *name, int local, int peer)
+{
+ printf("[0x%04x]%-45s : local = 0x%08x, peer = 0x%08x\n",
+ id, name, local, peer);
+}
+
+static int unipro_read(int fd, int idn, int id, __u8 all)
+{
+ int index, qts;
+ int mib_val_local, mib_val_peer;
+ int ret = OK;
+
+ qts = uic_attrs_group[idn].items;
+ struct ufs_uic_attr_fields *p = uic_attrs_group[idn].attrs;
+
+ if (all) {
+ printf("\nUFS Unipro %s layer Attributes:\n",
+ uic_attrs_group[idn].name);
+
+ for (index = 0; index < qts; index++) {
+ if (p[index].acc_mode & GETTABLE) {
+ mib_val_local =
+ ufshcd_dme_get_attr(fd,
+ UIC_ARG_MIB(p[index].id),
+ DME_LOCAL);
+ mib_val_peer =
+ ufshcd_dme_get_attr(fd,
+ UIC_ARG_MIB(p[index].id),
+ DME_PEER);
+
+ if (mib_val_local != ERROR &&
+ mib_val_peer != ERROR) {
+ display(p[index].id, p[index].name,
+ mib_val_local, mib_val_peer);
+ } else {
+ print_error("Read %s ID 0x%x Failed",
+ ((mib_val_local == ERROR) &&
+ (mib_val_peer == ERROR)) ?
+ ("local&peer") :
+ ((mib_val_local == ERROR) ?
+ ("local") : ("peer")),
+ p[index].id);
+
+ ret = ERROR;
+ }
+ }
+ }
+ } else {
+ /* read single item */
+ index = check_attr_id(idn, id);
+ if (index >= 0) {
+ mib_val_local =
+ ufshcd_dme_get_attr(fd,
+ UIC_ARG_MIB(p[index].id),
+ DME_LOCAL);
+ mib_val_peer =
+ ufshcd_dme_get_attr(fd,
+ UIC_ARG_MIB(p[index].id),
+ DME_PEER);
+
+ if (mib_val_local != ERROR &&
+ mib_val_peer != ERROR) {
+ display(p[index].id, p[index].name,
+ mib_val_local, mib_val_peer);
+ } else {
+ print_error("Read %s ID 0x%x failed",
+ ((mib_val_local == ERROR) &&
+ (mib_val_peer == ERROR)) ?
+ ("local&peer") :
+ ((mib_val_local == ERROR) ?
+ ("local") : ("peer")),
+ p[index].id);
+ ret = ERROR;
+ }
+
+ } else {
+ print_error("Unsupport ID 0x%02x in %s",
+ id, uic_attrs_group[idn].name);
+ ret = ERROR;
+ }
+ }
+
+ return ret;
+}
+
+static int unipro_write(int fd, int idn, int id, int mib_val,
+ int attr_set, int target)
+{
+ int index;
+ int ret = OK;
+ struct ufs_uic_attr_fields *p = uic_attrs_group[idn].attrs;
+
+ index = check_attr_id(idn, id);
+
+ if (index >= 0) {
+ if (p[index].acc_mode & SETTABLE) {
+ ret = ufshcd_dme_set_attr(fd,
+ UIC_ARG_MIB(p[index].id),
+ attr_set, mib_val, target);
+
+ printf("%s set %s 0x%04x:%s to 0x%08x\n",
+ (ret == OK ? "Successfully" : "Failed"),
+ (target == DME_PEER ? "PEER" : "LOCAL"),
+ p[index].id, p[index].name, mib_val);
+ } else {
+ print_error("un-settable id 0x%02x in %s", id,
+ uic_attrs_group[idn].name);
+ ret = ERROR;
+ }
+ } else {
+ print_error("unsupport id 0x%02x in %s", id,
+ uic_attrs_group[idn].name);
+ ret = ERROR;
+ }
+
+ return ret;
+}
+
+int do_uic(struct tool_options *opt)
+{
+ int fd;
+ int rt = OK;
+ int oflag = O_RDWR;
+
+ if (opt->opr == READ_ALL || opt->opr == READ)
+ oflag = O_RDONLY;
+
+ fd = open(opt->path, oflag);
+ if (fd < 0) {
+ print_error("open");
+ return ERROR;
+ }
+
+ switch (opt->opr) {
+ case READ_ALL:
+ rt = unipro_read(fd, opt->idn, 0, 1);
+ break;
+ case READ:
+ rt = unipro_read(fd, opt->idn, opt->index, 0);
+ break;
+ case WRITE:
+ rt = unipro_write(fd,
+ opt->idn, opt->index,
+ *(__u32 *)opt->data,
+ ATTR_SET_NOR, opt->target);
+ break;
+ default:
+ rt = INVALID;
+ break;
+ }
+ close(fd);
+ return rt;
+}
+
+const char *help_str =
+ "\nUnipro command usage:\n"
+ " %s uic [-t idn] [-a|-r] [-i ID] [-w data <peer|local>] [-p bsg]\n\n"
+ " -t idn\n"
+ " Supported Unipro layers attributes idn as below:\n"
+ " 0: MIPI M-PHY Attributes\n"
+ " 1: PHY-Adapter Attributes\n"
+ " 2: DME Attributes for QoS\n\n"
+ " -a Read all gettable attributes of peer & local, please\n"
+ " use -t to specify Unipro attributes idn\n\n"
+ " -r Read single attribute of peer & local, please use -i\n"
+ " to specify attribute ID, and -t for associated idn\n\n"
+ " -w data <peer|local>\n"
+ " Write settable attribute, followed by data writing,\n"
+ " Please use -i to specify which ID to write, --peer\n"
+ " and --local to specify accessed target\n\n"
+ " --peer : access to a peer device (UFS device)\n"
+ " --local : access to a local device (UFS host)\n\n"
+ " -i ID\n"
+ " Set attribute ID to read/write\n"
+ " -p bsg\n"
+ " Path to ufs-bsg device\n\n"
+ " Note :\n"
+ " As for the format of the data inputted, hex number should be\n"
+ " prefixed by 0x/0X\n"
+ " Eg :\n"
+ " 1. Set local PA_TxTrailingClocks:\n"
+ " %s uic -t 1 -w 0x44 -i 0x1564 --local -p /dev/ufs-bsg\n"
+ " 2. Read peer and local PA_TxTrailingClocks:\n"
+ " %s uic -t 1 -r -i 0x1564 -p /dev/ufs-bsg\n";
+void unipro_help(char *tool_name)
+{
+ printf(help_str, tool_name, tool_name, tool_name);
+}
diff --git a/libmtk_bsg/unipro.h b/libmtk_bsg/unipro.h
new file mode 100644
index 0000000..760a186
--- /dev/null
+++ b/libmtk_bsg/unipro.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2020, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef UNIPRO_H_
+#define UNIPRO_H_
+
+#define UIC_ARG_MIB_SEL(attr, sel) ((((attr) & 0xFFFF) << 16) |\
+ ((sel) & 0xFFFF))
+#define UIC_ARG_MIB(attr) UIC_ARG_MIB_SEL(attr, 0)
+#define UIC_ARG_ATTR_TYPE(t) (((t) & 0xFF) << 16)
+#define UIC_GET_ATTR_ID(v) (((v) >> 16) & 0xFFFF)
+
+/* UIC command interfaces for DME primitives */
+#define DME_LOCAL 0
+#define DME_PEER 1
+#define ATTR_SET_NOR 0 /* NORMAL */
+#define ATTR_SET_ST 1 /* STATIC */
+#define MASK_UIC_COMMAND_RESULT 0xFF
+
+#define UPIU_TRANSACTION_UIC_CMD 0x1F
+/* uic commands are 4DW long, per UFSHCI V2.1 paragraph 5.6.1 */
+#define UIC_CMD_SIZE (sizeof(__u32) * 4)
+
+/**
+ * struct uic_command - UIC command structure
+ * @command: UIC command
+ * @argument1: UIC command argument 1
+ * @argument2: UIC command argument 2
+ * @argument3: UIC command argument 3
+ */
+struct uic_command {
+ __u32 command;
+ __u32 argument1;
+ __u32 argument2;
+ __u32 argument3;
+};
+
+enum unipro_acc_mode {
+ GETTABLE = (1 << 0),
+ SETTABLE = (1 << 1),
+ STATIC = (1 << 2),
+ DYNAMIC = (1 << 3)
+};
+
+struct ufs_uic_attr_fields {
+ const char *name;
+ __u32 id;
+ enum unipro_acc_mode acc_mode;
+};
+
+struct ufs_unipro_attrs_info {
+ const char *name;
+ struct ufs_uic_attr_fields *attrs;
+ __u32 items;
+};
+
+struct uic_cmd_result_code {
+ __u8 value;
+ const char *def;
+};
+
+/* Unipro attribute idn */
+enum unipro_attr_idn {
+ MPHY = 0x00,
+ PHY_ADAPTER = 0x01,
+ DME_QOS = 0X02,
+ MAX_UNIPRO_IDN,
+};
+
+/* UIC Commands */
+enum uic_cmd_dme {
+ UIC_CMD_DME_GET = 0x01,
+ UIC_CMD_DME_SET = 0x02,
+ UIC_CMD_DME_PEER_GET = 0x03,
+ UIC_CMD_DME_PEER_SET = 0x04,
+ UIC_CMD_DME_POWERON = 0x10,
+ UIC_CMD_DME_POWEROFF = 0x11,
+ UIC_CMD_DME_ENABLE = 0x12,
+ UIC_CMD_DME_RESET = 0x14,
+ UIC_CMD_DME_END_PT_RST = 0x15,
+ UIC_CMD_DME_LINK_STARTUP = 0x16,
+ UIC_CMD_DME_HIBER_ENTER = 0x17,
+ UIC_CMD_DME_HIBER_EXIT = 0x18,
+ UIC_CMD_DME_TEST_MODE = 0x1A,
+};
+
+int do_uic(struct tool_options *opt);
+void unipro_help(char *tool_name);
+
+#endif /* END UNIPRO_H_ */