6
6
//
7
7
// Author: Mark Brown <broonie@opensource.wolfsonmicro.com>
8
8
9
+ #include <linux/array_size.h>
9
10
#include <linux/device.h>
10
11
#include <linux/export.h>
11
12
#include <linux/interrupt.h>
12
13
#include <linux/irq.h>
13
14
#include <linux/irqdomain.h>
15
+ #include <linux/overflow.h>
14
16
#include <linux/pm_runtime.h>
15
17
#include <linux/regmap.h>
16
18
#include <linux/slab.h>
@@ -33,6 +35,7 @@ struct regmap_irq_chip_data {
33
35
void * status_reg_buf ;
34
36
unsigned int * main_status_buf ;
35
37
unsigned int * status_buf ;
38
+ unsigned int * prev_status_buf ;
36
39
unsigned int * mask_buf ;
37
40
unsigned int * mask_buf_def ;
38
41
unsigned int * wake_buf ;
@@ -332,27 +335,13 @@ static inline int read_sub_irq_data(struct regmap_irq_chip_data *data,
332
335
return ret ;
333
336
}
334
337
335
- static irqreturn_t regmap_irq_thread ( int irq , void * d )
338
+ static int read_irq_data ( struct regmap_irq_chip_data * data )
336
339
{
337
- struct regmap_irq_chip_data * data = d ;
338
340
const struct regmap_irq_chip * chip = data -> chip ;
339
341
struct regmap * map = data -> map ;
340
342
int ret , i ;
341
- bool handled = false;
342
343
u32 reg ;
343
344
344
- if (chip -> handle_pre_irq )
345
- chip -> handle_pre_irq (chip -> irq_drv_data );
346
-
347
- if (chip -> runtime_pm ) {
348
- ret = pm_runtime_get_sync (map -> dev );
349
- if (ret < 0 ) {
350
- dev_err (map -> dev , "IRQ thread failed to resume: %d\n" ,
351
- ret );
352
- goto exit ;
353
- }
354
- }
355
-
356
345
/*
357
346
* Read only registers with active IRQs if the chip has 'main status
358
347
* register'. Else read in the statuses, using a single bulk read if
@@ -379,10 +368,8 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
379
368
reg = data -> get_irq_reg (data , chip -> main_status , i );
380
369
ret = regmap_read (map , reg , & data -> main_status_buf [i ]);
381
370
if (ret ) {
382
- dev_err (map -> dev ,
383
- "Failed to read IRQ status %d\n" ,
384
- ret );
385
- goto exit ;
371
+ dev_err (map -> dev , "Failed to read IRQ status %d\n" , ret );
372
+ return ret ;
386
373
}
387
374
}
388
375
@@ -398,10 +385,8 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
398
385
ret = read_sub_irq_data (data , b );
399
386
400
387
if (ret != 0 ) {
401
- dev_err (map -> dev ,
402
- "Failed to read IRQ status %d\n" ,
403
- ret );
404
- goto exit ;
388
+ dev_err (map -> dev , "Failed to read IRQ status %d\n" , ret );
389
+ return ret ;
405
390
}
406
391
}
407
392
@@ -418,9 +403,8 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
418
403
data -> status_reg_buf ,
419
404
chip -> num_regs );
420
405
if (ret != 0 ) {
421
- dev_err (map -> dev , "Failed to read IRQ status: %d\n" ,
422
- ret );
423
- goto exit ;
406
+ dev_err (map -> dev , "Failed to read IRQ status: %d\n" , ret );
407
+ return ret ;
424
408
}
425
409
426
410
for (i = 0 ; i < data -> chip -> num_regs ; i ++ ) {
@@ -436,7 +420,7 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
436
420
break ;
437
421
default :
438
422
BUG ();
439
- goto exit ;
423
+ return - EIO ;
440
424
}
441
425
}
442
426
@@ -447,10 +431,8 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
447
431
ret = regmap_read (map , reg , & data -> status_buf [i ]);
448
432
449
433
if (ret != 0 ) {
450
- dev_err (map -> dev ,
451
- "Failed to read IRQ status: %d\n" ,
452
- ret );
453
- goto exit ;
434
+ dev_err (map -> dev , "Failed to read IRQ status: %d\n" , ret );
435
+ return ret ;
454
436
}
455
437
}
456
438
}
@@ -459,6 +441,42 @@ static irqreturn_t regmap_irq_thread(int irq, void *d)
459
441
for (i = 0 ; i < data -> chip -> num_regs ; i ++ )
460
442
data -> status_buf [i ] = ~data -> status_buf [i ];
461
443
444
+ return 0 ;
445
+ }
446
+
447
+ static irqreturn_t regmap_irq_thread (int irq , void * d )
448
+ {
449
+ struct regmap_irq_chip_data * data = d ;
450
+ const struct regmap_irq_chip * chip = data -> chip ;
451
+ struct regmap * map = data -> map ;
452
+ int ret , i ;
453
+ bool handled = false;
454
+ u32 reg ;
455
+
456
+ if (chip -> handle_pre_irq )
457
+ chip -> handle_pre_irq (chip -> irq_drv_data );
458
+
459
+ if (chip -> runtime_pm ) {
460
+ ret = pm_runtime_get_sync (map -> dev );
461
+ if (ret < 0 ) {
462
+ dev_err (map -> dev , "IRQ thread failed to resume: %d\n" , ret );
463
+ goto exit ;
464
+ }
465
+ }
466
+
467
+ ret = read_irq_data (data );
468
+ if (ret < 0 )
469
+ goto exit ;
470
+
471
+ if (chip -> status_is_level ) {
472
+ for (i = 0 ; i < data -> chip -> num_regs ; i ++ ) {
473
+ unsigned int val = data -> status_buf [i ];
474
+
475
+ data -> status_buf [i ] ^= data -> prev_status_buf [i ];
476
+ data -> prev_status_buf [i ] = val ;
477
+ }
478
+ }
479
+
462
480
/*
463
481
* Ignore masked IRQs and ack if we need to; we ack early so
464
482
* there is no race between handling and acknowledging the
@@ -705,6 +723,13 @@ int regmap_add_irq_chip_fwnode(struct fwnode_handle *fwnode,
705
723
if (!d -> status_buf )
706
724
goto err_alloc ;
707
725
726
+ if (chip -> status_is_level ) {
727
+ d -> prev_status_buf = kcalloc (chip -> num_regs , sizeof (* d -> prev_status_buf ),
728
+ GFP_KERNEL );
729
+ if (!d -> prev_status_buf )
730
+ goto err_alloc ;
731
+ }
732
+
708
733
d -> mask_buf = kcalloc (chip -> num_regs , sizeof (* d -> mask_buf ),
709
734
GFP_KERNEL );
710
735
if (!d -> mask_buf )
@@ -881,6 +906,16 @@ int regmap_add_irq_chip_fwnode(struct fwnode_handle *fwnode,
881
906
}
882
907
}
883
908
909
+ /* Store current levels */
910
+ if (chip -> status_is_level ) {
911
+ ret = read_irq_data (d );
912
+ if (ret < 0 )
913
+ goto err_alloc ;
914
+
915
+ memcpy (d -> prev_status_buf , d -> status_buf ,
916
+ array_size (d -> chip -> num_regs , sizeof (d -> prev_status_buf [0 ])));
917
+ }
918
+
884
919
ret = regmap_irq_create_domain (fwnode , irq_base , chip , d );
885
920
if (ret )
886
921
goto err_alloc ;
@@ -908,6 +943,7 @@ int regmap_add_irq_chip_fwnode(struct fwnode_handle *fwnode,
908
943
kfree (d -> mask_buf );
909
944
kfree (d -> main_status_buf );
910
945
kfree (d -> status_buf );
946
+ kfree (d -> prev_status_buf );
911
947
kfree (d -> status_reg_buf );
912
948
if (d -> config_buf ) {
913
949
for (i = 0 ; i < chip -> num_config_bases ; i ++ )
@@ -985,6 +1021,7 @@ void regmap_del_irq_chip(int irq, struct regmap_irq_chip_data *d)
985
1021
kfree (d -> main_status_buf );
986
1022
kfree (d -> status_reg_buf );
987
1023
kfree (d -> status_buf );
1024
+ kfree (d -> prev_status_buf );
988
1025
if (d -> config_buf ) {
989
1026
for (i = 0 ; i < d -> chip -> num_config_bases ; i ++ )
990
1027
kfree (d -> config_buf [i ]);
0 commit comments