+ if (data->rec_count > RECORD_COUNT)
+ printf("Overflowed internal boot id table by %d entries\n"
+ "Please increase CONFIG_(SPL_)BOOTSTAGE_RECORD_COUNT\n",
+ data->rec_count - RECORD_COUNT);
+
+ puts("\nAccumulated time:\n");
+ for (i = 0, rec = data->record; i < data->rec_count; i++, rec++) {
+ if (rec->start_us)
+ prev = print_time_record(rec, -1);
+ }
+}
+
+/**
+ * Append data to a memory buffer
+ *
+ * Write data to the buffer if there is space. Whether there is space or not,
+ * the buffer pointer is incremented.
+ *
+ * @param ptrp Pointer to buffer, updated by this function
+ * @param end Pointer to end of buffer
+ * @param data Data to write to buffer
+ * @param size Size of data
+ */
+static void append_data(char **ptrp, char *end, const void *data, int size)
+{
+ char *ptr = *ptrp;
+
+ *ptrp += size;
+ if (*ptrp > end)
+ return;
+
+ memcpy(ptr, data, size);
+}
+
+int bootstage_stash(void *base, int size)
+{
+ const struct bootstage_data *data = gd->bootstage;
+ struct bootstage_hdr *hdr = (struct bootstage_hdr *)base;
+ const struct bootstage_record *rec;
+ char buf[20];
+ char *ptr = base, *end = ptr + size;
+ uint32_t count;
+ int i;
+
+ if (hdr + 1 > (struct bootstage_hdr *)end) {
+ debug("%s: Not enough space for bootstage hdr\n", __func__);
+ return -ENOSPC;
+ }
+
+ /* Write an arbitrary version number */
+ hdr->version = BOOTSTAGE_VERSION;
+
+ /* Count the number of records, and write that value first */
+ for (rec = data->record, i = count = 0; i < data->rec_count;
+ i++, rec++) {
+ if (rec->id != 0)
+ count++;
+ }
+ hdr->count = count;
+ hdr->size = 0;
+ hdr->magic = BOOTSTAGE_MAGIC;
+ ptr += sizeof(*hdr);
+
+ /* Write the records, silently stopping when we run out of space */
+ for (rec = data->record, i = 0; i < data->rec_count; i++, rec++) {
+ append_data(&ptr, end, rec, sizeof(*rec));
+ }
+
+ /* Write the name strings */
+ for (rec = data->record, i = 0; i < data->rec_count; i++, rec++) {
+ const char *name;
+
+ name = get_record_name(buf, sizeof(buf), rec);
+ append_data(&ptr, end, name, strlen(name) + 1);
+ }
+
+ /* Check for buffer overflow */
+ if (ptr > end) {
+ debug("%s: Not enough space for bootstage stash\n", __func__);
+ return -ENOSPC;
+ }
+
+ /* Update total data size */
+ hdr->size = ptr - (char *)base;
+ debug("Stashed %d records\n", hdr->count);
+
+ return 0;
+}
+
+int bootstage_unstash(const void *base, int size)
+{
+ const struct bootstage_hdr *hdr = (struct bootstage_hdr *)base;
+ struct bootstage_data *data = gd->bootstage;
+ const char *ptr = base, *end = ptr + size;
+ struct bootstage_record *rec;
+ uint rec_size;
+ int i;
+
+ if (size == -1)
+ end = (char *)(~(uintptr_t)0);
+
+ if (hdr + 1 > (struct bootstage_hdr *)end) {
+ debug("%s: Not enough space for bootstage hdr\n", __func__);
+ return -EPERM;
+ }
+
+ if (hdr->magic != BOOTSTAGE_MAGIC) {
+ debug("%s: Invalid bootstage magic\n", __func__);
+ return -ENOENT;
+ }
+
+ if (ptr + hdr->size > end) {
+ debug("%s: Bootstage data runs past buffer end\n", __func__);
+ return -ENOSPC;
+ }
+
+ if (hdr->count * sizeof(*rec) > hdr->size) {
+ debug("%s: Bootstage has %d records needing %lu bytes, but "
+ "only %d bytes is available\n", __func__, hdr->count,
+ (ulong)hdr->count * sizeof(*rec), hdr->size);
+ return -ENOSPC;
+ }
+
+ if (hdr->version != BOOTSTAGE_VERSION) {
+ debug("%s: Bootstage data version %#0x unrecognised\n",
+ __func__, hdr->version);
+ return -EINVAL;
+ }
+
+ if (data->rec_count + hdr->count > RECORD_COUNT) {
+ debug("%s: Bootstage has %d records, we have space for %d\n"
+ "Please increase CONFIG_(SPL_)BOOTSTAGE_RECORD_COUNT\n",
+ __func__, hdr->count, RECORD_COUNT - data->rec_count);
+ return -ENOSPC;
+ }
+
+ ptr += sizeof(*hdr);
+
+ /* Read the records */
+ rec_size = hdr->count * sizeof(*data->record);
+ memcpy(data->record + data->rec_count, ptr, rec_size);
+
+ /* Read the name strings */
+ ptr += rec_size;
+ for (rec = data->record + data->next_id, i = 0; i < hdr->count;
+ i++, rec++) {
+ rec->name = ptr;
+
+ /* Assume no data corruption here */
+ ptr += strlen(ptr) + 1;
+ }
+
+ /* Mark the records as read */
+ data->rec_count += hdr->count;
+ debug("Unstashed %d records\n", hdr->count);
+
+ return 0;
+}
+
+int bootstage_get_size(void)
+{
+ return sizeof(struct bootstage_data);
+}
+
+int bootstage_init(bool first)
+{
+ struct bootstage_data *data;
+ int size = sizeof(struct bootstage_data);
+
+ gd->bootstage = (struct bootstage_data *)malloc(size);
+ if (!gd->bootstage)
+ return -ENOMEM;
+ data = gd->bootstage;
+ memset(data, '\0', size);
+ if (first) {
+ data->next_id = BOOTSTAGE_ID_USER;
+ bootstage_add_record(BOOTSTAGE_ID_AWAKE, "reset", 0, 0);
+ }
+
+ return 0;