Add 'prioritize' command to cgpt tool.

This lets us reorder the priority of all the kernel partitions with a single
command, instead of a bunch of complicated and error-prone shell script
logic.

Change-Id: I21d39763ec5a748488d5319a987bcfe7c34ce4d0

BUG=chromium-os:9167
TEST=manual

In the chroot, do this:

cd ~/trunk/src/platform/vboot_reference
make
make runtests
make clean

Everything should pass.

Review URL: http://codereview.chromium.org/5352005
diff --git a/cgpt/Makefile b/cgpt/Makefile
index cd503ba..d8c2022 100644
--- a/cgpt/Makefile
+++ b/cgpt/Makefile
@@ -21,6 +21,7 @@
 	cmd_add.c \
 	cmd_boot.c \
 	cmd_find.c \
+	cmd_prioritize.c \
 	cgpt_common.c
 
 main: $(PROGNAME)
diff --git a/cgpt/cgpt.c b/cgpt/cgpt.c
index d8604b1..4c9fc32 100644
--- a/cgpt/cgpt.c
+++ b/cgpt/cgpt.c
@@ -27,18 +27,19 @@
   {"repair", cmd_repair, "Repair damaged GPT headers and tables"},
   {"boot", cmd_boot, "Edit the PMBR sector for legacy BIOSes"},
   {"find", cmd_find, "Locate a partition by its GUID"},
+  {"prioritize", cmd_prioritize,
+   "Reorder the priority of all kernel partitions"},
 };
 
-
 void Usage(void) {
   int i;
 
-  printf("Usage: %s COMMAND [OPTIONS] DRIVE\n\n"
+  printf("\nUsage: %s COMMAND [OPTIONS] DRIVE\n\n"
          "Supported COMMANDs:\n\n",
          progname);
 
   for (i = 0; i < sizeof(cmds)/sizeof(cmds[0]); ++i) {
-    printf("    %-10s  %s\n", cmds[i].name, cmds[i].comment);
+    printf("    %-15s  %s\n", cmds[i].name, cmds[i].comment);
   }
   printf("\nFor more detailed usage, use %s COMMAND -h\n\n", progname);
 }
@@ -47,6 +48,8 @@
 
 int main(int argc, char *argv[]) {
   int i;
+  int match_count = 0;
+  int match_index = 0;
 
   progname = strrchr(argv[0], '/');
   if (progname)
@@ -64,12 +67,23 @@
 
   // Find the command to invoke.
   for (i = 0; command && i < sizeof(cmds)/sizeof(cmds[0]); ++i) {
+    // exact match?
     if (0 == strcmp(cmds[i].name, command)) {
-      return cmds[i].fp(argc, argv);
+      match_index = i;
+      match_count = 1;
+      break;
+    }
+    // unique match?
+    else if (0 == strncmp(cmds[i].name, command, strlen(command))) {
+      match_index = i;
+      match_count++;
     }
   }
 
-  // Couldn't find the command.
+  if (match_count == 1)
+    return cmds[match_index].fp(argc, argv);
+
+  // Couldn't find a single matching command.
   Usage();
 
   return CGPT_FAILED;
diff --git a/cgpt/cgpt.h b/cgpt/cgpt.h
index 85702a4..3f117bb 100644
--- a/cgpt/cgpt.h
+++ b/cgpt/cgpt.h
@@ -72,13 +72,20 @@
 #define GUID_STRLEN 37
 int StrToGuid(const char *str, Guid *guid);
 void GuidToStr(const Guid *guid, char *str, unsigned int buflen);
+int GuidEqual(const Guid *guid1, const Guid *guid2);
 int IsZero(const Guid *guid);
 
+/* Constant global type values to compare against */
+extern const Guid guid_chromeos_kernel;
+extern const Guid guid_chromeos_rootfs;
+extern const Guid guid_linux_data;
+extern const Guid guid_chromeos_reserved;
+extern const Guid guid_efi;
+extern const Guid guid_unused;
 
 int ReadPMBR(struct drive *drive);
 int WritePMBR(struct drive *drive);
 
-
 /* Convert possibly unterminated UTF16 string to UTF8.
  * Caller must prepare enough space for UTF8, which could be up to
  * twice the byte length of UTF16 string plus the terminating '\0'.
@@ -133,6 +140,7 @@
 int cmd_add(int argc, char *argv[]);
 int cmd_boot(int argc, char *argv[]);
 int cmd_find(int argc, char *argv[]);
+int cmd_prioritize(int argc, char *argv[]);
 
 #define ARRAY_COUNT(array) (sizeof(array)/sizeof((array)[0]))
 const char *GptError(int errnum);
diff --git a/cgpt/cgpt_common.c b/cgpt/cgpt_common.c
index eafc661..90cd8b6 100644
--- a/cgpt/cgpt_common.c
+++ b/cgpt/cgpt_common.c
@@ -557,17 +557,25 @@
   return retval;
 }
 
-struct {
-  Guid type;
+/* global types to compare against */
+const Guid guid_chromeos_kernel =   GPT_ENT_TYPE_CHROMEOS_KERNEL;
+const Guid guid_chromeos_rootfs =   GPT_ENT_TYPE_CHROMEOS_ROOTFS;
+const Guid guid_linux_data =        GPT_ENT_TYPE_LINUX_DATA;
+const Guid guid_chromeos_reserved = GPT_ENT_TYPE_CHROMEOS_RESERVED;
+const Guid guid_efi =               GPT_ENT_TYPE_EFI;
+const Guid guid_unused =            GPT_ENT_TYPE_UNUSED;
+
+static struct {
+  const Guid *type;
   char *name;
   char *description;
 } supported_types[] = {
-  {GPT_ENT_TYPE_CHROMEOS_KERNEL, "kernel", "ChromeOS kernel"},
-  {GPT_ENT_TYPE_CHROMEOS_ROOTFS, "rootfs", "ChromeOS rootfs"},
-  {GPT_ENT_TYPE_LINUX_DATA, "data", "Linux data"},
-  {GPT_ENT_TYPE_CHROMEOS_RESERVED, "reserved", "ChromeOS reserved"},
-  {GPT_ENT_TYPE_EFI, "efi", "EFI System Partition"},
-  {GPT_ENT_TYPE_UNUSED, "unused", "Unused (nonexistent) partition"},
+  {&guid_chromeos_kernel, "kernel", "ChromeOS kernel"},
+  {&guid_chromeos_rootfs, "rootfs", "ChromeOS rootfs"},
+  {&guid_linux_data, "data", "Linux data"},
+  {&guid_chromeos_reserved, "reserved", "ChromeOS reserved"},
+  {&guid_efi, "efi", "EFI System Partition"},
+  {&guid_unused, "unused", "Unused (nonexistent) partition"},
 };
 
 /* Resolves human-readable GPT type.
@@ -576,7 +584,7 @@
 int ResolveType(const Guid *type, char *buf) {
   int i;
   for (i = 0; i < ARRAY_COUNT(supported_types); ++i) {
-    if (!memcmp(type, &supported_types[i].type, sizeof(Guid))) {
+    if (!memcmp(type, supported_types[i].type, sizeof(Guid))) {
       strcpy(buf, supported_types[i].description);
       return CGPT_OK;
     }
@@ -588,7 +596,7 @@
   int i;
   for (i = 0; i < ARRAY_COUNT(supported_types); ++i) {
     if (!strcmp(name, supported_types[i].name)) {
-      memcpy(type, &supported_types[i].type, sizeof(Guid));
+      memcpy(type, supported_types[i].type, sizeof(Guid));
       return CGPT_OK;
     }
   }
@@ -842,9 +850,12 @@
   return 0;
 }
 
+int GuidEqual(const Guid *guid1, const Guid *guid2) {
+  return (0 == memcmp(guid1, guid2, sizeof(Guid)));
+}
 
 int IsZero(const Guid *gp) {
-  return (0 == memcmp(gp, &guid_unused, sizeof(Guid)));
+  return GuidEqual(gp, &guid_unused);
 }
 
 void PMBRToStr(struct pmbr *pmbr, char *str, unsigned int buflen) {
diff --git a/cgpt/cmd_find.c b/cgpt/cmd_find.c
index 055777e..896119a 100644
--- a/cgpt/cmd_find.c
+++ b/cgpt/cmd_find.c
@@ -177,8 +177,8 @@
       continue;
 
     int found = 0;
-    if ((set_unique && !memcmp(&unique_guid, &entry->unique, sizeof(Guid))) ||
-        (set_type && !memcmp(&type_guid, &entry->type, sizeof(Guid)))) {
+    if ((set_unique && GuidEqual(&unique_guid, &entry->unique)) ||
+        (set_type && GuidEqual(&type_guid, &entry->type))) {
       found = 1;
     } else if (set_label) {
       if (CGPT_OK != UTF16ToUTF8(entry->name,
diff --git a/cgpt/cmd_prioritize.c b/cgpt/cmd_prioritize.c
new file mode 100644
index 0000000..3f34568
--- /dev/null
+++ b/cgpt/cmd_prioritize.c
@@ -0,0 +1,314 @@
+// Copyright (c) 2010 The Chromium OS Authors. All rights reserved.
+// Use of this source code is governed by a BSD-style license that can be
+// found in the LICENSE file.
+
+#include "cgpt.h"
+
+#include <getopt.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <uuid/uuid.h>
+
+#include "cgptlib_internal.h"
+
+static void Usage(void)
+{
+  printf("\nUsage: %s prioritize [OPTIONS] DRIVE\n\n"
+         "Reorder the priority of all active ChromeOS Kernel partitions.\n\n"
+         "Options:\n"
+         "  -P NUM       Highest priority to use in the new ordering. The\n"
+         "                 other partitions will be ranked in decreasing\n"
+         "                 priority while preserving their original order.\n"
+         "                 If necessary the lowest ranks will be coalesced.\n"
+         "                 No active kernels will be lowered to priority 0.\n"
+         "  -i NUM       Specify the partition to make the highest in the new\n"
+         "                 order.\n"
+         "  -f           Friends of the given partition (those with the same\n"
+         "                 starting priority) are also updated to the new\n"
+         "                 highest priority.\n"
+         "\n"
+         "With no options this will set the lowest active kernel to\n"
+         "priority 1 while maintaining the original order.\n"
+         "\n", progname);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// I want a sorted list of priority groups, where each element in the list
+// contains an unordered list of GPT partition numbers. This is a stupid
+// implementation, but our needs are simple and don't justify the time or space
+// it would take to write a "better" one.
+#define MAX_GROUPS 17                   // 0-15, plus one "higher"
+
+typedef struct {
+  int priority;                         // priority of this group
+  int num_parts;                        // number of partitions in this group
+  uint32_t *part;                       // array of partitions in this group
+} group_t;
+
+typedef struct {
+  int max_parts;                       // max number of partitions in any group
+  int num_groups;                      // number of non-empty groups
+  group_t group[MAX_GROUPS];           // array of groups
+} group_list_t;
+
+
+static group_list_t *NewGroupList(int max_p) {
+  int i;
+  group_list_t *gl = (group_list_t *)malloc(sizeof(group_list_t));
+  require(gl);
+  gl->max_parts = max_p;
+  gl->num_groups = 0;
+  // reserve space for the maximum number of partitions in every group
+  for (i=0; i<MAX_GROUPS; i++) {
+    gl->group[i].priority = -1;
+    gl->group[i].num_parts = 0;
+    gl->group[i].part = (uint32_t *)malloc(sizeof(uint32_t) * max_p);
+    require(gl->group[i].part);
+  }
+
+  return gl;
+}
+
+static void FreeGroups(group_list_t *gl) {
+  int i;
+  for (i=0; i<MAX_GROUPS; i++)
+    free(gl->group[i].part);
+  free(gl);
+}
+
+static void AddToGroup(group_list_t *gl, int priority, int partition) {
+  int i;
+  // See if I've already got a group with this priority
+  for (i=0; i<gl->num_groups; i++)
+    if (gl->group[i].priority == priority)
+      break;
+  if (i == gl->num_groups) {
+    // no, add a group
+    require(i < MAX_GROUPS);
+    gl->num_groups++;
+    gl->group[i].priority = priority;
+  }
+  // add the partition to it
+  int j = gl->group[i].num_parts;
+  gl->group[i].part[j] = partition;
+  gl->group[i].num_parts++;
+}
+
+static void ChangeGroup(group_list_t *gl, int old_priority, int new_priority) {
+  int i;
+  for (i=0; i<gl->num_groups; i++)
+    if (gl->group[i].priority == old_priority) {
+      gl->group[i].priority = new_priority;
+      break;
+    }
+}
+
+static void SortGroups(group_list_t *gl) {
+  int i, j;
+  group_t tmp;
+
+  // straight insertion sort is fast enough
+  for (i=1; i<gl->num_groups; i++) {
+    tmp = gl->group[i];
+    for (j=i; j && (gl->group[j-1].priority < tmp.priority); j--)
+      gl->group[j] = gl->group[j-1];
+    gl->group[j] = tmp;
+  }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+
+int cmd_prioritize(int argc, char *argv[]) {
+  struct drive drive;
+  uint32_t set_partition = 0;
+  int set_friends = 0;
+  int max_priority = 0;
+  int priority;
+  int orig_priority = 0;
+  int gpt_retval;
+  GptEntry *entry;
+  uint32_t index;
+  uint32_t max_part;
+  int num_kernels;
+  int i,j;
+  group_list_t *groups;
+
+  int c;
+  int errorcnt = 0;
+  char *e = 0;
+
+  opterr = 0;                     // quiet, you
+  while ((c=getopt(argc, argv, ":hi:fP:")) != -1)
+  {
+    switch (c)
+    {
+    case 'i':
+      set_partition = (uint32_t)strtoul(optarg, &e, 0);
+      if (!*optarg || (e && *e))
+      {
+        Error("invalid argument to -%c: \"%s\"\n", c, optarg);
+        errorcnt++;
+      }
+      break;
+    case 'f':
+      set_friends = 1;
+      break;
+    case 'P':
+      max_priority = (int)strtol(optarg, &e, 0);
+      if (!*optarg || (e && *e))
+      {
+        Error("invalid argument to -%c: \"%s\"\n", c, optarg);
+        errorcnt++;
+      }
+      if (max_priority < 1 || max_priority > 15) {
+        Error("value for -%c must be between 1 and 15\n", c);
+        errorcnt++;
+      }
+      break;
+
+    case 'h':
+      Usage();
+      return CGPT_OK;
+    case '?':
+      Error("unrecognized option: -%c\n", optopt);
+      errorcnt++;
+      break;
+    case ':':
+      Error("missing argument to -%c\n", optopt);
+      errorcnt++;
+      break;
+    default:
+      errorcnt++;
+      break;
+    }
+  }
+  if (errorcnt)
+  {
+    Usage();
+    return CGPT_FAILED;
+  }
+
+  if (set_friends && !set_partition) {
+    Error("the -f option is only useful with the -i option\n");
+    Usage();
+    return CGPT_FAILED;
+  }
+
+  if (optind >= argc) {
+    Error("missing drive argument\n");
+    return CGPT_FAILED;
+  }
+
+  if (CGPT_OK != DriveOpen(argv[optind], &drive))
+    return CGPT_FAILED;
+
+  if (GPT_SUCCESS != (gpt_retval = GptSanityCheck(&drive.gpt))) {
+    Error("GptSanityCheck() returned %d: %s\n",
+          gpt_retval, GptError(gpt_retval));
+    return CGPT_FAILED;
+  }
+
+  max_part = GetNumberOfEntries(&drive.gpt);
+
+  if (set_partition) {
+    if (set_partition < 1 || set_partition > max_part) {
+      Error("invalid partition number: %d (must be between 1 and %d\n",
+            set_partition, max_part);
+      goto bad;
+    }
+    index = set_partition - 1;
+    // it must be a kernel
+    entry = GetEntry(&drive.gpt, PRIMARY, index);
+    if (!GuidEqual(&entry->type, &guid_chromeos_kernel)) {
+      Error("partition %d is not a ChromeOS kernel\n", set_partition);
+      goto bad;
+    }
+  }
+
+  // How many kernel partitions do I have?
+  num_kernels = 0;
+  for (i = 0; i < max_part; i++) {
+    entry = GetEntry(&drive.gpt, PRIMARY, i);
+    if (GuidEqual(&entry->type, &guid_chromeos_kernel))
+      num_kernels++;
+  }
+
+  if (!num_kernels)
+    // nothing to do, so don't
+    goto good;
+
+  // Determine the current priority groups
+  groups = NewGroupList(num_kernels);
+  for (i = 0; i < max_part; i++) {
+    entry = GetEntry(&drive.gpt, PRIMARY, i);
+    if (!GuidEqual(&entry->type, &guid_chromeos_kernel))
+      continue;
+
+    priority = GetPriority(&drive.gpt, PRIMARY, i);
+
+    // Is this partition special?
+    if (set_partition && (i+1 == set_partition)) {
+      orig_priority = priority;         // remember the original priority
+      if (set_friends)
+        AddToGroup(groups, priority, i); // we'll move them all later
+      else
+        AddToGroup(groups, 99, i);      // move only this one
+    } else {
+      AddToGroup(groups, priority, i);  // just remember
+    }
+  }
+
+  // If we're including friends, then change the original group priority
+  if (set_partition && set_friends) {
+    ChangeGroup(groups, orig_priority, 99);
+  }
+
+  // Sorting gives the new order. Now we just need to reassign the
+  // priorities.
+  SortGroups(groups);
+
+  // We'll never lower anything to zero, so if the last group is priority zero
+  // we can ignore it.
+  i = groups->num_groups;
+  if (groups->group[i-1].priority == 0)
+    groups->num_groups--;
+
+  // Where do we start?
+  if (max_priority)
+    priority = max_priority;
+  else
+    priority = groups->num_groups > 15 ? 15 : groups->num_groups;
+
+  // Figure out what the new values should be
+  for (i=0; i<groups->num_groups; i++) {
+    groups->group[i].priority = priority;
+    if (priority > 1)
+      priority--;
+  }
+
+  // Now apply the ranking to the GPT
+  for (i=0; i<groups->num_groups; i++)
+    for (j=0; j<groups->group[i].num_parts; j++)
+      SetPriority(&drive.gpt, PRIMARY,
+                  groups->group[i].part[j], groups->group[i].priority);
+
+  FreeGroups(groups);
+
+
+  // Write it all out
+good:
+  RepairEntries(&drive.gpt, MASK_PRIMARY);
+  RepairHeader(&drive.gpt, MASK_PRIMARY);
+
+  drive.gpt.modified |= (GPT_MODIFIED_HEADER1 | GPT_MODIFIED_ENTRIES1 |
+                         GPT_MODIFIED_HEADER2 | GPT_MODIFIED_ENTRIES2);
+  UpdateCrc(&drive.gpt);
+
+  return DriveClose(&drive, 1);
+
+bad:
+  (void) DriveClose(&drive, 0);
+  return CGPT_FAILED;
+}
diff --git a/cgpt/cmd_show.c b/cgpt/cmd_show.c
index 920a30f..4942251 100644
--- a/cgpt/cmd_show.c
+++ b/cgpt/cmd_show.c
@@ -133,7 +133,7 @@
     }
     GuidToStr(&entry->unique, unique, GUID_STRLEN);
     printf(PARTITION_MORE, "UUID: ", unique);
-    if (!memcmp(&guid_chromeos_kernel, &entry->type, sizeof(Guid))) {
+    if (GuidEqual(&guid_chromeos_kernel, &entry->type)) {
       int tries = (entry->attrs.fields.gpt_att &
                    CGPT_ATTRIBUTE_TRIES_MASK) >>
           CGPT_ATTRIBUTE_TRIES_OFFSET;
@@ -176,7 +176,8 @@
     GptEntry *entry;
     entry = GetEntry(gpt, secondary, i);
 
-    if (!memcmp(&guid_unused, &entry->type, sizeof(Guid))) continue;
+    if (IsZero(&entry->type))
+      continue;
 
     EntryDetails(entry, i, raw);
   }
diff --git a/firmware/lib/cgptlib/cgptlib.c b/firmware/lib/cgptlib/cgptlib.c
index 20c0b9e..6590e80 100644
--- a/firmware/lib/cgptlib/cgptlib.c
+++ b/firmware/lib/cgptlib/cgptlib.c
@@ -9,11 +9,6 @@
 #include "gpt.h"
 #include "utility.h"
 
-/* global types to compare against */
-const Guid guid_unused = GPT_ENT_TYPE_UNUSED;
-const Guid guid_chromeos_kernel = GPT_ENT_TYPE_CHROMEOS_KERNEL;
-
-
 int GptInit(GptData *gpt) {
   int retval;
 
diff --git a/firmware/lib/cgptlib/include/gpt.h b/firmware/lib/cgptlib/include/gpt.h
index b55f921..b13b221 100644
--- a/firmware/lib/cgptlib/include/gpt.h
+++ b/firmware/lib/cgptlib/include/gpt.h
@@ -62,10 +62,6 @@
 
 #define GUID_EXPECTED_SIZE GUID_SIZE
 
-/* Some constant values */
-extern const Guid guid_unused;
-extern const Guid guid_chromeos_kernel;
-
 /* GPT header defines how many partitions exist on a drive and sectors managed.
  * For every drive device, there are 2 headers, primary and secondary.
  * Most of fields are duplicated except my_lba and entries_lba.
diff --git a/firmware/version.c b/firmware/version.c
index 836810d..e307395 100644
--- a/firmware/version.c
+++ b/firmware/version.c
@@ -1 +1 @@
-char* VbootVersion = "VBOOv=f66e9197";
+char* VbootVersion = "VBOOv=9e0713db";
diff --git a/tests/common.sh b/tests/common.sh
index cd7695f..5145004 100755
--- a/tests/common.sh
+++ b/tests/common.sh
@@ -27,23 +27,41 @@
 COL_STOP='\E[0;m'
 
 hash_algos=( sha1 sha256 sha512 )
-key_lengths=( 1024 2048 4096 8192 ) 
+key_lengths=( 1024 2048 4096 8192 )
 
 function happy {
   echo -e "${COL_GREEN}$*${COL_STOP}" 1>&2
 }
 
+# args: [nested level [message]]
 function warning {
   echo -e "${COL_YELLOW}WARNING: $*${COL_STOP}" 1>&2
 }
 
+# args: [nested level [message]]
 function error {
-  echo -e "${COL_RED}ERROR: $*${COL_STOP}" 1>&2
+  local lev=${1:-}
+  case "${1:-}" in
+    [0-9]*)
+      lev=$1
+      shift
+      ;;
+    *) lev=0
+      ;;
+  esac
+  local x=$(caller $lev)
+  local cline=${x%% *}
+  local cfunc=${x#* }
+  cfunc=${cfunc##*/}
+  local args="$*"
+  local spacer=${args:+: }
+  echo -e "${COL_RED}ERROR at ${cfunc}, line ${cline}${spacer}${args}" \
+    "${COL_STOP}" 1>&2
   exit 1
 }
 
 function check_test_keys {
   [ -d ${TESTKEY_DIR} ] || \
-    error "You must run gen_test_keys.sh to generate test keys first."
+    error 1 "You must run gen_test_keys.sh to generate test keys first."
 }
 
diff --git a/tests/run_cgpt_tests.sh b/tests/run_cgpt_tests.sh
index 577d7b1..ffd6f68 100755
--- a/tests/run_cgpt_tests.sh
+++ b/tests/run_cgpt_tests.sh
@@ -81,27 +81,27 @@
 
 X=$($GPT show -b -i $DATA_NUM ${DEV})
 Y=$($GPT show -s -i $DATA_NUM ${DEV})
-[ "$X $Y" = "$DATA_START $DATA_SIZE" ] || error "fail at line $LINENO"
+[ "$X $Y" = "$DATA_START $DATA_SIZE" ] || error
 
 X=$($GPT show -b -i $KERN_NUM ${DEV})
 Y=$($GPT show -s -i $KERN_NUM ${DEV})
-[ "$X $Y" = "$KERN_START $KERN_SIZE" ] || error "fail at line $LINENO"
+[ "$X $Y" = "$KERN_START $KERN_SIZE" ] || error
 
 X=$($GPT show -b -i $ROOTFS_NUM ${DEV})
 Y=$($GPT show -s -i $ROOTFS_NUM ${DEV})
-[ "$X $Y" = "$ROOTFS_START $ROOTFS_SIZE" ] || error "fail at line $LINENO"
+[ "$X $Y" = "$ROOTFS_START $ROOTFS_SIZE" ] || error
 
 X=$($GPT show -b -i $ESP_NUM ${DEV})
 Y=$($GPT show -s -i $ESP_NUM ${DEV})
-[ "$X $Y" = "$ESP_START $ESP_SIZE" ] || error "fail at line $LINENO"
+[ "$X $Y" = "$ESP_START $ESP_SIZE" ] || error
 
 X=$($GPT show -b -i $FUTURE_NUM ${DEV})
 Y=$($GPT show -s -i $FUTURE_NUM ${DEV})
-[ "$X $Y" = "$FUTURE_START $FUTURE_SIZE" ] || error "fail at line $LINENO"
+[ "$X $Y" = "$FUTURE_START $FUTURE_SIZE" ] || error
 
 X=$($GPT show -b -i $RANDOM_NUM ${DEV})
 Y=$($GPT show -s -i $RANDOM_NUM ${DEV})
-[ "$X $Y" = "$RANDOM_START $RANDOM_SIZE" ] || error "fail at line $LINENO"
+[ "$X $Y" = "$RANDOM_START $RANDOM_SIZE" ] || error
 
 
 echo "Set the boot partition.."
@@ -110,7 +110,127 @@
 echo "Check the PMBR's idea of the boot partition..."
 X=$($GPT boot ${DEV})
 Y=$($GPT show -u -i $KERN_NUM $DEV)
-[ "$X" = "$Y" ] || error "fail at line $LINENO"
+[ "$X" = "$Y" ] || error
+
+
+echo "Test the cgpt prioritize command..."
+
+# Input: sequence of priorities
+# Output: ${DEV} has kernel partitions with the given priorities
+make_pri() {
+  local idx=0
+  $GPT create ${DEV}
+  for pri in "$@"; do
+    idx=$((idx+1))
+    $GPT add -t kernel -l "kern$idx" -b $((100 + 2 * $idx)) -s 1 -P $pri ${DEV}
+  done
+}
+
+# Output: returns string containing priorities of all kernels
+get_pri() {
+  echo $(
+  for idx in $($GPT find -t kernel ${DEV} | sed -e s@${DEV}@@); do
+    $GPT show -i $idx -P ${DEV}
+  done
+  )
+}
+
+# Input: list of priorities
+# Operation: expects ${DEV} to contain those kernel priorities
+assert_pri() {
+  local expected="$*"
+  local actual=$(get_pri)
+  [ "$actual" = "$expected" ] || \
+    error 1 "expected priority \"$expected\", actual priority \"$actual\""
+}
+
+
+# no kernels at all. This should do nothing.
+$GPT create ${DEV}
+$GPT add -t rootfs -b 100 -s 1 ${DEV}
+$GPT prioritize ${DEV}
+assert_pri ""
+
+# common install/upgrade sequence
+make_pri   2 0 0
+$GPT prioritize -i 1 ${DEV}
+assert_pri 1 0 0
+$GPT prioritize -i 2 ${DEV}
+assert_pri 1 2 0
+$GPT prioritize -i 1 ${DEV}
+assert_pri 2 1 0
+$GPT prioritize -i 2 ${DEV}
+assert_pri 1 2 0
+
+# lots of kernels, all same starting priority, should go to priority 1
+make_pri   8 8 8 8 8 8 8 8 8 8 8 0 0 8
+$GPT prioritize ${DEV}
+assert_pri 1 1 1 1 1 1 1 1 1 1 1 0 0 1
+
+# now raise them all up again
+$GPT prioritize -P 4 ${DEV}
+assert_pri 4 4 4 4 4 4 4 4 4 4 4 0 0 4
+
+# set one of them higher, should leave the rest alone
+$GPT prioritize -P 5 -i 3 ${DEV}
+assert_pri 4 4 5 4 4 4 4 4 4 4 4 0 0 4
+
+# set one of them lower, should bring the rest down
+$GPT prioritize -P 3 -i 4 ${DEV}
+assert_pri 1 1 2 3 1 1 1 1 1 1 1 0 0 1
+
+# raise a group by including the friends of one partition
+$GPT prioritize -P 6 -i 1 -f ${DEV}
+assert_pri 6 6 4 5 6 6 6 6 6 6 6 0 0 6
+
+# resurrect one, should not affect the others
+make_pri   0 0 0 0 0 0 0 0 0 0 0 0 0 0
+$GPT prioritize -i 2 ${DEV}
+assert_pri 0 1 0 0 0 0 0 0 0 0 0 0 0 0
+
+# resurrect one and all its friends
+make_pri   0 0 0 0 0 0 0 0 1 2 0 0 0 0
+$GPT prioritize -P 5 -i 2 -f ${DEV}
+assert_pri 5 5 5 5 5 5 5 5 3 4 5 5 5 5
+
+# no options should maintain the same order
+$GPT prioritize ${DEV}
+assert_pri 3 3 3 3 3 3 3 3 1 2 3 3 3 3
+
+# squish all the ranks
+make_pri   1 1 2 2 3 3 4 4 5 5 0 6 7 7
+$GPT prioritize -P 6 ${DEV}
+assert_pri 1 1 1 1 2 2 3 3 4 4 0 5 6 6
+
+# squish the ranks by not leaving room
+make_pri   1 1 2 2 3 3 4 4 5 5 0 6 7 7
+$GPT prioritize -P 7 -i 3 ${DEV}
+assert_pri 1 1 7 1 2 2 3 3 4 4 0 5 6 6
+
+# squish the ranks while bringing the friends along
+make_pri   1 1 2 2 3 3 4 4 5 5 0 6 7 7
+$GPT prioritize -P 6 -i 3 -f ${DEV}
+assert_pri 1 1 6 6 1 1 2 2 3 3 0 4 5 5
+
+# squish them pretty hard
+make_pri   1 1 2 2 3 3 4 4 5 5 0 6 7 7
+$GPT prioritize -P 2 ${DEV}
+assert_pri 1 1 1 1 1 1 1 1 1 1 0 1 2 2
+
+# squish them really really hard (nobody gets reduced to zero, though)
+make_pri   1 1 2 2 3 3 4 4 5 5 0 6 7 7
+$GPT prioritize -P 1 -i 3 ${DEV}
+assert_pri 1 1 1 1 1 1 1 1 1 1 0 1 1 1
+
+# squish if we try to go too high
+make_pri   15 15 14 14 13 13 12 12 11 11 10 10 9 9 8 8 7 7 6 6 5 5 4 4 3 3 2 2 1 1 0
+$GPT prioritize -i 3 ${DEV}
+assert_pri 14 14 15 13 12 12 11 11 10 10  9  9 8 8 7 7 6 6 5 5 4 4 3 3 2 2 1 1 1 1 0
+$GPT prioritize -i 5 ${DEV}
+assert_pri 13 13 14 12 15 11 10 10  9  9  8  8 7 7 6 6 5 5 4 4 3 3 2 2 1 1 1 1 1 1 0
+# but if I bring friends I don't have to squish
+$GPT prioritize -i 1 -f ${DEV}
+assert_pri 15 15 13 12 14 11 10 10  9  9  8  8 7 7 6 6 5 5 4 4 3 3 2 2 1 1 1 1 1 1 0
 
 echo "Done."