arm-device.c 4.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Copyright (c) 2015, Linaro Limited, Shannon Zhao
  4. */
  5. #include <linux/platform_device.h>
  6. #include <linux/acpi.h>
  7. #include <xen/xen.h>
  8. #include <xen/page.h>
  9. #include <xen/interface/memory.h>
  10. #include <asm/xen/hypervisor.h>
  11. #include <asm/xen/hypercall.h>
  12. static int xen_unmap_device_mmio(const struct resource *resources,
  13. unsigned int count)
  14. {
  15. unsigned int i, j, nr;
  16. int rc = 0;
  17. const struct resource *r;
  18. struct xen_remove_from_physmap xrp;
  19. for (i = 0; i < count; i++) {
  20. r = &resources[i];
  21. nr = DIV_ROUND_UP(resource_size(r), XEN_PAGE_SIZE);
  22. if ((resource_type(r) != IORESOURCE_MEM) || (nr == 0))
  23. continue;
  24. for (j = 0; j < nr; j++) {
  25. xrp.domid = DOMID_SELF;
  26. xrp.gpfn = XEN_PFN_DOWN(r->start) + j;
  27. rc = HYPERVISOR_memory_op(XENMEM_remove_from_physmap,
  28. &xrp);
  29. if (rc)
  30. return rc;
  31. }
  32. }
  33. return rc;
  34. }
  35. static int xen_map_device_mmio(const struct resource *resources,
  36. unsigned int count)
  37. {
  38. unsigned int i, j, nr;
  39. int rc = 0;
  40. const struct resource *r;
  41. xen_pfn_t *gpfns;
  42. xen_ulong_t *idxs;
  43. int *errs;
  44. for (i = 0; i < count; i++) {
  45. struct xen_add_to_physmap_range xatp = {
  46. .domid = DOMID_SELF,
  47. .space = XENMAPSPACE_dev_mmio
  48. };
  49. r = &resources[i];
  50. nr = DIV_ROUND_UP(resource_size(r), XEN_PAGE_SIZE);
  51. if ((resource_type(r) != IORESOURCE_MEM) || (nr == 0))
  52. continue;
  53. gpfns = kcalloc(nr, sizeof(xen_pfn_t), GFP_KERNEL);
  54. idxs = kcalloc(nr, sizeof(xen_ulong_t), GFP_KERNEL);
  55. errs = kcalloc(nr, sizeof(int), GFP_KERNEL);
  56. if (!gpfns || !idxs || !errs) {
  57. kfree(gpfns);
  58. kfree(idxs);
  59. kfree(errs);
  60. rc = -ENOMEM;
  61. goto unmap;
  62. }
  63. for (j = 0; j < nr; j++) {
  64. /*
  65. * The regions are always mapped 1:1 to DOM0 and this is
  66. * fine because the memory map for DOM0 is the same as
  67. * the host (except for the RAM).
  68. */
  69. gpfns[j] = XEN_PFN_DOWN(r->start) + j;
  70. idxs[j] = XEN_PFN_DOWN(r->start) + j;
  71. }
  72. xatp.size = nr;
  73. set_xen_guest_handle(xatp.gpfns, gpfns);
  74. set_xen_guest_handle(xatp.idxs, idxs);
  75. set_xen_guest_handle(xatp.errs, errs);
  76. rc = HYPERVISOR_memory_op(XENMEM_add_to_physmap_range, &xatp);
  77. kfree(gpfns);
  78. kfree(idxs);
  79. kfree(errs);
  80. if (rc)
  81. goto unmap;
  82. }
  83. return rc;
  84. unmap:
  85. xen_unmap_device_mmio(resources, i);
  86. return rc;
  87. }
  88. static int xen_platform_notifier(struct notifier_block *nb,
  89. unsigned long action, void *data)
  90. {
  91. struct platform_device *pdev = to_platform_device(data);
  92. int r = 0;
  93. if (pdev->num_resources == 0 || pdev->resource == NULL)
  94. return NOTIFY_OK;
  95. switch (action) {
  96. case BUS_NOTIFY_ADD_DEVICE:
  97. r = xen_map_device_mmio(pdev->resource, pdev->num_resources);
  98. break;
  99. case BUS_NOTIFY_DEL_DEVICE:
  100. r = xen_unmap_device_mmio(pdev->resource, pdev->num_resources);
  101. break;
  102. default:
  103. return NOTIFY_DONE;
  104. }
  105. if (r)
  106. dev_err(&pdev->dev, "Platform: Failed to %s device %s MMIO!\n",
  107. action == BUS_NOTIFY_ADD_DEVICE ? "map" :
  108. (action == BUS_NOTIFY_DEL_DEVICE ? "unmap" : "?"),
  109. pdev->name);
  110. return NOTIFY_OK;
  111. }
  112. static struct notifier_block platform_device_nb = {
  113. .notifier_call = xen_platform_notifier,
  114. };
  115. static int __init register_xen_platform_notifier(void)
  116. {
  117. if (!xen_initial_domain() || acpi_disabled)
  118. return 0;
  119. return bus_register_notifier(&platform_bus_type, &platform_device_nb);
  120. }
  121. arch_initcall(register_xen_platform_notifier);
  122. #ifdef CONFIG_ARM_AMBA
  123. #include <linux/amba/bus.h>
  124. static int xen_amba_notifier(struct notifier_block *nb,
  125. unsigned long action, void *data)
  126. {
  127. struct amba_device *adev = to_amba_device(data);
  128. int r = 0;
  129. switch (action) {
  130. case BUS_NOTIFY_ADD_DEVICE:
  131. r = xen_map_device_mmio(&adev->res, 1);
  132. break;
  133. case BUS_NOTIFY_DEL_DEVICE:
  134. r = xen_unmap_device_mmio(&adev->res, 1);
  135. break;
  136. default:
  137. return NOTIFY_DONE;
  138. }
  139. if (r)
  140. dev_err(&adev->dev, "AMBA: Failed to %s device %s MMIO!\n",
  141. action == BUS_NOTIFY_ADD_DEVICE ? "map" :
  142. (action == BUS_NOTIFY_DEL_DEVICE ? "unmap" : "?"),
  143. adev->dev.init_name);
  144. return NOTIFY_OK;
  145. }
  146. static struct notifier_block amba_device_nb = {
  147. .notifier_call = xen_amba_notifier,
  148. };
  149. static int __init register_xen_amba_notifier(void)
  150. {
  151. if (!xen_initial_domain() || acpi_disabled)
  152. return 0;
  153. return bus_register_notifier(&amba_bustype, &amba_device_nb);
  154. }
  155. arch_initcall(register_xen_amba_notifier);
  156. #endif