sound/pci/ctxfi/ctsrc.c

   1 /**
   2  * Copyright (C) 2008, Creative Technology Ltd. All Rights Reserved.
   3  *
   4  * This source file is released under GPL v2 license (no other versions).
   5  * See the COPYING file included in the main directory of this source
   6  * distribution for the license terms and conditions.
   7  *
   8  * @File        ctsrc.c
   9  *
  10  * @Brief
  11  * This file contains the implementation of the Sample Rate Convertor
  12  * resource management object.
  13  *
  14  * @Author      Liu Chun
  15  * @Date        May 13 2008
  16  *
  17  */
  18
  19 #include "ctsrc.h"
  20 #include "cthardware.h"
  21 #include <linux/slab.h>
  22
  23 #define SRC_RESOURCE_NUM        256
  24 #define SRCIMP_RESOURCE_NUM     256
  25
  26 static unsigned int conj_mask;
  27
  28 static int src_default_config_memrd(struct src *src);
  29 static int src_default_config_memwr(struct src *src);
  30 static int src_default_config_arcrw(struct src *src);
  31
  32 static int (*src_default_config[3])(struct src *) = {
  33         [MEMRD] = src_default_config_memrd,
  34         [MEMWR] = src_default_config_memwr,
  35         [ARCRW] = src_default_config_arcrw
  36 };
  37
  38 static int src_set_state(struct src *src, unsigned int state)
  39 {
  40         struct hw *hw;
  41
  42         hw = src->rsc.hw;
  43         hw->src_set_state(src->rsc.ctrl_blk, state);
  44
  45         return 0;
  46 }
  47
  48 static int src_set_bm(struct src *src, unsigned int bm)
  49 {
  50         struct hw *hw;
  51
  52         hw = src->rsc.hw;
  53         hw->src_set_bm(src->rsc.ctrl_blk, bm);
  54
  55         return 0;
  56 }
  57
  58 static int src_set_sf(struct src *src, unsigned int sf)
  59 {
  60         struct hw *hw;
  61
  62         hw = src->rsc.hw;
  63         hw->src_set_sf(src->rsc.ctrl_blk, sf);
  64
  65         return 0;
  66 }
  67
  68 static int src_set_pm(struct src *src, unsigned int pm)
  69 {
  70         struct hw *hw;
  71
  72         hw = src->rsc.hw;
  73         hw->src_set_pm(src->rsc.ctrl_blk, pm);
  74
  75         return 0;
  76 }
  77
  78 static int src_set_rom(struct src *src, unsigned int rom)
  79 {
  80         struct hw *hw;
  81
  82         hw = src->rsc.hw;
  83         hw->src_set_rom(src->rsc.ctrl_blk, rom);
  84
  85         return 0;
  86 }
  87
  88 static int src_set_vo(struct src *src, unsigned int vo)
  89 {
  90         struct hw *hw;
  91
  92         hw = src->rsc.hw;
  93         hw->src_set_vo(src->rsc.ctrl_blk, vo);
  94
  95         return 0;
  96 }
  97
  98 static int src_set_st(struct src *src, unsigned int st)
  99 {
 100         struct hw *hw;
 101
 102         hw = src->rsc.hw;
 103         hw->src_set_st(src->rsc.ctrl_blk, st);
 104
 105         return 0;
 106 }
 107
 108 static int src_set_bp(struct src *src, unsigned int bp)
 109 {
 110         struct hw *hw;
 111
 112         hw = src->rsc.hw;
 113         hw->src_set_bp(src->rsc.ctrl_blk, bp);
 114
 115         return 0;
 116 }
 117
 118 static int src_set_cisz(struct src *src, unsigned int cisz)
 119 {
 120         struct hw *hw;
 121
 122         hw = src->rsc.hw;
 123         hw->src_set_cisz(src->rsc.ctrl_blk, cisz);
 124
 125         return 0;
 126 }
 127
 128 static int src_set_ca(struct src *src, unsigned int ca)
 129 {
 130         struct hw *hw;
 131
 132         hw = src->rsc.hw;
 133         hw->src_set_ca(src->rsc.ctrl_blk, ca);
 134
 135         return 0;
 136 }
 137
 138 static int src_set_sa(struct src *src, unsigned int sa)
 139 {
 140         struct hw *hw;
 141
 142         hw = src->rsc.hw;
 143         hw->src_set_sa(src->rsc.ctrl_blk, sa);
 144
 145         return 0;
 146 }
 147
 148 static int src_set_la(struct src *src, unsigned int la)
 149 {
 150         struct hw *hw;
 151
 152         hw = src->rsc.hw;
 153         hw->src_set_la(src->rsc.ctrl_blk, la);
 154
 155         return 0;
 156 }
 157
 158 static int src_set_pitch(struct src *src, unsigned int pitch)
 159 {
 160         struct hw *hw;
 161
 162         hw = src->rsc.hw;
 163         hw->src_set_pitch(src->rsc.ctrl_blk, pitch);
 164
 165         return 0;
 166 }
 167
 168 static int src_set_clear_zbufs(struct src *src)
 169 {
 170         struct hw *hw;
 171
 172         hw = src->rsc.hw;
 173         hw->src_set_clear_zbufs(src->rsc.ctrl_blk, 1);
 174
 175         return 0;
 176 }
 177
 178 static int src_commit_write(struct src *src)
 179 {
 180         struct hw *hw;
 181         int i;
 182         unsigned int dirty = 0;
 183
 184         hw = src->rsc.hw;
 185         src->rsc.ops->master(&src->rsc);
 186         if (src->rsc.msr > 1) {
 187                 /* Save dirty flags for conjugate resource programming */
 188                 dirty = hw->src_get_dirty(src->rsc.ctrl_blk) & conj_mask;
 189         }
 190         hw->src_commit_write(hw, src->rsc.ops->index(&src->rsc),
 191                                                 src->rsc.ctrl_blk);
 192
 193         /* Program conjugate parameter mixer resources */
 194         if (MEMWR == src->mode)
 195                 return 0;
 196
 197         for (i = 1; i < src->rsc.msr; i++) {
 198                 src->rsc.ops->next_conj(&src->rsc);
 199                 hw->src_set_dirty(src->rsc.ctrl_blk, dirty);
 200                 hw->src_commit_write(hw, src->rsc.ops->index(&src->rsc),
 201                                                         src->rsc.ctrl_blk);
 202         }
 203         src->rsc.ops->master(&src->rsc);
 204
 205         return 0;
 206 }
 207
 208 static int src_get_ca(struct src *src)
 209 {
 210         struct hw *hw;
 211
 212         hw = src->rsc.hw;
 213         return hw->src_get_ca(hw, src->rsc.ops->index(&src->rsc),
 214                                                 src->rsc.ctrl_blk);
 215 }
 216
 217 static int src_init(struct src *src)
 218 {
 219         src_default_config[src->mode](src);
 220
 221         return 0;
 222 }
 223
 224 static struct src *src_next_interleave(struct src *src)
 225 {
 226         return src->intlv;
 227 }
 228
 229 static int src_default_config_memrd(struct src *src)
 230 {
 231         struct hw *hw = src->rsc.hw;
 232         unsigned int rsr, msr;
 233
 234         hw->src_set_state(src->rsc.ctrl_blk, SRC_STATE_OFF);
 235         hw->src_set_bm(src->rsc.ctrl_blk, 1);
 236         for (rsr = 0, msr = src->rsc.msr; msr > 1; msr >>= 1)
 237                 rsr++;
 238
 239         hw->src_set_rsr(src->rsc.ctrl_blk, rsr);
 240         hw->src_set_sf(src->rsc.ctrl_blk, SRC_SF_S16);
 241         hw->src_set_wr(src->rsc.ctrl_blk, 0);
 242         hw->src_set_pm(src->rsc.ctrl_blk, 0);
 243         hw->src_set_rom(src->rsc.ctrl_blk, 0);
 244         hw->src_set_vo(src->rsc.ctrl_blk, 0);
 245         hw->src_set_st(src->rsc.ctrl_blk, 0);
 246         hw->src_set_ilsz(src->rsc.ctrl_blk, src->multi - 1);
 247         hw->src_set_cisz(src->rsc.ctrl_blk, 0x80);
 248         hw->src_set_sa(src->rsc.ctrl_blk, 0x0);
 249         hw->src_set_la(src->rsc.ctrl_blk, 0x1000);
 250         hw->src_set_ca(src->rsc.ctrl_blk, 0x80);
 251         hw->src_set_pitch(src->rsc.ctrl_blk, 0x1000000);
 252         hw->src_set_clear_zbufs(src->rsc.ctrl_blk, 1);
 253
 254         src->rsc.ops->master(&src->rsc);
 255         hw->src_commit_write(hw, src->rsc.ops->index(&src->rsc),
 256                                                 src->rsc.ctrl_blk);
 257
 258         for (msr = 1; msr < src->rsc.msr; msr++) {
 259                 src->rsc.ops->next_conj(&src->rsc);
 260                 hw->src_set_pitch(src->rsc.ctrl_blk, 0x1000000);
 261                 hw->src_commit_write(hw, src->rsc.ops->index(&src->rsc),
 262                                                         src->rsc.ctrl_blk);
 263         }
 264         src->rsc.ops->master(&src->rsc);
 265
 266         return 0;
 267 }
 268
 269 static int src_default_config_memwr(struct src *src)
 270 {
 271         struct hw *hw = src->rsc.hw;
 272
 273         hw->src_set_state(src->rsc.ctrl_blk, SRC_STATE_OFF);
 274         hw->src_set_bm(src->rsc.ctrl_blk, 1);
 275         hw->src_set_rsr(src->rsc.ctrl_blk, 0);
 276         hw->src_set_sf(src->rsc.ctrl_blk, SRC_SF_S16);
 277         hw->src_set_wr(src->rsc.ctrl_blk, 1);
 278         hw->src_set_pm(src->rsc.ctrl_blk, 0);
 279         hw->src_set_rom(src->rsc.ctrl_blk, 0);
 280         hw->src_set_vo(src->rsc.ctrl_blk, 0);
 281         hw->src_set_st(src->rsc.ctrl_blk, 0);
 282         hw->src_set_ilsz(src->rsc.ctrl_blk, 0);
 283         hw->src_set_cisz(src->rsc.ctrl_blk, 0x80);
 284         hw->src_set_sa(src->rsc.ctrl_blk, 0x0);
 285         hw->src_set_la(src->rsc.ctrl_blk, 0x1000);
 286         hw->src_set_ca(src->rsc.ctrl_blk, 0x80);
 287         hw->src_set_pitch(src->rsc.ctrl_blk, 0x1000000);
 288         hw->src_set_clear_zbufs(src->rsc.ctrl_blk, 1);
 289
 290         src->rsc.ops->master(&src->rsc);
 291         hw->src_commit_write(hw, src->rsc.ops->index(&src->rsc),
 292                                                 src->rsc.ctrl_blk);
 293
 294         return 0;
 295 }
 296
 297 static int src_default_config_arcrw(struct src *src)
 298 {
 299         struct hw *hw = src->rsc.hw;
 300         unsigned int rsr, msr;
 301         unsigned int dirty;
 302
 303         hw->src_set_state(src->rsc.ctrl_blk, SRC_STATE_OFF);
 304         hw->src_set_bm(src->rsc.ctrl_blk, 0);
 305         for (rsr = 0, msr = src->rsc.msr; msr > 1; msr >>= 1)
 306                 rsr++;
 307
 308         hw->src_set_rsr(src->rsc.ctrl_blk, rsr);
 309         hw->src_set_sf(src->rsc.ctrl_blk, SRC_SF_F32);
 310         hw->src_set_wr(src->rsc.ctrl_blk, 0);
 311         hw->src_set_pm(src->rsc.ctrl_blk, 0);
 312         hw->src_set_rom(src->rsc.ctrl_blk, 0);
 313         hw->src_set_vo(src->rsc.ctrl_blk, 0);
 314         hw->src_set_st(src->rsc.ctrl_blk, 0);
 315         hw->src_set_ilsz(src->rsc.ctrl_blk, 0);
 316         hw->src_set_cisz(src->rsc.ctrl_blk, 0x80);
 317         hw->src_set_sa(src->rsc.ctrl_blk, 0x0);
 318         /*hw->src_set_sa(src->rsc.ctrl_blk, 0x100);*/
 319         hw->src_set_la(src->rsc.ctrl_blk, 0x1000);
 320         /*hw->src_set_la(src->rsc.ctrl_blk, 0x03ffffe0);*/
 321         hw->src_set_ca(src->rsc.ctrl_blk, 0x80);
 322         hw->src_set_pitch(src->rsc.ctrl_blk, 0x1000000);
 323         hw->src_set_clear_zbufs(src->rsc.ctrl_blk, 1);
 324
 325         dirty = hw->src_get_dirty(src->rsc.ctrl_blk);
 326         src->rsc.ops->master(&src->rsc);
 327         for (msr = 0; msr < src->rsc.msr; msr++) {
 328                 hw->src_set_dirty(src->rsc.ctrl_blk, dirty);
 329                 hw->src_commit_write(hw, src->rsc.ops->index(&src->rsc),
 330                                                         src->rsc.ctrl_blk);
 331                 src->rsc.ops->next_conj(&src->rsc);
 332         }
 333         src->rsc.ops->master(&src->rsc);
 334
 335         return 0;
 336 }
 337
 338 static struct src_rsc_ops src_rsc_ops = {
 339         .set_state              = src_set_state,
 340         .set_bm                 = src_set_bm,
 341         .set_sf                 = src_set_sf,
 342         .set_pm                 = src_set_pm,
 343         .set_rom                = src_set_rom,
 344         .set_vo                 = src_set_vo,
 345         .set_st                 = src_set_st,
 346         .set_bp                 = src_set_bp,
 347         .set_cisz               = src_set_cisz,
 348         .set_ca                 = src_set_ca,
 349         .set_sa                 = src_set_sa,
 350         .set_la                 = src_set_la,
 351         .set_pitch              = src_set_pitch,
 352         .set_clr_zbufs          = src_set_clear_zbufs,
 353         .commit_write           = src_commit_write,
 354         .get_ca                 = src_get_ca,
 355         .init                   = src_init,
 356         .next_interleave        = src_next_interleave,
 357 };
 358
 359 static int
 360 src_rsc_init(struct src *src, u32 idx,
 361              const struct src_desc *desc, struct src_mgr *mgr)
 362 {
 363         int err;
 364         int i, n;
 365         struct src *p;
 366
 367         n = (MEMRD == desc->mode) ? desc->multi : 1;
 368         for (i = 0, p = src; i < n; i++, p++) {
 369                 err = rsc_init(&p->rsc, idx + i, SRC, desc->msr, mgr->mgr.hw);
 370                 if (err)
 371                         goto error1;
 372
 373                 /* Initialize src specific rsc operations */
 374                 p->ops = &src_rsc_ops;
 375                 p->multi = (0 == i) ? desc->multi : 1;
 376                 p->mode = desc->mode;
 377                 src_default_config[desc->mode](p);
 378                 mgr->src_enable(mgr, p);
 379                 p->intlv = p + 1;
 380         }
 381         (--p)->intlv = NULL;    /* Set @intlv of the last SRC to NULL */
 382
 383         mgr->commit_write(mgr);
 384
 385         return 0;
 386
 387 error1:
 388         for (i--, p--; i >= 0; i--, p--) {
 389                 mgr->src_disable(mgr, p);
 390                 rsc_uninit(&p->rsc);
 391         }
 392         mgr->commit_write(mgr);
 393         return err;
 394 }
 395
 396 static int src_rsc_uninit(struct src *src, struct src_mgr *mgr)
 397 {
 398         int i, n;
 399         struct src *p;
 400
 401         n = (MEMRD == src->mode) ? src->multi : 1;
 402         for (i = 0, p = src; i < n; i++, p++) {
 403                 mgr->src_disable(mgr, p);
 404                 rsc_uninit(&p->rsc);
 405                 p->multi = 0;
 406                 p->ops = NULL;
 407                 p->mode = NUM_SRCMODES;
 408                 p->intlv = NULL;
 409         }
 410         mgr->commit_write(mgr);
 411
 412         return 0;
 413 }
 414
 415 static int
 416 get_src_rsc(struct src_mgr *mgr, const struct src_desc *desc, struct src **rsrc)
 417 {
 418         unsigned int idx = SRC_RESOURCE_NUM;
 419         int err;
 420         struct src *src;
 421         unsigned long flags;
 422
 423         *rsrc = NULL;
 424
 425         /* Check whether there are sufficient src resources to meet request. */
 426         spin_lock_irqsave(&mgr->mgr_lock, flags);
 427         if (MEMRD == desc->mode)
 428                 err = mgr_get_resource(&mgr->mgr, desc->multi, &idx);
 429         else
 430                 err = mgr_get_resource(&mgr->mgr, 1, &idx);
 431
 432         spin_unlock_irqrestore(&mgr->mgr_lock, flags);
 433         if (err) {
 434                 printk(KERN_ERR "ctxfi: Can't meet SRC resource request!\n");
 435                 return err;
 436         }
 437
 438         /* Allocate mem for master src resource */
 439         if (MEMRD == desc->mode)
 440                 src = kzalloc(sizeof(*src)*desc->multi, GFP_KERNEL);
 441         else
 442                 src = kzalloc(sizeof(*src), GFP_KERNEL);
 443
 444         if (!src) {
 445                 err = -ENOMEM;
 446                 goto error1;
 447         }
 448
 449         err = src_rsc_init(src, idx, desc, mgr);
 450         if (err)
 451                 goto error2;
 452
 453         *rsrc = src;
 454
 455         return 0;
 456
 457 error2:
 458         kfree(src);
 459 error1:
 460         spin_lock_irqsave(&mgr->mgr_lock, flags);
 461         if (MEMRD == desc->mode)
 462                 mgr_put_resource(&mgr->mgr, desc->multi, idx);
 463         else
 464                 mgr_put_resource(&mgr->mgr, 1, idx);
 465
 466         spin_unlock_irqrestore(&mgr->mgr_lock, flags);
 467         return err;
 468 }
 469
 470 static int put_src_rsc(struct src_mgr *mgr, struct src *src)
 471 {
 472         unsigned long flags;
 473
 474         spin_lock_irqsave(&mgr->mgr_lock, flags);
 475         src->rsc.ops->master(&src->rsc);
 476         if (MEMRD == src->mode)
 477                 mgr_put_resource(&mgr->mgr, src->multi,
 478                                  src->rsc.ops->index(&src->rsc));
 479         else
 480                 mgr_put_resource(&mgr->mgr, 1, src->rsc.ops->index(&src->rsc));
 481
 482         spin_unlock_irqrestore(&mgr->mgr_lock, flags);
 483         src_rsc_uninit(src, mgr);
 484         kfree(src);
 485
 486         return 0;
 487 }
 488
 489 static int src_enable_s(struct src_mgr *mgr, struct src *src)
 490 {
 491         struct hw *hw = mgr->mgr.hw;
 492         int i;
 493
 494         src->rsc.ops->master(&src->rsc);
 495         for (i = 0; i < src->rsc.msr; i++) {
 496                 hw->src_mgr_enbs_src(mgr->mgr.ctrl_blk,
 497                                      src->rsc.ops->index(&src->rsc));
 498                 src->rsc.ops->next_conj(&src->rsc);
 499         }
 500         src->rsc.ops->master(&src->rsc);
 501
 502         return 0;
 503 }
 504
 505 static int src_enable(struct src_mgr *mgr, struct src *src)
 506 {
 507         struct hw *hw = mgr->mgr.hw;
 508         int i;
 509
 510         src->rsc.ops->master(&src->rsc);
 511         for (i = 0; i < src->rsc.msr; i++) {
 512                 hw->src_mgr_enb_src(mgr->mgr.ctrl_blk,
 513                                     src->rsc.ops->index(&src->rsc));
 514                 src->rsc.ops->next_conj(&src->rsc);
 515         }
 516         src->rsc.ops->master(&src->rsc);
 517
 518         return 0;
 519 }
 520
 521 static int src_disable(struct src_mgr *mgr, struct src *src)
 522 {
 523         struct hw *hw = mgr->mgr.hw;
 524         int i;
 525
 526         src->rsc.ops->master(&src->rsc);
 527         for (i = 0; i < src->rsc.msr; i++) {
 528                 hw->src_mgr_dsb_src(mgr->mgr.ctrl_blk,
 529                                     src->rsc.ops->index(&src->rsc));
 530                 src->rsc.ops->next_conj(&src->rsc);
 531         }
 532         src->rsc.ops->master(&src->rsc);
 533
 534         return 0;
 535 }
 536
 537 static int src_mgr_commit_write(struct src_mgr *mgr)
 538 {
 539         struct hw *hw = mgr->mgr.hw;
 540
 541         hw->src_mgr_commit_write(hw, mgr->mgr.ctrl_blk);
 542
 543         return 0;
 544 }
 545
 546 int src_mgr_create(void *hw, struct src_mgr **rsrc_mgr)
 547 {
 548         int err, i;
 549         struct src_mgr *src_mgr;
 550
 551         *rsrc_mgr = NULL;
 552         src_mgr = kzalloc(sizeof(*src_mgr), GFP_KERNEL);
 553         if (!src_mgr)
 554                 return -ENOMEM;
 555
 556         err = rsc_mgr_init(&src_mgr->mgr, SRC, SRC_RESOURCE_NUM, hw);
 557         if (err)
 558                 goto error1;
 559
 560         spin_lock_init(&src_mgr->mgr_lock);
 561         conj_mask = ((struct hw *)hw)->src_dirty_conj_mask();
 562
 563         src_mgr->get_src = get_src_rsc;
 564         src_mgr->put_src = put_src_rsc;
 565         src_mgr->src_enable_s = src_enable_s;
 566         src_mgr->src_enable = src_enable;
 567         src_mgr->src_disable = src_disable;
 568         src_mgr->commit_write = src_mgr_commit_write;
 569
 570         /* Disable all SRC resources. */
 571         for (i = 0; i < 256; i++)
 572                 ((struct hw *)hw)->src_mgr_dsb_src(src_mgr->mgr.ctrl_blk, i);
 573
 574         ((struct hw *)hw)->src_mgr_commit_write(hw, src_mgr->mgr.ctrl_blk);
 575
 576         *rsrc_mgr = src_mgr;
 577
 578         return 0;
 579
 580 error1:
 581         kfree(src_mgr);
 582         return err;
 583 }
 584
 585 int src_mgr_destroy(struct src_mgr *src_mgr)
 586 {
 587         rsc_mgr_uninit(&src_mgr->mgr);
 588         kfree(src_mgr);
 589
 590         return 0;
 591 }
 592
 593 /* SRCIMP resource manager operations */
 594
 595 static int srcimp_master(struct rsc *rsc)
 596 {
 597         rsc->conj = 0;
 598         return rsc->idx = container_of(rsc, struct srcimp, rsc)->idx[0];
 599 }
 600
 601 static int srcimp_next_conj(struct rsc *rsc)
 602 {
 603         rsc->conj++;
 604         return container_of(rsc, struct srcimp, rsc)->idx[rsc->conj];
 605 }
 606
 607 static int srcimp_index(const struct rsc *rsc)
 608 {
 609         return container_of(rsc, struct srcimp, rsc)->idx[rsc->conj];
 610 }
 611
 612 static struct rsc_ops srcimp_basic_rsc_ops = {
 613         .master         = srcimp_master,
 614         .next_conj      = srcimp_next_conj,
 615         .index          = srcimp_index,
 616         .output_slot    = NULL,
 617 };
 618
 619 static int srcimp_map(struct srcimp *srcimp, struct src *src, struct rsc *input)
 620 {
 621         struct imapper *entry;
 622         int i;
 623
 624         srcimp->rsc.ops->master(&srcimp->rsc);
 625         src->rsc.ops->master(&src->rsc);
 626         input->ops->master(input);
 627
 628         /* Program master and conjugate resources */
 629         for (i = 0; i < srcimp->rsc.msr; i++) {
 630                 entry = &srcimp->imappers[i];
 631                 entry->slot = input->ops->output_slot(input);
 632                 entry->user = src->rsc.ops->index(&src->rsc);
 633                 entry->addr = srcimp->rsc.ops->index(&srcimp->rsc);
 634                 srcimp->mgr->imap_add(srcimp->mgr, entry);
 635                 srcimp->mapped |= (0x1 << i);
 636
 637                 srcimp->rsc.ops->next_conj(&srcimp->rsc);
 638                 input->ops->next_conj(input);
 639         }
 640
 641         srcimp->rsc.ops->master(&srcimp->rsc);
 642         input->ops->master(input);
 643
 644         return 0;
 645 }
 646
 647 static int srcimp_unmap(struct srcimp *srcimp)
 648 {
 649         int i;
 650
 651         /* Program master and conjugate resources */
 652         for (i = 0; i < srcimp->rsc.msr; i++) {
 653                 if (srcimp->mapped & (0x1 << i)) {
 654                         srcimp->mgr->imap_delete(srcimp->mgr,
 655                                                  &srcimp->imappers[i]);
 656                         srcimp->mapped &= ~(0x1 << i);
 657                 }
 658         }
 659
 660         return 0;
 661 }
 662
 663 static struct srcimp_rsc_ops srcimp_ops = {
 664         .map = srcimp_map,
 665         .unmap = srcimp_unmap
 666 };
 667
 668 static int srcimp_rsc_init(struct srcimp *srcimp,
 669                            const struct srcimp_desc *desc,
 670                            struct srcimp_mgr *mgr)
 671 {
 672         int err;
 673
 674         err = rsc_init(&srcimp->rsc, srcimp->idx[0],
 675                        SRCIMP, desc->msr, mgr->mgr.hw);
 676         if (err)
 677                 return err;
 678
 679         /* Reserve memory for imapper nodes */
 680         srcimp->imappers = kzalloc(sizeof(struct imapper)*desc->msr,
 681                                    GFP_KERNEL);
 682         if (!srcimp->imappers) {
 683                 err = -ENOMEM;
 684                 goto error1;
 685         }
 686
 687         /* Set srcimp specific operations */
 688         srcimp->rsc.ops = &srcimp_basic_rsc_ops;
 689         srcimp->ops = &srcimp_ops;
 690         srcimp->mgr = mgr;
 691
 692         srcimp->rsc.ops->master(&srcimp->rsc);
 693
 694         return 0;
 695
 696 error1:
 697         rsc_uninit(&srcimp->rsc);
 698         return err;
 699 }
 700
 701 static int srcimp_rsc_uninit(struct srcimp *srcimp)
 702 {
 703         if (NULL != srcimp->imappers) {
 704                 kfree(srcimp->imappers);
 705                 srcimp->imappers = NULL;
 706         }
 707         srcimp->ops = NULL;
 708         srcimp->mgr = NULL;
 709         rsc_uninit(&srcimp->rsc);
 710
 711         return 0;
 712 }
 713
 714 static int get_srcimp_rsc(struct srcimp_mgr *mgr,
 715                           const struct srcimp_desc *desc,
 716                           struct srcimp **rsrcimp)
 717 {
 718         int err, i;
 719         unsigned int idx;
 720         struct srcimp *srcimp;
 721         unsigned long flags;
 722
 723         *rsrcimp = NULL;
 724
 725         /* Allocate mem for SRCIMP resource */
 726         srcimp = kzalloc(sizeof(*srcimp), GFP_KERNEL);
 727         if (!srcimp)
 728                 return -ENOMEM;
 729
 730         /* Check whether there are sufficient SRCIMP resources. */
 731         err = 0;
 732         spin_lock_irqsave(&mgr->mgr_lock, flags);
 733         for (i = 0; i < desc->msr; i++) {
 734                 err = mgr_get_resource(&mgr->mgr, 1, &idx);
 735                 if (err)
 736                         break;
 737
 738                 srcimp->idx[i] = idx;
 739         }
 740         spin_unlock_irqrestore(&mgr->mgr_lock, flags);
 741         if (err) {
 742                 printk(KERN_ERR "ctxfi: Can't meet SRCIMP resource request!\n");
 743                 goto error1;
 744         }
 745
 746         err = srcimp_rsc_init(srcimp, desc, mgr);
 747         if (err)
 748                 goto error1;
 749
 750         *rsrcimp = srcimp;
 751
 752         return 0;
 753
 754 error1:
 755         spin_lock_irqsave(&mgr->mgr_lock, flags);
 756         for (i--; i >= 0; i--)
 757                 mgr_put_resource(&mgr->mgr, 1, srcimp->idx[i]);
 758
 759         spin_unlock_irqrestore(&mgr->mgr_lock, flags);
 760         kfree(srcimp);
 761         return err;
 762 }
 763
 764 static int put_srcimp_rsc(struct srcimp_mgr *mgr, struct srcimp *srcimp)
 765 {
 766         unsigned long flags;
 767         int i;
 768
 769         spin_lock_irqsave(&mgr->mgr_lock, flags);
 770         for (i = 0; i < srcimp->rsc.msr; i++)
 771                 mgr_put_resource(&mgr->mgr, 1, srcimp->idx[i]);
 772
 773         spin_unlock_irqrestore(&mgr->mgr_lock, flags);
 774         srcimp_rsc_uninit(srcimp);
 775         kfree(srcimp);
 776
 777         return 0;
 778 }
 779
 780 static int srcimp_map_op(void *data, struct imapper *entry)
 781 {
 782         struct rsc_mgr *mgr = &((struct srcimp_mgr *)data)->mgr;
 783         struct hw *hw = mgr->hw;
 784
 785         hw->srcimp_mgr_set_imaparc(mgr->ctrl_blk, entry->slot);
 786         hw->srcimp_mgr_set_imapuser(mgr->ctrl_blk, entry->user);
 787         hw->srcimp_mgr_set_imapnxt(mgr->ctrl_blk, entry->next);
 788         hw->srcimp_mgr_set_imapaddr(mgr->ctrl_blk, entry->addr);
 789         hw->srcimp_mgr_commit_write(mgr->hw, mgr->ctrl_blk);
 790
 791         return 0;
 792 }
 793
 794 static int srcimp_imap_add(struct srcimp_mgr *mgr, struct imapper *entry)
 795 {
 796         unsigned long flags;
 797         int err;
 798
 799         spin_lock_irqsave(&mgr->imap_lock, flags);
 800         if ((0 == entry->addr) && (mgr->init_imap_added)) {
 801                 input_mapper_delete(&mgr->imappers,
 802                                     mgr->init_imap, srcimp_map_op, mgr);
 803                 mgr->init_imap_added = 0;
 804         }
 805         err = input_mapper_add(&mgr->imappers, entry, srcimp_map_op, mgr);
 806         spin_unlock_irqrestore(&mgr->imap_lock, flags);
 807
 808         return err;
 809 }
 810
 811 static int srcimp_imap_delete(struct srcimp_mgr *mgr, struct imapper *entry)
 812 {
 813         unsigned long flags;
 814         int err;
 815
 816         spin_lock_irqsave(&mgr->imap_lock, flags);
 817         err = input_mapper_delete(&mgr->imappers, entry, srcimp_map_op, mgr);
 818         if (list_empty(&mgr->imappers)) {
 819                 input_mapper_add(&mgr->imappers, mgr->init_imap,
 820                                  srcimp_map_op, mgr);
 821                 mgr->init_imap_added = 1;
 822         }
 823         spin_unlock_irqrestore(&mgr->imap_lock, flags);
 824
 825         return err;
 826 }
 827
 828 int srcimp_mgr_create(void *hw, struct srcimp_mgr **rsrcimp_mgr)
 829 {
 830         int err;
 831         struct srcimp_mgr *srcimp_mgr;
 832         struct imapper *entry;
 833
 834         *rsrcimp_mgr = NULL;
 835         srcimp_mgr = kzalloc(sizeof(*srcimp_mgr), GFP_KERNEL);
 836         if (!srcimp_mgr)
 837                 return -ENOMEM;
 838
 839         err = rsc_mgr_init(&srcimp_mgr->mgr, SRCIMP, SRCIMP_RESOURCE_NUM, hw);
 840         if (err)
 841                 goto error1;
 842
 843         spin_lock_init(&srcimp_mgr->mgr_lock);
 844         spin_lock_init(&srcimp_mgr->imap_lock);
 845         INIT_LIST_HEAD(&srcimp_mgr->imappers);
 846         entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 847         if (!entry) {
 848                 err = -ENOMEM;
 849                 goto error2;
 850         }
 851         entry->slot = entry->addr = entry->next = entry->user = 0;
 852         list_add(&entry->list, &srcimp_mgr->imappers);
 853         srcimp_mgr->init_imap = entry;
 854         srcimp_mgr->init_imap_added = 1;
 855
 856         srcimp_mgr->get_srcimp = get_srcimp_rsc;
 857         srcimp_mgr->put_srcimp = put_srcimp_rsc;
 858         srcimp_mgr->imap_add = srcimp_imap_add;
 859         srcimp_mgr->imap_delete = srcimp_imap_delete;
 860
 861         *rsrcimp_mgr = srcimp_mgr;
 862
 863         return 0;
 864
 865 error2:
 866         rsc_mgr_uninit(&srcimp_mgr->mgr);
 867 error1:
 868         kfree(srcimp_mgr);
 869         return err;
 870 }
 871
 872 int srcimp_mgr_destroy(struct srcimp_mgr *srcimp_mgr)
 873 {
 874         unsigned long flags;
 875
 876         /* free src input mapper list */
 877         spin_lock_irqsave(&srcimp_mgr->imap_lock, flags);
 878         free_input_mapper_list(&srcimp_mgr->imappers);
 879         spin_unlock_irqrestore(&srcimp_mgr->imap_lock, flags);
 880
 881         rsc_mgr_uninit(&srcimp_mgr->mgr);
 882         kfree(srcimp_mgr);
 883
 884         return 0;
 885 }