drivers/misc/inv_mpu/mlsl-kernel.c

   1 /*
   2         $License:
   3         Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
   4
   5         This program is free software; you can redistribute it and/or modify
   6         it under the terms of the GNU General Public License as published by
   7         the Free Software Foundation; either version 2 of the License, or
   8         (at your option) any later version.
   9
  10         This program is distributed in the hope that it will be useful,
  11         but WITHOUT ANY WARRANTY; without even the implied warranty of
  12         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13         GNU General Public License for more details.
  14
  15         You should have received a copy of the GNU General Public License
  16         along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17         $
  18  */
  19
  20 #include "mlsl.h"
  21 #include <linux/i2c.h>
  22 #include "log.h"
  23 #include "mpu6050b1.h"
  24
  25 #define MPU_I2C_RATE 100*1000
  26 static int inv_i2c_write(struct i2c_adapter *i2c_adap,
  27                             unsigned char address,
  28                             unsigned int len, unsigned char const *data)
  29 {
  30         struct i2c_msg msgs[1];
  31         int res;
  32
  33         if (!data || !i2c_adap) {
  34                 LOG_RESULT_LOCATION(-EINVAL);
  35                 return -EINVAL;
  36         }
  37
  38         msgs[0].addr = address;
  39         msgs[0].flags = 0;      /* write */
  40         msgs[0].buf = (unsigned char *)data;
  41         msgs[0].len = len;
  42         msgs[0].scl_rate = MPU_I2C_RATE;
  43
  44         res = i2c_transfer(i2c_adap, msgs, 1);
  45         if (res < 1) {
  46                 if (res == 0)
  47                         res = -EIO;
  48                 LOG_RESULT_LOCATION(res);
  49                 return res;
  50         } else
  51                 return 0;
  52 }
  53
  54 static int inv_i2c_write_register(struct i2c_adapter *i2c_adap,
  55                                      unsigned char address,
  56                                      unsigned char reg, unsigned char value)
  57 {
  58         unsigned char data[2];
  59
  60         data[0] = reg;
  61         data[1] = value;
  62         return inv_i2c_write(i2c_adap, address, 2, data);
  63 }
  64
  65 static int inv_i2c_read(struct i2c_adapter *i2c_adap,
  66                            unsigned char address, unsigned char reg,
  67                            unsigned int len, unsigned char *data)
  68 {
  69         struct i2c_msg msgs[2];
  70         int res;
  71
  72         if (!data || !i2c_adap) {
  73                 LOG_RESULT_LOCATION(-EINVAL);
  74                 return -EINVAL;
  75         }
  76
  77         msgs[0].addr = address;
  78         msgs[0].flags = 0;      /* write */
  79         msgs[0].buf = &reg;
  80         msgs[0].len = 1;
  81         msgs[0].scl_rate = MPU_I2C_RATE;
  82
  83         msgs[1].addr = address;
  84         msgs[1].flags = I2C_M_RD;
  85         msgs[1].buf = data;
  86         msgs[1].len = len;
  87         msgs[1].scl_rate = MPU_I2C_RATE;
  88
  89         res = i2c_transfer(i2c_adap, msgs, 2);
  90         if (res < 2) {
  91                 if (res >= 0)
  92                         res = -EIO;
  93                 LOG_RESULT_LOCATION(res);
  94                 return res;
  95         } else
  96                 return 0;
  97 }
  98
  99 static int mpu_memory_read(struct i2c_adapter *i2c_adap,
 100                            unsigned char mpu_addr,
 101                            unsigned short mem_addr,
 102                            unsigned int len, unsigned char *data)
 103 {
 104         unsigned char bank[2];
 105         unsigned char addr[2];
 106         unsigned char buf;
 107         struct i2c_msg msgs[2];
 108         int res;
 109
 110         if (!data || !i2c_adap) {
 111                 LOG_RESULT_LOCATION(-EINVAL);
 112                 return -EINVAL;
 113         }
 114
 115         bank[0] = MPUREG_BANK_SEL;
 116         bank[1] = mem_addr >> 8;
 117
 118         addr[0] = MPUREG_MEM_START_ADDR;
 119         addr[1] = mem_addr & 0xFF;
 120
 121         buf = MPUREG_MEM_R_W;
 122
 123         /* write message */
 124         msgs[0].addr = mpu_addr;
 125         msgs[0].flags = 0;
 126         msgs[0].buf = bank;
 127         msgs[0].len = sizeof(bank);
 128         msgs[0].scl_rate = MPU_I2C_RATE;
 129
 130         res = i2c_transfer(i2c_adap, msgs, 1);
 131         if (res != 1)
 132                 return res;
 133
 134         msgs[0].addr = mpu_addr;
 135         msgs[0].flags = 0;
 136         msgs[0].buf = addr;
 137         msgs[0].len = sizeof(addr);
 138         msgs[0].scl_rate = MPU_I2C_RATE;
 139
 140         res = i2c_transfer(i2c_adap, msgs, 1);
 141         if (res != 1)
 142                 return res;
 143
 144         msgs[0].addr = mpu_addr;
 145         msgs[0].flags = 0;
 146         msgs[0].buf = &buf;
 147         msgs[0].len = 1;
 148         msgs[0].scl_rate = MPU_I2C_RATE;
 149
 150         msgs[1].addr = mpu_addr;
 151         msgs[1].flags = I2C_M_RD;
 152         msgs[1].buf = data;
 153         msgs[1].len = len;
 154         msgs[1].scl_rate = MPU_I2C_RATE;
 155
 156         res = i2c_transfer(i2c_adap, msgs, 2);
 157         if (res == 2)
 158                 return 0;
 159         else if(res == 0)
 160                 return -EBUSY;
 161         else
 162                 return res;
 163 }
 164
 165 static int mpu_memory_write(struct i2c_adapter *i2c_adap,
 166                             unsigned char mpu_addr,
 167                             unsigned short mem_addr,
 168                             unsigned int len, unsigned char const *data)
 169 {
 170         unsigned char bank[2];
 171         unsigned char addr[2];
 172         unsigned char buf[513];
 173
 174         struct i2c_msg msgs[2];
 175         int res;
 176
 177         if (!data || !i2c_adap) {
 178                 LOG_RESULT_LOCATION(-EINVAL);
 179                 return -EINVAL;
 180         }
 181         if (len >= (sizeof(buf) - 1)) {
 182                 LOG_RESULT_LOCATION(-ENOMEM);
 183                 return -ENOMEM;
 184         }
 185
 186         bank[0] = MPUREG_BANK_SEL;
 187         bank[1] = mem_addr >> 8;
 188
 189         addr[0] = MPUREG_MEM_START_ADDR;
 190         addr[1] = mem_addr & 0xFF;
 191
 192         buf[0] = MPUREG_MEM_R_W;
 193         memcpy(buf + 1, data, len);
 194
 195         /* write message */
 196         msgs[0].addr = mpu_addr;
 197         msgs[0].flags = 0;
 198         msgs[0].buf = bank;
 199         msgs[0].len = sizeof(bank);
 200         msgs[0].scl_rate = MPU_I2C_RATE;
 201
 202         res = i2c_transfer(i2c_adap, msgs, 1);
 203         if (res != 1)
 204                 return res;
 205
 206         msgs[0].addr = mpu_addr;
 207         msgs[0].flags = 0;
 208         msgs[0].buf = addr;
 209         msgs[0].scl_rate = MPU_I2C_RATE;
 210
 211         res = i2c_transfer(i2c_adap, msgs, 1);
 212         if (res != 1)
 213                 return res;
 214
 215         msgs[0].addr = mpu_addr;
 216         msgs[0].flags = 0;
 217         msgs[0].buf = (unsigned char *)buf;
 218         msgs[0].len = len + 1;
 219         msgs[0].scl_rate = MPU_I2C_RATE;
 220
 221         res = i2c_transfer(i2c_adap, msgs, 1);
 222         if (res == 1)
 223                 return 0;
 224         else if(res == 0)
 225                 return -EBUSY;
 226         else
 227                 return res;
 228
 229 }
 230
 231 int inv_serial_single_write(
 232         void *sl_handle,
 233         unsigned char slave_addr,
 234         unsigned char register_addr,
 235         unsigned char data)
 236 {
 237         return inv_i2c_write_register((struct i2c_adapter *)sl_handle,
 238                                       slave_addr, register_addr, data);
 239 }
 240 EXPORT_SYMBOL(inv_serial_single_write);
 241
 242 int inv_serial_write(
 243         void *sl_handle,
 244         unsigned char slave_addr,
 245         unsigned short length,
 246         unsigned char const *data)
 247 {
 248         int result;
 249         const unsigned short data_length = length - 1;
 250         const unsigned char start_reg_addr = data[0];
 251         unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
 252         unsigned short bytes_written = 0;
 253
 254         while (bytes_written < data_length) {
 255                 unsigned short this_len = min(SERIAL_MAX_TRANSFER_SIZE,
 256                                              data_length - bytes_written);
 257                 if (bytes_written == 0) {
 258                         result = inv_i2c_write((struct i2c_adapter *)
 259                                                sl_handle, slave_addr,
 260                                                1 + this_len, data);
 261                 } else {
 262                         /* manually increment register addr between chunks */
 263                         i2c_write[0] = start_reg_addr + bytes_written;
 264                         memcpy(&i2c_write[1], &data[1 + bytes_written],
 265                                 this_len);
 266                         result = inv_i2c_write((struct i2c_adapter *)
 267                                                sl_handle, slave_addr,
 268                                                1 + this_len, i2c_write);
 269                 }
 270                 if (result) {
 271                         LOG_RESULT_LOCATION(result);
 272                         return result;
 273                 }
 274                 bytes_written += this_len;
 275         }
 276         return 0;
 277 }
 278 EXPORT_SYMBOL(inv_serial_write);
 279
 280 int inv_serial_read(
 281         void *sl_handle,
 282         unsigned char slave_addr,
 283         unsigned char register_addr,
 284         unsigned short length,
 285         unsigned char *data)
 286 {
 287         int result;
 288         unsigned short bytes_read = 0;
 289
 290         if ((slave_addr & 0x7E) == DEFAULT_MPU_SLAVEADDR
 291                 && (register_addr == MPUREG_FIFO_R_W ||
 292                     register_addr == MPUREG_MEM_R_W)) {
 293                 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
 294                 return INV_ERROR_INVALID_PARAMETER;
 295         }
 296
 297         while (bytes_read < length) {
 298                 unsigned short this_len =
 299                     min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
 300                 result = inv_i2c_read((struct i2c_adapter *)sl_handle,
 301                                       slave_addr, register_addr + bytes_read,
 302                                       this_len, &data[bytes_read]);
 303                 if (result) {
 304                         LOG_RESULT_LOCATION(result);
 305                         return result;
 306                 }
 307                 bytes_read += this_len;
 308         }
 309         return 0;
 310 }
 311 EXPORT_SYMBOL(inv_serial_read);
 312
 313 int inv_serial_write_mem(
 314         void *sl_handle,
 315         unsigned char slave_addr,
 316         unsigned short mem_addr,
 317         unsigned short length,
 318         unsigned char const *data)
 319 {
 320         int result;
 321         unsigned short bytes_written = 0;
 322
 323         if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
 324                 pr_err("memory read length (%d B) extends beyond its"
 325                        " limits (%d) if started at location %d\n", length,
 326                        MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
 327                 return INV_ERROR_INVALID_PARAMETER;
 328         }
 329         while (bytes_written < length) {
 330                 unsigned short this_len =
 331                     min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
 332                 result = mpu_memory_write((struct i2c_adapter *)sl_handle,
 333                                           slave_addr, mem_addr + bytes_written,
 334                                           this_len, &data[bytes_written]);
 335                 if (result) {
 336                         LOG_RESULT_LOCATION(result);
 337                         return result;
 338                 }
 339                 bytes_written += this_len;
 340         }
 341         return 0;
 342 }
 343 EXPORT_SYMBOL(inv_serial_write_mem);
 344
 345 int inv_serial_read_mem(
 346         void *sl_handle,
 347         unsigned char slave_addr,
 348         unsigned short mem_addr,
 349         unsigned short length,
 350         unsigned char *data)
 351 {
 352         int result;
 353         unsigned short bytes_read = 0;
 354
 355         if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
 356                 printk
 357                     ("memory read length (%d B) extends beyond its limits (%d) "
 358                      "if started at location %d\n", length,
 359                      MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
 360                 return INV_ERROR_INVALID_PARAMETER;
 361         }
 362         while (bytes_read < length) {
 363                 unsigned short this_len =
 364                     min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
 365                 result =
 366                     mpu_memory_read((struct i2c_adapter *)sl_handle,
 367                                     slave_addr, mem_addr + bytes_read,
 368                                     this_len, &data[bytes_read]);
 369                 if (result) {
 370                         LOG_RESULT_LOCATION(result);
 371                         return result;
 372                 }
 373                 bytes_read += this_len;
 374         }
 375         return 0;
 376 }
 377 EXPORT_SYMBOL(inv_serial_read_mem);
 378
 379 int inv_serial_write_fifo(
 380         void *sl_handle,
 381         unsigned char slave_addr,
 382         unsigned short length,
 383         unsigned char const *data)
 384 {
 385         int result;
 386         unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
 387         unsigned short bytes_written = 0;
 388
 389         if (length > FIFO_HW_SIZE) {
 390                 printk(KERN_ERR
 391                        "maximum fifo write length is %d\n", FIFO_HW_SIZE);
 392                 return INV_ERROR_INVALID_PARAMETER;
 393         }
 394         while (bytes_written < length) {
 395                 unsigned short this_len =
 396                     min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
 397                 i2c_write[0] = MPUREG_FIFO_R_W;
 398                 memcpy(&i2c_write[1], &data[bytes_written], this_len);
 399                 result = inv_i2c_write((struct i2c_adapter *)sl_handle,
 400                                        slave_addr, this_len + 1, i2c_write);
 401                 if (result) {
 402                         LOG_RESULT_LOCATION(result);
 403                         return result;
 404                 }
 405                 bytes_written += this_len;
 406         }
 407         return 0;
 408 }
 409 EXPORT_SYMBOL(inv_serial_write_fifo);
 410
 411 int inv_serial_read_fifo(
 412         void *sl_handle,
 413         unsigned char slave_addr,
 414         unsigned short length,
 415         unsigned char *data)
 416 {
 417         int result;
 418         unsigned short bytes_read = 0;
 419
 420         if (length > FIFO_HW_SIZE) {
 421                 printk(KERN_ERR
 422                        "maximum fifo read length is %d\n", FIFO_HW_SIZE);
 423                 return INV_ERROR_INVALID_PARAMETER;
 424         }
 425         while (bytes_read < length) {
 426                 unsigned short this_len =
 427                     min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
 428                 result = inv_i2c_read((struct i2c_adapter *)sl_handle,
 429                                       slave_addr, MPUREG_FIFO_R_W, this_len,
 430                                       &data[bytes_read]);
 431                 if (result) {
 432                         LOG_RESULT_LOCATION(result);
 433                         return result;
 434                 }
 435                 bytes_read += this_len;
 436         }
 437
 438         return 0;
 439 }
 440 EXPORT_SYMBOL(inv_serial_read_fifo);
 441
 442 /**
 443  *  @}
 444  */