sensor_bulk: New C file with helper code for sending bulk sensor measurements

Refactor the low-level "bulk sensor" management code in the mcu.  This
updates the sensor_adxl345.c, sensor_mpu9250.c, sensor_lis2dw.c, and
sensor_angle.c code to use the same "bulk sensor" messages.  All of
these sensors will now send "sensor_bulk_data" and
"sensor_bulk_status" messages.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

13 files changed, 153 insertions, 189 deletions

diff --git a/klippy/extras/adxl345.py b/klippy/extras/adxl345.py
index 76fd4ca4..b91224d5 100644
--- a/klippy/extras/adxl345.py
+++ b/klippy/extras/adxl345.py
@@ -187,7 +187,7 @@ def read_axes_map(config):
 MIN_MSG_TIME = 0.100
 
 BYTES_PER_SAMPLE = 5
-SAMPLES_PER_BLOCK = 10
+SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE
 
 BATCH_UPDATES = 0.100
 
@@ -205,13 +205,12 @@ class ADXL345:
         self.mcu = mcu = self.spi.get_mcu()
         self.oid = oid = mcu.create_oid()
         self.query_adxl345_cmd = None
-        self.query_adxl345_status_cmd = None
         mcu.add_config_cmd("config_adxl345 oid=%d spi_oid=%d"
                            % (oid, self.spi.get_oid()))
         mcu.add_config_cmd("query_adxl345 oid=%d clock=0 rest_ticks=0"
                            % (oid,), on_restart=True)
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "adxl345_data", oid)
+        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
         # Clock tracking
         chip_smooth = self.data_rate * BATCH_UPDATES * 2
         self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth)
@@ -230,10 +229,8 @@ class ADXL345:
         cmdqueue = self.spi.get_command_queue()
         self.query_adxl345_cmd = self.mcu.lookup_command(
             "query_adxl345 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue)
-        self.query_adxl345_status_cmd = self.mcu.lookup_query_command(
-            "query_adxl345_status oid=%c",
-            "adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
-            " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
+        self.clock_updater.setup_query_command(
+            self.mcu, "query_adxl345_status oid=%c", oid=self.oid, cq=cmdqueue)
     def read_reg(self, reg):
         params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00])
         response = bytearray(params['response'])
@@ -286,17 +283,6 @@ class ADXL345:
         self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
         del samples[count:]
         return samples
-    def _update_clock(self, minclock=0):
-        # Query current state
-        for retry in range(5):
-            params = self.query_adxl345_status_cmd.send([self.oid],
-                                                        minclock=minclock)
-            fifo = params['fifo'] & 0x7f
-            if fifo <= 32:
-                break
-        else:
-            raise self.printer.command_error("Unable to query adxl345 fifo")
-        self.clock_updater.update_clock(params)
     # Start, stop, and process message batches
     def _start_measurements(self):
         # In case of miswiring, testing ADXL345 device ID prevents treating
@@ -325,8 +311,6 @@ class ADXL345:
         logging.info("ADXL345 starting '%s' measurements", self.name)
         # Initialize clock tracking
         self.clock_updater.note_start(reqclock)
-        self._update_clock(minclock=reqclock)
-        self.clock_updater.clear_duration_filter()
         self.last_error_count = 0
     def _finish_measurements(self):
         # Halt bulk reading
@@ -334,7 +318,7 @@ class ADXL345:
         self.bulk_queue.clear_samples()
         logging.info("ADXL345 finished '%s' measurements", self.name)
     def _process_batch(self, eventtime):
-        self._update_clock()
+        self.clock_updater.update_clock()
         raw_samples = self.bulk_queue.pull_samples()
         if not raw_samples:
             return {}
@@ -342,7 +326,7 @@ class ADXL345:
         if not samples:
             return {}
         return {'data': samples, 'errors': self.last_error_count,
-                'overflows': self.clock_updater.get_last_limit_count()}
+                'overflows': self.clock_updater.get_last_overflows()}
 
 def load_config(config):
     return ADXL345(config)
diff --git a/klippy/extras/angle.py b/klippy/extras/angle.py
index 163168d0..23f402a7 100644
--- a/klippy/extras/angle.py
+++ b/klippy/extras/angle.py
@@ -412,7 +412,7 @@ class HelperTLE5012B:
         self._write_reg(reg, val)
 
 BYTES_PER_SAMPLE = 3
-SAMPLES_PER_BLOCK = 16
+SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE
 
 SAMPLE_PERIOD = 0.000400
 BATCH_UPDATES = 0.100
@@ -445,7 +445,7 @@ class Angle:
             "query_spi_angle oid=%d clock=0 rest_ticks=0 time_shift=0"
             % (oid,), on_restart=True)
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "spi_angle_data", oid)
+        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
         # Process messages in batches
         self.batch_bulk = bulk_sensor.BatchBulkHelper(
             self.printer, self._process_batch,
diff --git a/klippy/extras/bulk_sensor.py b/klippy/extras/bulk_sensor.py
index 8d0c0541..df5a5da2 100644
--- a/klippy/extras/bulk_sensor.py
+++ b/klippy/extras/bulk_sensor.py
@@ -114,7 +114,7 @@ class BatchWebhooksClient:
 
 # Helper class to store incoming messages in a queue
 class BulkDataQueue:
-    def __init__(self, mcu, msg_name, oid):
+    def __init__(self, mcu, msg_name="sensor_bulk_data", oid=None):
         # Measurement storage (accessed from background thread)
         self.lock = threading.Lock()
         self.raw_samples = []
@@ -206,31 +206,37 @@ class ChipClockUpdater:
         self.clock_sync = clock_sync
         self.bytes_per_sample = bytes_per_sample
         self.samples_per_block = MAX_BULK_MSG_SIZE // bytes_per_sample
-        self.mcu = clock_sync.mcu
         self.last_sequence = self.max_query_duration = 0
-        self.last_limit_count = 0
+        self.last_overflows = 0
+        self.mcu = self.oid = self.query_status_cmd = None
+    def setup_query_command(self, mcu, msgformat, oid, cq):
+        self.mcu = mcu
+        self.oid = oid
+        self.query_status_cmd = self.mcu.lookup_query_command(
+            msgformat, "sensor_bulk_status oid=%c clock=%u query_ticks=%u"
+            " next_sequence=%hu buffered=%u possible_overflows=%hu",
+            oid=oid, cq=cq)
     def get_last_sequence(self):
         return self.last_sequence
-    def get_last_limit_count(self):
-        return self.last_limit_count
+    def get_last_overflows(self):
+        return self.last_overflows
     def clear_duration_filter(self):
         self.max_query_duration = 1 << 31
     def note_start(self, reqclock):
         self.last_sequence = 0
-        self.last_limit_count = 0
+        self.last_overflows = 0
         self.clock_sync.reset(reqclock, 0)
         self.clear_duration_filter()
-    def update_clock(self, params):
-        # Handle a status response message of the form:
-        #   adxl345_status oid=x clock=x query_ticks=x next_sequence=x
-        #     buffered=x fifo=x limit_count=x
-        fifo = params['fifo']
+        self.update_clock(minclock=reqclock)
+        self.clear_duration_filter()
+    def update_clock(self, minclock=0):
+        params = self.query_status_cmd.send([self.oid], minclock=minclock)
         mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
         seq_diff = (params['next_sequence'] - self.last_sequence) & 0xffff
         self.last_sequence += seq_diff
         buffered = params['buffered']
-        lc_diff = (params['limit_count'] - self.last_limit_count) & 0xffff
-        self.last_limit_count += lc_diff
+        po_diff = (params['possible_overflows'] - self.last_overflows) & 0xffff
+        self.last_overflows += po_diff
         duration = params['query_ticks']
         if duration > self.max_query_duration:
             # Skip measurement as a high query time could skew clock tracking
@@ -239,7 +245,7 @@ class ChipClockUpdater:
             return
         self.max_query_duration = 2 * duration
         msg_count = (self.last_sequence * self.samples_per_block
-                     + buffered // self.bytes_per_sample + fifo)
+                     + buffered // self.bytes_per_sample)
         # The "chip clock" is the message counter plus .5 for average
         # inaccuracy of query responses and plus .5 for assumed offset
         # of hardware processing time.
diff --git a/klippy/extras/lis2dw.py b/klippy/extras/lis2dw.py
index a7fe54d7..74911e6f 100644
--- a/klippy/extras/lis2dw.py
+++ b/klippy/extras/lis2dw.py
@@ -33,7 +33,7 @@ SCALE = FREEFALL_ACCEL * 1.952 / 4
 MIN_MSG_TIME = 0.100
 
 BYTES_PER_SAMPLE = 6
-SAMPLES_PER_BLOCK = 8
+SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE
 
 BATCH_UPDATES = 0.100
 
@@ -49,13 +49,12 @@ class LIS2DW:
         self.mcu = mcu = self.spi.get_mcu()
         self.oid = oid = mcu.create_oid()
         self.query_lis2dw_cmd = None
-        self.query_lis2dw_status_cmd = None
         mcu.add_config_cmd("config_lis2dw oid=%d spi_oid=%d"
                            % (oid, self.spi.get_oid()))
         mcu.add_config_cmd("query_lis2dw oid=%d clock=0 rest_ticks=0"
                            % (oid,), on_restart=True)
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "lis2dw_data", oid)
+        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
         # Clock tracking
         chip_smooth = self.data_rate * BATCH_UPDATES * 2
         self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth)
@@ -75,10 +74,8 @@ class LIS2DW:
         cmdqueue = self.spi.get_command_queue()
         self.query_lis2dw_cmd = self.mcu.lookup_command(
             "query_lis2dw oid=%c clock=%u rest_ticks=%u", cq=cmdqueue)
-        self.query_lis2dw_status_cmd = self.mcu.lookup_query_command(
-            "query_lis2dw_status oid=%c",
-            "lis2dw_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
-            " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
+        self.clock_updater.setup_query_command(
+            self.mcu, "query_lis2dw_status oid=%c", oid=self.oid, cq=cmdqueue)
     def read_reg(self, reg):
         params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00])
         response = bytearray(params['response'])
@@ -133,10 +130,6 @@ class LIS2DW:
         self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
         del samples[count:]
         return samples
-    def _update_clock(self, minclock=0):
-        params = self.query_lis2dw_status_cmd.send([self.oid],
-                                                   minclock=minclock)
-        self.clock_updater.update_clock(params)
     # Start, stop, and process message batches
     def _start_measurements(self):
         # In case of miswiring, testing LIS2DW device ID prevents treating
@@ -170,8 +163,6 @@ class LIS2DW:
         logging.info("LIS2DW starting '%s' measurements", self.name)
         # Initialize clock tracking
         self.clock_updater.note_start(reqclock)
-        self._update_clock(minclock=reqclock)
-        self.clock_updater.clear_duration_filter()
         self.last_error_count = 0
     def _finish_measurements(self):
         # Halt bulk reading
@@ -180,7 +171,7 @@ class LIS2DW:
         logging.info("LIS2DW finished '%s' measurements", self.name)
         self.set_reg(REG_LIS2DW_FIFO_CTRL, 0x00)
     def _process_batch(self, eventtime):
-        self._update_clock()
+        self.clock_updater.update_clock()
         raw_samples = self.bulk_queue.pull_samples()
         if not raw_samples:
             return {}
@@ -188,7 +179,7 @@ class LIS2DW:
         if not samples:
             return {}
         return {'data': samples, 'errors': self.last_error_count,
-                'overflows': self.clock_updater.get_last_limit_count()}
+                'overflows': self.clock_updater.get_last_overflows()}
 
 def load_config(config):
     return LIS2DW(config)
diff --git a/klippy/extras/mpu9250.py b/klippy/extras/mpu9250.py
index 41376dc3..4626e1c0 100644
--- a/klippy/extras/mpu9250.py
+++ b/klippy/extras/mpu9250.py
@@ -50,7 +50,7 @@ FIFO_SIZE = 512
 MIN_MSG_TIME = 0.100
 
 BYTES_PER_SAMPLE = 6
-SAMPLES_PER_BLOCK = 8
+SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE
 
 BATCH_UPDATES = 0.100
 
@@ -70,9 +70,8 @@ class MPU9250:
         self.mcu = mcu = self.i2c.get_mcu()
         self.oid = oid = mcu.create_oid()
         self.query_mpu9250_cmd = None
-        self.query_mpu9250_status_cmd = None
         mcu.register_config_callback(self._build_config)
-        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "mpu9250_data", oid)
+        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, oid=oid)
         # Clock tracking
         chip_smooth = self.data_rate * BATCH_UPDATES * 2
         self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth)
@@ -95,10 +94,8 @@ class MPU9250:
                            % (self.oid,), on_restart=True)
         self.query_mpu9250_cmd = self.mcu.lookup_command(
             "query_mpu9250 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue)
-        self.query_mpu9250_status_cmd = self.mcu.lookup_query_command(
-            "query_mpu9250_status oid=%c",
-            "mpu9250_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
-            " buffered=%c fifo=%u limit_count=%hu", oid=self.oid, cq=cmdqueue)
+        self.clock_updater.setup_query_command(
+            self.mcu, "query_mpu9250_status oid=%c", oid=self.oid, cq=cmdqueue)
     def read_reg(self, reg):
         params = self.i2c.i2c_read([reg], 1)
         return bytearray(params['response'])[0]
@@ -142,11 +139,6 @@ class MPU9250:
         self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
         del samples[count:]
         return samples
-
-    def _update_clock(self, minclock=0):
-        params = self.query_mpu9250_status_cmd.send([self.oid],
-                                                    minclock=minclock)
-        self.clock_updater.update_clock(params)
     # Start, stop, and process message batches
     def _start_measurements(self):
         # In case of miswiring, testing MPU9250 device ID prevents treating
@@ -184,8 +176,6 @@ class MPU9250:
         logging.info("MPU9250 starting '%s' measurements", self.name)
         # Initialize clock tracking
         self.clock_updater.note_start(reqclock)
-        self._update_clock(minclock=reqclock)
-        self.clock_updater.clear_duration_filter()
         self.last_error_count = 0
     def _finish_measurements(self):
         # Halt bulk reading
@@ -195,7 +185,7 @@ class MPU9250:
         self.set_reg(REG_PWR_MGMT_1, SET_PWR_MGMT_1_SLEEP)
         self.set_reg(REG_PWR_MGMT_2, SET_PWR_MGMT_2_OFF)
     def _process_batch(self, eventtime):
-        self._update_clock()
+        self.clock_updater.update_clock()
         raw_samples = self.bulk_queue.pull_samples()
         if not raw_samples:
             return {}
@@ -203,7 +193,7 @@ class MPU9250:
         if not samples:
             return {}
         return {'data': samples, 'errors': self.last_error_count,
-                'overflows': self.clock_updater.get_last_limit_count()}
+                'overflows': self.clock_updater.get_last_overflows()}
 
 def load_config(config):
     return MPU9250(config)
diff --git a/src/Kconfig b/src/Kconfig
index aaf50653..91376910 100644
--- a/src/Kconfig
+++ b/src/Kconfig
@@ -112,6 +112,10 @@ config WANT_SOFTWARE_SPI
     bool
     depends on HAVE_GPIO && HAVE_GPIO_SPI
     default y
+config NEED_SENSOR_BULK
+    bool
+    depends on WANT_SENSORS || WANT_LIS2DW
+    default y
 menu "Optional features (to reduce code size)"
     depends on HAVE_LIMITED_CODE_SIZE
 config WANT_GPIO_BITBANGING
diff --git a/src/Makefile b/src/Makefile
index 8d771f9e..eddad978 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -16,6 +16,7 @@ src-$(CONFIG_WANT_SOFTWARE_SPI) += spi_software.c
 src-$(CONFIG_WANT_SOFTWARE_I2C) += i2c_software.c
 sensors-src-$(CONFIG_HAVE_GPIO_SPI) := thermocouple.c sensor_adxl345.c \
     sensor_angle.c
-src-$(CONFIG_WANT_LIS2DW) += sensor_lis2dw.c
 sensors-src-$(CONFIG_HAVE_GPIO_I2C) += sensor_mpu9250.c
 src-$(CONFIG_WANT_SENSORS) += $(sensors-src-y)
+src-$(CONFIG_WANT_LIS2DW) += sensor_lis2dw.c
+src-$(CONFIG_NEED_SENSOR_BULK) += sensor_bulk.c
diff --git a/src/sensor_adxl345.c b/src/sensor_adxl345.c
index 5ec3945e..8cd471ef 100644
--- a/src/sensor_adxl345.c
+++ b/src/sensor_adxl345.c
@@ -10,15 +10,15 @@
 #include "basecmd.h" // oid_alloc
 #include "command.h" // DECL_COMMAND
 #include "sched.h" // DECL_TASK
+#include "sensor_bulk.h" // sensor_bulk_report
 #include "spicmds.h" // spidev_transfer
 
 struct adxl345 {
     struct timer timer;
     uint32_t rest_ticks;
     struct spidev_s *spi;
-    uint16_t sequence, limit_count;
-    uint8_t flags, data_count;
-    uint8_t data[50];
+    uint8_t flags;
+    struct sensor_bulk sb;
 };
 
 enum {
@@ -47,27 +47,6 @@ command_config_adxl345(uint32_t *args)
 }
 DECL_COMMAND(command_config_adxl345, "config_adxl345 oid=%c spi_oid=%c");
 
-// Report local measurement buffer
-static void
-adxl_report(struct adxl345 *ax, uint8_t oid)
-{
-    sendf("adxl345_data oid=%c sequence=%hu data=%*s"
-          , oid, ax->sequence, ax->data_count, ax->data);
-    ax->data_count = 0;
-    ax->sequence++;
-}
-
-// Report buffer and fifo status
-static void
-adxl_status(struct adxl345 *ax, uint_fast8_t oid
-            , uint32_t time1, uint32_t time2, uint_fast8_t fifo)
-{
-    sendf("adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
-          " buffered=%c fifo=%c limit_count=%hu"
-          , oid, time1, time2-time1, ax->sequence
-          , ax->data_count, fifo, ax->limit_count);
-}
-
 // Helper code to reschedule the adxl345_event() timer
 static void
 adxl_reschedule_timer(struct adxl345 *ax)
@@ -87,6 +66,8 @@ adxl_reschedule_timer(struct adxl345 *ax)
 
 #define SET_FIFO_CTL 0x90
 
+#define BYTES_PER_SAMPLE 5
+
 // Query accelerometer data
 static void
 adxl_query(struct adxl345 *ax, uint8_t oid)
@@ -96,7 +77,7 @@ adxl_query(struct adxl345 *ax, uint8_t oid)
     spidev_transfer(ax->spi, 1, sizeof(msg), msg);
     // Extract x, y, z measurements
     uint_fast8_t fifo_status = msg[8] & ~0x80; // Ignore trigger bit
-    uint8_t *d = &ax->data[ax->data_count];
+    uint8_t *d = &ax->sb.data[ax->sb.data_count];
     if (((msg[2] & 0xf0) && (msg[2] & 0xf0) != 0xf0)
         || ((msg[4] & 0xf0) && (msg[4] & 0xf0) != 0xf0)
         || ((msg[6] & 0xf0) && (msg[6] & 0xf0) != 0xf0)
@@ -112,12 +93,12 @@ adxl_query(struct adxl345 *ax, uint8_t oid)
         d[3] = (msg[2] & 0x1f) | (msg[6] << 5); // x high bits and z high bits
         d[4] = (msg[4] & 0x1f) | ((msg[6] << 2) & 0x60); // y high and z high
     }
-    ax->data_count += 5;
-    if (ax->data_count + 5 > ARRAY_SIZE(ax->data))
-        adxl_report(ax, oid);
+    ax->sb.data_count += BYTES_PER_SAMPLE;
+    if (ax->sb.data_count + BYTES_PER_SAMPLE > ARRAY_SIZE(ax->sb.data))
+        sensor_bulk_report(&ax->sb, oid);
     // Check fifo status
     if (fifo_status >= 31)
-        ax->limit_count++;
+        ax->sb.possible_overflows++;
     if (fifo_status > 1 && fifo_status <= 32) {
         // More data in fifo - wake this task again
         sched_wake_task(&adxl345_wake);
@@ -166,8 +147,7 @@ command_query_adxl345(uint32_t *args)
     ax->timer.waketime = args[1];
     ax->rest_ticks = args[2];
     ax->flags = AX_HAVE_START;
-    ax->sequence = ax->limit_count = 0;
-    ax->data_count = 0;
+    sensor_bulk_reset(&ax->sb);
     sched_add_timer(&ax->timer);
 }
 DECL_COMMAND(command_query_adxl345,
@@ -178,10 +158,17 @@ command_query_adxl345_status(uint32_t *args)
 {
     struct adxl345 *ax = oid_lookup(args[0], command_config_adxl345);
     uint8_t msg[2] = { AR_FIFO_STATUS | AM_READ, 0x00 };
+
     uint32_t time1 = timer_read_time();
     spidev_transfer(ax->spi, 1, sizeof(msg), msg);
     uint32_t time2 = timer_read_time();
-    adxl_status(ax, args[0], time1, time2, msg[1]);
+
+    uint_fast8_t fifo_status = msg[1] & ~0x80; // Ignore trigger bit
+    if (fifo_status > 32)
+        // Query error - don't send response - host will retry
+        return;
+    sensor_bulk_status(&ax->sb, args[0], time1, time2-time1
+                       , fifo_status * BYTES_PER_SAMPLE);
 }
 DECL_COMMAND(command_query_adxl345_status, "query_adxl345_status oid=%c");
 
diff --git a/src/sensor_angle.c b/src/sensor_angle.c
index 865670b8..54caecc2 100644
--- a/src/sensor_angle.c
+++ b/src/sensor_angle.c
@@ -10,6 +10,7 @@
 #include "board/irq.h" // irq_disable
 #include "command.h" // DECL_COMMAND
 #include "sched.h" // DECL_TASK
+#include "sensor_bulk.h" // sensor_bulk_report
 #include "spicmds.h" // spidev_transfer
 
 enum { SA_CHIP_A1333, SA_CHIP_AS5047D, SA_CHIP_TLE5012B, SA_CHIP_MAX };
@@ -29,15 +30,16 @@ struct spi_angle {
     struct timer timer;
     uint32_t rest_ticks;
     struct spidev_s *spi;
-    uint16_t sequence;
-    uint8_t flags, chip_type, data_count, time_shift, overflow;
-    uint8_t data[48];
+    uint8_t flags, chip_type, time_shift, overflow;
+    struct sensor_bulk sb;
 };
 
 enum {
     SA_PENDING = 1<<2,
 };
 
+#define BYTES_PER_SAMPLE 3
+
 static struct task_wake angle_wake;
 
 // Event handler that wakes spi_angle_task() periodically
@@ -72,32 +74,22 @@ command_config_spi_angle(uint32_t *args)
 DECL_COMMAND(command_config_spi_angle,
              "config_spi_angle oid=%c spi_oid=%c spi_angle_type=%c");
 
-// Report local measurement buffer
-static void
-angle_report(struct spi_angle *sa, uint8_t oid)
-{
-    sendf("spi_angle_data oid=%c sequence=%hu data=%*s"
-          , oid, sa->sequence, sa->data_count, sa->data);
-    sa->data_count = 0;
-    sa->sequence++;
-}
-
 // Send spi_angle_data message if buffer is full
 static void
 angle_check_report(struct spi_angle *sa, uint8_t oid)
 {
-    if (sa->data_count + 3 > ARRAY_SIZE(sa->data))
-        angle_report(sa, oid);
+    if (sa->sb.data_count + BYTES_PER_SAMPLE > ARRAY_SIZE(sa->sb.data))
+        sensor_bulk_report(&sa->sb, oid);
 }
 
 // Add an entry to the measurement buffer
 static void
 angle_add(struct spi_angle *sa, uint_fast8_t tcode, uint_fast16_t data)
 {
-    sa->data[sa->data_count] = tcode;
-    sa->data[sa->data_count + 1] = data;
-    sa->data[sa->data_count + 2] = data >> 8;
-    sa->data_count += 3;
+    sa->sb.data[sa->sb.data_count] = tcode;
+    sa->sb.data[sa->sb.data_count + 1] = data;
+    sa->sb.data[sa->sb.data_count + 2] = data >> 8;
+    sa->sb.data_count += BYTES_PER_SAMPLE;
 }
 
 // Add an error indicator to the measurement buffer
@@ -237,8 +229,7 @@ command_query_spi_angle(uint32_t *args)
     // Start new measurements query
     sa->timer.waketime = args[1];
     sa->rest_ticks = args[2];
-    sa->sequence = 0;
-    sa->data_count = 0;
+    sensor_bulk_reset(&sa->sb);
     sa->time_shift = args[3];
     sched_add_timer(&sa->timer);
 }
diff --git a/src/sensor_bulk.c b/src/sensor_bulk.c
new file mode 100644
index 00000000..9b5c782c
--- /dev/null
+++ b/src/sensor_bulk.c
@@ -0,0 +1,38 @@
+// Helper code for collecting and sending bulk sensor measurements
+//
+// Copyright (C) 2020-2023  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include "command.h" // sendf
+#include "sensor_bulk.h" // sensor_bulk_report
+
+// Reset counters
+void
+sensor_bulk_reset(struct sensor_bulk *sb)
+{
+    sb->sequence = 0;
+    sb->possible_overflows = 0;
+    sb->data_count = 0;
+}
+
+// Report local measurement buffer
+void
+sensor_bulk_report(struct sensor_bulk *sb, uint8_t oid)
+{
+    sendf("sensor_bulk_data oid=%c sequence=%hu data=%*s"
+          , oid, sb->sequence, sb->data_count, sb->data);
+    sb->data_count = 0;
+    sb->sequence++;
+}
+
+// Report buffer and fifo status
+void
+sensor_bulk_status(struct sensor_bulk *sb, uint8_t oid
+                   , uint32_t time1, uint32_t query_ticks, uint32_t fifo)
+{
+    sendf("sensor_bulk_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
+          " buffered=%u possible_overflows=%hu"
+          , oid, time1, query_ticks, sb->sequence
+          , sb->data_count + fifo, sb->possible_overflows);
+}
diff --git a/src/sensor_bulk.h b/src/sensor_bulk.h
new file mode 100644
index 00000000..9c130bea
--- /dev/null
+++ b/src/sensor_bulk.h
@@ -0,0 +1,15 @@
+#ifndef __SENSOR_BULK_H
+#define __SENSOR_BULK_H
+
+struct sensor_bulk {
+    uint16_t sequence, possible_overflows;
+    uint8_t data_count;
+    uint8_t data[52];
+};
+
+void sensor_bulk_reset(struct sensor_bulk *sb);
+void sensor_bulk_report(struct sensor_bulk *sb, uint8_t oid);
+void sensor_bulk_status(struct sensor_bulk *sb, uint8_t oid
+                        , uint32_t time1, uint32_t query_ticks, uint32_t fifo);
+
+#endif // sensor_bulk.h
diff --git a/src/sensor_lis2dw.c b/src/sensor_lis2dw.c
index 06dd3206..579ee1f7 100644
--- a/src/sensor_lis2dw.c
+++ b/src/sensor_lis2dw.c
@@ -11,6 +11,7 @@
 #include "basecmd.h" // oid_alloc
 #include "command.h" // DECL_COMMAND
 #include "sched.h" // DECL_TASK
+#include "sensor_bulk.h" // sensor_bulk_report
 #include "spicmds.h" // spidev_transfer
 
 #define LIS_AR_DATAX0 0x28
@@ -18,13 +19,14 @@
 #define LIS_FIFO_CTRL    0x2E
 #define LIS_FIFO_SAMPLES 0x2F
 
+#define BYTES_PER_SAMPLE 6
+
 struct lis2dw {
     struct timer timer;
     uint32_t rest_ticks;
     struct spidev_s *spi;
-    uint16_t sequence, limit_count;
-    uint8_t flags, data_count;
-    uint8_t data[48];
+    uint8_t flags;
+    struct sensor_bulk sb;
 };
 
 enum {
@@ -53,27 +55,6 @@ command_config_lis2dw(uint32_t *args)
 }
 DECL_COMMAND(command_config_lis2dw, "config_lis2dw oid=%c spi_oid=%c");
 
-// Report local measurement buffer
-static void
-lis2dw_report(struct lis2dw *ax, uint8_t oid)
-{
-    sendf("lis2dw_data oid=%c sequence=%hu data=%*s"
-          , oid, ax->sequence, ax->data_count, ax->data);
-    ax->data_count = 0;
-    ax->sequence++;
-}
-
-// Report buffer and fifo status
-static void
-lis2dw_status(struct lis2dw *ax, uint_fast8_t oid
-            , uint32_t time1, uint32_t time2, uint_fast8_t fifo)
-{
-    sendf("lis2dw_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
-          " buffered=%c fifo=%c limit_count=%hu"
-          , oid, time1, time2-time1, ax->sequence
-          , ax->data_count, fifo, ax->limit_count);
-}
-
 // Helper code to reschedule the lis2dw_event() timer
 static void
 lis2dw_reschedule_timer(struct lis2dw *ax)
@@ -93,7 +74,7 @@ lis2dw_query(struct lis2dw *ax, uint8_t oid)
     uint8_t fifo_empty,fifo_ovrn = 0;
 
     msg[0] = LIS_AR_DATAX0 | LIS_AM_READ ;
-    uint8_t *d = &ax->data[ax->data_count];
+    uint8_t *d = &ax->sb.data[ax->sb.data_count];
 
     spidev_transfer(ax->spi, 1, sizeof(msg), msg);
 
@@ -108,13 +89,13 @@ lis2dw_query(struct lis2dw *ax, uint8_t oid)
     d[4] = msg[5]; // z low bits
     d[5] = msg[6]; // z high bits
 
-    ax->data_count += 6;
-    if (ax->data_count + 6 > ARRAY_SIZE(ax->data))
-        lis2dw_report(ax, oid);
+    ax->sb.data_count += BYTES_PER_SAMPLE;
+    if (ax->sb.data_count + BYTES_PER_SAMPLE > ARRAY_SIZE(ax->sb.data))
+        sensor_bulk_report(&ax->sb, oid);
 
     // Check fifo status
     if (fifo_ovrn)
-        ax->limit_count++;
+        ax->sb.possible_overflows++;
 
     // check if we need to run the task again (more packets in fifo?)
     if (!fifo_empty) {
@@ -165,8 +146,7 @@ command_query_lis2dw(uint32_t *args)
     ax->timer.waketime = args[1];
     ax->rest_ticks = args[2];
     ax->flags = LIS_HAVE_START;
-    ax->sequence = ax->limit_count = 0;
-    ax->data_count = 0;
+    sensor_bulk_reset(&ax->sb);
     sched_add_timer(&ax->timer);
 }
 DECL_COMMAND(command_query_lis2dw,
@@ -180,7 +160,8 @@ command_query_lis2dw_status(uint32_t *args)
     uint32_t time1 = timer_read_time();
     spidev_transfer(ax->spi, 1, sizeof(msg), msg);
     uint32_t time2 = timer_read_time();
-    lis2dw_status(ax, args[0], time1, time2, msg[1]&0x1f);
+    sensor_bulk_status(&ax->sb, args[0], time1, time2-time1
+                       , (msg[1] & 0x1f) * BYTES_PER_SAMPLE);
 }
 DECL_COMMAND(command_query_lis2dw_status, "query_lis2dw_status oid=%c");
 
diff --git a/src/sensor_mpu9250.c b/src/sensor_mpu9250.c
index 7792b4d8..d52de811 100644
--- a/src/sensor_mpu9250.c
+++ b/src/sensor_mpu9250.c
@@ -12,6 +12,7 @@
 #include "basecmd.h" // oid_alloc
 #include "command.h" // DECL_COMMAND
 #include "sched.h" // DECL_TASK
+#include "sensor_bulk.h" // sensor_bulk_report
 #include "board/gpio.h" // i2c_read
 #include "i2ccmds.h" // i2cdev_oid_lookup
 
@@ -46,11 +47,9 @@ struct mpu9250 {
     struct timer timer;
     uint32_t rest_ticks;
     struct i2cdev_s *i2c;
-    uint16_t sequence, limit_count, fifo_max, fifo_pkts_bytes;
-    uint8_t flags, data_count;
-    // msg size must be <= 255 due to Klipper api
-    // = SAMPLES_PER_BLOCK (from mpu9250.py) * BYTES_PER_FIFO_ENTRY + 1
-    uint8_t data[BYTES_PER_BLOCK];
+    uint16_t fifo_max, fifo_pkts_bytes;
+    uint8_t flags;
+    struct sensor_bulk sb;
 };
 
 enum {
@@ -92,27 +91,6 @@ command_config_mpu9250(uint32_t *args)
 }
 DECL_COMMAND(command_config_mpu9250, "config_mpu9250 oid=%c i2c_oid=%c");
 
-// Report local measurement buffer
-static void
-mp9250_report(struct mpu9250 *mp, uint8_t oid)
-{
-    sendf("mpu9250_data oid=%c sequence=%hu data=%*s"
-          , oid, mp->sequence, mp->data_count, mp->data);
-    mp->data_count = 0;
-    mp->sequence++;
-}
-
-// Report buffer and fifo status
-static void
-mp9250_status(struct mpu9250 *mp, uint_fast8_t oid
-            , uint32_t time1, uint32_t time2, uint16_t fifo)
-{
-    sendf("mpu9250_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
-          " buffered=%c fifo=%u limit_count=%hu"
-          , oid, time1, time2-time1, mp->sequence
-          , mp->data_count, fifo, mp->limit_count);
-}
-
 // Helper code to reschedule the mpu9250_event() timer
 static void
 mp9250_reschedule_timer(struct mpu9250 *mp)
@@ -135,10 +113,10 @@ mp9250_query(struct mpu9250 *mp, uint8_t oid)
     if (mp->fifo_pkts_bytes >= BYTES_PER_BLOCK) {
         uint8_t reg = AR_FIFO;
         i2c_read(mp->i2c->i2c_config, sizeof(reg), &reg
-                 , BYTES_PER_BLOCK, &mp->data[0]);
-        mp->data_count = BYTES_PER_BLOCK;
+                 , BYTES_PER_BLOCK, &mp->sb.data[0]);
+        mp->sb.data_count = BYTES_PER_BLOCK;
         mp->fifo_pkts_bytes -= BYTES_PER_BLOCK;
-        mp9250_report(mp, oid);
+        sensor_bulk_report(&mp->sb, oid);
     }
 
     // If we have enough bytes remaining to fill another report wake again
@@ -214,9 +192,7 @@ command_query_mpu9250(uint32_t *args)
     mp->timer.waketime = args[1];
     mp->rest_ticks = args[2];
     mp->flags = AX_HAVE_START;
-    mp->sequence = 0;
-    mp->limit_count = 0;
-    mp->data_count = 0;
+    sensor_bulk_reset(&mp->sb);
     mp->fifo_max = 0;
     mp->fifo_pkts_bytes = 0;
     sched_add_timer(&mp->timer);
@@ -235,7 +211,7 @@ command_query_mpu9250_status(uint32_t *args)
     i2c_read(mp->i2c->i2c_config, sizeof(int_reg), int_reg, sizeof(int_msg),
              &int_msg);
     if (int_msg & FIFO_OVERFLOW_INT)
-        mp->limit_count++;
+        mp->sb.possible_overflows++;
 
     // Read latest FIFO count (with precise timing)
     uint8_t reg[] = {AR_FIFO_COUNT_H};
@@ -246,7 +222,7 @@ command_query_mpu9250_status(uint32_t *args)
     uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];
 
     // Report status
-    mp9250_status(mp, args[0], time1, time2, fifo_bytes / BYTES_PER_FIFO_ENTRY);
+    sensor_bulk_status(&mp->sb, args[0], time1, time2-time1, fifo_bytes);
 }
 DECL_COMMAND(command_query_mpu9250_status, "query_mpu9250_status oid=%c");