3 files changed, 20 insertions, 9 deletions

diff --git a/arch/x86/kernel/apic/vector.c b/arch/x86/kernel/apic/vector.c
index 7654febd5102..652e7ffa9b9d 100644
--- a/arch/x86/kernel/apic/vector.c
+++ b/arch/x86/kernel/apic/vector.c
@@ -313,14 +313,13 @@ assign_managed_vector(struct irq_data *irqd, const struct cpumask *dest)
         struct apic_chip_data *apicd = apic_chip_data(irqd);
         int vector, cpu;
 
-        cpumask_and(vector_searchmask, vector_searchmask, affmsk);
-        cpu = cpumask_first(vector_searchmask);
-        if (cpu >= nr_cpu_ids)
-                return -EINVAL;
+        cpumask_and(vector_searchmask, dest, affmsk);
+
         /* set_affinity might call here for nothing */
         if (apicd->vector && cpumask_test_cpu(apicd->cpu, vector_searchmask))
                 return 0;
-        vector = irq_matrix_alloc_managed(vector_matrix, cpu);
+        vector = irq_matrix_alloc_managed(vector_matrix, vector_searchmask,
+                                          &cpu);
         trace_vector_alloc_managed(irqd->irq, vector, vector);
         if (vector < 0)
                 return vector;
diff --git a/include/linux/irq.h b/include/linux/irq.h
index 201de12a9957..c9bffda04a45 100644
--- a/include/linux/irq.h
+++ b/include/linux/irq.h
@@ -1151,7 +1151,8 @@ void irq_matrix_offline(struct irq_matrix *m);
 void irq_matrix_assign_system(struct irq_matrix *m, unsigned int bit, bool replace);
 int irq_matrix_reserve_managed(struct irq_matrix *m, const struct cpumask *msk);
 void irq_matrix_remove_managed(struct irq_matrix *m, const struct cpumask *msk);
-int irq_matrix_alloc_managed(struct irq_matrix *m, unsigned int cpu);
+int irq_matrix_alloc_managed(struct irq_matrix *m, const struct cpumask *msk,
+                                unsigned int *mapped_cpu);
 void irq_matrix_reserve(struct irq_matrix *m);
 void irq_matrix_remove_reserved(struct irq_matrix *m);
 int irq_matrix_alloc(struct irq_matrix *m, const struct cpumask *msk,
diff --git a/kernel/irq/matrix.c b/kernel/irq/matrix.c
index 67768bbe736e..6e6d467f3dec 100644
--- a/kernel/irq/matrix.c
+++ b/kernel/irq/matrix.c
@@ -260,11 +260,21 @@ void irq_matrix_remove_managed(struct irq_matrix *m, const struct cpumask *msk)
  * @m:          Matrix pointer
  * @cpu:        On which CPU the interrupt should be allocated
  */
-int irq_matrix_alloc_managed(struct irq_matrix *m, unsigned int cpu)
+int irq_matrix_alloc_managed(struct irq_matrix *m, const struct cpumask *msk,
+                             unsigned int *mapped_cpu)
 {
-        struct cpumap *cm = per_cpu_ptr(m->maps, cpu);
-        unsigned int bit, end = m->alloc_end;
+        unsigned int bit, cpu, end = m->alloc_end;
+        struct cpumap *cm;
+
+        if (cpumask_empty(msk))
+                return -EINVAL;
 
+        cpu = matrix_find_best_cpu(m, msk);
+        if (cpu == UINT_MAX)
+                return -ENOSPC;
+
+        cm = per_cpu_ptr(m->maps, cpu);
+        end = m->alloc_end;
         /* Get managed bit which are not allocated */
         bitmap_andnot(m->scratch_map, cm->managed_map, cm->alloc_map, end);
         bit = find_first_bit(m->scratch_map, end);
@@ -273,6 +283,7 @@ int irq_matrix_alloc_managed(struct irq_matrix *m, unsigned int cpu)
         set_bit(bit, cm->alloc_map);
         cm->allocated++;
         m->total_allocated++;
+        *mapped_cpu = cpu;
         trace_irq_matrix_alloc_managed(bit, cpu, m, cm);
         return bit;
 }