diff --git a/src/hal/drivers/mesa-hostmot2/capsense.c b/src/hal/drivers/mesa-hostmot2/capsense.c
index a0203ced1a..93a85304bd 100644
--- a/src/hal/drivers/mesa-hostmot2/capsense.c
+++ b/src/hal/drivers/mesa-hostmot2/capsense.c
@@ -62,9 +62,9 @@ int hm2_capsense_parse_md(hostmot2_t *hm2, int md_index) {
         return 0;
     }
 
-    // 
+    //
     // looks good, start initializing
-    // 
+    //
 
 
     if (hm2->config.num_capsensors == -1) {
@@ -82,16 +82,14 @@ int hm2_capsense_parse_md(hostmot2_t *hm2, int md_index) {
     }
 
     hm2->capsense.capsense_data_addr = md->base_address + (0 * md->register_stride);
-    hm2->capsense.capsense_hysteresis_addr = md->base_address + (1 * md->register_stride);
+    hm2->capsense.capsense_hysteresis_addr = md->base_address + (0 * md->register_stride) + (1 * sizeof(u32));
 
-//    r = hm2_register_tram_read_region(hm2, hm2->capsense.capsense_data_addr, (hm2->capsense.num_instances * sizeof(u32)), &hm2->capsense.capsense_data_reg);
     r = hm2_register_tram_read_region(hm2, hm2->capsense.capsense_data_addr, (1 * sizeof(u32)), &hm2->capsense.capsense_data_reg);
     if (r < 0) {
         HM2_ERR("error registering tram read region for CAPSENSE Data register (%d)\n", r);
         goto fail0;
     }
 
-//    hm2->capsense.capsense_data_reg = (u32 *)kmalloc(hm2->capsense.num_instances * sizeof(u32), GFP_KERNEL);
     hm2->capsense.capsense_data_reg = (u32 *)kmalloc(1 * sizeof(u32), GFP_KERNEL);
     if (hm2->capsense.capsense_data_reg == NULL) {
         HM2_ERR("out of memory!\n");
@@ -99,6 +97,19 @@ int hm2_capsense_parse_md(hostmot2_t *hm2, int md_index) {
         goto fail0;
     }
 
+    r = hm2_register_tram_read_region(hm2, hm2->capsense.capsense_hysteresis_addr, (1 * sizeof(u32)), &hm2->capsense.capsense_hysteresis_reg);
+    if (r < 0) {
+        HM2_ERR("error registering tram write region for CAPSENSE Hysteresis register (%d)\n", r);
+        goto fail0;
+    }
+
+    hm2->capsense.capsense_hysteresis_reg = (u32 *)kmalloc(1 * sizeof(u32), GFP_KERNEL);
+    if (hm2->capsense.capsense_hysteresis_reg == NULL) {
+        HM2_ERR("out of memory!\n");
+        r = -ENOMEM;
+        goto fail0;
+    }
+
     // export to HAL
     // FIXME: r hides the r in enclosing function, and it returns the wrong thing
     {
@@ -107,32 +118,14 @@ int hm2_capsense_parse_md(hostmot2_t *hm2, int md_index) {
         char name[HAL_NAME_LEN + 1];
 
         // parameters
-/*
-        // these hal parameters affect all capsense instances
-        r = hal_param_u32_newf(
-            HAL_RW,
-            &(hm2->capsense.hal->param.capsense_hysteresis),
-            hm2->llio->comp_id,
-            "%s.capsense.capsense_hysteresis",
-            hm2->llio->name
-        );
-        if (r < 0) {
-            HM2_ERR("error adding capsense.capsense_hysteresis param, aborting\n");
-            goto fail1;
-        }
-        hm2->capsense.hal->param.capsense_hysteresis = 0x33333333;
-//        hm2->capsense.written_capsense_hysteresis_reg = 0;
-*/
 
         rtapi_snprintf(name, sizeof(name), "%s.capsense.%02d.hysteresis", hm2->llio->name, 0);
-        r = hal_pin_u32_new(name, HAL_IN, &(hm2->capsense.hal->param.capsense_hysteresis), hm2->llio->comp_id);
+        r = hal_pin_u32_new(name, HAL_IN, &(hm2->capsense.hal->pin.capsense_hysteresis), hm2->llio->comp_id);
         if (r < 0) {
             HM2_ERR("error adding capsense.hysteresis pin, aborting\n");
             goto fail1;
         }
 
-        *hm2->capsense.hal->param.capsense_hysteresis = 0x33333333;
- 
        for (i = 0; i < hm2->capsense.num_instances; i ++) {
             // pins
             rtapi_snprintf(name, sizeof(name), "%s.capsense.%02d.trigged", hm2->llio->name, i);
@@ -141,73 +134,27 @@ int hm2_capsense_parse_md(hostmot2_t *hm2, int md_index) {
                 HM2_ERR("error adding pin '%s', aborting\n", name);
                 goto fail1;
             }
-            // init hal objects
-/*
-            for(i=0;i<hm2->capsense.num_instances;i++){
-                *(hm2->capsense.instance[i].hal.pin.sensepad) = 0;
-            }
-*/
         }
     }
 
-
     return hm2->capsense.num_instances;
 
 fail1:
     kfree(hm2->capsense.capsense_data_reg);
+    kfree(hm2->capsense.capsense_hysteresis_reg);
 
 fail0:
     hm2->capsense.num_instances = 0;
     return r;
 }
 
-
-/*
-int hm2_capsense_setup(hostmot2_t *hm2) {
-    int r,i;
-    char name[HAL_NAME_LEN + 1];
-
-
-    if (hm2->config.enable_capsense == 0) {
-        return 0;
-    }
-    
-    hm2->capsense = (capsense_t *)hal_malloc(sizeof(capsense_t));
-    if (hm2->capsense == NULL) {
-        HM2_ERR("out of memory!\n");
-        hm2->config.enable_capsense = 0;
-        return -ENOMEM;
-    }
-
-    for(i=0;i<NUM_capsense_SENSORS;i=i+1){
-        rtapi_snprintf(name, sizeof(name), "%s.capsense.sensepad.%d.out", hm2->llio->name, i);
-        r = hal_pin_bit_new(name, HAL_OUT, &(hm2->capsense->hal.pin.sensepad[i]), hm2->llio->comp_id);
-        if (r < 0) {
-            HM2_ERR("error adding pin '%s', aborting\n", name);
-            return -EINVAL;
-        }
-    }
-
-*/
-    // init hal objects
-/*
-    for(i=0;i<NUM_capsense_SENSORS;i++){
-        *(hm2->capsense->hal.pin.sensepad[i]) = 0;
-    }
-
-	 hm2_set_pin_direction(hm2, 36, HM2_PIN_DIR_IS_OUTPUT);	
-    
-    return 0;
-}
-*/
-
 void hm2_capsense_read(hostmot2_t *hm2) {
     int i;
     u32 val;
     hal_bit_t bit;
 
-     if (hm2->capsense.num_instances == 0) return;
-	 
+    if (hm2->capsense.num_instances == 0) return;
+
     hm2->llio->read(
         hm2->llio,
         hm2->capsense.capsense_data_addr,
@@ -221,6 +168,26 @@ void hm2_capsense_read(hostmot2_t *hm2) {
     }
 }
 
-//void hm2_capsense_allocate_pins(hostmot2_t *hm2) {
-//    hm2_set_pin_direction(hm2, 36, HM2_PIN_DIR_IS_OUTPUT);
-//}
+void hm2_capsense_write(hostmot2_t *hm2) {
+
+    if (hm2->capsense.num_instances == 0) return;
+    *hm2->capsense.capsense_hysteresis_reg = *hm2->capsense.hal->pin.capsense_hysteresis;
+
+    // update register
+    if (*hm2->capsense.capsense_hysteresis_reg && hm2->capsense.written_capsense_hysteresis_reg != *hm2->capsense.capsense_hysteresis_reg) {
+        hm2->llio->write(
+            hm2->llio,
+            hm2->capsense.capsense_hysteresis_addr,
+            hm2->capsense.capsense_hysteresis_reg,
+            sizeof(u32)
+        );
+        hm2->capsense.written_capsense_hysteresis_reg = *hm2->capsense.capsense_hysteresis_reg;
+    } else {
+        hm2->llio->read(
+            hm2->llio,
+            hm2->capsense.capsense_hysteresis_addr,
+            (void *)hm2->capsense.hal->pin.capsense_hysteresis,
+            sizeof(u32)
+        );
+    }
+}
diff --git a/src/hal/drivers/mesa-hostmot2/hostmot2.c b/src/hal/drivers/mesa-hostmot2/hostmot2.c
index fe56bb4724..ef46d504e0 100755
--- a/src/hal/drivers/mesa-hostmot2/hostmot2.c
+++ b/src/hal/drivers/mesa-hostmot2/hostmot2.c
@@ -137,6 +137,8 @@ static int hm2_write(void *void_hm2, const hal_funct_args_t *fa) {
     hm2_led_write(hm2);	      // Update on-board LEDs
 
     hm2_raw_write(hm2);
+
+    hm2_capsense_write(hm2); // handles set hysteresis in capsense mksocfpga hm2 core
     return 0;
 }
 
diff --git a/src/hal/drivers/mesa-hostmot2/hostmot2.h b/src/hal/drivers/mesa-hostmot2/hostmot2.h
index b17ff1fdfc..217c60a675 100644
--- a/src/hal/drivers/mesa-hostmot2/hostmot2.h
+++ b/src/hal/drivers/mesa-hostmot2/hostmot2.h
@@ -1105,7 +1105,7 @@ typedef struct {
 typedef struct {
     struct {
         hal_u32_t *capsense_hysteresis;
-    } param;
+    } pin;
 } hm2_capsense_module_global_t;
 
 typedef struct {
@@ -1540,9 +1540,8 @@ void de0_nano_soc_adc_read(hostmot2_t *hm2);
 //
 
 int hm2_capsense_parse_md(hostmot2_t *hm2, int md_index);
-//int hm2_capsense_setup(hostmot2_t *hm2);
 void hm2_capsense_read(hostmot2_t *hm2);
-//void hm2_capsense_allocate_pins(hostmot2_t *hm2);
+void hm2_capsense_write(hostmot2_t *hm2);