From 76d6170ec9c307fb4e2d1bcc356dfad7f873f8b3 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Thu, 19 May 2022 11:54:22 +0530
Subject: [PATCH] AP_HAL_ChibiOS: add mechanism to set register rw register
 using device name for SPI

---
 libraries/AP_HAL_ChibiOS/SPIDevice.cpp | 19 +++++++++++++++++++
 libraries/AP_HAL_ChibiOS/SPIDevice.h   |  5 ++++-
 2 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp
index 73f7d99bf7..223be6bdb1 100644
--- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp
+++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp
@@ -126,6 +126,7 @@ SPIDevice::SPIDevice(SPIBus &_bus, SPIDesc &_device_desc)
              device_desc.name,
              (unsigned)bus.bus, (unsigned)device_desc.device);
     AP_HAL::SPIDevice::setup_bankselect_callback(device_desc.bank_select_cb);
+    AP_HAL::SPIDevice::set_register_rw_callback(device_desc.register_rw_cb);
     //printf("SPI device %s on %u:%u at speed %u mode %u\n",
     //       device_desc.name,
     //       (unsigned)bus.bus, (unsigned)device_desc.device,
@@ -441,6 +442,24 @@ SPIDeviceManager::get_device(const char *name)
     return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(new SPIDevice(*busp, desc));
 }
 
+void SPIDeviceManager::set_register_rw_callback(const char* name, AP_HAL::Device::RegisterRWCb cb)
+{
+    /* Find the bus description in the table */
+    uint8_t i;
+    for (i = 0; i<ARRAY_SIZE(device_table); i++) {
+        if (strcmp(device_table[i].name, name) == 0) {
+            break;
+        }
+    }
+    if (i == ARRAY_SIZE(device_table)) {
+        return;
+    }
+
+    device_table[i].register_rw_cb = cb;
+
+}
+
+
 #ifdef HAL_SPI_CHECK_CLOCK_FREQ
 
 /*
diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.h b/libraries/AP_HAL_ChibiOS/SPIDevice.h
index aa1e48ad3c..325fc26460 100644
--- a/libraries/AP_HAL_ChibiOS/SPIDevice.h
+++ b/libraries/AP_HAL_ChibiOS/SPIDevice.h
@@ -52,7 +52,7 @@ struct SPIDesc {
         : name(_name), bus(_bus), device(_device),
           pal_line(_pal_line), mode(_mode),
           lowspeed(_lowspeed), highspeed(_highspeed),
-          bank_select_cb(nullptr)
+          bank_select_cb(nullptr), register_rw_cb(nullptr)
     {
     }
 
@@ -64,6 +64,7 @@ struct SPIDesc {
     uint32_t lowspeed;
     uint32_t highspeed;
     AP_HAL::Device::BankSelectCb bank_select_cb;
+    AP_HAL::Device::RegisterRWCb register_rw_cb;
 };
 
 
@@ -160,6 +161,8 @@ public:
 
     AP_HAL::OwnPtr<AP_HAL::SPIDevice> get_device(const char *name) override;
 
+    void set_register_rw_callback(const char* name, AP_HAL::Device::RegisterRWCb cb) override;
+
 private:
     static SPIDesc device_table[];
     SPIBus *buses;