// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (c) 2019 Samsung Electronics Co., Ltd.
 *            http://www.samsung.com/
 *
 * Samsung Exynos 5422 SoC Adaptive Supply Voltage support
 */

#include <linux/bitrev.h>
#include <linux/errno.h>
#include <linux/regmap.h>
#include <linux/soc/samsung/exynos-chipid.h>
#include <linux/slab.h>

#include "exynos-asv.h"
#include "exynos5422-asv.h"

#define ASV_GROUPS_NUM          14
#define ASV_ARM_DVFS_NUM        20
#define ASV_ARM_BIN2_DVFS_NUM   17
#define ASV_KFC_DVFS_NUM        14
#define ASV_KFC_BIN2_DVFS_NUM   12

/*
 * This array is a set of 4 ASV data tables, first column of each ASV table
 * contains frequency value in MHz and subsequent columns contain the CPU
 * cluster's supply voltage values in uV.
 * In order to create a set of OPPs for specific SoC revision one of the voltage
 * columns (1...14) from one of the tables (0...3) is selected during
 * initialization. There are separate ASV tables for the big (ARM) and little
 * (KFC) CPU cluster. Only OPPs which are already defined in devicetree
 * will be updated.
 */

static const u32 asv_arm_table[][ASV_ARM_DVFS_NUM][ASV_GROUPS_NUM + 1] = {
{
        /* ARM 0, 1 */
        { 2100,    1362500, 1362500, 1350000, 1337500, 1325000, 1312500, 1300000,
          1275000, 1262500, 1250000, 1237500, 1225000, 1212500, 1200000 },
        { 2000,    1312500, 1312500, 1300000, 1287500, 1275000, 1262500, 1250000,
          1237500, 1225000, 1237500, 1225000, 1212500, 1200000, 1187500 },
        { 1900,    1250000, 1237500, 1225000, 1212500, 1200000, 1187500, 1175000,
          1162500, 1150000, 1162500, 1150000, 1137500, 1125000, 1112500 },
        { 1800,    1200000, 1187500, 1175000, 1162500, 1150000, 1137500, 1125000,
          1112500, 1100000, 1112500, 1100000, 1087500, 1075000, 1062500 },
        { 1700,    1162500, 1150000, 1137500, 1125000, 1112500, 1100000, 1087500,
          1075000, 1062500, 1075000, 1062500, 1050000, 1037500, 1025000 },
        { 1600,    1125000, 1112500, 1100000, 1087500, 1075000, 1062500, 1050000,
          1037500, 1025000, 1037500, 1025000, 1012500, 1000000, 987500 },
        { 1500,    1087500, 1075000, 1062500, 1050000, 1037500, 1025000, 1012500,
          1000000, 987500,  1000000, 987500,  975000,  962500,  950000 },
        { 1400,    1062500, 1050000, 1037500, 1025000, 1012500, 1000000, 987500,
          975000,  962500,  975000,  962500,  950000,  937500,  925000 },
        { 1300,    1050000, 1037500, 1025000, 1012500, 1000000, 987500,  975000,
          962500,  950000,  962500,  950000,  937500,  925000,  912500 },
        { 1200,    1025000, 1012500, 1000000, 987500,  975000,  962500,  950000,
          937500,  925000,  937500,  925000,  912500,  900000,  900000 },
        { 1100,    1000000, 987500,  975000,  962500,  950000,  937500,  925000,
          912500,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 1000,    975000,  962500,  950000,  937500,  925000,  912500,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 900,     950000,  937500,  925000,  912500,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 800,     925000,  912500,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 700,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 600,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 500,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 400,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 300,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 200,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
}, {
        /* ARM 2 */
        { 2100,    1362500, 1362500, 1350000, 1337500, 1325000, 1312500, 1300000,
          1275000, 1262500, 1250000, 1237500, 1225000, 1212500, 1200000 },
        { 2000,    1312500, 1312500, 1312500, 1300000, 1275000, 1262500, 1250000,
          1237500, 1225000, 1237500, 1225000, 1212500, 1200000, 1187500 },
        { 1900,    1262500, 1250000, 1250000, 1237500, 1212500, 1200000, 1187500,
          1175000, 1162500, 1175000, 1162500, 1150000, 1137500, 1125000 },
        { 1800,    1212500, 1200000, 1187500, 1175000, 1162500, 1150000, 1137500,
          1125000, 1112500, 1125000, 1112500, 1100000, 1087500, 1075000 },
        { 1700,    1175000, 1162500, 1150000, 1137500, 1125000, 1112500, 1100000,
          1087500, 1075000, 1087500, 1075000, 1062500, 1050000, 1037500 },
        { 1600,    1137500, 1125000, 1112500, 1100000, 1087500, 1075000, 1062500,
          1050000, 1037500, 1050000, 1037500, 1025000, 1012500, 1000000 },
        { 1500,    1100000, 1087500, 1075000, 1062500, 1050000, 1037500, 1025000,
          1012500, 1000000, 1012500, 1000000, 987500,  975000,  962500 },
        { 1400,    1075000, 1062500, 1050000, 1037500, 1025000, 1012500, 1000000,
          987500,  975000,  987500,  975000,  962500,  950000,  937500 },
        { 1300,    1050000, 1037500, 1025000, 1012500, 1000000, 987500,  975000,
          962500,  950000,  962500,  950000,  937500,  925000,  912500 },
        { 1200,    1025000, 1012500, 1000000, 987500,  975000,  962500,  950000,
          937500,  925000,  937500,  925000,  912500,  900000,  900000 },
        { 1100,    1000000, 987500,  975000,  962500,  950000,  937500,  925000,
          912500,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 1000,    975000,  962500,  950000,  937500,  925000,  912500,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 900,     950000,  937500,  925000,  912500,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 800,     925000,  912500,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 700,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 600,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 500,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 400,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 300,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 200,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
}, {
        /* ARM 3 */
        { 2100,    1362500, 1362500, 1350000, 1337500, 1325000, 1312500, 1300000,
          1275000, 1262500, 1250000, 1237500, 1225000, 1212500, 1200000 },
        { 2000,    1312500, 1312500, 1300000, 1287500, 1275000, 1262500, 1250000,
          1237500, 1225000, 1237500, 1225000, 1212500, 1200000, 1187500 },
        { 1900,    1262500, 1250000, 1237500, 1225000, 1212500, 1200000, 1187500,
          1175000, 1162500, 1175000, 1162500, 1150000, 1137500, 1125000 },
        { 1800,    1212500, 1200000, 1187500, 1175000, 1162500, 1150000, 1137500,
          1125000, 1112500, 1125000, 1112500, 1100000, 1087500, 1075000 },
        { 1700,    1175000, 1162500, 1150000, 1137500, 1125000, 1112500, 1100000,
          1087500, 1075000, 1087500, 1075000, 1062500, 1050000, 1037500 },
        { 1600,    1137500, 1125000, 1112500, 1100000, 1087500, 1075000, 1062500,
          1050000, 1037500, 1050000, 1037500, 1025000, 1012500, 1000000 },
        { 1500,    1100000, 1087500, 1075000, 1062500, 1050000, 1037500, 1025000,
          1012500, 1000000, 1012500, 1000000, 987500,  975000,  962500 },
        { 1400,    1075000, 1062500, 1050000, 1037500, 1025000, 1012500, 1000000,
          987500,  975000,  987500,  975000,  962500,  950000,  937500 },
        { 1300,    1050000, 1037500, 1025000, 1012500, 1000000, 987500,  975000,
          962500,  950000,  962500,  950000,  937500,  925000,  912500 },
        { 1200,    1025000, 1012500, 1000000, 987500,  975000,  962500,  950000,
          937500,  925000,  937500,  925000,  912500,  900000,  900000 },
        { 1100,    1000000, 987500,  975000,  962500,  950000,  937500,  925000,
          912500,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 1000,    975000,  962500,  950000,  937500,  925000,  912500,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 900,     950000,  937500,  925000,  912500,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 800,     925000,  912500,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 700,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 600,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 500,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 400,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 300,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 200,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
}, {
        /* ARM bin 2 */
        { 1800,    1237500, 1225000, 1212500, 1200000, 1187500, 1175000, 1162500,
          1150000, 1137500, 1150000, 1137500, 1125000, 1112500, 1100000 },
        { 1700,    1200000, 1187500, 1175000, 1162500, 1150000, 1137500, 1125000,
          1112500, 1100000, 1112500, 1100000, 1087500, 1075000, 1062500 },
        { 1600,    1162500, 1150000, 1137500, 1125000, 1112500, 1100000, 1087500,
          1075000, 1062500, 1075000, 1062500, 1050000, 1037500, 1025000 },
        { 1500,    1125000, 1112500, 1100000, 1087500, 1075000, 1062500, 1050000,
          1037500, 1025000, 1037500, 1025000, 1012500, 1000000, 987500 },
        { 1400,    1100000, 1087500, 1075000, 1062500, 1050000, 1037500, 1025000,
          1012500, 1000000, 1012500, 1000000, 987500,  975000,  962500 },
        { 1300,    1087500, 1075000, 1062500, 1050000, 1037500, 1025000, 1012500,
          1000000, 987500,  1000000, 987500,  975000,  962500,  950000 },
        { 1200,    1062500, 1050000, 1037500, 1025000, 1012500, 1000000, 987500,
          975000,  962500,  975000,  962500,  950000,  937500,  925000 },
        { 1100,    1037500, 1025000, 1012500, 1000000, 987500,  975000,  962500,
          950000,  937500,  950000,  937500,  925000,  912500,  900000 },
        { 1000,    1012500, 1000000, 987500,  975000,  962500,  950000,  937500,
          925000,  912500,  925000,  912500,  900000,  900000,  900000 },
        { 900,     987500,  975000,  962500,  950000,  937500,  925000,  912500,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 800,     962500,  950000,  937500,  925000,  912500,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 700,     937500,  925000,  912500,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 600,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 500,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 400,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 300,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 200,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
}
};

static const u32 asv_kfc_table[][ASV_KFC_DVFS_NUM][ASV_GROUPS_NUM + 1] = {
{
        /* KFC 0, 1 */
        { 1500000, 1300000, 1300000, 1300000, 1287500, 1287500, 1287500, 1275000,
          1262500, 1250000, 1237500, 1225000, 1212500, 1200000, 1187500 },
        { 1400000, 1275000, 1262500, 1250000, 1237500, 1225000, 1212500, 1200000,
          1187500, 1175000, 1162500, 1150000, 1137500, 1125000, 1112500 },
        { 1300000, 1225000, 1212500, 1200000, 1187500, 1175000, 1162500, 1150000,
          1137500, 1125000, 1112500, 1100000, 1087500, 1075000, 1062500 },
        { 1200000, 1175000, 1162500, 1150000, 1137500, 1125000, 1112500, 1100000,
          1087500, 1075000, 1062500, 1050000, 1037500, 1025000, 1012500 },
        { 1100000, 1137500, 1125000, 1112500, 1100000, 1087500, 1075000, 1062500,
          1050000, 1037500, 1025000, 1012500, 1000000, 987500,  975000 },
        { 1000000, 1100000, 1087500, 1075000, 1062500, 1050000, 1037500, 1025000,
          1012500, 1000000, 987500,  975000,  962500,  950000,  937500 },
        { 900000,  1062500, 1050000, 1037500, 1025000, 1012500, 1000000, 987500,
          975000,  962500,  950000,  937500,  925000,  912500,  900000 },
        { 800000,  1025000, 1012500, 1000000, 987500,  975000,  962500,  950000,
          937500,  925000,  912500,  900000,  900000,  900000,  900000 },
        { 700000,  987500,  975000,  962500,  950000,  937500,  925000,  912500,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 600000,  950000,  937500,  925000,  912500,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 500000,  912500,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 400000,  900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 300000,  900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 200000,  900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
}, {
        /* KFC 2 */
        { 1500,    1300000, 1300000, 1300000, 1287500, 1287500, 1287500, 1275000,
          1262500, 1250000, 1237500, 1225000, 1212500, 1200000, 1187500 },
        { 1400,    1275000, 1262500, 1250000, 1237500, 1225000, 1212500, 1200000,
          1187500, 1175000, 1162500, 1150000, 1137500, 1125000, 1112500 },
        { 1300,    1225000, 1212500, 1200000, 1187500, 1175000, 1162500, 1150000,
          1137500, 1125000, 1112500, 1100000, 1087500, 1075000, 1062500 },
        { 1200,    1175000, 1162500, 1150000, 1137500, 1125000, 1112500, 1100000,
          1087500, 1075000, 1062500, 1050000, 1037500, 1025000, 1012500 },
        { 1100,    1137500, 1125000, 1112500, 1100000, 1087500, 1075000, 1062500,
          1050000, 1037500, 1025000, 1012500, 1000000, 987500,  975000 },
        { 1000,    1100000, 1087500, 1075000, 1062500, 1050000, 1037500, 1025000,
          1012500, 1000000, 987500,  975000,  962500,  950000,  937500 },
        { 900,     1062500, 1050000, 1037500, 1025000, 1012500, 1000000, 987500,
          975000,  962500,  950000,  937500,  925000,  912500,  900000 },
        { 800,     1025000, 1012500, 1000000, 987500,  975000,  962500,  950000,
          937500,  925000,  912500,  900000,  900000,  900000,  900000 },
        { 700,     987500,  975000,  962500,  950000,  937500,  925000,  912500,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 600,     950000,  937500,  925000,  912500,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 500,     912500,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 400,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 300,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 200,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
}, {
        /* KFC 3 */
        { 1500,    1300000, 1300000, 1300000, 1287500, 1287500, 1287500, 1275000,
          1262500, 1250000, 1237500, 1225000, 1212500, 1200000, 1187500 },
        { 1400,    1275000, 1262500, 1250000, 1237500, 1225000, 1212500, 1200000,
          1187500, 1175000, 1162500, 1150000, 1137500, 1125000, 1112500 },
        { 1300,    1225000, 1212500, 1200000, 1187500, 1175000, 1162500, 1150000,
          1137500, 1125000, 1112500, 1100000, 1087500, 1075000, 1062500 },
        { 1200,    1175000, 1162500, 1150000, 1137500, 1125000, 1112500, 1100000,
          1087500, 1075000, 1062500, 1050000, 1037500, 1025000, 1012500 },
        { 1100,    1137500, 1125000, 1112500, 1100000, 1087500, 1075000, 1062500,
          1050000, 1037500, 1025000, 1012500, 1000000, 987500,  975000 },
        { 1000,    1100000, 1087500, 1075000, 1062500, 1050000, 1037500, 1025000,
          1012500, 1000000, 987500,  975000,  962500,  950000,  937500 },
        { 900,     1062500, 1050000, 1037500, 1025000, 1012500, 1000000, 987500,
          975000,  962500,  950000,  937500,  925000,  912500,  900000 },
        { 800,     1025000, 1012500, 1000000, 987500,  975000,  962500,  950000,
          937500,  925000,  912500,  900000,  900000,  900000,  900000 },
        { 700,     987500,  975000,  962500,  950000,  937500,  925000,  912500,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 600,     950000,  937500,  925000,  912500,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 500,     912500,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 400,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 300,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 200,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
}, {
        /* KFC bin 2 */
        { 1300,    1250000, 1237500, 1225000, 1212500, 1200000, 1187500, 1175000,
          1162500, 1150000, 1137500, 1125000, 1112500, 1100000, 1087500 },
        { 1200,    1200000, 1187500, 1175000, 1162500, 1150000, 1137500, 1125000,
          1112500, 1100000, 1087500, 1075000, 1062500, 1050000, 1037500 },
        { 1100,    1162500, 1150000, 1137500, 1125000, 1112500, 1100000, 1087500,
          1075000, 1062500, 1050000, 1037500, 1025000, 1012500, 1000000 },
        { 1000,    1125000, 1112500, 1100000, 1087500, 1075000, 1062500, 1050000,
          1037500, 1025000, 1012500, 1000000, 987500,  975000,  962500 },
        { 900,     1087500, 1075000, 1062500, 1050000, 1037500, 1025000, 1012500,
          1000000, 987500,  975000,  962500,  950000,  937500,  925000 },
        { 800,     1050000, 1037500, 1025000, 1012500, 1000000, 987500,  975000,
          962500,  950000,  937500,  925000,  912500,  900000,  900000 },
        { 700,     1012500, 1000000, 987500,  975000,  962500,  950000,  937500,
          925000,  912500,  900000,  900000,  900000,  900000,  900000 },
        { 600,     975000,  962500,  950000,  937500,  925000,  912500,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 500,     937500,  925000,  912500,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 400,     925000,  912500,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 300,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
        { 200,     900000,  900000,  900000,  900000,  900000,  900000,  900000,
          900000,  900000,  900000,  900000,  900000,  900000,  900000 },
}
};

static const struct asv_limit_entry __asv_limits[ASV_GROUPS_NUM] = {
        { 13, 55 },
        { 21, 65 },
        { 25, 69 },
        { 30, 72 },
        { 36, 74 },
        { 43, 76 },
        { 51, 78 },
        { 65, 80 },
        { 81, 82 },
        { 98, 84 },
        { 119, 87 },
        { 135, 89 },
        { 150, 92 },
        { 999, 999 },
};

static int exynos5422_asv_get_group(struct exynos_asv *asv)
{
        unsigned int pkgid_reg, auxi_reg;
        int hpm, ids, i;

        regmap_read(asv->chipid_regmap, EXYNOS_CHIPID_REG_PKG_ID, &pkgid_reg);
        regmap_read(asv->chipid_regmap, EXYNOS_CHIPID_REG_AUX_INFO, &auxi_reg);

        if (asv->use_sg) {
                u32 sga = (pkgid_reg >> EXYNOS5422_SG_A_OFFSET) &
                           EXYNOS5422_SG_A_MASK;

                u32 sgb = (pkgid_reg >> EXYNOS5422_SG_B_OFFSET) &
                           EXYNOS5422_SG_B_MASK;

                if ((pkgid_reg >> EXYNOS5422_SG_BSIGN_OFFSET) &
                     EXYNOS5422_SG_BSIGN_MASK)
                        return sga + sgb;
                else
                        return sga - sgb;
        }

        hpm = (auxi_reg >> EXYNOS5422_TMCB_OFFSET) & EXYNOS5422_TMCB_MASK;
        ids = (pkgid_reg >> EXYNOS5422_IDS_OFFSET) & EXYNOS5422_IDS_MASK;

        for (i = 0; i < ASV_GROUPS_NUM; i++) {
                if (ids <= __asv_limits[i].ids)
                        break;
                if (hpm <= __asv_limits[i].hpm)
                        break;
        }
        if (i < ASV_GROUPS_NUM)
                return i;

        return 0;
}

static int __asv_offset_voltage(unsigned int index)
{
        switch (index) {
        case 1:
                return 12500;
        case 2:
                return 50000;
        case 3:
                return 25000;
        default:
                return 0;
        }
}

static void exynos5422_asv_offset_voltage_setup(struct exynos_asv *asv)
{
        struct exynos_asv_subsys *subsys;
        unsigned int reg, value;

        regmap_read(asv->chipid_regmap, EXYNOS_CHIPID_REG_AUX_INFO, &reg);

        /* ARM offset voltage setup */
        subsys = &asv->subsys[EXYNOS_ASV_SUBSYS_ID_ARM];

        subsys->base_volt = 1000000;

        value = (reg >> EXYNOS5422_ARM_UP_OFFSET) & EXYNOS5422_ARM_UP_MASK;
        subsys->offset_volt_h = __asv_offset_voltage(value);

        value = (reg >> EXYNOS5422_ARM_DN_OFFSET) & EXYNOS5422_ARM_DN_MASK;
        subsys->offset_volt_l = __asv_offset_voltage(value);

        /* KFC offset voltage setup */
        subsys = &asv->subsys[EXYNOS_ASV_SUBSYS_ID_KFC];

        subsys->base_volt = 1000000;

        value = (reg >> EXYNOS5422_KFC_UP_OFFSET) & EXYNOS5422_KFC_UP_MASK;
        subsys->offset_volt_h = __asv_offset_voltage(value);

        value = (reg >> EXYNOS5422_KFC_DN_OFFSET) & EXYNOS5422_KFC_DN_MASK;
        subsys->offset_volt_l = __asv_offset_voltage(value);
}

static int exynos5422_asv_opp_get_voltage(const struct exynos_asv_subsys *subsys,
                                          int level, unsigned int volt)
{
        unsigned int asv_volt;

        if (level >= subsys->table.num_rows)
                return volt;

        asv_volt = exynos_asv_opp_get_voltage(subsys, level,
                                              subsys->asv->group);

        if (volt > subsys->base_volt)
                asv_volt += subsys->offset_volt_h;
        else
                asv_volt += subsys->offset_volt_l;

        return asv_volt;
}

static unsigned int exynos5422_asv_parse_table(unsigned int pkg_id)
{
        return (pkg_id >> EXYNOS5422_TABLE_OFFSET) & EXYNOS5422_TABLE_MASK;
}

static bool exynos5422_asv_parse_bin2(unsigned int pkg_id)
{
        return (pkg_id >> EXYNOS5422_BIN2_OFFSET) & EXYNOS5422_BIN2_MASK;
}

static bool exynos5422_asv_parse_sg(unsigned int pkg_id)
{
        return (pkg_id >> EXYNOS5422_USESG_OFFSET) & EXYNOS5422_USESG_MASK;
}

int exynos5422_asv_init(struct exynos_asv *asv)
{
        struct exynos_asv_subsys *subsys;
        unsigned int table_index;
        unsigned int pkg_id;
        bool bin2;

        regmap_read(asv->chipid_regmap, EXYNOS_CHIPID_REG_PKG_ID, &pkg_id);

        if (asv->of_bin == 2) {
                bin2 = true;
                asv->use_sg = false;
        } else {
                asv->use_sg = exynos5422_asv_parse_sg(pkg_id);
                bin2 = exynos5422_asv_parse_bin2(pkg_id);
        }

        asv->group = exynos5422_asv_get_group(asv);
        asv->table = exynos5422_asv_parse_table(pkg_id);

        exynos5422_asv_offset_voltage_setup(asv);

        if (bin2) {
                table_index = 3;
        } else {
                if (asv->table == 2 || asv->table == 3)
                        table_index = asv->table - 1;
                else
                        table_index = 0;
        }

        subsys = &asv->subsys[EXYNOS_ASV_SUBSYS_ID_ARM];
        subsys->cpu_dt_compat = "arm,cortex-a15";
        if (bin2)
                subsys->table.num_rows = ASV_ARM_BIN2_DVFS_NUM;
        else
                subsys->table.num_rows = ASV_ARM_DVFS_NUM;
        subsys->table.num_cols = ASV_GROUPS_NUM + 1;
        subsys->table.buf = (u32 *)asv_arm_table[table_index];

        subsys = &asv->subsys[EXYNOS_ASV_SUBSYS_ID_KFC];
        subsys->cpu_dt_compat = "arm,cortex-a7";
        if (bin2)
                subsys->table.num_rows = ASV_KFC_BIN2_DVFS_NUM;
        else
                subsys->table.num_rows = ASV_KFC_DVFS_NUM;
        subsys->table.num_cols = ASV_GROUPS_NUM + 1;
        subsys->table.buf = (u32 *)asv_kfc_table[table_index];

        asv->opp_get_voltage = exynos5422_asv_opp_get_voltage;

        return 0;
}
EXPORT_SYMBOL_GPL(exynos5422_asv_init);