drivers/media/i2c/smiapp/smiapp-regs.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * drivers/media/i2c/smiapp/smiapp-regs.c
   4  *
   5  * Generic driver for SMIA/SMIA++ compliant camera modules
   6  *
   7  * Copyright (C) 2011--2012 Nokia Corporation
   8  * Contact: Sakari Ailus <sakari.ailus@iki.fi>
   9  */
  10
  11 #include <asm/unaligned.h>
  12
  13 #include <linux/delay.h>
  14 #include <linux/i2c.h>
  15
  16 #include "smiapp.h"
  17 #include "smiapp-regs.h"
  18
  19 static uint32_t float_to_u32_mul_1000000(struct i2c_client *client,
  20                                          uint32_t phloat)
  21 {
  22         int32_t exp;
  23         uint64_t 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  */
  69 static int ____smiapp_read(struct smiapp_sensor *sensor, u16 reg,
  70                            u16 len, 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
 109 err:
 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. */
 116 static int ____smiapp_read_8only(struct smiapp_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 = ____smiapp_read(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
 136 /*
 137  * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
 138  * Returns zero if successful, or non-zero otherwise.
 139  */
 140 static int __smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val,
 141                          bool only8)
 142 {
 143         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 144         u8 len = SMIAPP_REG_WIDTH(reg);
 145         int rval;
 146
 147         if (len != SMIAPP_REG_8BIT && len != SMIAPP_REG_16BIT
 148             && len != SMIAPP_REG_32BIT)
 149                 return -EINVAL;
 150
 151         if (!only8)
 152                 rval = ____smiapp_read(sensor, SMIAPP_REG_ADDR(reg), len, val);
 153         else
 154                 rval = ____smiapp_read_8only(sensor, SMIAPP_REG_ADDR(reg), len,
 155                                              val);
 156         if (rval < 0)
 157                 return rval;
 158
 159         if (reg & SMIAPP_REG_FLAG_FLOAT)
 160                 *val = float_to_u32_mul_1000000(client, *val);
 161
 162         return 0;
 163 }
 164
 165 int smiapp_read_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 166 {
 167         return __smiapp_read(
 168                 sensor, reg, val,
 169                 smiapp_needs_quirk(sensor,
 170                                    SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY));
 171 }
 172
 173 static int smiapp_read_quirk(struct smiapp_sensor *sensor, u32 reg, u32 *val,
 174                              bool force8)
 175 {
 176         int rval;
 177
 178         *val = 0;
 179         rval = smiapp_call_quirk(sensor, reg_access, false, &reg, val);
 180         if (rval == -ENOIOCTLCMD)
 181                 return 0;
 182         if (rval < 0)
 183                 return rval;
 184
 185         if (force8)
 186                 return __smiapp_read(sensor, reg, val, true);
 187
 188         return smiapp_read_no_quirk(sensor, reg, val);
 189 }
 190
 191 int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 192 {
 193         return smiapp_read_quirk(sensor, reg, val, false);
 194 }
 195
 196 int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 197 {
 198         return smiapp_read_quirk(sensor, reg, val, true);
 199 }
 200
 201 int smiapp_write_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 val)
 202 {
 203         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 204         struct i2c_msg msg;
 205         unsigned char data[6];
 206         unsigned int retries;
 207         u8 len = SMIAPP_REG_WIDTH(reg);
 208         int r;
 209
 210         if (len > sizeof(data) - 2)
 211                 return -EINVAL;
 212
 213         msg.addr = client->addr;
 214         msg.flags = 0; /* Write */
 215         msg.len = 2 + len;
 216         msg.buf = data;
 217
 218         put_unaligned_be16(SMIAPP_REG_ADDR(reg), data);
 219         put_unaligned_be32(val << (8 * (sizeof(val) - len)), data + 2);
 220
 221         for (retries = 0; retries < 5; retries++) {
 222                 /*
 223                  * Due to unknown reason sensor stops responding. This
 224                  * loop is a temporaty solution until the root cause
 225                  * is found.
 226                  */
 227                 r = i2c_transfer(client->adapter, &msg, 1);
 228                 if (r == 1) {
 229                         if (retries)
 230                                 dev_err(&client->dev,
 231                                         "sensor i2c stall encountered. retries: %d\n",
 232                                         retries);
 233                         return 0;
 234                 }
 235
 236                 usleep_range(2000, 2000);
 237         }
 238
 239         dev_err(&client->dev,
 240                 "wrote 0x%x to offset 0x%x error %d\n", val,
 241                 SMIAPP_REG_ADDR(reg), r);
 242
 243         return r;
 244 }
 245
 246 /*
 247  * Write to a 8/16-bit register.
 248  * Returns zero if successful, or non-zero otherwise.
 249  */
 250 int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val)
 251 {
 252         int rval;
 253
 254         rval = smiapp_call_quirk(sensor, reg_access, true, &reg, &val);
 255         if (rval == -ENOIOCTLCMD)
 256                 return 0;
 257         if (rval < 0)
 258                 return rval;
 259
 260         return smiapp_write_no_quirk(sensor, reg, val);
 261 }