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

DEFINITIONS

1. float_to_u32_mul_1000000
2. ____smiapp_read
3. ____smiapp_read_8only
4. __smiapp_read
5. smiapp_read_no_quirk
6. smiapp_read_quirk
7. smiapp_read
8. smiapp_read_8only
9. smiapp_write_no_quirk
10. smiapp_write

   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 <linux/delay.h>
  12 #include <linux/i2c.h>
  13 
  14 #include "smiapp.h"
  15 #include "smiapp-regs.h"
  16 
  17 static uint32_t float_to_u32_mul_1000000(struct i2c_client *client,
  18                                          uint32_t phloat)
  19 {
  20         int32_t exp;
  21         uint64_t man;
  22 
  23         if (phloat >= 0x80000000) {
  24                 dev_err(&client->dev, "this is a negative number\n");
  25                 return 0;
  26         }
  27 
  28         if (phloat == 0x7f800000)
  29                 return ~0; /* Inf. */
  30 
  31         if ((phloat & 0x7f800000) == 0x7f800000) {
  32                 dev_err(&client->dev, "NaN or other special number\n");
  33                 return 0;
  34         }
  35 
  36         /* Valid cases begin here */
  37         if (phloat == 0)
  38                 return 0; /* Valid zero */
  39 
  40         if (phloat > 0x4f800000)
  41                 return ~0; /* larger than 4294967295 */
  42 
  43         /*
  44          * Unbias exponent (note how phloat is now guaranteed to
  45          * have 0 in the high bit)
  46          */
  47         exp = ((int32_t)phloat >> 23) - 127;
  48 
  49         /* Extract mantissa, add missing '1' bit and it's in MHz */
  50         man = ((phloat & 0x7fffff) | 0x800000) * 1000000ULL;
  51 
  52         if (exp < 0)
  53                 man >>= -exp;
  54         else
  55                 man <<= exp;
  56 
  57         man >>= 23; /* Remove mantissa bias */
  58 
  59         return man & 0xffffffff;
  60 }
  61 
  62 
  63 /*
  64  * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
  65  * Returns zero if successful, or non-zero otherwise.
  66  */
  67 static int ____smiapp_read(struct smiapp_sensor *sensor, u16 reg,
  68                            u16 len, u32 *val)
  69 {
  70         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
  71         struct i2c_msg msg;
  72         unsigned char data[4];
  73         u16 offset = reg;
  74         int r;
  75 
  76         msg.addr = client->addr;
  77         msg.flags = 0;
  78         msg.len = 2;
  79         msg.buf = data;
  80 
  81         /* high byte goes out first */
  82         data[0] = (u8) (offset >> 8);
  83         data[1] = (u8) offset;
  84         r = i2c_transfer(client->adapter, &msg, 1);
  85         if (r != 1) {
  86                 if (r >= 0)
  87                         r = -EBUSY;
  88                 goto err;
  89         }
  90 
  91         msg.len = len;
  92         msg.flags = I2C_M_RD;
  93         r = i2c_transfer(client->adapter, &msg, 1);
  94         if (r != 1) {
  95                 if (r >= 0)
  96                         r = -EBUSY;
  97                 goto err;
  98         }
  99 
 100         *val = 0;
 101         /* high byte comes first */
 102         switch (len) {
 103         case SMIAPP_REG_32BIT:
 104                 *val = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) +
 105                         data[3];
 106                 break;
 107         case SMIAPP_REG_16BIT:
 108                 *val = (data[0] << 8) + data[1];
 109                 break;
 110         case SMIAPP_REG_8BIT:
 111                 *val = data[0];
 112                 break;
 113         default:
 114                 BUG();
 115         }
 116 
 117         return 0;
 118 
 119 err:
 120         dev_err(&client->dev, "read from offset 0x%x error %d\n", offset, r);
 121 
 122         return r;
 123 }
 124 
 125 /* Read a register using 8-bit access only. */
 126 static int ____smiapp_read_8only(struct smiapp_sensor *sensor, u16 reg,
 127                                  u16 len, u32 *val)
 128 {
 129         unsigned int i;
 130         int rval;
 131 
 132         *val = 0;
 133 
 134         for (i = 0; i < len; i++) {
 135                 u32 val8;
 136 
 137                 rval = ____smiapp_read(sensor, reg + i, 1, &val8);
 138                 if (rval < 0)
 139                         return rval;
 140                 *val |= val8 << ((len - i - 1) << 3);
 141         }
 142 
 143         return 0;
 144 }
 145 
 146 /*
 147  * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
 148  * Returns zero if successful, or non-zero otherwise.
 149  */
 150 static int __smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val,
 151                          bool only8)
 152 {
 153         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 154         u8 len = SMIAPP_REG_WIDTH(reg);
 155         int rval;
 156 
 157         if (len != SMIAPP_REG_8BIT && len != SMIAPP_REG_16BIT
 158             && len != SMIAPP_REG_32BIT)
 159                 return -EINVAL;
 160 
 161         if (len == SMIAPP_REG_8BIT || !only8)
 162                 rval = ____smiapp_read(sensor, SMIAPP_REG_ADDR(reg), len, val);
 163         else
 164                 rval = ____smiapp_read_8only(sensor, SMIAPP_REG_ADDR(reg), len,
 165                                              val);
 166         if (rval < 0)
 167                 return rval;
 168 
 169         if (reg & SMIAPP_REG_FLAG_FLOAT)
 170                 *val = float_to_u32_mul_1000000(client, *val);
 171 
 172         return 0;
 173 }
 174 
 175 int smiapp_read_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 176 {
 177         return __smiapp_read(
 178                 sensor, reg, val,
 179                 smiapp_needs_quirk(sensor,
 180                                    SMIAPP_QUIRK_FLAG_8BIT_READ_ONLY));
 181 }
 182 
 183 static int smiapp_read_quirk(struct smiapp_sensor *sensor, u32 reg, u32 *val,
 184                              bool force8)
 185 {
 186         int rval;
 187 
 188         *val = 0;
 189         rval = smiapp_call_quirk(sensor, reg_access, false, &reg, val);
 190         if (rval == -ENOIOCTLCMD)
 191                 return 0;
 192         if (rval < 0)
 193                 return rval;
 194 
 195         if (force8)
 196                 return __smiapp_read(sensor, reg, val, true);
 197 
 198         return smiapp_read_no_quirk(sensor, reg, val);
 199 }
 200 
 201 int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 202 {
 203         return smiapp_read_quirk(sensor, reg, val, false);
 204 }
 205 
 206 int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 207 {
 208         return smiapp_read_quirk(sensor, reg, val, true);
 209 }
 210 
 211 int smiapp_write_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 val)
 212 {
 213         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 214         struct i2c_msg msg;
 215         unsigned char data[6];
 216         unsigned int retries;
 217         u8 flags = SMIAPP_REG_FLAGS(reg);
 218         u8 len = SMIAPP_REG_WIDTH(reg);
 219         u16 offset = SMIAPP_REG_ADDR(reg);
 220         int r;
 221 
 222         if ((len != SMIAPP_REG_8BIT && len != SMIAPP_REG_16BIT &&
 223              len != SMIAPP_REG_32BIT) || flags)
 224                 return -EINVAL;
 225 
 226         if (!sensor->active)
 227                 return 0;
 228 
 229         msg.addr = client->addr;
 230         msg.flags = 0; /* Write */
 231         msg.len = 2 + len;
 232         msg.buf = data;
 233 
 234         /* high byte goes out first */
 235         data[0] = (u8) (reg >> 8);
 236         data[1] = (u8) (reg & 0xff);
 237 
 238         switch (len) {
 239         case SMIAPP_REG_8BIT:
 240                 data[2] = val;
 241                 break;
 242         case SMIAPP_REG_16BIT:
 243                 data[2] = val >> 8;
 244                 data[3] = val;
 245                 break;
 246         case SMIAPP_REG_32BIT:
 247                 data[2] = val >> 24;
 248                 data[3] = val >> 16;
 249                 data[4] = val >> 8;
 250                 data[5] = val;
 251                 break;
 252         default:
 253                 BUG();
 254         }
 255 
 256         for (retries = 0; retries < 5; retries++) {
 257                 /*
 258                  * Due to unknown reason sensor stops responding. This
 259                  * loop is a temporaty solution until the root cause
 260                  * is found.
 261                  */
 262                 r = i2c_transfer(client->adapter, &msg, 1);
 263                 if (r == 1) {
 264                         if (retries)
 265                                 dev_err(&client->dev,
 266                                         "sensor i2c stall encountered. retries: %d\n",
 267                                         retries);
 268                         return 0;
 269                 }
 270 
 271                 usleep_range(2000, 2000);
 272         }
 273 
 274         dev_err(&client->dev,
 275                 "wrote 0x%x to offset 0x%x error %d\n", val, offset, r);
 276 
 277         return r;
 278 }
 279 
 280 /*
 281  * Write to a 8/16-bit register.
 282  * Returns zero if successful, or non-zero otherwise.
 283  */
 284 int smiapp_write(struct smiapp_sensor *sensor, u32 reg, u32 val)
 285 {
 286         int rval;
 287 
 288         rval = smiapp_call_quirk(sensor, reg_access, true, &reg, &val);
 289         if (rval == -ENOIOCTLCMD)
 290                 return 0;
 291         if (rval < 0)
 292                 return rval;
 293 
 294         return smiapp_write_no_quirk(sensor, reg, val);
 295 }