1
2
3
4
5
6
7
8
9
10
11#include <linux/mm.h>
12#include <linux/slab.h>
13#include <linux/iommu.h>
14#include <linux/vfio.h>
15#include <asm/idals.h>
16
17#include "vfio_ccw_cp.h"
18
19
20
21
22
23#define CCWCHAIN_LEN_MAX 256
24
25struct pfn_array {
26
27 unsigned long pa_iova;
28
29 unsigned long *pa_iova_pfn;
30
31 unsigned long *pa_pfn;
32
33 int pa_nr;
34};
35
36struct pfn_array_table {
37 struct pfn_array *pat_pa;
38 int pat_nr;
39};
40
41struct ccwchain {
42 struct list_head next;
43 struct ccw1 *ch_ccw;
44
45 u64 ch_iova;
46
47 int ch_len;
48
49 struct pfn_array_table *ch_pat;
50};
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71static int pfn_array_alloc_pin(struct pfn_array *pa, struct device *mdev,
72 u64 iova, unsigned int len)
73{
74 int i, ret = 0;
75
76 if (!len)
77 return 0;
78
79 if (pa->pa_nr || pa->pa_iova_pfn)
80 return -EINVAL;
81
82 pa->pa_iova = iova;
83
84 pa->pa_nr = ((iova & ~PAGE_MASK) + len + (PAGE_SIZE - 1)) >> PAGE_SHIFT;
85 if (!pa->pa_nr)
86 return -EINVAL;
87
88 pa->pa_iova_pfn = kcalloc(pa->pa_nr,
89 sizeof(*pa->pa_iova_pfn) +
90 sizeof(*pa->pa_pfn),
91 GFP_KERNEL);
92 if (unlikely(!pa->pa_iova_pfn))
93 return -ENOMEM;
94 pa->pa_pfn = pa->pa_iova_pfn + pa->pa_nr;
95
96 pa->pa_iova_pfn[0] = pa->pa_iova >> PAGE_SHIFT;
97 for (i = 1; i < pa->pa_nr; i++)
98 pa->pa_iova_pfn[i] = pa->pa_iova_pfn[i - 1] + 1;
99
100 ret = vfio_pin_pages(mdev, pa->pa_iova_pfn, pa->pa_nr,
101 IOMMU_READ | IOMMU_WRITE, pa->pa_pfn);
102
103 if (ret < 0) {
104 goto err_out;
105 } else if (ret > 0 && ret != pa->pa_nr) {
106 vfio_unpin_pages(mdev, pa->pa_iova_pfn, ret);
107 ret = -EINVAL;
108 goto err_out;
109 }
110
111 return ret;
112
113err_out:
114 pa->pa_nr = 0;
115 kfree(pa->pa_iova_pfn);
116 pa->pa_iova_pfn = NULL;
117
118 return ret;
119}
120
121
122static void pfn_array_unpin_free(struct pfn_array *pa, struct device *mdev)
123{
124 vfio_unpin_pages(mdev, pa->pa_iova_pfn, pa->pa_nr);
125 pa->pa_nr = 0;
126 kfree(pa->pa_iova_pfn);
127}
128
129static int pfn_array_table_init(struct pfn_array_table *pat, int nr)
130{
131 pat->pat_pa = kcalloc(nr, sizeof(*pat->pat_pa), GFP_KERNEL);
132 if (unlikely(ZERO_OR_NULL_PTR(pat->pat_pa))) {
133 pat->pat_nr = 0;
134 return -ENOMEM;
135 }
136
137 pat->pat_nr = nr;
138
139 return 0;
140}
141
142static void pfn_array_table_unpin_free(struct pfn_array_table *pat,
143 struct device *mdev)
144{
145 int i;
146
147 for (i = 0; i < pat->pat_nr; i++)
148 pfn_array_unpin_free(pat->pat_pa + i, mdev);
149
150 if (pat->pat_nr) {
151 kfree(pat->pat_pa);
152 pat->pat_pa = NULL;
153 pat->pat_nr = 0;
154 }
155}
156
157static bool pfn_array_table_iova_pinned(struct pfn_array_table *pat,
158 unsigned long iova)
159{
160 struct pfn_array *pa = pat->pat_pa;
161 unsigned long iova_pfn = iova >> PAGE_SHIFT;
162 int i, j;
163
164 for (i = 0; i < pat->pat_nr; i++, pa++)
165 for (j = 0; j < pa->pa_nr; j++)
166 if (pa->pa_iova_pfn[j] == iova_pfn)
167 return true;
168
169 return false;
170}
171
172static inline void pfn_array_table_idal_create_words(
173 struct pfn_array_table *pat,
174 unsigned long *idaws)
175{
176 struct pfn_array *pa;
177 int i, j, k;
178
179
180
181
182
183
184
185
186 k = 0;
187 for (i = 0; i < pat->pat_nr; i++) {
188 pa = pat->pat_pa + i;
189 for (j = 0; j < pa->pa_nr; j++) {
190 idaws[k] = pa->pa_pfn[j] << PAGE_SHIFT;
191 if (k == 0)
192 idaws[k] += pa->pa_iova & (PAGE_SIZE - 1);
193 k++;
194 }
195 }
196}
197
198
199
200
201
202
203static long copy_from_iova(struct device *mdev,
204 void *to, u64 iova,
205 unsigned long n)
206{
207 struct pfn_array pa = {0};
208 u64 from;
209 int i, ret;
210 unsigned long l, m;
211
212 ret = pfn_array_alloc_pin(&pa, mdev, iova, n);
213 if (ret <= 0)
214 return ret;
215
216 l = n;
217 for (i = 0; i < pa.pa_nr; i++) {
218 from = pa.pa_pfn[i] << PAGE_SHIFT;
219 m = PAGE_SIZE;
220 if (i == 0) {
221 from += iova & (PAGE_SIZE - 1);
222 m -= iova & (PAGE_SIZE - 1);
223 }
224
225 m = min(l, m);
226 memcpy(to + (n - l), (void *)from, m);
227
228 l -= m;
229 if (l == 0)
230 break;
231 }
232
233 pfn_array_unpin_free(&pa, mdev);
234
235 return l;
236}
237
238static long copy_ccw_from_iova(struct channel_program *cp,
239 struct ccw1 *to, u64 iova,
240 unsigned long len)
241{
242 struct ccw0 ccw0;
243 struct ccw1 *pccw1;
244 int ret;
245 int i;
246
247 ret = copy_from_iova(cp->mdev, to, iova, len * sizeof(struct ccw1));
248 if (ret)
249 return ret;
250
251 if (!cp->orb.cmd.fmt) {
252 pccw1 = to;
253 for (i = 0; i < len; i++) {
254 ccw0 = *(struct ccw0 *)pccw1;
255 if ((pccw1->cmd_code & 0x0f) == CCW_CMD_TIC) {
256 pccw1->cmd_code = CCW_CMD_TIC;
257 pccw1->flags = 0;
258 pccw1->count = 0;
259 } else {
260 pccw1->cmd_code = ccw0.cmd_code;
261 pccw1->flags = ccw0.flags;
262 pccw1->count = ccw0.count;
263 }
264 pccw1->cda = ccw0.cda;
265 pccw1++;
266 }
267 }
268
269 return ret;
270}
271
272
273
274
275#define ccw_is_test(_ccw) (((_ccw)->cmd_code & 0x0F) == 0)
276
277#define ccw_is_noop(_ccw) ((_ccw)->cmd_code == CCW_CMD_NOOP)
278
279#define ccw_is_tic(_ccw) ((_ccw)->cmd_code == CCW_CMD_TIC)
280
281#define ccw_is_idal(_ccw) ((_ccw)->flags & CCW_FLAG_IDA)
282
283
284#define ccw_is_chain(_ccw) ((_ccw)->flags & (CCW_FLAG_CC | CCW_FLAG_DC))
285
286static struct ccwchain *ccwchain_alloc(struct channel_program *cp, int len)
287{
288 struct ccwchain *chain;
289 void *data;
290 size_t size;
291
292
293 size = ((sizeof(*chain) + 7L) & -8L) +
294 sizeof(*chain->ch_ccw) * len +
295 sizeof(*chain->ch_pat) * len;
296 chain = kzalloc(size, GFP_DMA | GFP_KERNEL);
297 if (!chain)
298 return NULL;
299
300 data = (u8 *)chain + ((sizeof(*chain) + 7L) & -8L);
301 chain->ch_ccw = (struct ccw1 *)data;
302
303 data = (u8 *)(chain->ch_ccw) + sizeof(*chain->ch_ccw) * len;
304 chain->ch_pat = (struct pfn_array_table *)data;
305
306 chain->ch_len = len;
307
308 list_add_tail(&chain->next, &cp->ccwchain_list);
309
310 return chain;
311}
312
313static void ccwchain_free(struct ccwchain *chain)
314{
315 list_del(&chain->next);
316 kfree(chain);
317}
318
319
320static void ccwchain_cda_free(struct ccwchain *chain, int idx)
321{
322 struct ccw1 *ccw = chain->ch_ccw + idx;
323
324 if (ccw_is_test(ccw) || ccw_is_noop(ccw) || ccw_is_tic(ccw))
325 return;
326 if (!ccw->count)
327 return;
328
329 kfree((void *)(u64)ccw->cda);
330}
331
332
333static void cp_unpin_free(struct channel_program *cp)
334{
335 struct ccwchain *chain, *temp;
336 int i;
337
338 list_for_each_entry_safe(chain, temp, &cp->ccwchain_list, next) {
339 for (i = 0; i < chain->ch_len; i++) {
340 pfn_array_table_unpin_free(chain->ch_pat + i,
341 cp->mdev);
342 ccwchain_cda_free(chain, i);
343 }
344 ccwchain_free(chain);
345 }
346}
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361static int ccwchain_calc_length(u64 iova, struct channel_program *cp)
362{
363 struct ccw1 *ccw, *p;
364 int cnt;
365
366
367
368
369
370
371 p = ccw = kcalloc(CCWCHAIN_LEN_MAX, sizeof(*ccw), GFP_KERNEL);
372 if (!ccw)
373 return -ENOMEM;
374
375 cnt = copy_ccw_from_iova(cp, ccw, iova, CCWCHAIN_LEN_MAX);
376 if (cnt) {
377 kfree(ccw);
378 return cnt;
379 }
380
381 cnt = 0;
382 do {
383 cnt++;
384
385
386
387
388
389
390 if ((!cp->orb.cmd.c64 || cp->orb.cmd.i2k) && ccw_is_idal(ccw)) {
391 kfree(p);
392 return -EOPNOTSUPP;
393 }
394
395 if ((!ccw_is_chain(ccw)) && (!ccw_is_tic(ccw)))
396 break;
397
398 ccw++;
399 } while (cnt < CCWCHAIN_LEN_MAX + 1);
400
401 if (cnt == CCWCHAIN_LEN_MAX + 1)
402 cnt = -EINVAL;
403
404 kfree(p);
405 return cnt;
406}
407
408static int tic_target_chain_exists(struct ccw1 *tic, struct channel_program *cp)
409{
410 struct ccwchain *chain;
411 u32 ccw_head, ccw_tail;
412
413 list_for_each_entry(chain, &cp->ccwchain_list, next) {
414 ccw_head = chain->ch_iova;
415 ccw_tail = ccw_head + (chain->ch_len - 1) * sizeof(struct ccw1);
416
417 if ((ccw_head <= tic->cda) && (tic->cda <= ccw_tail))
418 return 1;
419 }
420
421 return 0;
422}
423
424static int ccwchain_loop_tic(struct ccwchain *chain,
425 struct channel_program *cp);
426
427static int ccwchain_handle_tic(struct ccw1 *tic, struct channel_program *cp)
428{
429 struct ccwchain *chain;
430 int len, ret;
431
432
433 if (tic_target_chain_exists(tic, cp))
434 return 0;
435
436
437 len = ccwchain_calc_length(tic->cda, cp);
438 if (len < 0)
439 return len;
440
441
442 chain = ccwchain_alloc(cp, len);
443 if (!chain)
444 return -ENOMEM;
445 chain->ch_iova = tic->cda;
446
447
448 ret = copy_ccw_from_iova(cp, chain->ch_ccw, tic->cda, len);
449 if (ret) {
450 ccwchain_free(chain);
451 return ret;
452 }
453
454
455 return ccwchain_loop_tic(chain, cp);
456}
457
458
459static int ccwchain_loop_tic(struct ccwchain *chain, struct channel_program *cp)
460{
461 struct ccw1 *tic;
462 int i, ret;
463
464 for (i = 0; i < chain->ch_len; i++) {
465 tic = chain->ch_ccw + i;
466
467 if (!ccw_is_tic(tic))
468 continue;
469
470 ret = ccwchain_handle_tic(tic, cp);
471 if (ret)
472 return ret;
473 }
474
475 return 0;
476}
477
478static int ccwchain_fetch_tic(struct ccwchain *chain,
479 int idx,
480 struct channel_program *cp)
481{
482 struct ccw1 *ccw = chain->ch_ccw + idx;
483 struct ccwchain *iter;
484 u32 ccw_head, ccw_tail;
485
486 list_for_each_entry(iter, &cp->ccwchain_list, next) {
487 ccw_head = iter->ch_iova;
488 ccw_tail = ccw_head + (iter->ch_len - 1) * sizeof(struct ccw1);
489
490 if ((ccw_head <= ccw->cda) && (ccw->cda <= ccw_tail)) {
491 ccw->cda = (__u32) (addr_t) (((char *)iter->ch_ccw) +
492 (ccw->cda - ccw_head));
493 return 0;
494 }
495 }
496
497 return -EFAULT;
498}
499
500static int ccwchain_fetch_direct(struct ccwchain *chain,
501 int idx,
502 struct channel_program *cp)
503{
504 struct ccw1 *ccw;
505 struct pfn_array_table *pat;
506 unsigned long *idaws;
507 int ret;
508
509 ccw = chain->ch_ccw + idx;
510
511 if (!ccw->count) {
512
513
514
515
516
517 ccw->flags |= CCW_FLAG_IDA;
518 return 0;
519 }
520
521
522
523
524
525
526 pat = chain->ch_pat + idx;
527 ret = pfn_array_table_init(pat, 1);
528 if (ret)
529 goto out_init;
530
531 ret = pfn_array_alloc_pin(pat->pat_pa, cp->mdev, ccw->cda, ccw->count);
532 if (ret < 0)
533 goto out_unpin;
534
535
536 idaws = kcalloc(ret, sizeof(*idaws), GFP_DMA | GFP_KERNEL);
537 if (!idaws) {
538 ret = -ENOMEM;
539 goto out_unpin;
540 }
541 ccw->cda = (__u32) virt_to_phys(idaws);
542 ccw->flags |= CCW_FLAG_IDA;
543
544 pfn_array_table_idal_create_words(pat, idaws);
545
546 return 0;
547
548out_unpin:
549 pfn_array_table_unpin_free(pat, cp->mdev);
550out_init:
551 ccw->cda = 0;
552 return ret;
553}
554
555static int ccwchain_fetch_idal(struct ccwchain *chain,
556 int idx,
557 struct channel_program *cp)
558{
559 struct ccw1 *ccw;
560 struct pfn_array_table *pat;
561 unsigned long *idaws;
562 u64 idaw_iova;
563 unsigned int idaw_nr, idaw_len;
564 int i, ret;
565
566 ccw = chain->ch_ccw + idx;
567
568 if (!ccw->count)
569 return 0;
570
571
572 ret = copy_from_iova(cp->mdev, &idaw_iova, ccw->cda, sizeof(idaw_iova));
573 if (ret)
574 return ret;
575 idaw_nr = idal_nr_words((void *)(idaw_iova), ccw->count);
576 idaw_len = idaw_nr * sizeof(*idaws);
577
578
579 pat = chain->ch_pat + idx;
580 ret = pfn_array_table_init(pat, idaw_nr);
581 if (ret)
582 goto out_init;
583
584
585 idaws = kzalloc(idaw_len, GFP_DMA | GFP_KERNEL);
586 if (!idaws) {
587 ret = -ENOMEM;
588 goto out_unpin;
589 }
590
591 ret = copy_from_iova(cp->mdev, idaws, ccw->cda, idaw_len);
592 if (ret)
593 goto out_free_idaws;
594
595 ccw->cda = virt_to_phys(idaws);
596
597 for (i = 0; i < idaw_nr; i++) {
598 idaw_iova = *(idaws + i);
599
600 ret = pfn_array_alloc_pin(pat->pat_pa + i, cp->mdev,
601 idaw_iova, 1);
602 if (ret < 0)
603 goto out_free_idaws;
604 }
605
606 pfn_array_table_idal_create_words(pat, idaws);
607
608 return 0;
609
610out_free_idaws:
611 kfree(idaws);
612out_unpin:
613 pfn_array_table_unpin_free(pat, cp->mdev);
614out_init:
615 ccw->cda = 0;
616 return ret;
617}
618
619
620
621
622
623
624
625static int ccwchain_fetch_one(struct ccwchain *chain,
626 int idx,
627 struct channel_program *cp)
628{
629 struct ccw1 *ccw = chain->ch_ccw + idx;
630
631 if (ccw_is_test(ccw) || ccw_is_noop(ccw))
632 return 0;
633
634 if (ccw_is_tic(ccw))
635 return ccwchain_fetch_tic(chain, idx, cp);
636
637 if (ccw_is_idal(ccw))
638 return ccwchain_fetch_idal(chain, idx, cp);
639
640 return ccwchain_fetch_direct(chain, idx, cp);
641}
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660int cp_init(struct channel_program *cp, struct device *mdev, union orb *orb)
661{
662 u64 iova = orb->cmd.cpa;
663 struct ccwchain *chain;
664 int len, ret;
665
666
667
668
669
670 if (!orb->cmd.pfch)
671 return -EOPNOTSUPP;
672
673 INIT_LIST_HEAD(&cp->ccwchain_list);
674 memcpy(&cp->orb, orb, sizeof(*orb));
675 cp->mdev = mdev;
676
677
678 len = ccwchain_calc_length(iova, cp);
679 if (len < 0)
680 return len;
681
682
683 chain = ccwchain_alloc(cp, len);
684 if (!chain)
685 return -ENOMEM;
686 chain->ch_iova = iova;
687
688
689 ret = copy_ccw_from_iova(cp, chain->ch_ccw, iova, len);
690 if (ret) {
691 ccwchain_free(chain);
692 return ret;
693 }
694
695
696 ret = ccwchain_loop_tic(chain, cp);
697 if (ret)
698 cp_unpin_free(cp);
699
700
701
702 cp->orb.cmd.c64 = 1;
703
704 return ret;
705}
706
707
708
709
710
711
712
713
714
715
716void cp_free(struct channel_program *cp)
717{
718 cp_unpin_free(cp);
719}
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758int cp_prefetch(struct channel_program *cp)
759{
760 struct ccwchain *chain;
761 int len, idx, ret;
762
763 list_for_each_entry(chain, &cp->ccwchain_list, next) {
764 len = chain->ch_len;
765 for (idx = 0; idx < len; idx++) {
766 ret = ccwchain_fetch_one(chain, idx, cp);
767 if (ret)
768 goto out_err;
769 }
770 }
771
772 return 0;
773out_err:
774
775 chain->ch_len = idx;
776 list_for_each_entry_continue(chain, &cp->ccwchain_list, next) {
777 chain->ch_len = 0;
778 }
779 return ret;
780}
781
782
783
784
785
786
787
788
789
790
791
792union orb *cp_get_orb(struct channel_program *cp, u32 intparm, u8 lpm)
793{
794 union orb *orb;
795 struct ccwchain *chain;
796 struct ccw1 *cpa;
797
798 orb = &cp->orb;
799
800 orb->cmd.intparm = intparm;
801 orb->cmd.fmt = 1;
802 orb->cmd.key = PAGE_DEFAULT_KEY >> 4;
803
804 if (orb->cmd.lpm == 0)
805 orb->cmd.lpm = lpm;
806
807 chain = list_first_entry(&cp->ccwchain_list, struct ccwchain, next);
808 cpa = chain->ch_ccw;
809 orb->cmd.cpa = (__u32) __pa(cpa);
810
811 return orb;
812}
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828void cp_update_scsw(struct channel_program *cp, union scsw *scsw)
829{
830 struct ccwchain *chain;
831 u32 cpa = scsw->cmd.cpa;
832 u32 ccw_head, ccw_tail;
833
834
835
836
837
838
839
840 list_for_each_entry(chain, &cp->ccwchain_list, next) {
841 ccw_head = (u32)(u64)chain->ch_ccw;
842 ccw_tail = (u32)(u64)(chain->ch_ccw + chain->ch_len - 1);
843
844 if ((ccw_head <= cpa) && (cpa <= ccw_tail)) {
845
846
847
848
849
850
851 cpa = chain->ch_iova + (cpa - ccw_head);
852 break;
853 }
854 }
855
856 scsw->cmd.cpa = cpa;
857}
858
859
860
861
862
863
864
865
866
867bool cp_iova_pinned(struct channel_program *cp, u64 iova)
868{
869 struct ccwchain *chain;
870 int i;
871
872 list_for_each_entry(chain, &cp->ccwchain_list, next) {
873 for (i = 0; i < chain->ch_len; i++)
874 if (pfn_array_table_iova_pinned(chain->ch_pat + i,
875 iova))
876 return true;
877 }
878
879 return false;
880}
881