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

   1 /*
   2  * drivers/media/video/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@maxwell.research.nokia.com>
   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  * You should have received a copy of the GNU General Public License
  19  * along with this program; if not, write to the Free Software
  20  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
  21  * 02110-1301 USA
  22  *
  23  */
  24
  25 #include "smiapp-debug.h"
  26
  27 #include <linux/delay.h>
  28 #include <linux/i2c.h>
  29
  30 #include "smiapp-regs.h"
  31
  32 static uint32_t float_to_u32_mul_1000000(struct i2c_client *client,
  33                                          uint32_t phloat)
  34 {
  35         int32_t exp;
  36         uint64_t man;
  37
  38         if (phloat >= 0x80000000) {
  39                 dev_err(&client->dev, "this is a negative number\n");
  40                 return 0;
  41         }
  42
  43         if (phloat == 0x7f800000)
  44                 return ~0; /* Inf. */
  45
  46         if ((phloat & 0x7f800000) == 0x7f800000) {
  47                 dev_err(&client->dev, "NaN or other special number\n");
  48                 return 0;
  49         }
  50
  51         /* Valid cases begin here */
  52         if (phloat == 0)
  53                 return 0; /* Valid zero */
  54
  55         if (phloat > 0x4f800000)
  56                 return ~0; /* larger than 4294967295 */
  57
  58         /*
  59          * Unbias exponent (note how phloat is now guaranteed to
  60          * have 0 in the high bit)
  61          */
  62         exp = ((int32_t)phloat >> 23) - 127;
  63
  64         /* Extract mantissa, add missing '1' bit and it's in MHz */
  65         man = ((phloat & 0x7fffff) | 0x800000) * 1000000ULL;
  66
  67         if (exp < 0)
  68                 man >>= -exp;
  69         else
  70                 man <<= exp;
  71
  72         man >>= 23; /* Remove mantissa bias */
  73
  74         return man & 0xffffffff;
  75 }
  76
  77
  78 /*
  79  * Read a 8/16/32-bit i2c register.  The value is returned in 'val'.
  80  * Returns zero if successful, or non-zero otherwise.
  81  */
  82 int smiapp_read(struct i2c_client *client, u32 reg, u32 *val)
  83 {
  84         struct i2c_msg msg;
  85         unsigned char data[4];
  86         unsigned int len = (u8)(reg >> 16);
  87         u16 offset = reg;
  88         int r;
  89
  90         if (len != SMIA_REG_8BIT && len != SMIA_REG_16BIT
  91             && len != SMIA_REG_32BIT)
  92                 return -EINVAL;
  93
  94         msg.addr = client->addr;
  95         msg.flags = 0;
  96         msg.len = 2;
  97         msg.buf = data;
  98
  99         /* high byte goes out first */
 100         data[0] = (u8) (offset >> 8);
 101         data[1] = (u8) offset;
 102         r = i2c_transfer(client->adapter, &msg, 1);
 103         if (r != 1) {
 104                 if (r >= 0)
 105                         r = -EBUSY;
 106                 goto err;
 107         }
 108
 109         msg.len = len;
 110         msg.flags = I2C_M_RD;
 111         r = i2c_transfer(client->adapter, &msg, 1);
 112         if (r != 1) {
 113                 if (r >= 0)
 114                         r = -EBUSY;
 115                 goto err;
 116         }
 117
 118         *val = 0;
 119         /* high byte comes first */
 120         switch (len) {
 121         case SMIA_REG_32BIT:
 122                 *val = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) +
 123                         data[3];
 124                 break;
 125         case SMIA_REG_16BIT:
 126                 *val = (data[0] << 8) + data[1];
 127                 break;
 128         case SMIA_REG_8BIT:
 129                 *val = data[0];
 130                 break;
 131         default:
 132                 BUG();
 133         }
 134
 135         if (reg & SMIA_REG_FLAG_FLOAT)
 136                 *val = float_to_u32_mul_1000000(client, *val);
 137
 138         return 0;
 139
 140 err:
 141         dev_err(&client->dev, "read from offset 0x%x error %d\n", offset, r);
 142
 143         return r;
 144 }
 145
 146 /*
 147  * Write to a 8/16-bit register.
 148  * Returns zero if successful, or non-zero otherwise.
 149  */
 150 int smiapp_write(struct i2c_client *client, u32 reg, u32 val)
 151 {
 152         struct i2c_msg msg;
 153         unsigned char data[6];
 154         unsigned int retries;
 155         unsigned int flags = reg >> 24;
 156         unsigned int len = (u8)(reg >> 16);
 157         u16 offset = reg;
 158         int r;
 159
 160         if ((len != SMIA_REG_8BIT && len != SMIA_REG_16BIT &&
 161              len != SMIA_REG_32BIT) || flags)
 162                 return -EINVAL;
 163
 164         msg.addr = client->addr;
 165         msg.flags = 0; /* Write */
 166         msg.len = 2 + len;
 167         msg.buf = data;
 168
 169         /* high byte goes out first */
 170         data[0] = (u8) (reg >> 8);
 171         data[1] = (u8) (reg & 0xff);
 172
 173         switch (len) {
 174         case SMIA_REG_8BIT:
 175                 data[2] = val;
 176                 break;
 177         case SMIA_REG_16BIT:
 178                 data[2] = val >> 8;
 179                 data[3] = val;
 180                 break;
 181         case SMIA_REG_32BIT:
 182                 data[2] = val >> 24;
 183                 data[3] = val >> 16;
 184                 data[4] = val >> 8;
 185                 data[5] = val;
 186                 break;
 187         default:
 188                 BUG();
 189         }
 190
 191         for (retries = 0; retries < 5; retries++) {
 192                 /*
 193                  * Due to unknown reason sensor stops responding. This
 194                  * loop is a temporaty solution until the root cause
 195                  * is found.
 196                  */
 197                 r = i2c_transfer(client->adapter, &msg, 1);
 198                 if (r == 1) {
 199                         if (retries)
 200                                 dev_err(&client->dev,
 201                                         "sensor i2c stall encountered. "
 202                                         "retries: %d\n", retries);
 203                         return 0;
 204                 }
 205
 206                 usleep_range(2000, 2000);
 207         }
 208
 209         dev_err(&client->dev,
 210                 "wrote 0x%x to offset 0x%x error %d\n", val, offset, r);
 211
 212         return r;
 213 }