drivers/media/video/rk_camsys/camsys_drv.c

   1 #include <media/camsys_head.h>
   2
   3 #include "camsys_cif.h"
   4 #include "camsys_marvin.h"
   5 #include "camsys_mipicsi_phy.h"
   6 #include "camsys_gpio.h"
   7 #include "camsys_soc_priv.h"
   8 #include "ext_flashled_drv/rk_ext_fshled_ctl.h"
   9
  10 unsigned int camsys_debug=1;
  11 module_param(camsys_debug, int, S_IRUGO|S_IWUSR);
  12
  13 static int drv_version = CAMSYS_DRIVER_VERSION;
  14 module_param(drv_version, int, S_IRUGO);
  15 static int head_version = CAMSYS_HEAD_VERSION;
  16 module_param(head_version, int, S_IRUGO);
  17
  18
  19 typedef struct camsys_devs_s {
  20     spinlock_t lock;
  21     struct list_head devs;
  22 } camsys_devs_t;
  23
  24 static camsys_devs_t camsys_devs;
  25
  26 static int camsys_i2c_write(camsys_i2c_info_t *i2cinfo, camsys_dev_t *camsys_dev)
  27 {
  28     int err = 0,i,j;
  29     unsigned char buf[8],*bufp;
  30     unsigned short msg_times,totallen,onelen;
  31     struct i2c_msg msg[1];
  32     struct i2c_adapter *adapter;
  33
  34     adapter = i2c_get_adapter(i2cinfo->bus_num);
  35     if (adapter == NULL) {
  36         camsys_err("Get %d i2c adapter is failed!",i2cinfo->bus_num);
  37         err = -EINVAL;
  38         goto end;
  39     }
  40
  41     if (i2cinfo->i2cbuf_directly) {
  42         if (camsys_dev->devmems.i2cmem == NULL) {
  43             camsys_err("%s has not i2c mem, it isn't support i2c buf write!",dev_name(camsys_dev->miscdev.this_device));
  44             err = -EINVAL;
  45             goto end;
  46         }
  47         totallen = (i2cinfo->i2cbuf_bytes&0xffff);
  48         onelen = (i2cinfo->i2cbuf_bytes&0xffff0000)>>16;
  49         msg_times = totallen/onelen;
  50         if (totallen > camsys_dev->devmems.i2cmem->size) {
  51             camsys_err("Want to write 0x%x bytes, i2c memory(size: 0x%x) is overlap",totallen,camsys_dev->devmems.i2cmem->size);
  52             err = -EINVAL;
  53             goto end;
  54         }
  55         bufp = (unsigned char*)camsys_dev->devmems.i2cmem->vir_base;
  56     } else {
  57         for (i=0; i<i2cinfo->reg_size; i++) {
  58             buf[i] = (i2cinfo->reg_addr>>((i2cinfo->reg_size-1-i)*8))&0xff;
  59         }
  60         for (j=0; j<i2cinfo->val_size; j++) {
  61             buf[i+j] = (i2cinfo->val>>((i2cinfo->val_size-1-j)*8))&0xff;  /* ddl@rock-chips.com: v0.a.0 */
  62         }
  63         bufp = buf;
  64         onelen = i2cinfo->val_size + i2cinfo->reg_size;
  65         msg_times = 1;
  66     }
  67
  68         err = -EAGAIN;
  69     msg->addr = (i2cinfo->slave_addr>>1);
  70     msg->flags = 0;
  71     msg->scl_rate = i2cinfo->speed;
  72    // msg->read_type = 0;
  73     msg->len = onelen;
  74     for (i=0; i<msg_times; i++) {
  75         msg->buf = bufp+i*onelen;
  76                 err = i2c_transfer(adapter, msg, 1);
  77                 if (err < 0) {
  78             camsys_err("i2c write dev(addr:0x%x) failed!",i2cinfo->slave_addr);
  79                         udelay(10);
  80                 }
  81     }
  82
  83 end:
  84     return err;
  85 }
  86
  87 static int camsys_i2c_read(camsys_i2c_info_t *i2cinfo, camsys_dev_t *camsys_dev)
  88 {
  89     int err = 0,i,retry=2,tmp, num_msg;
  90     unsigned char buf[8];
  91     struct i2c_msg msg[2];
  92     struct i2c_adapter *adapter;
  93
  94     adapter = i2c_get_adapter(i2cinfo->bus_num);
  95     if (adapter == NULL) {
  96         camsys_err("Get %d i2c adapter is failed!",i2cinfo->bus_num);
  97         err = -EINVAL;
  98         goto end;
  99     }
 100
 101         num_msg = 0;
 102         if (i2cinfo->reg_size) {                /* ddl@rock-chips.com: v0.a.0 */
 103             for (i=0; i<i2cinfo->reg_size; i++) {
 104                 buf[i] = (i2cinfo->reg_addr>>((i2cinfo->reg_size-1-i)*8))&0xff;
 105             }
 106
 107             msg[0].addr = (i2cinfo->slave_addr>>1);
 108                 msg[0].flags = 0;
 109                 msg[0].scl_rate = i2cinfo->speed;
 110                 //msg[0].read_type = 0;
 111             msg[0].buf = buf;
 112             msg[0].len = i2cinfo->reg_size;
 113                 num_msg++;
 114         }
 115
 116     msg[1].addr = (i2cinfo->slave_addr>>1);
 117         msg[1].flags = I2C_M_RD;
 118         msg[1].scl_rate = i2cinfo->speed;
 119 //      msg[1].read_type = 0;
 120     msg[1].buf = buf;
 121     msg[1].len = (unsigned short)i2cinfo->val_size;
 122         err = -EAGAIN;
 123         num_msg++;
 124
 125         while ((retry-- > 0) && (err < 0)) {                                             /* ddl@rock-chips.com :  Transfer again if transent is failed   */
 126                 if (num_msg==1) {
 127                         err = i2c_transfer(adapter, &msg[1], num_msg);
 128                 } else {
 129                         err = i2c_transfer(adapter, msg, num_msg);
 130                 }
 131
 132                 if (err >= 0) {
 133             err = 0;
 134                 } else {
 135                         camsys_err("i2c read dev(addr:0x%x) failed,try again-%d!",i2cinfo->slave_addr,retry);
 136                         udelay(10);
 137                 }
 138         }
 139
 140
 141     if (err==0) {
 142         i2cinfo->val = 0x00;
 143         for(i=0; i<i2cinfo->val_size; i++) {
 144             tmp = buf[i];
 145             i2cinfo->val |= (tmp<<((i2cinfo->val_size-1-i)*8));
 146         }
 147     }
 148
 149 end:
 150     return err;
 151 }
 152
 153
 154 static int camsys_extdev_register(camsys_devio_name_t *devio, camsys_dev_t *camsys_dev)
 155 {
 156     int err = 0,i;
 157     camsys_extdev_t *extdev;
 158     camsys_regulator_info_t *regulator_info;
 159     camsys_regulator_t *regulator;
 160     camsys_gpio_info_t *gpio_info;
 161     camsys_gpio_t *gpio;
 162
 163     if ((devio->dev_id & CAMSYS_DEVID_EXTERNAL) == 0) {
 164         err = -EINVAL;
 165         camsys_err("dev_id: 0x%x is not support for camsys!",devio->dev_id);
 166         goto end;
 167     }
 168
 169     extdev = camsys_find_extdev(devio->dev_id, camsys_dev);
 170     if (extdev != NULL) {
 171         if (strcmp(extdev->dev_name, devio->dev_name) == 0) {
 172             err = 0;
 173         } else {
 174             err = -EINVAL;    /* ddl@rock-chips.com: v0.0x13.0 */
 175             camsys_warn("Extdev(dev_id: 0x%x dev_name: %s) has been registered in %s!",
 176                 extdev->dev_id, extdev->dev_name,dev_name(camsys_dev->miscdev.this_device));
 177         }
 178         goto end;
 179     }
 180
 181     extdev = kzalloc(sizeof(camsys_extdev_t),GFP_KERNEL);
 182     if (extdev == NULL) {
 183         camsys_err("alloc camsys_extdev_t failed!");
 184         err = -ENOMEM;
 185         goto end;
 186     }
 187
 188     extdev->dev_cfg = devio->dev_cfg;
 189     extdev->fl.fl.active = devio->fl.fl.active;
 190     extdev->fl.ext_fsh_dev = NULL;
 191     //should register external flash device ?
 192     if(strlen(devio->fl.fl_drv_name) && (strcmp(devio->fl.fl_drv_name,"Internal") != 0)
 193         && (strcmp(devio->fl.fl_drv_name,"NC") != 0)){
 194         //register flash device
 195         extdev->fl.ext_fsh_dev = camsys_register_ext_fsh_dev(&devio->fl);
 196         if(extdev->fl.ext_fsh_dev == NULL){
 197             camsys_err("register ext flash %s failed!",devio->fl.fl_drv_name);
 198             err = -EINVAL;
 199             goto fail;
 200         }
 201     }
 202     regulator_info = &devio->avdd;
 203     regulator = &extdev->avdd;
 204     for (i=(CamSys_Vdd_Start_Tag+1); i<CamSys_Vdd_End_Tag; i++) {
 205         if (strcmp(regulator_info->name,"NC")) {
 206             regulator->ldo = regulator_get(NULL,regulator_info->name);
 207             if (IS_ERR_OR_NULL(regulator->ldo)) {
 208                 camsys_err("Get %s regulator for dev_id 0x%x failed!",regulator_info->name,devio->dev_id);
 209                 err = -EINVAL;
 210                 goto fail;
 211             }
 212
 213             regulator->min_uv = regulator_info->min_uv;
 214             regulator->max_uv = regulator_info->max_uv;
 215             camsys_trace(1,"Get %s regulator(min: %duv  max: %duv) for dev_id 0x%x success",
 216                         regulator_info->name,regulator->min_uv,regulator->max_uv,
 217                         devio->dev_id);
 218         } else {
 219             regulator->ldo = NULL;
 220             regulator->min_uv = 0;
 221             regulator->max_uv = 0;
 222         }
 223
 224         regulator++;
 225         regulator_info++;
 226     }
 227
 228     gpio_info = &devio->pwrdn;
 229     gpio = &extdev->pwrdn;
 230     for (i=(CamSys_Gpio_Start_Tag+1); i<CamSys_Gpio_End_Tag; i++) {
 231         if (strcmp(gpio_info->name,"NC")) {
 232             gpio->io = camsys_gpio_get(gpio_info->name);
 233             if (gpio->io < 0) {
 234                 camsys_err("Get %s gpio for dev_id 0x%x failed!",gpio_info->name,devio->dev_id);
 235                 err = -EINVAL;
 236                 goto fail;
 237             }
 238             if (gpio_request(gpio->io,"camsys_gpio")<0) {
 239                 camsys_err("Request %s(%d) failed",gpio_info->name,gpio->io);
 240             }
 241             gpio->active = gpio_info->active;
 242             camsys_trace(1,"Get %s(%d) gpio(active: %d) for dev_id 0x%x success!",
 243                         gpio_info->name,gpio->io,gpio->active,devio->dev_id);
 244         } else {
 245             gpio->io = 0xffffffff;
 246             gpio->active = 0xffffffff;
 247         }
 248
 249         gpio++;
 250         gpio_info++;
 251     }
 252
 253     extdev->pdev = camsys_dev->pdev;
 254     extdev->phy = devio->phy;
 255     extdev->clk = devio->clk;
 256     extdev->dev_id = devio->dev_id;
 257     //spin_lock(&camsys_dev->lock);
 258     mutex_lock(&camsys_dev->extdevs.mut);
 259     list_add_tail(&extdev->list, &camsys_dev->extdevs.list);
 260     //spin_unlock(&camsys_dev->lock);
 261     mutex_unlock(&camsys_dev->extdevs.mut);
 262
 263     camsys_dev->iomux(extdev, (void*)camsys_dev);
 264
 265     memcpy(extdev->dev_name,devio->dev_name, sizeof(extdev->dev_name));
 266     camsys_trace(1,"Extdev(dev_id: 0x%x  dev_name: %s) register success",
 267         extdev->dev_id,
 268         extdev->dev_name);
 269
 270     return 0;
 271 fail:
 272     if (extdev) {
 273         kfree(extdev);
 274         extdev = NULL;
 275     }
 276 end:
 277
 278     return err;
 279 }
 280
 281 static int camsys_extdev_deregister(unsigned int dev_id, camsys_dev_t *camsys_dev, bool all)
 282 {
 283     int err = 0,i;
 284     camsys_extdev_t *extdev;
 285     camsys_regulator_t *regulator;
 286     camsys_gpio_t *gpio;
 287
 288     if (all == false) {
 289         if ((dev_id & CAMSYS_DEVID_EXTERNAL) == 0) {
 290             err = -EINVAL;
 291             camsys_err("dev_id: 0x%x is not support for %s!",dev_id, dev_name(camsys_dev->miscdev.this_device));
 292             goto end;
 293         }
 294
 295         extdev = camsys_find_extdev(dev_id, camsys_dev);
 296         if (extdev == NULL) {
 297             err = -EINVAL;
 298             camsys_warn("Extdev(dev_id: 0x%x) isn't registered in %s!",
 299                 dev_id, dev_name(camsys_dev->miscdev.this_device));
 300             goto end;
 301         }
 302
 303         regulator = &extdev->avdd;
 304         for (i=(CamSys_Vdd_Start_Tag+1); i<CamSys_Vdd_End_Tag; i++) {
 305             if (!IS_ERR_OR_NULL(regulator->ldo)) {
 306                 while(regulator_is_enabled(regulator->ldo)>0)
 307                             regulator_disable(regulator->ldo);
 308                         regulator_put(regulator->ldo);
 309             }
 310             regulator++;
 311         }
 312
 313         gpio = &extdev->pwrdn;
 314         for (i=(CamSys_Gpio_Start_Tag+1); i<CamSys_Gpio_End_Tag; i++) {
 315             if (gpio->io!=0xffffffff) {
 316                 gpio_free(gpio->io);
 317             }
 318             gpio++;
 319         }
 320
 321         if(extdev->fl.ext_fsh_dev != NULL){
 322             camsys_deregister_ext_fsh_dev(extdev->fl.ext_fsh_dev);
 323         }
 324         //spin_lock(&camsys_dev->lock);
 325         mutex_lock(&camsys_dev->extdevs.mut);
 326         list_del_init(&extdev->list);
 327         list_del_init(&extdev->active);
 328         //spin_unlock(&camsys_dev->lock);
 329         mutex_unlock(&camsys_dev->extdevs.mut);
 330
 331         camsys_trace(1,"Extdev(dev_id: 0x%x) is deregister success", extdev->dev_id);
 332         kfree(extdev);
 333         extdev = NULL;
 334
 335     } else {
 336         //spin_lock(&camsys_dev->lock);
 337         mutex_lock(&camsys_dev->extdevs.mut);
 338         while (!list_empty(&camsys_dev->extdevs.list)) {
 339
 340             extdev = list_first_entry(&camsys_dev->extdevs.list, camsys_extdev_t, list);
 341             if (extdev) {
 342                 regulator = &extdev->avdd;
 343                 for (i=(CamSys_Vdd_Start_Tag+1); i<CamSys_Vdd_End_Tag; i++) {
 344                     if (!IS_ERR(regulator->ldo)) {
 345                         while(regulator_is_enabled(regulator->ldo)>0)
 346                                     regulator_disable(regulator->ldo);
 347                                 regulator_put(regulator->ldo);
 348                     }
 349                     regulator++;
 350                 }
 351
 352                 gpio = &extdev->pwrdn;
 353                 for (i=(CamSys_Gpio_Start_Tag+1); i<CamSys_Gpio_End_Tag; i++) {
 354                     if (gpio->io!=0xffffffff) {
 355                         gpio_free(gpio->io);
 356                     }
 357                     gpio++;
 358                 }
 359
 360                 if(extdev->fl.ext_fsh_dev != NULL){
 361                     camsys_deregister_ext_fsh_dev(extdev->fl.ext_fsh_dev);
 362                 }
 363                 camsys_trace(1,"Extdev(dev_id: 0x%x) is deregister success", extdev->dev_id);
 364                 list_del_init(&extdev->list);
 365                 list_del_init(&extdev->active);
 366                 kfree(extdev);
 367                 extdev=NULL;
 368             }
 369         }
 370         //spin_unlock(&camsys_dev->lock);
 371         mutex_unlock(&camsys_dev->extdevs.mut);
 372         camsys_trace(1, "All extdev is deregister success!");
 373     }
 374
 375
 376 end:
 377     return err;
 378
 379 }
 380
 381 static int camsys_sysctl(camsys_sysctrl_t *devctl, camsys_dev_t *camsys_dev)
 382 {
 383     int i;
 384     int err = 0;
 385     camsys_extdev_t *extdev,*extdev2;
 386
 387     //spin_lock(&camsys_dev->lock);
 388     mutex_lock(&camsys_dev->extdevs.mut);
 389         if(devctl->ops == 0xaa){
 390                 dump_stack();
 391                 return 0;
 392         }
 393     //Internal
 394     if (camsys_dev->dev_id & devctl->dev_mask) {
 395         switch (devctl->ops)
 396         {
 397             case CamSys_ClkIn:
 398             {
 399                 camsys_dev->clkin_cb(camsys_dev,devctl->on);
 400                 break;
 401             }
 402
 403             case CamSys_Rst:
 404             {
 405                 camsys_dev->reset_cb(camsys_dev, devctl->on);
 406                 break;
 407             }
 408             case CamSys_Flash_Trigger:
 409             {
 410                 camsys_dev->flash_trigger_cb(camsys_dev,devctl->rev[0], devctl->on);
 411                 break;
 412             }
 413             case CamSys_IOMMU:
 414             {
 415                 if(camsys_dev->iommu_cb(camsys_dev, devctl) < 0){
 416                     err = -1;
 417                     }
 418                 break;
 419             }
 420             default:
 421                 break;
 422
 423         }
 424     }
 425
 426     //External
 427     for (i=0; i<8; i++) {
 428         if (devctl->dev_mask & (1<<(i+24))) {
 429             extdev = camsys_find_extdev((1<<(i+24)), camsys_dev);
 430             if (extdev) {
 431                 camsys_sysctl_extdev(extdev, devctl, camsys_dev);
 432
 433                 if (devctl->ops == CamSys_ClkIn) {
 434                     if (devctl->on) {
 435                         list_add_tail(&extdev->active,&camsys_dev->extdevs.active);
 436                     } else {
 437                         if (!list_empty(&camsys_dev->extdevs.active)) {    /* ddla@rock-chips.com: v0.0.7 */
 438                             list_for_each_entry(extdev2, &camsys_dev->extdevs.active, active) {
 439                                 if (extdev2 == extdev) {
 440                                     list_del_init(&extdev->active);
 441                                     break;
 442                                 }
 443                             }
 444                         }
 445                     }
 446                 }else if(devctl->ops == CamSys_Flash_Trigger){
 447                     err = camsys_ext_fsh_ctrl(extdev->fl.ext_fsh_dev,devctl->rev[0],devctl->on);
 448                 }
 449
 450             } else {
 451                 camsys_err("Can not find dev_id 0x%x device in %s!", (1<<(i+24)), dev_name(camsys_dev->miscdev.this_device));
 452             }
 453         }
 454     }
 455
 456     //spin_unlock(&camsys_dev->lock);
 457     mutex_unlock(&camsys_dev->extdevs.mut);
 458     return err;
 459 }
 460 static int camsys_phy_ops (camsys_extdev_t *extdev, camsys_sysctrl_t *devctl, void *ptr)
 461 {
 462     camsys_dev_t *camsys_dev = (camsys_dev_t*)ptr;
 463     camsys_mipiphy_t *mipiphy;
 464     int err = 0;
 465
 466     if (extdev->phy.type == CamSys_Phy_Mipi) {
 467         mipiphy = (camsys_mipiphy_t*)devctl->rev;
 468         if (devctl->on == 0) {
 469             mipiphy->phy_index = extdev->phy.info.mipi.phy_index;
 470             mipiphy->bit_rate = 0;
 471             mipiphy->data_en_bit = 0x00;
 472         } else {
 473             if ((mipiphy->bit_rate == 0) || (mipiphy->data_en_bit == 0)) {
 474                 *mipiphy = extdev->phy.info.mipi;
 475             }
 476             if (mipiphy->phy_index != extdev->phy.info.mipi.phy_index) {
 477                 camsys_warn("mipiphy->phy_index(%d) != extdev->phy.info.mipi.phy_index(%d)!",
 478                     mipiphy->phy_index,extdev->phy.info.mipi.phy_index);
 479                 mipiphy->phy_index = extdev->phy.info.mipi.phy_index;
 480
 481             }
 482         }
 483         err = camsys_dev->mipiphy[mipiphy->phy_index].ops(ptr,mipiphy);
 484         if (err < 0) {
 485             camsys_err("extdev(0x%x) mipi phy ops config failed!",extdev->dev_id);
 486         }
 487     }
 488
 489     return err;
 490 }
 491 static int camsys_irq_connect(camsys_irqcnnt_t *irqcnnt, camsys_dev_t *camsys_dev)
 492 {
 493     int err = 0,i;
 494     camsys_irqpool_t *irqpool;
 495     unsigned long int flags;
 496
 497     if ((irqcnnt->mis != MRV_ISP_MIS) &&
 498         (irqcnnt->mis != MRV_MIPI_MIS) &&
 499         (irqcnnt->mis != MRV_MI_MIS) &&
 500         (irqcnnt->mis != MRV_JPG_MIS) &&
 501         (irqcnnt->mis != MRV_JPG_ERR_MIS)) {
 502
 503         camsys_err("this thread(pid: %d) irqcnnt->mis(0x%x) is invalidate, irq connect failed!",
 504             irqcnnt->pid, irqcnnt->mis);
 505
 506         err = -EINVAL;
 507         goto end;
 508     }
 509
 510     spin_lock_irqsave(&camsys_dev->irq.lock,flags);
 511     if (!list_empty(&camsys_dev->irq.irq_pool)) {
 512         list_for_each_entry(irqpool, &camsys_dev->irq.irq_pool, list) {
 513             if (irqpool->pid == irqcnnt->pid) {
 514                 camsys_warn("this thread(pid: %d) had been connect irq!",current->pid);
 515                 spin_unlock(&camsys_dev->irq.lock);
 516                 goto end;
 517             }
 518         }
 519     }
 520     spin_unlock_irqrestore(&camsys_dev->irq.lock,flags);
 521
 522     irqpool = kzalloc(sizeof(camsys_irqpool_t),GFP_KERNEL);
 523     if (irqpool) {
 524         spin_lock_init(&irqpool->lock);
 525         irqpool->pid = irqcnnt->pid;
 526         irqpool->timeout = irqcnnt->timeout;
 527         irqpool->mis = irqcnnt->mis;
 528         irqpool->icr = irqcnnt->icr;
 529         INIT_LIST_HEAD(&irqpool->active);
 530         INIT_LIST_HEAD(&irqpool->deactive);
 531         init_waitqueue_head(&irqpool->done);
 532         for (i=0; i<CAMSYS_IRQPOOL_NUM; i++) {
 533             list_add_tail(&irqpool->pool[i].list, &irqpool->deactive);
 534         }
 535     }
 536
 537     spin_lock_irqsave(&camsys_dev->irq.lock,flags);
 538     //camsys_dev->irq.timeout = irqcnnt->timeout;
 539     list_add_tail(&irqpool->list, &camsys_dev->irq.irq_pool);
 540     spin_unlock_irqrestore(&camsys_dev->irq.lock,flags);
 541     camsys_trace(1, "Thread(pid: %d) connect %s irq success! mis: 0x%x icr: 0x%x ", irqpool->pid, dev_name(camsys_dev->miscdev.this_device),
 542         irqpool->mis,irqpool->icr);
 543
 544 end:
 545     return err;
 546 }
 547 static int active_list_isnot_empty(camsys_irqpool_t *irqpool)
 548 {
 549     int err;
 550     unsigned long int flags;
 551
 552     spin_lock_irqsave(&irqpool->lock,flags);
 553     err = list_empty(&irqpool->active);
 554     spin_unlock_irqrestore(&irqpool->lock,flags);
 555
 556     return !err;
 557
 558 }
 559 static int camsys_irq_wait(camsys_irqsta_t *irqsta, camsys_dev_t *camsys_dev)
 560 {
 561     int err = 0;
 562     bool find_pool = false;
 563     camsys_irqstas_t *irqstas;
 564     camsys_irqpool_t *irqpool;
 565     unsigned long int flags;
 566
 567     spin_lock_irqsave(&camsys_dev->irq.lock,flags);
 568     if (!list_empty(&camsys_dev->irq.irq_pool)) {
 569         list_for_each_entry(irqpool, &camsys_dev->irq.irq_pool, list) {
 570             if (irqpool->pid == current->pid) {
 571                 find_pool = true;
 572                 break;
 573             }
 574         }
 575     }
 576     spin_unlock_irqrestore(&camsys_dev->irq.lock,flags);
 577
 578     if (find_pool == false) {
 579         camsys_err("this thread(pid: %d) hasn't been connect irq, so wait irq failed!",current->pid);
 580         err = -EINVAL;
 581         goto end;
 582     }
 583
 584
 585     spin_lock_irqsave(&irqpool->lock,flags);
 586     if (!list_empty(&irqpool->active)) {
 587         irqstas = list_first_entry(&irqpool->active, camsys_irqstas_t, list);
 588         *irqsta = irqstas->sta;
 589         list_del_init(&irqstas->list);
 590         list_add_tail(&irqstas->list,&irqpool->deactive);
 591         spin_unlock_irqrestore(&irqpool->lock,flags);
 592     } else {
 593         spin_unlock_irqrestore(&irqpool->lock,flags);
 594
 595         wait_event_interruptible_timeout(irqpool->done,
 596             active_list_isnot_empty(irqpool),
 597             usecs_to_jiffies(irqpool->timeout));
 598
 599         if (irqpool->pid == current->pid) {
 600             if (active_list_isnot_empty(irqpool)) {
 601                 spin_lock_irqsave(&irqpool->lock,flags);
 602                 irqstas = list_first_entry(&irqpool->active, camsys_irqstas_t, list);
 603                 *irqsta = irqstas->sta;
 604                 list_del_init(&irqstas->list);
 605                 list_add_tail(&irqstas->list,&irqpool->deactive);
 606                 spin_unlock_irqrestore(&irqpool->lock,flags);
 607             } else {
 608                 err = -EAGAIN;
 609             }
 610         } else {
 611             camsys_warn("Thread(pid: %d) has been disconnect!",current->pid);
 612             err = -EAGAIN;
 613         }
 614     }
 615
 616     if (err == 0) {
 617         camsys_trace(3,"Thread(pid: %d) has been wake up for irq(mis: 0x%x ris:0x%x)!",
 618                      current->pid, irqsta->mis, irqsta->ris);
 619     }
 620
 621 end:
 622     return err;
 623 }
 624
 625 static int camsys_irq_disconnect(camsys_irqcnnt_t *irqcnnt, camsys_dev_t *camsys_dev, bool all)
 626 {
 627     int err = 0;
 628     bool find_pool = false;
 629     camsys_irqpool_t *irqpool;
 630     unsigned long int flags;
 631
 632     if (all == false) {
 633         spin_lock_irqsave(&camsys_dev->irq.lock,flags);
 634                 if (!list_empty(&camsys_dev->irq.irq_pool)) {
 635             list_for_each_entry(irqpool, &camsys_dev->irq.irq_pool, list) {
 636                 if (irqpool->pid == irqcnnt->pid) {
 637                     find_pool = true;
 638                     irqpool->pid = 0;
 639                     break;
 640                 }
 641             }
 642         }
 643         spin_unlock_irqrestore(&camsys_dev->irq.lock,flags);
 644
 645         if (find_pool == false) {
 646             camsys_err("this thread(pid: %d) have not been connect irq!, disconnect failed",current->pid);
 647         } else {
 648             wake_up_all(&irqpool->done);
 649         }
 650
 651         camsys_trace(1, "Thread(pid: %d) disconnect %s irq success!", irqcnnt->pid, dev_name(camsys_dev->miscdev.this_device));
 652    } else {
 653         spin_lock_irqsave(&camsys_dev->irq.lock,flags);
 654         while (!list_empty(&camsys_dev->irq.irq_pool)) {
 655             irqpool = list_first_entry(&camsys_dev->irq.irq_pool, camsys_irqpool_t, list);
 656             list_del_init(&irqpool->list);
 657             irqpool->pid = 0;
 658             wake_up_all(&irqpool->done);
 659             kfree(irqpool);
 660             irqpool = NULL;
 661         }
 662         spin_unlock_irqrestore(&camsys_dev->irq.lock,flags);
 663
 664         camsys_trace(1, "All thread disconnect %s irq success!", dev_name(camsys_dev->miscdev.this_device));
 665    }
 666
 667
 668     return err;
 669 }
 670
 671 static int camsys_querymem (camsys_dev_t *camsys_dev,  camsys_querymem_t *qmem)
 672 {
 673     int err = 0;
 674
 675     if (qmem->mem_type == CamSys_Mmap_RegisterMem) {
 676         if (camsys_dev->devmems.registermem == NULL) {
 677             camsys_err("%s register memory isn't been register!", dev_name(camsys_dev->miscdev.this_device));
 678             err = -EINVAL;
 679             goto end;
 680         }
 681
 682         qmem->mem_size = camsys_dev->devmems.registermem->size;
 683         qmem->mem_offset = CamSys_Mmap_RegisterMem*PAGE_SIZE;
 684     } else if (qmem->mem_type == CamSys_Mmap_I2cMem) {
 685         if (camsys_dev->devmems.i2cmem== NULL) {
 686             camsys_err("%s i2c memory isn't been register!", dev_name(camsys_dev->miscdev.this_device));
 687             err = -EINVAL;
 688             goto end;
 689         }
 690
 691         qmem->mem_size = camsys_dev->devmems.i2cmem->size;
 692         qmem->mem_offset = CamSys_Mmap_I2cMem*PAGE_SIZE;
 693     } else {
 694         camsys_err("%d memory type have not in %s memory list",qmem->mem_type,dev_name(camsys_dev->miscdev.this_device));
 695         err = -EINVAL;
 696         goto end;
 697     }
 698
 699
 700     return 0;
 701 end:
 702     return err;
 703 }
 704 static int camsys_open(struct inode *inode, struct file *file)
 705 {
 706     int err = 0;
 707     int minor = iminor(inode);
 708     camsys_dev_t *camsys_dev;
 709     unsigned int i,phycnt;
 710
 711     spin_lock(&camsys_devs.lock);
 712     list_for_each_entry(camsys_dev, &camsys_devs.devs, list) {
 713         if (camsys_dev->miscdev.minor == minor) {
 714             file->private_data = (void*)(camsys_dev);
 715             break;
 716         }
 717     }
 718     spin_unlock(&camsys_devs.lock);
 719
 720     //zyc add
 721     INIT_LIST_HEAD(&camsys_dev->extdevs.active);
 722
 723     if (camsys_dev->mipiphy != NULL) {
 724         phycnt = camsys_dev->mipiphy[0].phycnt;
 725
 726         for (i=0; i<phycnt; i++) {
 727             if (camsys_dev->mipiphy[i].clkin_cb != NULL) {
 728                 camsys_dev->mipiphy[i].clkin_cb(camsys_dev,1);
 729             }
 730         }
 731     }
 732
 733
 734     if (file->private_data == NULL) {
 735         camsys_err("Cann't find camsys_dev!");
 736         err = -ENODEV;
 737         goto end;
 738     } else {
 739         camsys_trace(1,"%s(%p) is opened!",dev_name(camsys_dev->miscdev.this_device),camsys_dev);
 740     }
 741
 742 end:
 743     return err;
 744 }
 745
 746 static int camsys_release(struct inode *inode, struct file *file)
 747 {
 748     camsys_dev_t *camsys_dev = (camsys_dev_t*)file->private_data;
 749     unsigned int i,phycnt;
 750
 751     camsys_irq_disconnect(NULL,camsys_dev, true);
 752
 753     if (camsys_dev->mipiphy != NULL) {
 754         phycnt = camsys_dev->mipiphy[0].phycnt;
 755
 756         for (i=0; i<phycnt; i++) {
 757             if (camsys_dev->mipiphy[i].clkin_cb != NULL) {
 758                 camsys_dev->mipiphy[i].clkin_cb(camsys_dev,0);
 759             }
 760         }
 761     }
 762
 763     camsys_trace(1,"%s(%p) is closed",dev_name(camsys_dev->miscdev.this_device),camsys_dev);
 764
 765     return 0;
 766 }
 767
 768 /*
 769 * The ioctl() implementation
 770 */
 771
 772 static long camsys_ioctl(struct file *filp,unsigned int cmd, unsigned long arg)
 773 {
 774         long err = 0;
 775     camsys_dev_t *camsys_dev = (camsys_dev_t*)filp->private_data;
 776
 777         if (_IOC_TYPE(cmd) != CAMSYS_IOC_MAGIC) {
 778         camsys_err("ioctl type(%c!=%c) is invalidate\n",_IOC_TYPE(cmd),CAMSYS_IOC_MAGIC);
 779         err = -ENOTTY;
 780         goto end;
 781         }
 782         if (_IOC_NR(cmd) > CAMSYS_IOC_MAXNR) {
 783         camsys_err("ioctl index(%d>%d) is invalidate\n",_IOC_NR(cmd),CAMSYS_IOC_MAXNR);
 784         err = -ENOTTY;
 785         goto end;
 786         }
 787
 788     if (_IOC_DIR(cmd) & _IOC_READ)
 789         err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
 790     else if (_IOC_DIR(cmd) & _IOC_WRITE)
 791         err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
 792
 793     if (err) {
 794         camsys_err("ioctl(0x%x) operation not permitted for %s",cmd,dev_name(camsys_dev->miscdev.this_device));
 795         err = -EFAULT;
 796         goto end;
 797     }
 798
 799         switch (cmd) {
 800
 801             case CAMSYS_VERCHK:
 802             {
 803                 camsys_version_t camsys_ver;
 804
 805             camsys_ver.drv_ver = CAMSYS_DRIVER_VERSION;
 806             camsys_ver.head_ver = CAMSYS_HEAD_VERSION;
 807             if (copy_to_user((void __user *)arg,(void*)&camsys_ver, sizeof(camsys_version_t)))
 808                 return -EFAULT;
 809             break;
 810             }
 811             case CAMSYS_QUREYIOMMU:
 812             {
 813             int iommu_enabled = 0;
 814             #ifdef CONFIG_ROCKCHIP_IOMMU
 815                                 struct device_node * vpu_node =NULL;
 816                                 int vpu_iommu_enabled = 0;
 817                 vpu_node = of_find_node_by_name(NULL, "vpu_service");
 818                                 if(vpu_node){
 819                                         of_property_read_u32(vpu_node, "iommu_enabled", &vpu_iommu_enabled);
 820                                         of_property_read_u32(camsys_dev->pdev->dev.of_node, "rockchip,isp,iommu_enable", &iommu_enabled);
 821                                         of_node_put(vpu_node);
 822                                         if(iommu_enabled != vpu_iommu_enabled){
 823                                                 camsys_err("iommu status not consistent,check the dts file ! isp:%d,vpu:%d",iommu_enabled,vpu_iommu_enabled);
 824                                                 return -EFAULT;
 825                                         }
 826                                 }
 827                         #endif
 828             if (copy_to_user((void __user *)arg,(void*)&iommu_enabled, sizeof(iommu_enabled)))
 829                 return -EFAULT;
 830             break;
 831             }
 832             case CAMSYS_I2CRD:
 833             {
 834                 camsys_i2c_info_t i2cinfo;
 835
 836             if (copy_from_user((void*)&i2cinfo,(void __user *)arg, sizeof(camsys_i2c_info_t)))
 837                 return -EFAULT;
 838
 839             err = camsys_i2c_read(&i2cinfo,camsys_dev);
 840             if (err==0) {
 841                 if (copy_to_user((void __user *)arg,(void*)&i2cinfo, sizeof(camsys_i2c_info_t)))
 842                     return -EFAULT;
 843             }
 844             break;
 845             }
 846
 847             case CAMSYS_I2CWR:
 848             {
 849             camsys_i2c_info_t i2cinfo;
 850
 851             if (copy_from_user((void*)&i2cinfo,(void __user *)arg, sizeof(camsys_i2c_info_t)))
 852                 return -EFAULT;
 853
 854             err = camsys_i2c_write(&i2cinfo,camsys_dev);
 855             break;
 856             }
 857
 858         case CAMSYS_SYSCTRL:
 859         {
 860             camsys_sysctrl_t devctl;
 861
 862             if (copy_from_user((void*)&devctl,(void __user *)arg, sizeof(camsys_sysctrl_t)))
 863                 return -EFAULT;
 864
 865             err = camsys_sysctl(&devctl, camsys_dev);
 866             if ((err==0) && (devctl.ops == CamSys_IOMMU)){
 867                 if (copy_to_user((void __user *)arg,(void*)&devctl, sizeof(camsys_sysctrl_t)))
 868                     return -EFAULT;
 869             }
 870             break;
 871         }
 872
 873         case CAMSYS_REGRD:
 874         {
 875
 876             break;
 877         }
 878
 879         case CAMSYS_REGWR:
 880         {
 881
 882             break;
 883         }
 884
 885         case CAMSYS_REGISTER_DEVIO:
 886         {
 887             camsys_devio_name_t devio;
 888
 889             if (copy_from_user((void*)&devio,(void __user *)arg, sizeof(camsys_devio_name_t)))
 890                 return -EFAULT;
 891
 892             err = camsys_extdev_register(&devio,camsys_dev);
 893             break;
 894         }
 895
 896         case CAMSYS_DEREGISTER_DEVIO:
 897         {
 898             unsigned int dev_id;
 899
 900             if (copy_from_user((void*)&dev_id,(void __user *)arg, sizeof(unsigned int)))
 901                 return -EFAULT;
 902
 903             err = camsys_extdev_deregister(dev_id, camsys_dev, false);
 904             break;
 905         }
 906
 907         case CAMSYS_IRQCONNECT:
 908         {
 909             camsys_irqcnnt_t irqcnnt;
 910
 911             if (copy_from_user((void*)&irqcnnt,(void __user *)arg, sizeof(camsys_irqcnnt_t)))
 912                 return -EFAULT;
 913
 914             err = camsys_irq_connect(&irqcnnt, camsys_dev);
 915
 916             break;
 917         }
 918
 919         case CAMSYS_IRQWAIT:
 920         {
 921             camsys_irqsta_t irqsta;
 922
 923             err = camsys_irq_wait(&irqsta, camsys_dev);
 924             if (err==0) {
 925                 if (copy_to_user((void __user *)arg,(void*)&irqsta, sizeof(camsys_irqsta_t)))
 926                     return -EFAULT;
 927             }
 928             break;
 929         }
 930
 931         case CAMSYS_IRQDISCONNECT:
 932         {
 933             camsys_irqcnnt_t irqcnnt;
 934
 935             if (copy_from_user((void*)&irqcnnt,(void __user *)arg, sizeof(camsys_irqcnnt_t)))
 936                 return -EFAULT;
 937             err = camsys_irq_disconnect(&irqcnnt,camsys_dev,false);
 938                         break;
 939         }
 940
 941
 942         case CAMSYS_QUREYMEM:
 943         {
 944             camsys_querymem_t qmem;
 945
 946             if (copy_from_user((void*)&qmem,(void __user *)arg, sizeof(camsys_querymem_t)))
 947                 return -EFAULT;
 948
 949             err = camsys_querymem(camsys_dev,&qmem);
 950             if (err == 0) {
 951                 if (copy_to_user((void __user *)arg,(void*)&qmem, sizeof(camsys_querymem_t)))
 952                     return -EFAULT;
 953             }
 954             break;
 955         }
 956
 957         default :
 958             break;
 959         }
 960
 961 end:
 962         return err;
 963
 964 }
 965 /*
 966  * VMA operations.
 967  */
 968 static void camsys_vm_open(struct vm_area_struct *vma)
 969 {
 970     camsys_meminfo_t *meminfo = (camsys_meminfo_t*)vma->vm_private_data;
 971
 972     meminfo->vmas++;
 973     return;
 974 }
 975
 976 static void camsys_vm_close(struct vm_area_struct *vma)
 977 {
 978     camsys_meminfo_t *meminfo = (camsys_meminfo_t*)vma->vm_private_data;
 979
 980     meminfo->vmas--;
 981     return;
 982 }
 983
 984 static const struct vm_operations_struct camsys_vm_ops = {
 985         .open           = camsys_vm_open,
 986         .close          = camsys_vm_close,
 987 };
 988
 989 int camsys_mmap(struct file *flip, struct vm_area_struct *vma)
 990 {
 991     camsys_dev_t *camsys_dev = (camsys_dev_t*)flip->private_data;
 992         unsigned long addr, start, size;
 993         camsys_mmap_type_t mem_type;
 994         camsys_meminfo_t *meminfo;
 995         int ret = 0;
 996
 997     mem_type = vma->vm_pgoff;
 998
 999     if (mem_type == CamSys_Mmap_RegisterMem) {
1000         if (camsys_dev->devmems.registermem != NULL) {
1001             meminfo = camsys_dev->devmems.registermem;
1002         } else {
1003             camsys_err("this camsys device has not register mem!");
1004             ret = -EINVAL;
1005             goto done;
1006         }
1007     } else if (mem_type == CamSys_Mmap_I2cMem) {
1008         if (camsys_dev->devmems.i2cmem != NULL) {
1009             meminfo = camsys_dev->devmems.i2cmem;
1010         } else {
1011             camsys_err("this camsys device has not i2c mem!");
1012             ret = -EINVAL;
1013             goto done;
1014         }
1015     } else {
1016         camsys_err("mmap buffer type %d is invalidate!",mem_type);
1017         ret = -EINVAL;
1018         goto done;
1019     }
1020
1021     size = vma->vm_end - vma->vm_start;
1022     if (size > meminfo->size) {
1023         ret = -ENOMEM;
1024         camsys_err("mmap size(0x%lx) > memory size(0x%x), so failed!",size,meminfo->size);
1025         goto done;
1026     }
1027
1028         start = vma->vm_start;
1029         addr = __phys_to_pfn(meminfo->phy_base);
1030
1031     if (remap_pfn_range(vma, start, addr,size,pgprot_noncached(vma->vm_page_prot))) {
1032
1033         ret = -EAGAIN;
1034         goto done;
1035     }
1036
1037     vma->vm_ops = &camsys_vm_ops;
1038     vma->vm_flags |= VM_IO;
1039     vma->vm_flags |=VM_ACCOUNT;//same as VM_RESERVED;
1040     vma->vm_private_data = (void*)meminfo;
1041     camsys_vm_open(vma);
1042
1043 done:
1044         return ret;
1045 }
1046
1047 struct file_operations camsys_fops = {
1048         .owner =            THIS_MODULE,
1049     .open =             camsys_open,
1050     .release =          camsys_release,
1051         .unlocked_ioctl =   camsys_ioctl,
1052         .mmap =             camsys_mmap,
1053 };
1054
1055 static int camsys_platform_probe(struct platform_device *pdev){
1056     int err = 0;
1057         camsys_dev_t *camsys_dev;
1058     struct resource register_res ;
1059     struct device *dev = &pdev->dev;
1060     unsigned long i2cmem;
1061         camsys_meminfo_t *meminfo;
1062     unsigned int irq_id;
1063
1064     err = of_address_to_resource(dev->of_node, 0, &register_res);
1065     if (err < 0){
1066         camsys_err("Get register resource from %s platform device failed!",pdev->name);
1067         err = -ENODEV;
1068         goto fail_end;
1069     }
1070
1071     //map irqs
1072     irq_id = irq_of_parse_and_map(dev->of_node, 0);
1073     if (irq_id < 0) {
1074         camsys_err("Get irq resource from %s platform device failed!",pdev->name);
1075         err = -ENODEV;
1076         goto fail_end;
1077     }
1078
1079     camsys_dev = (camsys_dev_t*)devm_kzalloc(&pdev->dev,sizeof(camsys_dev_t), GFP_KERNEL);
1080     if (camsys_dev == NULL) {
1081         camsys_err("Allocate camsys_dev for %s platform device failed",pdev->name);
1082         err = -ENOMEM;
1083         goto fail_end;
1084     }
1085
1086     //spin_lock_init(&camsys_dev->lock);
1087     mutex_init(&camsys_dev->extdevs.mut);
1088     INIT_LIST_HEAD(&camsys_dev->extdevs.list);
1089     INIT_LIST_HEAD(&camsys_dev->extdevs.active);
1090     INIT_LIST_HEAD(&camsys_dev->list);
1091
1092
1093     //IRQ init
1094     camsys_dev->irq.irq_id = irq_id;
1095     spin_lock_init(&camsys_dev->irq.lock);
1096     INIT_LIST_HEAD(&camsys_dev->irq.irq_pool);
1097     //init_waitqueue_head(&camsys_dev->irq.irq_done);
1098
1099     INIT_LIST_HEAD(&camsys_dev->devmems.memslist);
1100
1101     // get soc operation
1102     camsys_dev->soc = (void*)camsys_soc_get();
1103     if (camsys_dev->soc == NULL) {
1104         err = -ENODEV;
1105         goto fail_end;
1106     }
1107
1108     //Register mem init
1109     meminfo = kzalloc(sizeof(camsys_meminfo_t),GFP_KERNEL);
1110     if (meminfo == NULL) {
1111         err = -ENOMEM;
1112         goto request_mem_fail;
1113     }
1114
1115     meminfo->vir_base = (unsigned int)devm_ioremap_resource(dev, &register_res);
1116     if (!meminfo->vir_base){
1117         camsys_err("%s ioremap %s failed",dev_name(&pdev->dev), CAMSYS_REGISTER_MEM_NAME);
1118         err = -ENXIO;
1119         goto request_mem_fail;
1120     }
1121
1122     strlcpy(meminfo->name, CAMSYS_REGISTER_MEM_NAME,sizeof(meminfo->name));
1123     meminfo->phy_base = register_res.start;
1124     meminfo->size = register_res.end - register_res.start + 1;
1125     list_add_tail(&meminfo->list, &camsys_dev->devmems.memslist);
1126
1127
1128     //I2c mem init
1129     i2cmem = __get_free_page(GFP_KERNEL);
1130     if (i2cmem == 0) {
1131         camsys_err("Allocate i2cmem failed!");
1132         err = -ENOMEM;
1133         goto request_mem_fail;
1134     }
1135     SetPageReserved(virt_to_page(i2cmem));
1136
1137     meminfo = kzalloc(sizeof(camsys_meminfo_t),GFP_KERNEL);
1138     if (meminfo == NULL) {
1139         err = -ENOMEM;
1140         goto request_mem_fail;
1141     }
1142     strlcpy(meminfo->name,CAMSYS_I2C_MEM_NAME,sizeof(meminfo->name));
1143     meminfo->vir_base = i2cmem;
1144     meminfo->phy_base = virt_to_phys((void*)i2cmem);
1145     meminfo->size = PAGE_SIZE;
1146     list_add_tail(&meminfo->list, &camsys_dev->devmems.memslist);
1147
1148     {
1149         unsigned int *tmpp;
1150
1151         tmpp = (unsigned int*)meminfo->vir_base;
1152         *tmpp = 0xfa561243;
1153     }
1154
1155     //Special init
1156
1157     {
1158         if (camsys_mipiphy_probe_cb(pdev, camsys_dev) <0) {
1159             camsys_err("Mipi phy probe failed!");
1160         }
1161     }
1162
1163 #if 0
1164     if (strcmp(dev_name(&pdev->dev),CAMSYS_PLATFORM_MARVIN_NAME) == 0) {
1165         #if (defined(CONFIG_CAMSYS_MRV))
1166         camsys_mrv_probe_cb(pdev, camsys_dev);
1167         #else
1168         camsys_err("Marvin controller camsys driver haven't been complie!!!");
1169         #endif
1170     } else {
1171         #if (defined(CONFIG_CAMSYS_CIF))
1172         camsys_cif_probe_cb(pdev,camsys_dev);
1173         #else
1174         camsys_err("CIF controller camsys driver haven't been complie!!!");
1175         #endif
1176     }
1177 #else
1178         #if (defined(CONFIG_CAMSYS_MRV))
1179         camsys_mrv_probe_cb(pdev, camsys_dev);
1180         #elif (defined(CONFIG_CAMSYS_CIF))
1181         camsys_cif_probe_cb(pdev,camsys_dev);
1182         #else
1183         camsys_err("camsys driver haven't been complie!!!");
1184         #endif
1185 #endif
1186     camsys_trace(1, "%s memory:",dev_name(&pdev->dev));
1187     list_for_each_entry(meminfo, &camsys_dev->devmems.memslist, list) {
1188         if (strcmp(meminfo->name,CAMSYS_I2C_MEM_NAME) == 0) {
1189             camsys_dev->devmems.i2cmem = meminfo;
1190             camsys_trace(1,"    I2c memory (phy: 0x%x vir: 0x%x size: 0x%x)",
1191                         meminfo->phy_base,meminfo->vir_base,meminfo->size);
1192         }
1193         if (strcmp(meminfo->name,CAMSYS_REGISTER_MEM_NAME) == 0) {
1194             camsys_dev->devmems.registermem = meminfo;
1195             camsys_trace(1,"    Register memory (phy: 0x%x vir: 0x%x size: 0x%x)",
1196                         meminfo->phy_base,meminfo->vir_base,meminfo->size);
1197         }
1198     }
1199
1200
1201     camsys_dev->phy_cb = camsys_phy_ops;
1202     camsys_dev->pdev    =   pdev;
1203
1204     platform_set_drvdata(pdev,(void*)camsys_dev);
1205     //Camsys_devs list add
1206     spin_lock(&camsys_devs.lock);
1207     list_add_tail(&camsys_dev->list, &camsys_devs.devs);
1208     spin_unlock(&camsys_devs.lock);
1209
1210     camsys_init_ext_fsh_module();
1211     camsys_trace(1, "Probe %s device success ", dev_name(&pdev->dev));
1212     return 0;
1213 request_mem_fail:
1214     if (camsys_dev != NULL) {
1215
1216         while(!list_empty(&camsys_dev->devmems.memslist)) {
1217             meminfo = list_first_entry(&camsys_dev->devmems.memslist, camsys_meminfo_t, list);
1218             if (meminfo) {
1219                 list_del_init(&meminfo->list);
1220                 if (strcmp(meminfo->name,CAMSYS_REGISTER_MEM_NAME)==0) {
1221                     iounmap((void __iomem *)meminfo->vir_base);
1222                     release_mem_region(meminfo->phy_base,meminfo->size);
1223                 } else if (strcmp(meminfo->name,CAMSYS_I2C_MEM_NAME)==0) {
1224                     kfree((void*)meminfo->vir_base);
1225                 }
1226                 kfree(meminfo);
1227                 meminfo = NULL;
1228             }
1229         }
1230
1231         kfree(camsys_dev);
1232         camsys_dev = NULL;
1233     }
1234 fail_end:
1235     return -1;
1236
1237
1238 }
1239 static int  camsys_platform_remove(struct platform_device *pdev)
1240 {
1241     camsys_dev_t *camsys_dev = platform_get_drvdata(pdev);
1242     camsys_meminfo_t *meminfo;
1243
1244     if (camsys_dev) {
1245
1246         //Mem deinit
1247         while(!list_empty(&camsys_dev->devmems.memslist)) {
1248             meminfo = list_first_entry(&camsys_dev->devmems.memslist, camsys_meminfo_t, list);
1249             if (meminfo) {
1250                 list_del_init(&meminfo->list);
1251                 if (strcmp(meminfo->name,CAMSYS_REGISTER_MEM_NAME)==0) {
1252                     iounmap((void __iomem *)meminfo->vir_base);
1253                     release_mem_region(meminfo->phy_base,meminfo->size);
1254                 } else if (strcmp(meminfo->name,CAMSYS_I2C_MEM_NAME)==0) {
1255                     kfree((void*)meminfo->vir_base);
1256                 }
1257                 kfree(meminfo);
1258                 meminfo = NULL;
1259             }
1260         }
1261
1262         //Irq deinit
1263         if (camsys_dev->irq.irq_id) {
1264             free_irq(camsys_dev->irq.irq_id, camsys_dev);
1265             camsys_irq_disconnect(NULL,camsys_dev,true);
1266         }
1267
1268         //Extdev deinit
1269         if (!list_empty(&camsys_dev->extdevs.list)) {
1270             camsys_extdev_deregister(0,camsys_dev,true);
1271         }
1272         if (camsys_dev->mipiphy != NULL) {
1273             camsys_dev->mipiphy->remove(pdev);
1274         }
1275         if (camsys_dev->cifphy.remove)
1276             camsys_dev->cifphy.remove(pdev);
1277         camsys_dev->platform_remove(pdev);
1278
1279         misc_deregister(&camsys_dev->miscdev);
1280
1281         spin_lock(&camsys_devs.lock);
1282         list_del_init(&camsys_dev->list);
1283         spin_unlock(&camsys_devs.lock);
1284
1285         camsys_deinit_ext_fsh_module();
1286
1287         kfree(camsys_dev);
1288         camsys_dev=NULL;
1289     } else {
1290         camsys_err("This platform device havn't obtain camsys_dev!");
1291     }
1292
1293     return 0;
1294 }
1295
1296
1297 static const struct of_device_id cif_of_match[] = {
1298     { .compatible = "rockchip,isp" },
1299 };
1300 MODULE_DEVICE_TABLE(of, cif_of_match);
1301
1302 static struct platform_driver camsys_platform_driver =
1303 {
1304     .driver     = {
1305         .name   = CAMSYS_PLATFORM_DRV_NAME,
1306         .of_match_table = of_match_ptr(cif_of_match),
1307     },
1308     .probe              = camsys_platform_probe,
1309     .remove             = (camsys_platform_remove),
1310 };
1311
1312 MODULE_ALIAS(CAMSYS_PLATFORM_DRV_NAME);
1313 static int __init camsys_platform_init(void)
1314 {
1315     printk("CamSys driver version: v%d.%d.%d,  CamSys head file version: v%d.%d.%d\n",
1316         (CAMSYS_DRIVER_VERSION&0xff0000)>>16, (CAMSYS_DRIVER_VERSION&0xff00)>>8,
1317         CAMSYS_DRIVER_VERSION&0xff,
1318         (CAMSYS_HEAD_VERSION&0xff0000)>>16, (CAMSYS_HEAD_VERSION&0xff00)>>8,
1319         CAMSYS_HEAD_VERSION&0xff);
1320
1321     spin_lock_init(&camsys_devs.lock);
1322     INIT_LIST_HEAD(&camsys_devs.devs);
1323     camsys_soc_init();
1324     platform_driver_register(&camsys_platform_driver);
1325    // platform_driver_probe(&camsys_platform_driver, camsys_platform_probe_new);
1326
1327     return 0;
1328 }
1329
1330 static void __exit camsys_platform_exit(void)
1331 {
1332     platform_driver_unregister(&camsys_platform_driver);
1333     camsys_soc_deinit();
1334 }
1335
1336 module_init(camsys_platform_init);
1337 module_exit(camsys_platform_exit);
1338
1339 MODULE_DESCRIPTION("RockChip Camera System");
1340 MODULE_AUTHOR("<ddl@rock-chips>");
1341 MODULE_LICENSE("GPL");
1342