From 57dd0ca9eaabe325f8e9195907cc0d4ba965a26d Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Wed, 15 Jul 2020 20:59:13 +0530
Subject: [PATCH] AP_HAL: add support for Bank based addressing

---
 libraries/AP_HAL/Device.cpp | 21 ++++++++++-
 libraries/AP_HAL/Device.h   | 69 +++++++++++++++++++++++++++++++++++++
 2 files changed, 89 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL/Device.cpp b/libraries/AP_HAL/Device.cpp
index 6376ec0db6..3d5bff9255 100644
--- a/libraries/AP_HAL/Device.cpp
+++ b/libraries/AP_HAL/Device.cpp
@@ -58,13 +58,18 @@ bool AP_HAL::Device::setup_checked_registers(uint8_t nregs, uint8_t frequency)
   set value of one checked register
  */
 void AP_HAL::Device::set_checked_register(uint8_t reg, uint8_t val)
+{
+    set_checked_register(0, reg, val);
+}
+
+void AP_HAL::Device::set_checked_register(uint8_t bank, uint8_t reg, uint8_t val)
 {
     if (_checked.regs == nullptr) {
         return;
     }
     struct checkreg *regs = _checked.regs;
     for (uint8_t i=0; i<_checked.n_set; i++) {
-        if (regs[i].regnum == reg) {
+        if (regs[i].regnum == reg && regs[i].bank == bank) {
             regs[i].value = val;
             return;
         }
@@ -74,6 +79,7 @@ void AP_HAL::Device::set_checked_register(uint8_t reg, uint8_t val)
                (unsigned)reg, (unsigned)get_bus_id());
         return;
     }
+    regs[_checked.n_set].bank = bank;
     regs[_checked.n_set].regnum = reg;
     regs[_checked.n_set].value = val;
     _checked.n_set++;
@@ -94,6 +100,19 @@ bool AP_HAL::Device::check_next_register(void)
 
     struct checkreg &reg = _checked.regs[_checked.next];
     uint8_t v;
+
+    if (_bank_select) {
+        if (!_bank_select(reg.bank)) {
+        // Cannot set bank
+#if 0
+        printf("Device 0x%x set bank 0x%02x\n",
+               (unsigned)get_bus_id(),
+               (unsigned)reg.bank);
+#endif
+            return false;
+        }
+    }
+
     if (!read_registers(reg.regnum, &v, 1) || v != reg.value) {
         // a register has changed value unexpectedly. Try changing it back
         // and re-check it next time
diff --git a/libraries/AP_HAL/Device.h b/libraries/AP_HAL/Device.h
index 6700a91575..6bf731e894 100644
--- a/libraries/AP_HAL/Device.h
+++ b/libraries/AP_HAL/Device.h
@@ -42,6 +42,8 @@ public:
     FUNCTOR_TYPEDEF(PeriodicCb, void);
     typedef void* PeriodicHandle;
 
+    FUNCTOR_TYPEDEF(BankSelectCb, bool, uint8_t);
+
     Device(enum BusType type)
     {
         _bus_id.devid_s.bus_type = type;
@@ -132,6 +134,56 @@ public:
         return transfer(buf, sizeof(buf), nullptr, 0);
     }
 
+    /**
+     * Wrapper function over #transfer() to call bank selection callback
+     * and then invoke the transfer call
+     *
+     * Return: true on a successful transfer, false on failure.
+     */
+    bool transfer_bank(uint8_t bank, const uint8_t *send, uint32_t send_len,
+                          uint8_t *recv, uint32_t recv_len) {
+        if (_bank_select) {
+            if (!_bank_select(bank)) {
+                return false;
+            }
+        }
+        return transfer(send, send_len, recv, recv_len);
+    }
+
+    /**
+     * Wrapper function over #transfer_bank() to read recv_len registers, starting
+     * by first_reg, into the array pointed by recv. The read flag passed to
+     * #set_read_flag(uint8_t) is ORed with first_reg before performing the
+     * transfer.
+     *
+     * Return: true on a successful transfer, false on failure.
+     */
+    bool read_bank_registers(uint8_t bank, uint8_t first_reg, uint8_t *recv, uint32_t recv_len)
+    {
+        first_reg |= _read_flag;
+        return transfer_bank(bank, &first_reg, 1, recv, recv_len);
+    }
+
+    /**
+     * Wrapper function over #transfer_bank() to write a byte to the register reg.
+     * The transfer is done by sending reg and val in that order.
+     *
+     * Return: true on a successful transfer, false on failure.
+     */
+    bool write_bank_register(uint8_t bank, uint8_t reg, uint8_t val, bool checked=false)
+    {
+        uint8_t buf[2] = { reg, val };
+        if (checked) {
+            set_checked_register(bank, reg, val);
+        }
+        return transfer_bank(bank, buf, sizeof(buf), nullptr, 0);
+    }
+
+    /**
+     * set a value for a checked register in a bank
+     */
+    void set_checked_register(uint8_t bank, uint8_t reg, uint8_t val);
+
     /**
      * set a value for a checked register
      */
@@ -198,6 +250,20 @@ public:
      */
     virtual bool unregister_callback(PeriodicHandle h) { return false; }
 
+    /*
+     * Sets a bank_select callback to be used for bank selection during register check
+     */
+    virtual void setup_bankselect_callback(BankSelectCb bank_select) {
+        _bank_select = bank_select;
+    }
+
+    /*
+     * Sets a bank_select callback to be used for bank selection during register check
+     */
+    virtual void deregister_bankselect_callback() {
+        _bank_select = nullptr;
+    }
+
 
     /*
         allows to set callback that will be called after DMA transfer complete.
@@ -322,8 +388,11 @@ protected:
     }
 
 private:
+    BankSelectCb _bank_select;
+
     // checked registers
     struct checkreg {
+        uint8_t bank;
         uint8_t regnum;
         uint8_t value;
     };