linux/drivers/media/i2c/smiapp/smiapp-regs.c
<<
>>
Prefs
   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
  17static 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 */
  67static 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
 119err:
 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. */
 126static 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 */
 150static 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
 175int 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
 183static 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
 201int smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 202{
 203        return smiapp_read_quirk(sensor, reg, val, false);
 204}
 205
 206int smiapp_read_8only(struct smiapp_sensor *sensor, u32 reg, u32 *val)
 207{
 208        return smiapp_read_quirk(sensor, reg, val, true);
 209}
 210
 211int 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 */
 284int 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}
 296