From 8e1da5c7a78dbb60a2c4fdd0eae343a1ccc6b5c8 Mon Sep 17 00:00:00 2001
From: hitech95 <nicveronese@gmail.com>
Date: Tue, 27 Sep 2022 23:44:37 +0200
Subject: [PATCH] I2C slave: allow function wrapped callbacks

Signed-off-by: hitech95 <nicveronese@gmail.com>
---
 libraries/Wire/src/Wire.cpp |  4 ++--
 libraries/Wire/src/Wire.h   | 16 ++++++++++++----
 2 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/libraries/Wire/src/Wire.cpp b/libraries/Wire/src/Wire.cpp
index dc9d14ba30..1d8d3ee556 100644
--- a/libraries/Wire/src/Wire.cpp
+++ b/libraries/Wire/src/Wire.cpp
@@ -438,13 +438,13 @@ void TwoWire::onRequestService(i2c_t *obj)
 }
 
 // sets function called on slave write
-void TwoWire::onReceive(void (*function)(int))
+void TwoWire::onReceive(cb_function_receive_t function)
 {
   user_onReceive = function;
 }
 
 // sets function called on slave read
-void TwoWire::onRequest(void (*function)(void))
+void TwoWire::onRequest(cb_function_request_t function)
 {
   user_onRequest = function;
 }
diff --git a/libraries/Wire/src/Wire.h b/libraries/Wire/src/Wire.h
index 23fb564b7e..e5a5d8d4e3 100644
--- a/libraries/Wire/src/Wire.h
+++ b/libraries/Wire/src/Wire.h
@@ -22,6 +22,8 @@
 #ifndef TwoWire_h
 #define TwoWire_h
 
+#include <functional>
+
 #include "Stream.h"
 #include "Arduino.h"
 extern "C" {
@@ -40,6 +42,10 @@ extern "C" {
 #define WIRE_HAS_END 1
 
 class TwoWire : public Stream {
+  public:
+    typedef std::function<void(int)> cb_function_receive_t;
+    typedef std::function<void(void)> cb_function_request_t;
+
   private:
     uint8_t *rxBuffer;
     uint16_t rxBufferAllocated;
@@ -56,8 +62,9 @@ class TwoWire : public Stream {
     uint8_t ownAddress;
     i2c_t _i2c;
 
-    void (*user_onRequest)(void);
-    void (*user_onReceive)(int);
+    std::function<void(int)> user_onReceive;
+    std::function<void(void)> user_onRequest;
+
     static void onRequestService(i2c_t *);
     static void onReceiveService(i2c_t *);
 
@@ -110,8 +117,9 @@ class TwoWire : public Stream {
     virtual int read(void);
     virtual int peek(void);
     virtual void flush(void);
-    void onReceive(void (*)(int));
-    void onRequest(void (*)(void));
+
+    void onReceive(cb_function_receive_t callback);
+    void onRequest(cb_function_request_t callback);
 
     inline size_t write(unsigned long n)
     {