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

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