int Load_D_LogBlockAddr(BYTE *redundant)
{
WORD addr1, addr2;
addr1 = (WORD)*(redundant + REDT_ADDR1H)*0x0100 + (WORD)*(redundant + REDT_ADDR1L);
addr2 = (WORD)*(redundant + REDT_ADDR2H)*0x0100 + (WORD)*(redundant + REDT_ADDR2L);
if (addr1 == addr2)
if ((addr1 & 0xF000) == 0x1000) {
Media.LogBlock = (addr1 & 0x0FFF) / 2;
return SMSUCCESS;
}
if (hweight16((WORD)(addr1^addr2)) != 0x01)
return ERROR;
if ((addr1 & 0xF000) == 0x1000)
if (!(hweight16(addr1) & 0x01)) {
Media.LogBlock = (addr1 & 0x0FFF) / 2;
return SMSUCCESS;
}
if ((addr2 & 0xF000) == 0x1000)
if (!(hweight16(addr2) & 0x01)) {
Media.LogBlock = (addr2 & 0x0FFF) / 2;
return SMSUCCESS;
}
return ERROR;
}
//----- Load_D_LogBlockAddr() ------------------------------------------
int Load_D_LogBlockAddr(BYTE *redundant)
{
WORD addr1,addr2;
//SSFDCTYPE_T aa = (SSFDCTYPE_T ) &Ssfdc;
//ADDRESS_T bb = (ADDRESS_T) &Media;
addr1=(WORD)*(redundant+REDT_ADDR1H)*0x0100+(WORD)*(redundant+REDT_ADDR1L);
addr2=(WORD)*(redundant+REDT_ADDR2H)*0x0100+(WORD)*(redundant+REDT_ADDR2L);
if (addr1==addr2)
if ((addr1 &0xF000)==0x1000)
{ Media.LogBlock=(addr1 &0x0FFF)/2; return(SMSUCCESS); }
if (hweight16((WORD)(addr1^addr2))!=0x01) return(ERROR);
if ((addr1 &0xF000)==0x1000)
if (!(hweight16(addr1) &0x01))
{ Media.LogBlock=(addr1 &0x0FFF)/2; return(SMSUCCESS); }
if ((addr2 &0xF000)==0x1000)
if (!(hweight16(addr2) &0x01))
{ Media.LogBlock=(addr2 &0x0FFF)/2; return(SMSUCCESS); }
return(ERROR);
}
/*
* The logical block number assigned to a physical block is stored in the OOB
* of the first page, in 3 16-bit copies with the following layout:
*
* 01234567 89abcdef
* -------- --------
* ECC BB xyxyxy
*
* When reading we check that the first two copies agree.
* In case of error, matching is tried using the following pairs.
* Reserved values 0xffff mean the block is kept for wear leveling.
*
* 01234567 89abcdef
* -------- --------
* ECC BB xyxy oob[8]==oob[10] && oob[9]==oob[11] -> byte0=8 byte1=9
* ECC BB xyxy oob[10]==oob[12] && oob[11]==oob[13] -> byte0=10 byte1=11
* ECC BB xy xy oob[12]==oob[8] && oob[13]==oob[9] -> byte0=12 byte1=13
*/
static int sharpsl_nand_get_logical_num(u8 *oob)
{
u16 us;
int good0, good1;
if (oob[NAND_NOOB_LOGADDR_00] == oob[NAND_NOOB_LOGADDR_10] &&
oob[NAND_NOOB_LOGADDR_01] == oob[NAND_NOOB_LOGADDR_11]) {
good0 = NAND_NOOB_LOGADDR_00;
good1 = NAND_NOOB_LOGADDR_01;
} else if (oob[NAND_NOOB_LOGADDR_10] == oob[NAND_NOOB_LOGADDR_20] &&
oob[NAND_NOOB_LOGADDR_11] == oob[NAND_NOOB_LOGADDR_21]) {
good0 = NAND_NOOB_LOGADDR_10;
good1 = NAND_NOOB_LOGADDR_11;
} else if (oob[NAND_NOOB_LOGADDR_20] == oob[NAND_NOOB_LOGADDR_00] &&
oob[NAND_NOOB_LOGADDR_21] == oob[NAND_NOOB_LOGADDR_01]) {
good0 = NAND_NOOB_LOGADDR_20;
good1 = NAND_NOOB_LOGADDR_21;
} else {
return -EINVAL;
}
us = oob[good0] | oob[good1] << 8;
/* parity check */
if (hweight16(us) & BLOCK_UNMASK_COMPLEMENT)
return -EINVAL;
/* reserved */
if (us == BLOCK_IS_RESERVED)
return BLOCK_IS_RESERVED;
return (us >> 1) & GENMASK(9, 0);
}
void hweight16_test()
{
for (int i = 0; i < 100000; ++i) {
uint16_t r = RAND_NR_NEXT(u, v, w);
assert(__builtin_popcount(r) == hweight16(r));
}
}
static u32 count_free(u8 *map[],unsigned blocksize,u32 numbits) {
u32 sum = 0;
unsigned blocks = DIV_ROUND_UP(numbits,blocksize * 8);
while(blocks--) {
unsigned words = blocksize / 2;
u16 *p = (u16 *)(*map++);
while(words--)
sum += 16 - hweight16(*p++);
}
return sum;
}
static __u32 count_free(struct buffer_head *map[], unsigned blocksize, __u32 numbits)
{
__u32 sum = 0;
unsigned blocks = DIV_ROUND_UP(numbits, blocksize * 8);
while (blocks--) {
unsigned words = blocksize / 2;
__u16 *p = (__u16 *)(*map++)->b_data;
while (words--)
sum += 16 - hweight16(*p++);
}
return sum;
}
void Set_D_LogBlockAddr(BYTE *redundant)
{
WORD addr;
*(redundant + REDT_BLOCK) = 0xFF;
*(redundant + REDT_DATA) = 0xFF;
addr = Media.LogBlock*2 + 0x1000;
if ((hweight16(addr) % 2))
addr++;
*(redundant + REDT_ADDR1H) = *(redundant + REDT_ADDR2H) = (BYTE)(addr / 0x0100);
*(redundant + REDT_ADDR1L) = *(redundant + REDT_ADDR2L) = (BYTE)addr;
}
static int twl4030_is_in_ghost_state(struct twl4030_keypad *kp, u16 *key_state)
{
int i;
u16 check = 0;
for (i = 0; i < kp->n_rows; i++) {
u16 col = key_state[i];
if ((col & check) && hweight16(col) > 1)
return 1;
check |= col;
}
return 0;
}
static inline int ina3221_wait_for_data(struct ina3221_data *ina)
{
u32 channels = hweight16(ina->reg_config & INA3221_CONFIG_CHs_EN_MASK);
u32 vbus_ct_idx = INA3221_CONFIG_VBUS_CT(ina->reg_config);
u32 vsh_ct_idx = INA3221_CONFIG_VSH_CT(ina->reg_config);
u32 vbus_ct = ina3221_conv_time[vbus_ct_idx];
u32 vsh_ct = ina3221_conv_time[vsh_ct_idx];
u32 wait, cvrf;
/* Calculate total conversion time */
wait = channels * (vbus_ct + vsh_ct);
/* Polling the CVRF bit to make sure read data is ready */
return regmap_field_read_poll_timeout(ina->fields[F_CVRF],
cvrf, cvrf, wait, 100000);
}
static bool pmic8xxx_detect_ghost_keys(struct pmic8xxx_kp *kp, u16 *new_state)
{
int row, found_first = -1;
u16 check, row_state;
/*
* for u8860, c8860 and u8860lp, add the codes means:
* when volumn-up and volumn-down keys are pressed in the sametime,
* the state of kp scan matrix read from the register is wrong because of hardwared's wrong,
* and it will make system think the state as ghost keys mistakenly ,
* so these board ids should not be check for ghost keys
*/
#ifdef CONFIG_HUAWEI_KERNEL
if (machine_is_msm8255_u8860()
|| machine_is_msm8255_c8860()
|| machine_is_msm8255_u8860lp()
|| machine_is_msm8255_u8860_r()
|| machine_is_msm8255_u8860_51())
{
return 0;
}
#endif
check = 0;
for (row = 0; row < kp->pdata->num_rows; row++) {
row_state = (~new_state[row]) &
((1 << kp->pdata->num_cols) - 1);
if (hweight16(row_state) > 1) {
if (found_first == -1)
found_first = row;
if (check & row_state) {
dev_dbg(kp->dev, "detected ghost key on row[%d]"
" and row[%d]\n", found_first, row);
return true;
}
}
check |= row_state;
}
return false;
}
static bool pmic8xxx_detect_ghost_keys(struct pmic8xxx_kp *kp, u16 *new_state)
{
int row, found_first = -1;
u16 check, row_state;
check = 0;
for (row = 0; row < kp->pdata->num_rows; row++) {
row_state = (~new_state[row]) &
((1 << kp->pdata->num_cols) - 1);
if (hweight16(row_state) > 1) {
if (found_first == -1)
found_first = row;
if (check & row_state) {
dev_dbg(kp->dev, "detected ghost key on row[%d]"
" and row[%d]\n", found_first, row);
return true;
}
}
check |= row_state;
}
return false;
}
static int i5k_amb_hwmon_init(struct platform_device *pdev)
{
int i, j, k, d = 0;
u16 c;
int res = 0;
int num_ambs = 0;
struct i5k_amb_data *data = platform_get_drvdata(pdev);
/* Count the number of AMBs found */
/* ignore the high-order bit, see "Ugly hack" comment above */
for (i = 0; i < MAX_MEM_CHANNELS; i++)
num_ambs += hweight16(data->amb_present[i] & 0x7fff);
/* Set up sysfs stuff */
data->attrs = kzalloc(sizeof(*data->attrs) * num_ambs * KNOBS_PER_AMB,
GFP_KERNEL);
if (!data->attrs)
return -ENOMEM;
data->num_attrs = 0;
for (i = 0; i < MAX_MEM_CHANNELS; i++) {
c = data->amb_present[i];
for (j = 0; j < REAL_MAX_AMBS_PER_CHANNEL; j++, c >>= 1) {
struct i5k_device_attribute *iattr;
k = amb_num_from_reg(i, j);
if (!(c & 0x1))
continue;
d++;
/* sysfs label */
iattr = data->attrs + data->num_attrs;
snprintf(iattr->name, AMB_SYSFS_NAME_LEN,
"temp%d_label", d);
iattr->s_attr.dev_attr.attr.name = iattr->name;
iattr->s_attr.dev_attr.attr.mode = S_IRUGO;
iattr->s_attr.dev_attr.show = show_label;
iattr->s_attr.index = k;
sysfs_attr_init(&iattr->s_attr.dev_attr.attr);
res = device_create_file(&pdev->dev,
&iattr->s_attr.dev_attr);
if (res)
goto exit_remove;
data->num_attrs++;
/* Temperature sysfs knob */
iattr = data->attrs + data->num_attrs;
snprintf(iattr->name, AMB_SYSFS_NAME_LEN,
"temp%d_input", d);
iattr->s_attr.dev_attr.attr.name = iattr->name;
iattr->s_attr.dev_attr.attr.mode = S_IRUGO;
iattr->s_attr.dev_attr.show = show_amb_temp;
iattr->s_attr.index = k;
sysfs_attr_init(&iattr->s_attr.dev_attr.attr);
res = device_create_file(&pdev->dev,
&iattr->s_attr.dev_attr);
if (res)
goto exit_remove;
data->num_attrs++;
/* Temperature min sysfs knob */
iattr = data->attrs + data->num_attrs;
snprintf(iattr->name, AMB_SYSFS_NAME_LEN,
"temp%d_min", d);
iattr->s_attr.dev_attr.attr.name = iattr->name;
iattr->s_attr.dev_attr.attr.mode = S_IWUSR | S_IRUGO;
iattr->s_attr.dev_attr.show = show_amb_min;
iattr->s_attr.dev_attr.store = store_amb_min;
iattr->s_attr.index = k;
sysfs_attr_init(&iattr->s_attr.dev_attr.attr);
res = device_create_file(&pdev->dev,
&iattr->s_attr.dev_attr);
if (res)
goto exit_remove;
data->num_attrs++;
/* Temperature mid sysfs knob */
iattr = data->attrs + data->num_attrs;
snprintf(iattr->name, AMB_SYSFS_NAME_LEN,
"temp%d_mid", d);
iattr->s_attr.dev_attr.attr.name = iattr->name;
iattr->s_attr.dev_attr.attr.mode = S_IWUSR | S_IRUGO;
iattr->s_attr.dev_attr.show = show_amb_mid;
iattr->s_attr.dev_attr.store = store_amb_mid;
iattr->s_attr.index = k;
sysfs_attr_init(&iattr->s_attr.dev_attr.attr);
res = device_create_file(&pdev->dev,
&iattr->s_attr.dev_attr);
if (res)
goto exit_remove;
data->num_attrs++;
/* Temperature max sysfs knob */
iattr = data->attrs + data->num_attrs;
snprintf(iattr->name, AMB_SYSFS_NAME_LEN,
"temp%d_max", d);
iattr->s_attr.dev_attr.attr.name = iattr->name;
iattr->s_attr.dev_attr.attr.mode = S_IWUSR | S_IRUGO;
iattr->s_attr.dev_attr.show = show_amb_max;
iattr->s_attr.dev_attr.store = store_amb_max;
iattr->s_attr.index = k;
sysfs_attr_init(&iattr->s_attr.dev_attr.attr);
res = device_create_file(&pdev->dev,
&iattr->s_attr.dev_attr);
if (res)
goto exit_remove;
data->num_attrs++;
/* Temperature alarm sysfs knob */
iattr = data->attrs + data->num_attrs;
snprintf(iattr->name, AMB_SYSFS_NAME_LEN,
"temp%d_alarm", d);
iattr->s_attr.dev_attr.attr.name = iattr->name;
iattr->s_attr.dev_attr.attr.mode = S_IRUGO;
iattr->s_attr.dev_attr.show = show_amb_alarm;
iattr->s_attr.index = k;
sysfs_attr_init(&iattr->s_attr.dev_attr.attr);
res = device_create_file(&pdev->dev,
&iattr->s_attr.dev_attr);
if (res)
goto exit_remove;
data->num_attrs++;
}
}
res = device_create_file(&pdev->dev, &dev_attr_name);
if (res)
goto exit_remove;
data->hwmon_dev = hwmon_device_register(&pdev->dev);
if (IS_ERR(data->hwmon_dev)) {
res = PTR_ERR(data->hwmon_dev);
goto exit_remove;
}
return res;
exit_remove:
device_remove_file(&pdev->dev, &dev_attr_name);
for (i = 0; i < data->num_attrs; i++)
device_remove_file(&pdev->dev, &data->attrs[i].s_attr.dev_attr);
kfree(data->attrs);
return res;
}
/*
* LDM/STM alignment handler.
*
* There are 4 variants of this instruction:
*
* B = rn pointer before instruction, A = rn pointer after instruction
* ------ increasing address ----->
* | | r0 | r1 | ... | rx | |
* PU = 01 B A
* PU = 11 B A
* PU = 00 A B
* PU = 10 A B
*/
static int
do_alignment_ldmstm(unsigned long addr, unsigned long instr, struct pt_regs *regs)
{
unsigned int rd, rn, correction, nr_regs, regbits;
unsigned long eaddr, newaddr;
if (LDM_S_BIT(instr))
goto bad;
correction = 4; /* processor implementation defined */
regs->ARM_pc += correction;
ai_multi += 1;
/* count the number of registers in the mask to be transferred */
nr_regs = hweight16(REGMASK_BITS(instr)) * 4;
rn = RN_BITS(instr);
newaddr = eaddr = regs->uregs[rn];
if (!LDST_U_BIT(instr))
nr_regs = -nr_regs;
newaddr += nr_regs;
if (!LDST_U_BIT(instr))
eaddr = newaddr;
if (LDST_P_EQ_U(instr)) /* U = P */
eaddr += 4;
/*
* For alignment faults on the ARM922T/ARM920T the MMU makes
* the FSR (and hence addr) equal to the updated base address
* of the multiple access rather than the restored value.
* Switch this message off if we've got a ARM92[02], otherwise
* [ls]dm alignment faults are noisy!
*/
#if !(defined CONFIG_CPU_ARM922T) && !(defined CONFIG_CPU_ARM920T)
/*
* This is a "hint" - we already have eaddr worked out by the
* processor for us.
*/
if (addr != eaddr) {
printk(KERN_ERR "LDMSTM: PC = %08lx, instr = %08lx, "
"addr = %08lx, eaddr = %08lx\n",
instruction_pointer(regs), instr, addr, eaddr);
show_regs(regs);
}
#endif
if (user_mode(regs)) {
for (regbits = REGMASK_BITS(instr), rd = 0; regbits;
regbits >>= 1, rd += 1)
if (regbits & 1) {
if (LDST_L_BIT(instr)) {
unsigned int val;
get32t_unaligned_check(val, eaddr);
regs->uregs[rd] = val;
} else
put32t_unaligned_check(regs->uregs[rd], eaddr);
eaddr += 4;
}
} else {
for (regbits = REGMASK_BITS(instr), rd = 0; regbits;
static int __pmic8058_kp_scan_matrix(struct pmic8058_kp *kp, u16 *new_state,
u16 *old_state)
{
int row, col, code;
#ifdef CONFIG_KEYBOARD_ZTE
for (row = 0; row < KEYP_NUM_ROWS; row++) {
#else
for (row = 0; row < kp->pdata->num_rows; row++) {
#endif
int bits_changed = new_state[row] ^ old_state[row];
if (!bits_changed)
continue;
#ifdef CONFIG_KEYBOARD_ZTE
for (col = 0; col < KEYP_NUM_COLS; col++) {
#else
for (col = 0; col < kp->pdata->num_cols; col++) {
#endif
if (!(bits_changed & (1 << col)))
continue;
dev_dbg(kp->dev, "key [%d:%d] %s\n", row, col,
!(new_state[row] & (1 << col)) ?
"pressed" : "released");
code = MATRIX_SCAN_CODE(row, col, PM8058_ROW_SHIFT);
input_event(kp->input, EV_MSC, MSC_SCAN, code);
input_report_key(kp->input,
kp->keycodes[code],
!(new_state[row] & (1 << col)));
input_sync(kp->input);
}
}
return 0;
}
static int pmic8058_detect_ghost_keys(struct pmic8058_kp *kp, u16 *new_state)
{
int row, found_first = -1;
u16 check, row_state;
check = 0;
for (row = 0; row < kp->pdata->num_rows; row++) {
row_state = (~new_state[row]) &
((1 << kp->pdata->num_cols) - 1);
if (hweight16(row_state) > 1) {
if (found_first == -1)
found_first = row;
if (check & row_state) {
dev_dbg(kp->dev, "detected ghost key on row[%d]"
"row[%d]\n", found_first, row);
return 1;
}
}
check |= row_state;
}
return 0;
}
static int pmic8058_kp_scan_matrix(struct pmic8058_kp *kp, unsigned int events)
{
u16 new_state[PM8058_MAX_ROWS];
u16 old_state[PM8058_MAX_ROWS];
int rc;
switch (events) {
case 0x1:
rc = pmic8058_kp_read_matrix(kp, new_state, NULL);
if (pmic8058_detect_ghost_keys(kp, new_state))
return -EINVAL;
__pmic8058_kp_scan_matrix(kp, new_state, kp->keystate);
memcpy(kp->keystate, new_state, sizeof(new_state));
break;
case 0x3: /* two events - eventcounter is gray-coded */
rc = pmic8058_kp_read_matrix(kp, new_state, old_state);
__pmic8058_kp_scan_matrix(kp, old_state, kp->keystate);
__pmic8058_kp_scan_matrix(kp, new_state, old_state);
memcpy(kp->keystate, new_state, sizeof(new_state));
break;
case 0x2:
dev_dbg(kp->dev, "Some key events are missed\n");
rc = pmic8058_kp_read_matrix(kp, new_state, old_state);
__pmic8058_kp_scan_matrix(kp, old_state, kp->keystate);
__pmic8058_kp_scan_matrix(kp, new_state, old_state);
memcpy(kp->keystate, new_state, sizeof(new_state));
break;
default:
rc = -1;
}
return rc;
}
/*
* LDM/STM alignment handler.
*
* There are 4 variants of this instruction:
*
* B = rn pointer before instruction, A = rn pointer after instruction
* ------ increasing address ----->
* | | r0 | r1 | ... | rx | |
* PU = 01 B A
* PU = 11 B A
* PU = 00 A B
* PU = 10 A B
*/
static int
do_alignment_ldmstm(unsigned long addr, unsigned long instr, struct pt_regs *regs)
{
unsigned int rd, rn, pc_correction, reg_correction, nr_regs, regbits;
unsigned long eaddr, newaddr;
if (LDM_S_BIT(instr))
goto bad;
pc_correction = 4; /* processor implementation defined */
ai_multi += 1;
/* count the number of registers in the mask to be transferred */
nr_regs = hweight16(REGMASK_BITS(instr)) * 4;
rn = RN_BITS(instr);
newaddr = eaddr = regs->uregs[rn];
if (!LDST_U_BIT(instr))
nr_regs = -nr_regs;
newaddr += nr_regs;
if (!LDST_U_BIT(instr))
eaddr = newaddr;
if (LDST_P_EQ_U(instr)) /* U = P */
eaddr += 4;
/*
* This is a "hint" - we already have eaddr worked out by the
* processor for us.
*/
if (addr != eaddr) {
printk(KERN_ERR "LDMSTM: PC = %08lx, instr = %08lx, "
"addr = %08lx, eaddr = %08lx\n",
instruction_pointer(regs), instr, addr, eaddr);
show_regs(regs);
}
if (LDM_H_BIT(instr))
reg_correction = 0x10;
else
reg_correction = 0x00;
for (regbits = REGMASK_BITS(instr), rd = 0; regbits; regbits >>= 1, rd += 1)
if (regbits & 1) {
if (LDST_L_BIT(instr))
get32_unaligned_check(regs->uregs[rd + reg_correction], eaddr);
else
put32_unaligned_check(regs->uregs[rd + reg_correction], eaddr);
eaddr += 4;
}
if (LDST_W_BIT(instr))
regs->uregs[rn] = newaddr;
return TYPE_DONE;
fault:
regs->UCreg_pc -= pc_correction;
return TYPE_FAULT;
bad:
printk(KERN_ERR "Alignment trap: not handling ldm with s-bit set\n");
return TYPE_ERROR;
}
