From 169e65a922bfda0d91fbfe22b8f42473d963105e Mon Sep 17 00:00:00 2001
From: Theo Arends <11044339+arendst@users.noreply.github.com>
Date: Sat, 15 Oct 2022 14:33:21 +0200
Subject: [PATCH] Add ESP32 non-persistent phy selection

---
 .../ESP32-to-ESP8266-compat/src/ESP32Wifi.cpp     | 15 ++++++++++++---
 .../ESP32-to-ESP8266-compat/src/ESP8266WiFi.h     |  6 ++++++
 tasmota/tasmota.ino                               |  1 +
 tasmota/tasmota_support/support_command.ino       |  8 ++++++--
 tasmota/tasmota_support/support_wifi.ino          |  5 +++++
 5 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/lib/libesp32/ESP32-to-ESP8266-compat/src/ESP32Wifi.cpp b/lib/libesp32/ESP32-to-ESP8266-compat/src/ESP32Wifi.cpp
index 9f932040c..b700f2955 100644
--- a/lib/libesp32/ESP32-to-ESP8266-compat/src/ESP32Wifi.cpp
+++ b/lib/libesp32/ESP32-to-ESP8266-compat/src/ESP32Wifi.cpp
@@ -37,14 +37,23 @@ int WiFiClass32::getPhyMode() {
   int phy_mode = 0;  // " BGNL"
   uint8_t protocol_bitmap;
   if (esp_wifi_get_protocol(WIFI_IF_STA, &protocol_bitmap) == ESP_OK) {
-    if (protocol_bitmap & 1) { phy_mode = 1; }  // 11b
-    if (protocol_bitmap & 2) { phy_mode = 2; }  // 11g
-    if (protocol_bitmap & 4) { phy_mode = 3; }  // 11n
+    if (protocol_bitmap & 1) { phy_mode = WIFI_PHY_MODE_11B; }  // 1 = 11b
+    if (protocol_bitmap & 2) { phy_mode = WIFI_PHY_MODE_11G; }  // 2 = 11bg
+    if (protocol_bitmap & 4) { phy_mode = WIFI_PHY_MODE_11N; }  // 3 = 11bgn
     if (protocol_bitmap & 8) { phy_mode = 4; }  // Low rate
   }
   return phy_mode;
 }
 
+bool WiFiClass32::setPhyMode(WiFiPhyMode_t mode) {
+  uint8_t protocol_bitmap = WIFI_PROTOCOL_11B;     // 1
+  switch (mode) {
+    case 3: protocol_bitmap |= WIFI_PROTOCOL_11N;  // 4
+    case 2: protocol_bitmap |= WIFI_PROTOCOL_11G;  // 2
+  }
+  return (ESP_OK == esp_wifi_set_protocol(WIFI_IF_STA, protocol_bitmap));
+}
+
 void WiFiClass32::wps_disable() {
 }
 
diff --git a/lib/libesp32/ESP32-to-ESP8266-compat/src/ESP8266WiFi.h b/lib/libesp32/ESP32-to-ESP8266-compat/src/ESP8266WiFi.h
index 97f9c842c..e7a760164 100644
--- a/lib/libesp32/ESP32-to-ESP8266-compat/src/ESP8266WiFi.h
+++ b/lib/libesp32/ESP32-to-ESP8266-compat/src/ESP8266WiFi.h
@@ -32,6 +32,11 @@
 #define WIFI_LIGHT_SLEEP	1
 #define WIFI_MODEM_SLEEP	2
 
+typedef enum WiFiPhyMode
+{
+    WIFI_PHY_MODE_11B = 1, WIFI_PHY_MODE_11G = 2, WIFI_PHY_MODE_11N = 3
+} WiFiPhyMode_t;
+
 class WiFiClass32 : public WiFiClass
 {
 public:
@@ -41,6 +46,7 @@ public:
     }
     static void setSleepMode(int iSleepMode);
     static int getPhyMode();
+    static bool setPhyMode(WiFiPhyMode_t mode);
 
     static void wps_disable();
     static void setOutputPower(int n);
diff --git a/tasmota/tasmota.ino b/tasmota/tasmota.ino
index 493028b1e..4a820a229 100644
--- a/tasmota/tasmota.ino
+++ b/tasmota/tasmota.ino
@@ -114,6 +114,7 @@ struct WIFI {
   uint8_t wifi_test_counter = 0;
   uint16_t save_data_counter = 0;
   uint8_t old_wificonfig = MAX_WIFI_OPTION; // means "nothing yet saved here"
+  uint8_t phy_mode = 0;
   bool wifi_test_AP_TIMEOUT = false;
   bool wifi_Test_Restart = false;
   bool wifi_Test_Save_SSID2 = false;
diff --git a/tasmota/tasmota_support/support_command.ino b/tasmota/tasmota_support/support_command.ino
index 8c1a1744e..6086572db 100644
--- a/tasmota/tasmota_support/support_command.ino
+++ b/tasmota/tasmota_support/support_command.ino
@@ -2478,10 +2478,14 @@ void CmndWifi(void)
       WifiEnable();
 #endif
     }
-#ifdef ESP8266
   } else if ((XdrvMailbox.payload >= 2) && (XdrvMailbox.payload <= 4)) {
-    WiFi.setPhyMode(WiFiPhyMode_t(XdrvMailbox.payload - 1));  // 1-B/2-BG/3-BGN
+    // Wifi 2 = B
+    // Wifi 3 = BG
+    // Wifi 4 = BGN
+#ifdef ESP32
+    Wifi.phy_mode = XdrvMailbox.payload - 1;
 #endif
+    WiFi.setPhyMode(WiFiPhyMode_t(XdrvMailbox.payload - 1));  // 1-B/2-BG/3-BGN
   }
   Response_P(PSTR("{\"" D_JSON_WIFI "\":\"%s\",\"" D_JSON_WIFI_MODE "\":\"11%c\"}"), GetStateText(Settings->flag4.network_wifi), pgm_read_byte(&kWifiPhyMode[WiFi.getPhyMode() & 0x3]) );
 }
diff --git a/tasmota/tasmota_support/support_wifi.ino b/tasmota/tasmota_support/support_wifi.ino
index 9bce4cda4..0700fcc2d 100644
--- a/tasmota/tasmota_support/support_wifi.ino
+++ b/tasmota/tasmota_support/support_wifi.ino
@@ -217,6 +217,11 @@ void WifiBegin(uint8_t flag, uint8_t channel)
   WiFiSetSleepMode();
 //  if (WiFi.getPhyMode() != WIFI_PHY_MODE_11N) { WiFi.setPhyMode(WIFI_PHY_MODE_11N); }  // B/G/N
 //  if (WiFi.getPhyMode() != WIFI_PHY_MODE_11G) { WiFi.setPhyMode(WIFI_PHY_MODE_11G); }  // B/G
+#ifdef ESP32
+  if (Wifi.phy_mode) {
+    WiFi.setPhyMode(WiFiPhyMode_t(Wifi.phy_mode));  // 1-B/2-BG/3-BGN
+  }
+#endif
   if (!WiFi.getAutoConnect()) { WiFi.setAutoConnect(true); }
 //  WiFi.setAutoReconnect(true);
   switch (flag) {