cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

ccs-reg-access.c (8984B)


      1// SPDX-License-Identifier: GPL-2.0-only
      2/*
      3 * drivers/media/i2c/ccs/ccs-reg-access.c
      4 *
      5 * Generic driver for MIPI CCS/SMIA/SMIA++ compliant camera sensors
      6 *
      7 * Copyright (C) 2020 Intel Corporation
      8 * Copyright (C) 2011--2012 Nokia Corporation
      9 * Contact: Sakari Ailus <sakari.ailus@linux.intel.com>
     10 */
     11
     12#include <asm/unaligned.h>
     13
     14#include <linux/delay.h>
     15#include <linux/i2c.h>
     16
     17#include "ccs.h"
     18#include "ccs-limits.h"
     19
     20static u32 float_to_u32_mul_1000000(struct i2c_client *client, u32 phloat)
     21{
     22	s32 exp;
     23	u64 man;
     24
     25	if (phloat >= 0x80000000) {
     26		dev_err(&client->dev, "this is a negative number\n");
     27		return 0;
     28	}
     29
     30	if (phloat == 0x7f800000)
     31		return ~0; /* Inf. */
     32
     33	if ((phloat & 0x7f800000) == 0x7f800000) {
     34		dev_err(&client->dev, "NaN or other special number\n");
     35		return 0;
     36	}
     37
     38	/* Valid cases begin here */
     39	if (phloat == 0)
     40		return 0; /* Valid zero */
     41
     42	if (phloat > 0x4f800000)
     43		return ~0; /* larger than 4294967295 */
     44
     45	/*
     46	 * Unbias exponent (note how phloat is now guaranteed to
     47	 * have 0 in the high bit)
     48	 */
     49	exp = ((int32_t)phloat >> 23) - 127;
     50
     51	/* Extract mantissa, add missing '1' bit and it's in MHz */
     52	man = ((phloat & 0x7fffff) | 0x800000) * 1000000ULL;
     53
     54	if (exp < 0)
     55		man >>= -exp;
     56	else
     57		man <<= exp;
     58
     59	man >>= 23; /* Remove mantissa bias */
     60
     61	return man & 0xffffffff;
     62}
     63
     64
     65/*
     66 * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
     67 * Returns zero if successful, or non-zero otherwise.
     68 */
     69static int ____ccs_read_addr(struct ccs_sensor *sensor, u16 reg, u16 len,
     70			     u32 *val)
     71{
     72	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
     73	struct i2c_msg msg;
     74	unsigned char data_buf[sizeof(u32)] = { 0 };
     75	unsigned char offset_buf[sizeof(u16)];
     76	int r;
     77
     78	if (len > sizeof(data_buf))
     79		return -EINVAL;
     80
     81	msg.addr = client->addr;
     82	msg.flags = 0;
     83	msg.len = sizeof(offset_buf);
     84	msg.buf = offset_buf;
     85	put_unaligned_be16(reg, offset_buf);
     86
     87	r = i2c_transfer(client->adapter, &msg, 1);
     88	if (r != 1) {
     89		if (r >= 0)
     90			r = -EBUSY;
     91		goto err;
     92	}
     93
     94	msg.len = len;
     95	msg.flags = I2C_M_RD;
     96	msg.buf = &data_buf[sizeof(data_buf) - len];
     97
     98	r = i2c_transfer(client->adapter, &msg, 1);
     99	if (r != 1) {
    100		if (r >= 0)
    101			r = -EBUSY;
    102		goto err;
    103	}
    104
    105	*val = get_unaligned_be32(data_buf);
    106
    107	return 0;
    108
    109err:
    110	dev_err(&client->dev, "read from offset 0x%x error %d\n", reg, r);
    111
    112	return r;
    113}
    114
    115/* Read a register using 8-bit access only. */
    116static int ____ccs_read_addr_8only(struct ccs_sensor *sensor, u16 reg,
    117				   u16 len, u32 *val)
    118{
    119	unsigned int i;
    120	int rval;
    121
    122	*val = 0;
    123
    124	for (i = 0; i < len; i++) {
    125		u32 val8;
    126
    127		rval = ____ccs_read_addr(sensor, reg + i, 1, &val8);
    128		if (rval < 0)
    129			return rval;
    130		*val |= val8 << ((len - i - 1) << 3);
    131	}
    132
    133	return 0;
    134}
    135
    136unsigned int ccs_reg_width(u32 reg)
    137{
    138	if (reg & CCS_FL_16BIT)
    139		return sizeof(u16);
    140	if (reg & CCS_FL_32BIT)
    141		return sizeof(u32);
    142
    143	return sizeof(u8);
    144}
    145
    146static u32 ireal32_to_u32_mul_1000000(struct i2c_client *client, u32 val)
    147{
    148	if (val >> 10 > U32_MAX / 15625) {
    149		dev_warn(&client->dev, "value %u overflows!\n", val);
    150		return U32_MAX;
    151	}
    152
    153	return ((val >> 10) * 15625) +
    154		(val & GENMASK(9, 0)) * 15625 / 1024;
    155}
    156
    157u32 ccs_reg_conv(struct ccs_sensor *sensor, u32 reg, u32 val)
    158{
    159	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
    160
    161	if (reg & CCS_FL_FLOAT_IREAL) {
    162		if (CCS_LIM(sensor, CLOCK_CAPA_TYPE_CAPABILITY) &
    163		    CCS_CLOCK_CAPA_TYPE_CAPABILITY_IREAL)
    164			val = ireal32_to_u32_mul_1000000(client, val);
    165		else
    166			val = float_to_u32_mul_1000000(client, val);
    167	} else if (reg & CCS_FL_IREAL) {
    168		val = ireal32_to_u32_mul_1000000(client, val);
    169	}
    170
    171	return val;
    172}
    173
    174/*
    175 * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
    176 * Returns zero if successful, or non-zero otherwise.
    177 */
    178static int __ccs_read_addr(struct ccs_sensor *sensor, u32 reg, u32 *val,
    179			   bool only8, bool conv)
    180{
    181	unsigned int len = ccs_reg_width(reg);
    182	int rval;
    183
    184	if (!only8)
    185		rval = ____ccs_read_addr(sensor, CCS_REG_ADDR(reg), len, val);
    186	else
    187		rval = ____ccs_read_addr_8only(sensor, CCS_REG_ADDR(reg), len,
    188					       val);
    189	if (rval < 0)
    190		return rval;
    191
    192	if (!conv)
    193		return 0;
    194
    195	*val = ccs_reg_conv(sensor, reg, *val);
    196
    197	return 0;
    198}
    199
    200static int __ccs_read_data(struct ccs_reg *regs, size_t num_regs,
    201			   u32 reg, u32 *val)
    202{
    203	unsigned int width = ccs_reg_width(reg);
    204	size_t i;
    205
    206	for (i = 0; i < num_regs; i++, regs++) {
    207		u8 *data;
    208
    209		if (regs->addr + regs->len < CCS_REG_ADDR(reg) + width)
    210			continue;
    211
    212		if (regs->addr > CCS_REG_ADDR(reg))
    213			break;
    214
    215		data = &regs->value[CCS_REG_ADDR(reg) - regs->addr];
    216
    217		switch (width) {
    218		case sizeof(u8):
    219			*val = *data;
    220			break;
    221		case sizeof(u16):
    222			*val = get_unaligned_be16(data);
    223			break;
    224		case sizeof(u32):
    225			*val = get_unaligned_be32(data);
    226			break;
    227		default:
    228			WARN_ON(1);
    229			return -EINVAL;
    230		}
    231
    232		return 0;
    233	}
    234
    235	return -ENOENT;
    236}
    237
    238static int ccs_read_data(struct ccs_sensor *sensor, u32 reg, u32 *val)
    239{
    240	if (!__ccs_read_data(sensor->sdata.sensor_read_only_regs,
    241			     sensor->sdata.num_sensor_read_only_regs,
    242			     reg, val))
    243		return 0;
    244
    245	return __ccs_read_data(sensor->mdata.module_read_only_regs,
    246			       sensor->mdata.num_module_read_only_regs,
    247			       reg, val);
    248}
    249
    250static int ccs_read_addr_raw(struct ccs_sensor *sensor, u32 reg, u32 *val,
    251			     bool force8, bool quirk, bool conv, bool data)
    252{
    253	int rval;
    254
    255	if (data) {
    256		rval = ccs_read_data(sensor, reg, val);
    257		if (!rval)
    258			return 0;
    259	}
    260
    261	if (quirk) {
    262		*val = 0;
    263		rval = ccs_call_quirk(sensor, reg_access, false, &reg, val);
    264		if (rval == -ENOIOCTLCMD)
    265			return 0;
    266		if (rval < 0)
    267			return rval;
    268
    269		if (force8)
    270			return __ccs_read_addr(sensor, reg, val, true, conv);
    271	}
    272
    273	return __ccs_read_addr(sensor, reg, val,
    274			       ccs_needs_quirk(sensor,
    275					       CCS_QUIRK_FLAG_8BIT_READ_ONLY),
    276			       conv);
    277}
    278
    279int ccs_read_addr(struct ccs_sensor *sensor, u32 reg, u32 *val)
    280{
    281	return ccs_read_addr_raw(sensor, reg, val, false, true, true, true);
    282}
    283
    284int ccs_read_addr_8only(struct ccs_sensor *sensor, u32 reg, u32 *val)
    285{
    286	return ccs_read_addr_raw(sensor, reg, val, true, true, true, true);
    287}
    288
    289int ccs_read_addr_noconv(struct ccs_sensor *sensor, u32 reg, u32 *val)
    290{
    291	return ccs_read_addr_raw(sensor, reg, val, false, true, false, true);
    292}
    293
    294static int ccs_write_retry(struct i2c_client *client, struct i2c_msg *msg)
    295{
    296	unsigned int retries;
    297	int r;
    298
    299	for (retries = 0; retries < 10; retries++) {
    300		/*
    301		 * Due to unknown reason sensor stops responding. This
    302		 * loop is a temporaty solution until the root cause
    303		 * is found.
    304		 */
    305		r = i2c_transfer(client->adapter, msg, 1);
    306		if (r != 1) {
    307			usleep_range(1000, 2000);
    308			continue;
    309		}
    310
    311		if (retries)
    312			dev_err(&client->dev,
    313				"sensor i2c stall encountered. retries: %d\n",
    314				retries);
    315		return 0;
    316	}
    317
    318	return r;
    319}
    320
    321int ccs_write_addr_no_quirk(struct ccs_sensor *sensor, u32 reg, u32 val)
    322{
    323	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
    324	struct i2c_msg msg;
    325	unsigned char data[6];
    326	unsigned int len = ccs_reg_width(reg);
    327	int r;
    328
    329	if (len > sizeof(data) - 2)
    330		return -EINVAL;
    331
    332	msg.addr = client->addr;
    333	msg.flags = 0; /* Write */
    334	msg.len = 2 + len;
    335	msg.buf = data;
    336
    337	put_unaligned_be16(CCS_REG_ADDR(reg), data);
    338	put_unaligned_be32(val << (8 * (sizeof(val) - len)), data + 2);
    339
    340	dev_dbg(&client->dev, "writing reg 0x%4.4x value 0x%*.*x (%u)\n",
    341		CCS_REG_ADDR(reg), ccs_reg_width(reg) << 1,
    342		ccs_reg_width(reg) << 1, val, val);
    343
    344	r = ccs_write_retry(client, &msg);
    345	if (r)
    346		dev_err(&client->dev,
    347			"wrote 0x%x to offset 0x%x error %d\n", val,
    348			CCS_REG_ADDR(reg), r);
    349
    350	return r;
    351}
    352
    353/*
    354 * Write to a 8/16-bit register.
    355 * Returns zero if successful, or non-zero otherwise.
    356 */
    357int ccs_write_addr(struct ccs_sensor *sensor, u32 reg, u32 val)
    358{
    359	int rval;
    360
    361	rval = ccs_call_quirk(sensor, reg_access, true, &reg, &val);
    362	if (rval == -ENOIOCTLCMD)
    363		return 0;
    364	if (rval < 0)
    365		return rval;
    366
    367	return ccs_write_addr_no_quirk(sensor, reg, val);
    368}
    369
    370#define MAX_WRITE_LEN	32U
    371
    372int ccs_write_data_regs(struct ccs_sensor *sensor, struct ccs_reg *regs,
    373			size_t num_regs)
    374{
    375	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
    376	unsigned char buf[2 + MAX_WRITE_LEN];
    377	struct i2c_msg msg = {
    378		.addr = client->addr,
    379		.buf = buf,
    380	};
    381	size_t i;
    382
    383	for (i = 0; i < num_regs; i++, regs++) {
    384		unsigned char *regdata = regs->value;
    385		unsigned int j;
    386
    387		for (j = 0; j < regs->len;
    388		     j += msg.len - 2, regdata += msg.len - 2) {
    389			char printbuf[(MAX_WRITE_LEN << 1) +
    390				      1 /* \0 */] = { 0 };
    391			int rval;
    392
    393			msg.len = min(regs->len - j, MAX_WRITE_LEN);
    394
    395			bin2hex(printbuf, regdata, msg.len);
    396			dev_dbg(&client->dev,
    397				"writing msr reg 0x%4.4x value 0x%s\n",
    398				regs->addr + j, printbuf);
    399
    400			put_unaligned_be16(regs->addr + j, buf);
    401			memcpy(buf + 2, regdata, msg.len);
    402
    403			msg.len += 2;
    404
    405			rval = ccs_write_retry(client, &msg);
    406			if (rval) {
    407				dev_err(&client->dev,
    408					"error writing %u octets to address 0x%4.4x\n",
    409					msg.len, regs->addr + j);
    410				return rval;
    411			}
    412		}
    413	}
    414
    415	return 0;
    416}