Add the WiFi Bluetooth control features.

1 files changed, 36 insertions, 34 deletions

diff --git a/io-module/mts_io.c b/io-module/mts_io.c
index c9a0c71..dd3a9ae 100644
--- a/io-module/mts_io.c
+++ b/io-module/mts_io.c
@@ -882,16 +882,36 @@ mts_id_eeprom_load(void)
 		log_info("detected board %s", HW_VERSION_MTRV1_0_1);
 	} else if (((tmp=HW_VERSION_MTCDT_0_1),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0) ||
             ((tmp=HW_VERSION_MTCDTIP_0_0),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0)) {
-		attr_blength = sizeof  mtcdt_0_1_platform_attributes;
+		current_blength = attr_blength = sizeof  mtcdt_0_1_platform_attributes;
+
+                /* See if we have no radio, and if so, prune out the stuff that follows */
+                if((strncmp(id_eeprom.product_id, PRODUCT_ID_MTCDT, sizeof PRODUCT_ID_MTCDT - 1) == 0) && 
+                    (id_eeprom.product_id[sizeof PRODUCT_ID_MTCDT-1] == '-')  &&
+                      isdigit(id_eeprom.product_id[sizeof PRODUCT_ID_MTCDT])) {
+                    struct attribute **ap = mtcdt_0_1_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_power_attr_mtcdt(ap[j])) {
+                            log_info("Pruning radio feature from mts-io",j);
+                            ap[j] = NULL;
+                            current_blength = attr_blength = j * sizeof (ap[j]);
+                            break;
+                        }
+                    }
+                }
+                
 		if(DEVICE_CAPA(id_eeprom.capa, CAPA_WIFI)) {
 			attr_blength += sizeof mtcdt_0_1_wifi_bt_attributes;
 		}
 		if(DEVICE_CAPA(id_eeprom.capa, CAPA_GPS)) {
 			attr_blength += sizeof mtcdt_0_1_gnss_attributes;
 		}
-		if (attr_blength != sizeof mtcdt_0_1_platform_attributes) {
+		if (current_blength != attr_blength) {
 			freelater = all_attrs = kmalloc(attr_blength,GFP_KERNEL);
-			current_blength = sizeof mtcdt_0_1_platform_attributes - sizeof (struct attribute *);
 			current_count = current_blength/(sizeof (struct attribute *));
 			memcpy(all_attrs,mtcdt_0_1_platform_attributes,current_blength);
 			if(DEVICE_CAPA(id_eeprom.capa, CAPA_WIFI)) {
@@ -906,10 +926,17 @@ mts_id_eeprom_load(void)
 			all_attrs[current_count] = (struct attribute *)NULL;
 			mtcdt_0_1_platform_attribute_group.attrs = all_attrs;
 		}
-		
-                if((strncmp(id_eeprom.product_id, PRODUCT_ID_MTCDT, sizeof PRODUCT_ID_MTCDT - 1) == 0) && 
-                    (id_eeprom.product_id[sizeof PRODUCT_ID_MTCDT-1] == '-')  &&
-                      isdigit(id_eeprom.product_id[sizeof PRODUCT_ID_MTCDT])) {
+
+		attr_group = &mtcdt_0_1_platform_attribute_group;
+		gpio_pins = gpio_pins_mtcdt_0_1;
+                log_info("detected board %s", tmp);
+        } else if ((tmp=HW_VERSION_MTCDTIPHP_0_0),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0) {
+                current_blength = attr_blength = sizeof  mtcdt_0_1_platform_attributes;
+
+                /* See if we have no radio, and if so, prune out the stuff that follows */
+                if((strncmp(id_eeprom.product_id, PRODUCT_ID_MTCDTIPHP, sizeof PRODUCT_ID_MTCDTIPHP - 1) == 0) && 
+                    (id_eeprom.product_id[sizeof PRODUCT_ID_MTCDTIPHP-1] == '-')  &&
+                      isdigit(id_eeprom.product_id[sizeof PRODUCT_ID_MTCDTIPHP])) {
                     struct attribute **ap = mtcdt_0_1_platform_attribute_group.attrs;
                     while(1) {
                         if(ap[j] == NULL) {
@@ -920,26 +947,19 @@ mts_id_eeprom_load(void)
                         if (is_radio_power_attr_mtcdt(ap[j])) {
                             log_info("Pruning radio feature from mts-io",j);
                             ap[j] = NULL;
+                            current_blength = attr_blength = j * sizeof (ap[j]);
                             break;
                         }
                     }
                 }
-
-
-		attr_group = &mtcdt_0_1_platform_attribute_group;
-		gpio_pins = gpio_pins_mtcdt_0_1;
-                log_info("detected board %s", tmp);
-        } else if ((tmp=HW_VERSION_MTCDTIPHP_0_0),strncmp(id_eeprom.hw_version, tmp, strlen(tmp)) == 0) {
-		attr_blength = sizeof  mtcdt_0_1_platform_attributes;
 		if(DEVICE_CAPA(id_eeprom.capa, CAPA_WIFI)) {
 			attr_blength += sizeof mtcdt_0_1_wifi_bt_attributes;
 		}
 		if(DEVICE_CAPA(id_eeprom.capa, CAPA_GPS)) {
 			attr_blength += sizeof mtcdt_0_1_gnss_attributes;
 		}
-		if (attr_blength != sizeof mtcdt_0_1_platform_attributes) {
+		if (current_blength != attr_blength) {
 			freelater = all_attrs = kmalloc(attr_blength,GFP_KERNEL);
-			current_blength = sizeof mtcdt_0_1_platform_attributes - sizeof (struct attribute *);
 			current_count = current_blength/(sizeof (struct attribute *));
 			memcpy(all_attrs,mtcdt_0_1_platform_attributes,current_blength);
 			if(DEVICE_CAPA(id_eeprom.capa, CAPA_WIFI)) {
@@ -955,24 +975,6 @@ mts_id_eeprom_load(void)
 			mtcdt_0_1_platform_attribute_group.attrs = all_attrs;
 		}
                 
-                /* See if we have no radio, and if so, prune out the stuff that follows */
-                if((strncmp(id_eeprom.product_id, PRODUCT_ID_MTCDTIPHP, sizeof PRODUCT_ID_MTCDTIPHP - 1) == 0) && 
-                    (id_eeprom.product_id[sizeof PRODUCT_ID_MTCDTIPHP-1] == '-')  &&
-                      isdigit(id_eeprom.product_id[sizeof PRODUCT_ID_MTCDTIPHP])) {
-                    struct attribute **ap = mtcdt_0_1_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_power_attr_mtcdt(ap[j])) {
-                            log_info("Pruning radio feature from mts-io",j);
-                            ap[j] = NULL;
-                            break;
-                        }
-                    }
-                }
 		attr_group = &mtcdt_0_1_platform_attribute_group;
 		gpio_pins = gpio_pins_mtcdt_0_1;
 		if (DEVICE_CAPA(id_eeprom.capa, CAPA_LORA)) {