Advertise Sink_Capabilities correctly

Now the Sink_Capabilities message contains a PDO from inside the range
if applicable, and a PPS APDO if we're using PD 3.0.

lib/include/pd.h

 /* PD Source Fixed PDO voltage */
 #define PD_PDO_SRC_FIXED_VOLTAGE_GET(msg, i) (((msg)->obj[(i)] & PD_PDO_SRC_FIXED_VOLTAGE) >> PD_PDO_SRC_FIXED_VOLTAGE_SHIFT)
 
-/* PD Source Programmable Power Supply APDO */
-#define PD_APDO_SRC_PPS_MAX_VOLTAGE_SHIFT 17
-#define PD_APDO_SRC_PPS_MAX_VOLTAGE (0xFF << PD_APDO_SRC_PPS_MAX_VOLTAGE_SHIFT)
-#define PD_APDO_SRC_PPS_MIN_VOLTAGE_SHIFT 8
-#define PD_APDO_SRC_PPS_MIN_VOLTAGE (0xFF << PD_APDO_SRC_PPS_MIN_VOLTAGE_SHIFT)
-#define PD_APDO_SRC_PPS_CURRENT_SHIFT 0
-#define PD_APDO_SRC_PPS_CURRENT (0x7F << PD_APDO_SRC_PPS_CURRENT_SHIFT)
-
-/* PD Source Programmable Power Supply APDO voltages */
-#define PD_APDO_SRC_PPS_MAX_VOLTAGE_GET(msg, i) (((msg)->obj[(i)] & PD_APDO_SRC_PPS_MAX_VOLTAGE) >> PD_APDO_SRC_PPS_MAX_VOLTAGE_SHIFT)
-#define PD_APDO_SRC_PPS_MIN_VOLTAGE_GET(msg, i) (((msg)->obj[(i)] & PD_APDO_SRC_PPS_MIN_VOLTAGE) >> PD_APDO_SRC_PPS_MIN_VOLTAGE_SHIFT)
-
-/* PD Source Programmable Power Supply APDO current */
-#define PD_APDO_SRC_PPS_CURRENT_GET(msg, i) (((msg)->obj[(i)] & PD_APDO_SRC_PPS_CURRENT) >> PD_APDO_SRC_PPS_CURRENT_SHIFT)
+/* PD Programmable Power Supply APDO */
+#define PD_APDO_PPS_MAX_VOLTAGE_SHIFT 17
+#define PD_APDO_PPS_MAX_VOLTAGE (0xFF << PD_APDO_PPS_MAX_VOLTAGE_SHIFT)
+#define PD_APDO_PPS_MIN_VOLTAGE_SHIFT 8
+#define PD_APDO_PPS_MIN_VOLTAGE (0xFF << PD_APDO_PPS_MIN_VOLTAGE_SHIFT)
+#define PD_APDO_PPS_CURRENT_SHIFT 0
+#define PD_APDO_PPS_CURRENT (0x7F << PD_APDO_PPS_CURRENT_SHIFT)
+
+/* PD Programmable Power Supply APDO voltages */
+#define PD_APDO_PPS_MAX_VOLTAGE_GET(msg, i) (((msg)->obj[(i)] & PD_APDO_PPS_MAX_VOLTAGE) >> PD_APDO_PPS_MAX_VOLTAGE_SHIFT)
+#define PD_APDO_PPS_MIN_VOLTAGE_GET(msg, i) (((msg)->obj[(i)] & PD_APDO_PPS_MIN_VOLTAGE) >> PD_APDO_PPS_MIN_VOLTAGE_SHIFT)
+
+#define PD_APDO_PPS_MAX_VOLTAGE_SET(v) (((v) << PD_APDO_PPS_MAX_VOLTAGE_SHIFT) & PD_APDO_PPS_MAX_VOLTAGE)
+#define PD_APDO_PPS_MIN_VOLTAGE_SET(v) (((v) << PD_APDO_PPS_MIN_VOLTAGE_SHIFT) & PD_APDO_PPS_MIN_VOLTAGE)
+
+/* PD Programmable Power Supply APDO current */
+#define PD_APDO_PPS_CURRENT_GET(msg, i) (((msg)->obj[(i)] & PD_APDO_PPS_CURRENT) >> PD_APDO_PPS_CURRENT_SHIFT)
+
+#define PD_APDO_PPS_CURRENT_SET(i) (((i) << PD_APDO_PPS_CURRENT_SHIFT) & PD_APDO_PPS_CURRENT)
 
 /* TODO: other types of source PDO */
 

src/device_policy_manager.c

              * its I is at least our desired I */
             if ((capabilities->obj[i] & PD_PDO_TYPE) == PD_PDO_TYPE_AUGMENTED
                     && (capabilities->obj[i] & PD_APDO_TYPE) == PD_APDO_TYPE_PPS
-                    && PD_APDO_SRC_PPS_MAX_VOLTAGE_GET(capabilities, i) >= PD_MV2PAV(scfg->v)
-                    && PD_APDO_SRC_PPS_MIN_VOLTAGE_GET(capabilities, i) <= PD_MV2PAV(scfg->v)
-                    && PD_APDO_SRC_PPS_CURRENT_GET(capabilities, i) >= PD_CA2PAI(scfg->i)) {
+                    && PD_APDO_PPS_MAX_VOLTAGE_GET(capabilities, i) >= PD_MV2PAV(scfg->v)
+                    && PD_APDO_PPS_MIN_VOLTAGE_GET(capabilities, i) <= PD_MV2PAV(scfg->v)
+                    && PD_APDO_PPS_CURRENT_GET(capabilities, i) >= PD_CA2PAI(scfg->i)) {
                 /* We got what we wanted, so build a request for that */
                 request->hdr = cfg->pe.hdr_template | PD_MSGTYPE_REQUEST
                     | PD_NUMOBJ(1);
             | PD_PDO_SNK_FIXED_CURRENT_SET(DPM_MIN_CURRENT);
     }
 
-    /* Add a PDO for the desired power. */
     if (scfg != NULL) {
+        /* Add a PDO for the desired power. */
         cap->obj[numobj++] = PD_PDO_TYPE_FIXED
             | PD_PDO_SNK_FIXED_VOLTAGE_SET(PD_MV2PDV(scfg->v))
             | PD_PDO_SNK_FIXED_CURRENT_SET(scfg->i);
-        /* If we want more than 5 V, set the Higher Capability flag */
-        if (PD_MV2PDV(scfg->v) != PD_MV2PDV(5000)) {
-            cap->obj[0] |= PD_PDO_SNK_FIXED_HIGHER_CAP;
+
+        /* Get the PDO from the voltage range */
+        int8_t i = dpm_get_range_fixed_pdo_index(dpm_data->capabilities, scfg);
+
+        /* If it's vSafe5V, set our vSafe5V's current to what we want */
+        if (i == 0) {
+            cap->obj[0] &= ~PD_PDO_SNK_FIXED_CURRENT;
+            cap->obj[0] |= PD_PDO_SNK_FIXED_CURRENT_SET(scfg->i);
+        } else {
+            /* If we want more than 5 V, set the Higher Capability flag */
+            if (PD_MV2PDV(scfg->v) != PD_MV2PDV(5000)) {
+                cap->obj[0] |= PD_PDO_SNK_FIXED_HIGHER_CAP;
+            }
+
+            /* If the range PDO is a different voltage than the preferred
+             * voltage, add it to the array. */
+            if (i > 0 && PD_PDO_SRC_FIXED_VOLTAGE_GET(dpm_data->capabilities, i) != PD_MV2PDV(scfg->v)) {
+                cap->obj[numobj++] = PD_PDO_TYPE_FIXED
+                    | PD_PDO_SNK_FIXED_VOLTAGE_SET(PD_PDO_SRC_FIXED_VOLTAGE_GET(dpm_data->capabilities, i))
+                    | PD_PDO_SNK_FIXED_CURRENT_SET(PD_PDO_SRC_FIXED_CURRENT_GET(dpm_data->capabilities, i));
+            }
+
+            /* If we have three PDOs at this point, make sure the last two are
+             * sorted by voltage. */
+            if (numobj == 3
+                    && (cap->obj[1] & PD_PDO_SNK_FIXED_VOLTAGE)
+                    > (cap->obj[2] & PD_PDO_SNK_FIXED_VOLTAGE)) {
+                cap->obj[1] ^= cap->obj[2];
+                cap->obj[2] ^= cap->obj[1];
+                cap->obj[1] ^= cap->obj[2];
+            }
+        }
+
+        /* If we're using PD 3.0, add a PPS APDO for our desired voltage */
+        if ((cfg->pe._message->hdr & PD_HDR_SPECREV) >= PD_SPECREV_3_0) {
+            cap->obj[numobj++] = PD_PDO_TYPE_AUGMENTED | PD_APDO_TYPE_PPS
+                | PD_APDO_PPS_MAX_VOLTAGE_SET(PD_MV2PAV(scfg->v))
+                | PD_APDO_PPS_MIN_VOLTAGE_SET(PD_MV2PAV(scfg->v))
+                | PD_APDO_PPS_CURRENT_SET(PD_CA2PAI(scfg->i));
         }
     }
 

src/shell.c

     chprintf(chp, "pps\r\n");
 
     /* Minimum voltage */
-    tmp = (pdo & PD_APDO_SRC_PPS_MIN_VOLTAGE) >> PD_APDO_SRC_PPS_MIN_VOLTAGE_SHIFT;
+    tmp = (pdo & PD_APDO_PPS_MIN_VOLTAGE) >> PD_APDO_PPS_MIN_VOLTAGE_SHIFT;
     chprintf(chp, "\tvmin: %d.%02d V\r\n", PD_PAV_V(tmp), PD_PAV_CV(tmp));
 
     /* Maximum voltage */
-    tmp = (pdo & PD_APDO_SRC_PPS_MAX_VOLTAGE) >> PD_APDO_SRC_PPS_MAX_VOLTAGE_SHIFT;
+    tmp = (pdo & PD_APDO_PPS_MAX_VOLTAGE) >> PD_APDO_PPS_MAX_VOLTAGE_SHIFT;
     chprintf(chp, "\tvmax: %d.%02d V\r\n", PD_PAV_V(tmp), PD_PAV_CV(tmp));
 
     /* Maximum current */
-    tmp = (pdo & PD_APDO_SRC_PPS_CURRENT) >> PD_APDO_SRC_PPS_CURRENT_SHIFT;
+    tmp = (pdo & PD_APDO_PPS_CURRENT) >> PD_APDO_PPS_CURRENT_SHIFT;
     chprintf(chp, "\ti: %d.%02d A\r\n", PD_PAI_A(tmp), PD_PAI_CA(tmp));
 }