sound/pci/ctxfi/ctsrc.c

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