1 files changed, 10 insertions, 0 deletions

diff --git a/arch/sh/drivers/heartbeat.c b/arch/sh/drivers/heartbeat.c
index 938817e34e2b..a9339a6174fc 100644
--- a/arch/sh/drivers/heartbeat.c
+++ b/arch/sh/drivers/heartbeat.c
@@ -40,14 +40,19 @@ static inline void heartbeat_toggle_bit(struct heartbeat_data *hd,
         if (inverted)
                 new = ~new;
 
+        new &= hd->mask;
+
         switch (hd->regsize) {
         case 32:
+                new |= ioread32(hd->base) & ~hd->mask;
                 iowrite32(new, hd->base);
                 break;
         case 16:
+                new |= ioread16(hd->base) & ~hd->mask;
                 iowrite16(new, hd->base);
                 break;
         default:
+                new |= ioread8(hd->base) & ~hd->mask;
                 iowrite8(new, hd->base);
                 break;
         }
@@ -72,6 +77,7 @@ static int heartbeat_drv_probe(struct platform_device *pdev)
 {
         struct resource *res;
         struct heartbeat_data *hd;
+        int i;
 
         if (unlikely(pdev->num_resources != 1)) {
                 dev_err(&pdev->dev, "invalid number of resources\n");
@@ -107,6 +113,10 @@ static int heartbeat_drv_probe(struct platform_device *pdev)
                 hd->nr_bits = ARRAY_SIZE(default_bit_pos);
         }
 
+        hd->mask = 0;
+        for (i = 0; i < hd->nr_bits; i++)
+                hd->mask |= (1 << hd->bit_pos[i]);
+
         if (!hd->regsize)
                 hd->regsize = 8;        /* default access size */