Add I2C debug commands to console

Fix up console help text

main.c

@@ -233,23 +233,25 @@ static void prompt(void)
 static void help(void)
 {
     puts("Available commands:");
-    puts("help                            - this command");
-    puts("reboot                          - reboot BMC CPU");
-    puts("poweron                         - Turn chassis power on, start IPL, "
+    puts("help                             - this command");
+    puts("reboot                           - reboot BMC CPU");
+    puts("poweron                          - Turn chassis power on, start IPL, "
          "and attach to host console");
-    puts("console                         - Attach to host console");
-    puts("status                          - Print system status");
-    puts("ipl                             - Start IPL sequence");
-    puts("chassison                       - Turn chassis power on and prepare "
+    puts("console                          - Attach to host console");
+    puts("status                           - Print system status");
+    puts("ipl                              - Start IPL sequence");
+    puts("chassison                        - Turn chassis power on and prepare "
          "for IPL");
-    puts("chassisoff                      - Turn chassis power off");
-    puts("sbe_status                      - Get SBE status register");
-    puts("post_codes                      - Enable or disable output of POST "
+    puts("chassisoff                       - Turn chassis power off");
+    puts("sbe_status                       - Get SBE status register");
+    puts("post_codes                       - Enable or disable output of POST "
          "codes on console");
-    puts("mr <address> <length>           - Read data from BMC internal address "
+    puts("mr <address> <length>            - Read data from BMC internal address "
          "in 32-bit words");
-    puts("mw <address> <length> <data>    - Write data from BMC internal address "
+    puts("mw <address> <length> <data>     - Write data from BMC internal address "
          "in 32-bit words");
+    puts("i2cget <bus> <addr> <reg>        - Read I2C register");
+    puts("i2cset <bus> <addr> <reg> <data> - Write I2C register");
 }
 
 static void reboot(void)
@@ -2103,7 +2105,9 @@ static void console_service(void)
 {
     char *str;
     char *token;
+    uint64_t bus;
     uint64_t address;
+    uint64_t reg;
     uint32_t data;
     unsigned int i;
     unsigned int length;
@@ -2192,6 +2196,79 @@ static void console_service(void)
             printf("USAGE: mr <memory address>\n");
         }
     }
+    else if (strcmp(token, "i2cget") == 0)
+    {
+        if (*str)
+        {
+            token = get_token(&str);
+            bus = parse_user_provided_number(token);
+            if (*str)
+            {
+                token = get_token(&str);
+                address = parse_user_provided_number(token);
+                if (*str)
+                {
+                    token = get_token(&str);
+                    reg = parse_user_provided_number(token);
+                    data = i2c_read_register_byte((uint8_t *)(I2CMASTER1_BASE + (0x20 * (bus - 1))), address, reg, NULL);
+                    printf("I2C device %d:0x%02x<0x%02x>: 0x%02x\n", bus, address, reg, data);
+                }
+                else
+                {
+                    printf("USAGE: i2cget <bus> <address> <register>\n");
+                }
+            }
+            else
+            {
+                printf("USAGE: i2cget <bus> <address> <register>\n");
+            }
+        }
+        else
+        {
+            printf("USAGE: i2cget <bus> <address> <register>\n");
+        }
+    }
+    else if (strcmp(token, "i2cset") == 0)
+    {
+        if (*str)
+        {
+            token = get_token(&str);
+            bus = parse_user_provided_number(token);
+            if (*str)
+            {
+                token = get_token(&str);
+                address = parse_user_provided_number(token);
+                if (*str)
+                {
+                    token = get_token(&str);
+                    reg = parse_user_provided_number(token);
+                    if (*str)
+                    {
+                        token = get_token(&str);
+                        data = parse_user_provided_number(token);
+                        i2c_write_register_byte((uint8_t *)(I2CMASTER1_BASE + (0x20 * (bus - 1))), address, reg, data);
+                        printf("I2C device %d:0x%02x<0x%02x> set to 0x%02x\n", bus, address, reg, data);
+                    }
+                    else
+                    {
+                        printf("USAGE: i2cset <bus> <address> <register> <data>\n");
+                    }
+                }
+                else
+                {
+                    printf("USAGE: i2cset <bus> <address> <register> <data>\n");
+                }
+            }
+            else
+            {
+                printf("USAGE: i2cset <bus> <address> <register> <data>\n");
+            }
+        }
+        else
+        {
+            printf("USAGE: i2cset <bus> <address> <register> <data>\n");
+        }
+    }
     else if (strcmp(token, "flash_write") == 0)
     {
         if (*str)
@@ -2560,8 +2637,18 @@ int main(void)
     {
         initialize_i2c_master(g_cpu_info[i].i2c_master, g_cpu_info[i].i2c_frequency);
     }
-    // initialize_i2c_master((uint8_t*)I2CMASTER3_BASE, 100000);
+#ifdef I2CMASTER3_BASE
+    initialize_i2c_master((uint8_t *)I2CMASTER3_BASE, 100000);
+#endif
+#ifdef I2CMASTER4_BASE
     initialize_i2c_master((uint8_t *)I2CMASTER4_BASE, 100000);
+#endif
+#ifdef I2CMASTER5_BASE
+    initialize_i2c_master((uint8_t *)I2CMASTER5_BASE, 100000);
+#endif
+#ifdef I2CMASTER6_BASE
+    initialize_i2c_master((uint8_t *)I2CMASTER6_BASE, 100000);
+#endif
 
     // Check for Aquila core presence
     if ((read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DEVICE_ID_HIGH) == AQUILA_LPC_DEVICE_ID_HIGH) &&