Fix radio-related issues in mts-io4.4.4

4 files changed, 95 insertions, 29 deletions

diff --git a/configure.ac b/configure.ac
index 1e8e1df..9332560 100644
--- a/configure.ac
+++ b/configure.ac
@@ -1,4 +1,4 @@
-AC_INIT([mts-io], [4.4.3])
+AC_INIT([mts-io], [4.4.4])
 AC_CONFIG_SRCDIR([util/mts_util_lora2_reset.c])
 AM_INIT_AUTOMAKE
 AM_CONFIG_HEADER([config.h])
diff --git a/io-module/machine/mtcpm.c b/io-module/machine/mtcpm.c
index df771cd..dfffef6 100644
--- a/io-module/machine/mtcpm.c
+++ b/io-module/machine/mtcpm.c
@@ -202,40 +202,40 @@ static struct attribute *mtcpm_platform_attributes[] = {
 
 	&dev_attr_usbhub_reset.attr,
 	&dev_attr_eth_reset.attr,
-        
-  // radio feature is last to be able to 
-  // easily remove radio.
-  // is_radio_power_attr_mtcdt() searches
-  // for this for truncation.
-    &dev_attr_radio_power.attr,  /* Must be first radio attribute */
+        &dev_attr_has_radio.attr,
+
+	// radio-reset must be first for mtcdt3
 	&dev_attr_radio_reset.attr,
 
 	&dev_attr_radio_reset_backoffs.attr,
 	&dev_attr_radio_reset_backoff_index.attr,
 	&dev_attr_radio_reset_backoff_seconds.attr,
 		
+	NULL,
+};
+
+static struct attribute *mtcpm_wifi_bt_attributes[] = {
     &dev_attr_wifi_reset_mtcpm.attr,
 	&dev_attr_bt_reset_mtcpm.attr,
 	&dev_attr_wifi_bt_int_mtcpm.attr,
-		
-    &dev_attr_gnss_reset_mtcpm.attr,
-	&dev_attr_gnss_int_mtcpm.attr,
 	&dev_attr_wifi_mac_mtcpm.attr,
-        &dev_attr_bluetooth_mac_mtcpm.attr,
-	NULL,
+	&dev_attr_bluetooth_mac_mtcpm.attr,
 };
 
-
+static struct attribute *mtcpm_gnss_attributes[] = {
+	&dev_attr_gnss_reset_mtcpm.attr,
+	&dev_attr_gnss_int_mtcpm.attr,
+};
 
 static struct attribute_group mtcpm_platform_attribute_group = {
 	.attrs = mtcpm_platform_attributes
 };
 
 static int 
-is_radio_power_attr_mtcpm(struct attribute *attr)
+is_radio_reset_attr_mtcpm(struct attribute *attr)
 {
     return (attr ==  &dev_attr_radio_power.attr);
-}
+};
 
 static struct attribute *mtcpm_lora_attributes[] = {
 		&dev_attr_lora_eui_mtcpm.attr,
@@ -295,3 +295,8 @@ static void mts_cpu_dir_delete(void)
 	}
 }
 
+static int
+is_radio_reset_attr_mtcdt(struct attribute *attr)
+{
+    return (attr ==  &dev_attr_radio_reset.attr);
+}
diff --git a/io-module/mts-io.c b/io-module/mts-io.c
index 3df4732..807df78 100644
--- a/io-module/mts-io.c
+++ b/io-module/mts-io.c
@@ -656,15 +656,16 @@ struct attribute **freelater = NULL; // Storage to free when driver is unloaded.
 static int 
 mts_id_eeprom_load(void)
 {
-	int i, j = 0;
-	char buf[64] = {0};
-	char* ptr;
-	int attr_blength;     // Byte length of base attribute array
-	int current_blength;  // Current length in bytes of attribute array
-	int current_count;    // Number of items in array
-	struct attribute **all_attrs = NULL;
+  int i, j = 0;
+  char buf[64] = {0};
+  char* ptr;
+  int attr_blength;     // Byte length of base attribute array
+  int current_blength;  // Current length in bytes of attribute array
+  int current_count;    // Number of items in array
+  struct attribute **all_attrs = NULL;
   char *tmp;
   int noradio, has_cellular_capaflag = 0;
+  int need_append;
   int ret;
   const struct firmware* fw = NULL;
 
@@ -701,6 +702,7 @@ mts_id_eeprom_load(void)
             ((tmp=HW_VERSION_MTCAP_0_2),(mts_hw_version=MTCAP_0_2),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0) ||
             ((tmp=HW_VERSION_MTCAP_0_3),(mts_hw_version=MTCAP_0_3),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0)) {
                 int need_radio_enable = 0;
+                need_append = 0;
                 current_blength = attr_blength = sizeof  mtcap_0_0_platform_attributes;
                 current_blength -= sizeof(struct attribute *);  /* Length without terminating NULL */
                 /* See if we have no radio, and if so, prune out the stuff that follows */
@@ -723,20 +725,24 @@ mts_id_eeprom_load(void)
                 }
 		if(DEVICE_CAPA(id_eeprom.capa, CAPA_GPS)) {
 			attr_blength += sizeof mtcap_0_3_gnss_attributes;
+                        need_append = 1;
 		}
 		if(DEVICE_CAPA(id_eeprom.capa, CAPA_SUPERCAP)) {
 			attr_blength += sizeof mtcap_0_3_supercap_attributes;
+                        need_append = 1;
 		}
 		if(DEVICE_CAPA(id_eeprom.capa, CAPA_WIFI)) {
 			attr_blength += sizeof mtcap_0_0_wifi_attributes;
+                        need_append = 1;
 		}
 		if(((tmp=HW_VERSION_MTCAP_0_2),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0) ||
                     ((tmp=HW_VERSION_MTCAP_0_3),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0)) {
                         need_radio_enable = 1;
+                        need_append = 1;
 			attr_blength += sizeof mtcap_0_2_enable_radio_attribute;
 		}
 
-		if (current_blength+(sizeof(struct attribute *)) != attr_blength) {
+		if (need_append) {
 			freelater = all_attrs = kmalloc(attr_blength,GFP_KERNEL);
 			current_count = current_blength/(sizeof (struct attribute *));
 			memcpy(all_attrs,mtcap_0_0_platform_attributes,current_blength);
@@ -821,7 +827,8 @@ mts_id_eeprom_load(void)
 		log_info("detected board %s", HW_VERSION_MTHS_0_1);
 	} else if (((tmp=HW_VERSION_MTCDT_0_1),(mts_hw_version=MTCDT_0_1),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0) ||
             ((tmp=HW_VERSION_MTCDTIP_0_0),(mts_hw_version=MTCDTIP_0_0),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0)) {
-		current_blength = attr_blength = sizeof  mtcdt_0_1_platform_attributes;
+                need_append = 0;
+                current_blength = attr_blength = sizeof  mtcdt_0_1_platform_attributes;
                 current_blength -= sizeof(struct attribute *);  /* Length without terminating NULL */
 
                 /* See if we have no radio, and if so, prune out the stuff that follows */
@@ -842,14 +849,16 @@ mts_id_eeprom_load(void)
                         }
                     }
                 }
-                
+
 		if(DEVICE_CAPA(id_eeprom.capa, CAPA_WIFI)) {
 			attr_blength += sizeof mtcdt_0_1_wifi_bt_attributes;
+                        need_append = 1;
 		}
 		if(DEVICE_CAPA(id_eeprom.capa, CAPA_GPS)) {
 			attr_blength += sizeof mtcdt_0_1_gnss_attributes;
+                        need_append = 1;
 		}
-		if (current_blength+(sizeof(struct attribute *)) != attr_blength) {
+		if (need_append) {
 			freelater = all_attrs = kmalloc(attr_blength,GFP_KERNEL);
 			current_count = current_blength/(sizeof (struct attribute *));
 			memcpy(all_attrs,mtcdt_0_1_platform_attributes,current_blength);
@@ -873,6 +882,7 @@ mts_id_eeprom_load(void)
 		set_buttons(default_buttons);
                 log_info("detected board %s", tmp);
         } else if ((tmp=HW_VERSION_MTCDTIPHP_0_0),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0) {
+                need_append = 0;
                 current_blength = attr_blength = sizeof  mtcdt_0_1_platform_attributes;
                 current_blength -= sizeof(struct attribute *);  /* Length without terminating NULL */
 
@@ -896,8 +906,9 @@ mts_id_eeprom_load(void)
                 }
 		if(DEVICE_CAPA(id_eeprom.capa, CAPA_WIFI)) {
 			attr_blength += sizeof mtcdt_0_1_wifi_bt_attributes;
+			need_append = 1;
 		}
-		if (current_blength+(sizeof(struct attribute *)) != attr_blength) {
+		if (need_append) {
 			freelater = all_attrs = kmalloc(attr_blength,GFP_KERNEL);
 			current_count = current_blength/(sizeof (struct attribute *));
 			memcpy(all_attrs,mtcdt_0_1_platform_attributes,current_blength);
@@ -944,10 +955,60 @@ mts_id_eeprom_load(void)
 		log_info("detected board %s", tmp);
     } else if (((tmp=HW_VERSION_MTCPM_0_0),(mts_hw_version=MTCPM_0_0),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0) ||
             ((tmp=HW_VERSION_MTCPM_0_1),(mts_hw_version=MTCPM_0_1),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0)) {
+                need_append = 0;
+                current_blength = attr_blength = sizeof  mtcpm_platform_attributes;
+                current_blength -= sizeof(struct attribute *);  /* Length without terminating NULL */
+
+                /* See if we have no radio, and if so, prune out the stuff that follows */
+                if(noradio) {
+                    struct attribute **ap = mtcpm_platform_attribute_group.attrs;
+                    while(1) {
+                        if(ap[j] == NULL) {
+                            log_info("Did not find radio power attribute.  Possible driver fault.");
+                            break;
+                        }
+                        j++;
+                        if (is_radio_reset_attr_mtcpm(ap[j])) {
+                            log_info("Pruning radio feature from mts-io",j);
+                            ap[j] = NULL;
+                            current_blength = j * sizeof (ap[j]); /* Size without NULL */
+                            attr_blength += sizeof (ap[j]);  /* Size of attr array with NULL */
+                            break;
+                        }
+                    }
+                }
+
+		if(DEVICE_CAPA(id_eeprom.capa, CAPA_WIFI)) {
+			attr_blength += sizeof mtcpm_wifi_bt_attributes;
+			need_append = 1;
+		}
+		if(DEVICE_CAPA(id_eeprom.capa, CAPA_GPS)) {
+			attr_blength += sizeof mtcpm_gnss_attributes;
+			need_append = 1;
+		}
+		if (need_append) {
+			freelater = all_attrs = kmalloc(attr_blength,GFP_KERNEL);
+			current_count = current_blength/(sizeof (struct attribute *));
+			memcpy(all_attrs,mtcpm_platform_attributes,current_blength);
+			if(DEVICE_CAPA(id_eeprom.capa, CAPA_WIFI)) {
+				log_info("Adding WiFi/BT to mts-io driver");
+				memcpy(all_attrs + current_count,mtcpm_wifi_bt_attributes,sizeof mtcpm_wifi_bt_attributes);
+				current_count += sizeof mtcpm_wifi_bt_attributes / (sizeof  (struct attribute *));
+			}
+			if(DEVICE_CAPA(id_eeprom.capa, CAPA_GPS)) {
+				log_info("Adding GPS to mts-io driver");
+				attr_blength += sizeof mtcpm_gnss_attributes;
+				memcpy(all_attrs + current_count,mtcpm_gnss_attributes,sizeof mtcpm_gnss_attributes);
+				current_count += sizeof mtcpm_gnss_attributes / (sizeof  (struct attribute *));
+			}
+			all_attrs[current_count] = (struct attribute *)NULL;
+			mtcpm_platform_attribute_group.attrs = all_attrs;
+		}
+
 		attr_group = &mtcpm_platform_attribute_group;
 		gpio_pins = gpio_pins_mtcpm;
 		set_buttons(default_buttons);
-		log_info("detected board %s", tmp);
+                log_info("detected board %s", tmp);
     } else if (strncmp(id_eeprom.product_id, PRODUCT_ID_MT100EOCG, strlen(PRODUCT_ID_MT100EOCG)) == 0) {
 		attr_group = &mt100eocg_platform_attribute_group;
 		gpio_pins = gpio_pins_mt100eocg_0_0;
diff --git a/io-module/mts_io_module.h b/io-module/mts_io_module.h
index 1374032..f0c4445 100644
--- a/io-module/mts_io_module.h
+++ b/io-module/mts_io_module.h
@@ -5,7 +5,7 @@
  * MTAC cards.
  */
 
-#define DRIVER_VERSION  "v4.4.3"
+#define DRIVER_VERSION  "v4.4.4"
 #define DRIVER_AUTHOR   "Multitech Systems"
 #define DRIVER_DESC "MTS-IO Controller"
 #define DRIVER_NAME "mts-io"