Fix radio-related issues in mts-io4.4.4

1 files changed, 74 insertions, 13 deletions

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;