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 return -EOPNOTSUPP;
392
393 if ((!ccw_is_chain(ccw)) && (!ccw_is_tic(ccw)))
394 break;
395
396 ccw++;
397 } while (cnt < CCWCHAIN_LEN_MAX + 1);
398
399 if (cnt == CCWCHAIN_LEN_MAX + 1)
400 cnt = -EINVAL;
401
402 kfree(p);
403 return cnt;
404}
405
406static int tic_target_chain_exists(struct ccw1 *tic, struct channel_program *cp)
407{
408 struct ccwchain *chain;
409 u32 ccw_head, ccw_tail;
410
411 list_for_each_entry(chain, &cp->ccwchain_list, next) {
412 ccw_head = chain->ch_iova;
413 ccw_tail = ccw_head + (chain->ch_len - 1) * sizeof(struct ccw1);
414
415 if ((ccw_head <= tic->cda) && (tic->cda <= ccw_tail))
416 return 1;
417 }
418
419 return 0;
420}
421
422static int ccwchain_loop_tic(struct ccwchain *chain,
423 struct channel_program *cp);
424
425static int ccwchain_handle_tic(struct ccw1 *tic, struct channel_program *cp)
426{
427 struct ccwchain *chain;
428 int len, ret;
429
430
431 if (tic_target_chain_exists(tic, cp))
432 return 0;
433
434
435 len = ccwchain_calc_length(tic->cda, cp);
436 if (len < 0)
437 return len;
438
439
440 chain = ccwchain_alloc(cp, len);
441 if (!chain)
442 return -ENOMEM;
443 chain->ch_iova = tic->cda;
444
445
446 ret = copy_ccw_from_iova(cp, chain->ch_ccw, tic->cda, len);
447 if (ret) {
448 ccwchain_free(chain);
449 return ret;
450 }
451
452
453 return ccwchain_loop_tic(chain, cp);
454}
455
456
457static int ccwchain_loop_tic(struct ccwchain *chain, struct channel_program *cp)
458{
459 struct ccw1 *tic;
460 int i, ret;
461
462 for (i = 0; i < chain->ch_len; i++) {
463 tic = chain->ch_ccw + i;
464
465 if (!ccw_is_tic(tic))
466 continue;
467
468 ret = ccwchain_handle_tic(tic, cp);
469 if (ret)
470 return ret;
471 }
472
473 return 0;
474}
475
476static int ccwchain_fetch_tic(struct ccwchain *chain,
477 int idx,
478 struct channel_program *cp)
479{
480 struct ccw1 *ccw = chain->ch_ccw + idx;
481 struct ccwchain *iter;
482 u32 ccw_head, ccw_tail;
483
484 list_for_each_entry(iter, &cp->ccwchain_list, next) {
485 ccw_head = iter->ch_iova;
486 ccw_tail = ccw_head + (iter->ch_len - 1) * sizeof(struct ccw1);
487
488 if ((ccw_head <= ccw->cda) && (ccw->cda <= ccw_tail)) {
489 ccw->cda = (__u32) (addr_t) (((char *)iter->ch_ccw) +
490 (ccw->cda - ccw_head));
491 return 0;
492 }
493 }
494
495 return -EFAULT;
496}
497
498static int ccwchain_fetch_direct(struct ccwchain *chain,
499 int idx,
500 struct channel_program *cp)
501{
502 struct ccw1 *ccw;
503 struct pfn_array_table *pat;
504 unsigned long *idaws;
505 int ret;
506
507 ccw = chain->ch_ccw + idx;
508
509 if (!ccw->count) {
510
511
512
513
514
515 ccw->flags |= CCW_FLAG_IDA;
516 return 0;
517 }
518
519
520
521
522
523
524 pat = chain->ch_pat + idx;
525 ret = pfn_array_table_init(pat, 1);
526 if (ret)
527 goto out_init;
528
529 ret = pfn_array_alloc_pin(pat->pat_pa, cp->mdev, ccw->cda, ccw->count);
530 if (ret < 0)
531 goto out_init;
532
533
534 idaws = kcalloc(ret, sizeof(*idaws), GFP_DMA | GFP_KERNEL);
535 if (!idaws) {
536 ret = -ENOMEM;
537 goto out_unpin;
538 }
539 ccw->cda = (__u32) virt_to_phys(idaws);
540 ccw->flags |= CCW_FLAG_IDA;
541
542 pfn_array_table_idal_create_words(pat, idaws);
543
544 return 0;
545
546out_unpin:
547 pfn_array_table_unpin_free(pat, cp->mdev);
548out_init:
549 ccw->cda = 0;
550 return ret;
551}
552
553static int ccwchain_fetch_idal(struct ccwchain *chain,
554 int idx,
555 struct channel_program *cp)
556{
557 struct ccw1 *ccw;
558 struct pfn_array_table *pat;
559 unsigned long *idaws;
560 u64 idaw_iova;
561 unsigned int idaw_nr, idaw_len;
562 int i, ret;
563
564 ccw = chain->ch_ccw + idx;
565
566 if (!ccw->count)
567 return 0;
568
569
570 ret = copy_from_iova(cp->mdev, &idaw_iova, ccw->cda, sizeof(idaw_iova));
571 if (ret)
572 return ret;
573 idaw_nr = idal_nr_words((void *)(idaw_iova), ccw->count);
574 idaw_len = idaw_nr * sizeof(*idaws);
575
576
577 pat = chain->ch_pat + idx;
578 ret = pfn_array_table_init(pat, idaw_nr);
579 if (ret)
580 goto out_init;
581
582
583 idaws = kzalloc(idaw_len, GFP_DMA | GFP_KERNEL);
584 if (!idaws) {
585 ret = -ENOMEM;
586 goto out_unpin;
587 }
588
589 ret = copy_from_iova(cp->mdev, idaws, ccw->cda, idaw_len);
590 if (ret)
591 goto out_free_idaws;
592
593 ccw->cda = virt_to_phys(idaws);
594
595 for (i = 0; i < idaw_nr; i++) {
596 idaw_iova = *(idaws + i);
597
598 ret = pfn_array_alloc_pin(pat->pat_pa + i, cp->mdev,
599 idaw_iova, 1);
600 if (ret < 0)
601 goto out_free_idaws;
602 }
603
604 pfn_array_table_idal_create_words(pat, idaws);
605
606 return 0;
607
608out_free_idaws:
609 kfree(idaws);
610out_unpin:
611 pfn_array_table_unpin_free(pat, cp->mdev);
612out_init:
613 ccw->cda = 0;
614 return ret;
615}
616
617
618
619
620
621
622
623static int ccwchain_fetch_one(struct ccwchain *chain,
624 int idx,
625 struct channel_program *cp)
626{
627 struct ccw1 *ccw = chain->ch_ccw + idx;
628
629 if (ccw_is_test(ccw) || ccw_is_noop(ccw))
630 return 0;
631
632 if (ccw_is_tic(ccw))
633 return ccwchain_fetch_tic(chain, idx, cp);
634
635 if (ccw_is_idal(ccw))
636 return ccwchain_fetch_idal(chain, idx, cp);
637
638 return ccwchain_fetch_direct(chain, idx, cp);
639}
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658int cp_init(struct channel_program *cp, struct device *mdev, union orb *orb)
659{
660 u64 iova = orb->cmd.cpa;
661 struct ccwchain *chain;
662 int len, ret;
663
664
665
666
667
668 if (!orb->cmd.pfch)
669 return -EOPNOTSUPP;
670
671 INIT_LIST_HEAD(&cp->ccwchain_list);
672 memcpy(&cp->orb, orb, sizeof(*orb));
673 cp->mdev = mdev;
674
675
676 len = ccwchain_calc_length(iova, cp);
677 if (len < 0)
678 return len;
679
680
681 chain = ccwchain_alloc(cp, len);
682 if (!chain)
683 return -ENOMEM;
684 chain->ch_iova = iova;
685
686
687 ret = copy_ccw_from_iova(cp, chain->ch_ccw, iova, len);
688 if (ret) {
689 ccwchain_free(chain);
690 return ret;
691 }
692
693
694 ret = ccwchain_loop_tic(chain, cp);
695 if (ret)
696 cp_unpin_free(cp);
697
698
699
700 cp->orb.cmd.c64 = 1;
701
702 return ret;
703}
704
705
706
707
708
709
710
711
712
713
714void cp_free(struct channel_program *cp)
715{
716 cp_unpin_free(cp);
717}
718
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
756int cp_prefetch(struct channel_program *cp)
757{
758 struct ccwchain *chain;
759 int len, idx, ret;
760
761 list_for_each_entry(chain, &cp->ccwchain_list, next) {
762 len = chain->ch_len;
763 for (idx = 0; idx < len; idx++) {
764 ret = ccwchain_fetch_one(chain, idx, cp);
765 if (ret)
766 goto out_err;
767 }
768 }
769
770 return 0;
771out_err:
772
773 chain->ch_len = idx;
774 list_for_each_entry_continue(chain, &cp->ccwchain_list, next) {
775 chain->ch_len = 0;
776 }
777 return ret;
778}
779
780
781
782
783
784
785
786
787
788
789
790union orb *cp_get_orb(struct channel_program *cp, u32 intparm, u8 lpm)
791{
792 union orb *orb;
793 struct ccwchain *chain;
794 struct ccw1 *cpa;
795
796 orb = &cp->orb;
797
798 orb->cmd.intparm = intparm;
799 orb->cmd.fmt = 1;
800 orb->cmd.key = PAGE_DEFAULT_KEY >> 4;
801
802 if (orb->cmd.lpm == 0)
803 orb->cmd.lpm = lpm;
804
805 chain = list_first_entry(&cp->ccwchain_list, struct ccwchain, next);
806 cpa = chain->ch_ccw;
807 orb->cmd.cpa = (__u32) __pa(cpa);
808
809 return orb;
810}
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826void cp_update_scsw(struct channel_program *cp, union scsw *scsw)
827{
828 struct ccwchain *chain;
829 u32 cpa = scsw->cmd.cpa;
830 u32 ccw_head, ccw_tail;
831
832
833
834
835
836
837
838 list_for_each_entry(chain, &cp->ccwchain_list, next) {
839 ccw_head = (u32)(u64)chain->ch_ccw;
840 ccw_tail = (u32)(u64)(chain->ch_ccw + chain->ch_len - 1);
841
842 if ((ccw_head <= cpa) && (cpa <= ccw_tail)) {
843
844
845
846
847
848
849 cpa = chain->ch_iova + (cpa - ccw_head);
850 break;
851 }
852 }
853
854 scsw->cmd.cpa = cpa;
855}
856
857
858
859
860
861
862
863
864
865bool cp_iova_pinned(struct channel_program *cp, u64 iova)
866{
867 struct ccwchain *chain;
868 int i;
869
870 list_for_each_entry(chain, &cp->ccwchain_list, next) {
871 for (i = 0; i < chain->ch_len; i++)
872 if (pfn_array_table_iova_pinned(chain->ch_pat + i,
873 iova))
874 return true;
875 }
876
877 return false;
878}
879