14 #include "sw/device/lib/base/status.h"
18 #include "spi_device_regs.h"
31 kUnsignedHexLow =
'x',
32 kUnsignedHexHigh =
'X',
50 static const char kDigitsLow[16] =
"0123456789abcdef";
51 static const char kDigitsHigh[16] =
"0123456789ABCDEF";
53 static const char kErrorNul[17] =
"%<unexpected nul>";
54 static const char kUnknownSpec[15] =
"%<unknown spec>";
55 static const char kErrorTooWide[12] =
"%<bad width>";
57 static size_t base_dev_null(
void *data,
const char *buf,
size_t len) {
68 if (out.sink == NULL) {
69 out.sink = &base_dev_null;
74 static const size_t kSpiDeviceReadBufferSizeBytes =
75 SPI_DEVICE_PARAM_SRAM_READ_BUFFER_DEPTH *
sizeof(uint32_t);
76 static const size_t kSpiDeviceFrameHeaderSizeBytes = 12;
77 static const size_t kSpiDeviceBufferPreservedSizeBytes =
78 kSpiDeviceFrameHeaderSizeBytes;
79 static const size_t kSpiDeviceMaxFramePayloadSizeBytes =
80 kSpiDeviceReadBufferSizeBytes - kSpiDeviceFrameHeaderSizeBytes -
81 kSpiDeviceBufferPreservedSizeBytes - 4;
82 static const uint32_t kSpiDeviceFrameMagicNumber = 0xa5a5beef;
83 static uint32_t spi_device_frame_num = 0;
86 const uint8_t *buf,
size_t len,
92 size_t space_to_end_of_buffer = kSpiDeviceReadBufferSizeBytes - address;
93 size_t first_part_size =
94 space_to_end_of_buffer < len ? space_to_end_of_buffer : len;
98 address, first_part_size, buf));
101 if (first_part_size < len) {
102 size_t second_part_size = len - first_part_size;
105 (uint8_t *)(buf + first_part_size)));
130 static size_t spi_device_send_frame(
void *data,
const char *buf,
size_t len) {
132 const size_t data_packet_size_bytes = ((len + 3u) & ~3u);
133 const size_t frame_size_bytes =
134 kSpiDeviceFrameHeaderSizeBytes + data_packet_size_bytes;
135 uint8_t frame_header_bytes[kSpiDeviceFrameHeaderSizeBytes];
137 static uint32_t next_write_address = 0;
139 if (frame_size_bytes >= kSpiDeviceReadBufferSizeBytes) {
144 for (
size_t i = 0; i < 4; ++i) {
145 frame_header_bytes[i] = (kSpiDeviceFrameMagicNumber >> (i * 8)) & 0xff;
148 for (
size_t i = 0; i < 4; ++i) {
149 frame_header_bytes[i + 4] = (spi_device_frame_num >> (i * 8)) & 0xff;
152 for (
size_t i = 0; i < 4; ++i) {
153 frame_header_bytes[i + 8] = (len >> (i * 8)) & 0xff;
156 uint32_t available_buffer_size = 0;
158 uint32_t last_read_address = 0;
171 uint32_t adjusted_last_read_address =
172 (kSpiDeviceReadBufferSizeBytes + last_read_address -
173 kSpiDeviceFrameHeaderSizeBytes) %
174 kSpiDeviceReadBufferSizeBytes;
175 uint32_t next_read_address = ((adjusted_last_read_address + 1) & ~3u) %
176 kSpiDeviceReadBufferSizeBytes;
178 if (next_read_address > next_write_address) {
179 available_buffer_size = next_read_address - next_write_address - 1;
181 available_buffer_size =
183 (kSpiDeviceReadBufferSizeBytes - next_write_address) - 1;
185 }
while ((frame_size_bytes + kSpiDeviceBufferPreservedSizeBytes) >
186 available_buffer_size);
189 size_t data_write_address =
190 (next_write_address + kSpiDeviceFrameHeaderSizeBytes) %
191 kSpiDeviceReadBufferSizeBytes;
192 size_t aligned_data_len = len & (~3u);
193 if (!status_ok(spi_device_send_data(spi_device, (uint8_t *)buf,
194 aligned_data_len, data_write_address))) {
199 if (len != aligned_data_len) {
200 uint8_t pad_bytes[4] = {0xff, 0xff, 0xff, 0xff};
201 size_t pad_write_address =
202 (data_write_address + aligned_data_len) % kSpiDeviceReadBufferSizeBytes;
204 for (
size_t i = 0; i + aligned_data_len < len; i++) {
205 pad_bytes[i] = buf[aligned_data_len + i];
207 if (!status_ok(spi_device_send_data(spi_device, pad_bytes, 4,
208 pad_write_address))) {
214 if (!status_ok(spi_device_send_data(spi_device, frame_header_bytes,
215 kSpiDeviceFrameHeaderSizeBytes,
216 next_write_address))) {
221 (next_write_address + frame_size_bytes) % kSpiDeviceReadBufferSizeBytes;
222 spi_device_frame_num++;
227 static size_t base_dev_spi_device(
void *data,
const char *buf,
size_t len) {
228 size_t write_data_len = 0;
230 while (write_data_len < len) {
231 size_t payload_len = len - write_data_len;
232 if (payload_len > kSpiDeviceMaxFramePayloadSizeBytes) {
233 payload_len = kSpiDeviceMaxFramePayloadSizeBytes;
236 if (spi_device_send_frame(data, buf + write_data_len, payload_len) ==
238 write_data_len += payload_len;
242 return write_data_len;
247 static size_t base_dev_uart(
void *data,
const char *buf,
size_t len) {
249 for (
size_t i = 0; i < len; ++i) {
261 spi_device_frame_num = 0;
263 .sink = &base_dev_spi_device});
268 (
buffer_sink_t){.data = (
void *)uart, .sink = &base_dev_uart});
273 va_start(args, format);
288 static size_t snprintf_sink(
void *data,
const char *buf,
size_t len) {
290 if (captures->bytes_left == 0) {
294 if (len > captures->bytes_left) {
295 len = captures->bytes_left;
297 memcpy(captures->buf, buf, len);
298 captures->buf += len;
299 captures->bytes_left -= len;
303 size_t base_snprintf(
char *buf,
size_t len,
const char *format, ...) {
305 va_start(args, format);
313 .sink = &snprintf_sink,
322 va_start(args, format);
337 static bool consume_until_percent(
buffer_sink_t out,
const char **format,
338 size_t *bytes_written) {
341 char c = (*format)[text_len];
342 if (c ==
'\0' || c == kPercent) {
344 *bytes_written += out.sink(out.data, *format, text_len);
372 static bool consume_format_specifier(
buffer_sink_t out,
const char **format,
373 size_t *bytes_written,
382 if ((*format)[0] ==
'!') {
383 spec->is_nonstd =
true;
395 char c = (*format)[spec_len];
397 *bytes_written += out.sink(out.data, kErrorNul,
sizeof(kErrorNul));
400 if (c < '0' || c >
'9') {
403 if (spec->padding == 0) {
412 spec->width += (c -
'0');
416 if ((spec->width == 0 && spec->padding != 0) || spec->width > 32) {
417 *bytes_written += out.sink(out.data, kErrorTooWide,
sizeof(kErrorTooWide));
421 spec->type = (*format)[spec_len];
422 *format += spec_len + 1;
438 static size_t write_digits(
buffer_sink_t out, uint32_t value, uint32_t width,
439 char padding, uint32_t base,
const char *glyphs) {
442 static const int kWordBits =
sizeof(uint32_t) * 8;
443 char buffer[kWordBits];
447 buffer[kWordBits - 1] = glyphs[0];
451 uint32_t digit = value % base;
453 buffer[kWordBits - 1 - len] = glyphs[digit];
456 width = width == 0 ? 1 : width;
457 width = width > kWordBits ? kWordBits : width;
458 while (len < width) {
459 buffer[kWordBits - len - 1] = padding;
462 return out.sink(out.data, buffer + (kWordBits - len), len);
467 char mod[] = {
'"', 0, 0, 0,
'"',
','};
470 bool err = status_extract(value, &start, &arg, &mod[1]);
473 const char *end = start;
478 len += out.sink(out.data,
"{\"", as_json ? 2 : 0);
479 len += out.sink(out.data, start, (
size_t)(end - start));
480 len += out.sink(out.data,
"\"", as_json ? 1 : 0);
482 len += out.sink(out.data,
":", 1);
485 len += out.sink(out.data,
"[", 1);
486 len += out.sink(out.data, mod,
sizeof(mod));
487 len += write_digits(out, (uint32_t)arg, 0, 0, 10, kDigitsLow);
488 len += out.sink(out.data,
"]", 1);
491 len += write_digits(out, (uint32_t)arg, 0, 0, 10, kDigitsLow);
493 len += out.sink(out.data,
"}", as_json ? 1 : 0);
511 static size_t hex_dump(
buffer_sink_t out,
const char *bytes,
size_t len,
512 uint32_t width,
char padding,
bool big_endian,
513 const char *glyphs) {
514 size_t bytes_written = 0;
519 memset(buf, padding,
sizeof(buf));
521 size_t to_write = width >
ARRAYSIZE(buf) ? 32 : width;
522 bytes_written += out.sink(out.data, buf, to_write);
527 for (
size_t i = 0; i < len; ++i) {
528 size_t idx = big_endian ? len - i - 1 : i;
529 buf[buffered] = glyphs[(bytes[idx] >> 4) & 0xf];
530 buf[buffered + 1] = glyphs[bytes[idx] & 0xf];
534 bytes_written += out.sink(out.data, buf, buffered);
540 bytes_written += out.sink(out.data, buf, buffered);
542 return bytes_written;
557 size_t *bytes_written, va_list *args) {
563 if (spec.is_nonstd) {
566 *bytes_written += out.sink(out.data,
"%", 1);
570 if (spec.is_nonstd) {
573 char value = (char)va_arg(*args, uint32_t);
574 *bytes_written += out.sink(out.data, &value, 1);
578 uint32_t value = va_arg(*args, uint32_t);
579 for (
size_t i = 0; i <
sizeof(uint32_t); ++i, value >>= 8) {
580 uint8_t ch = (uint8_t)value;
581 if (ch >= 32 && ch < 127) {
582 *bytes_written += out.sink(out.data, (
const char *)&ch, 1);
584 *bytes_written += out.sink(out.data,
"\\x", 2);
585 *bytes_written += out.sink(out.data, &kDigitsLow[ch >> 4], 1);
586 *bytes_written += out.sink(out.data, &kDigitsLow[ch & 15], 1);
593 if (spec.is_nonstd) {
595 len = va_arg(*args,
size_t);
598 char *value = va_arg(*args,
char *);
599 while (!spec.is_nonstd && value[len] !=
'\0') {
604 *bytes_written += out.sink(out.data, value, len);
609 if (spec.is_nonstd) {
612 uint32_t value = va_arg(*args, uint32_t);
613 if (((int32_t)value) < 0) {
614 *bytes_written += out.sink(out.data,
"-", 1);
618 write_digits(out, value, spec.width, spec.padding, 10, kDigitsLow);
622 if (spec.is_nonstd) {
625 uint32_t value = va_arg(*args, uint32_t);
627 write_digits(out, value, spec.width, spec.padding, 8, kDigitsLow);
631 if (spec.is_nonstd) {
640 *bytes_written += out.sink(out.data,
"0x", 2);
641 uintptr_t value = va_arg(*args, uintptr_t);
643 write_digits(out, value,
sizeof(uintptr_t) * 2,
'0', 16, kDigitsLow);
646 case kUnsignedHexLow:
647 if (spec.is_nonstd) {
648 size_t len = va_arg(*args,
size_t);
649 char *value = va_arg(*args,
char *);
650 *bytes_written += hex_dump(out, value, len, spec.width, spec.padding,
656 uint32_t value = va_arg(*args, uint32_t);
658 write_digits(out, value, spec.width, spec.padding, 16, kDigitsLow);
661 case kUnsignedHexHigh:
662 if (spec.is_nonstd) {
663 size_t len = va_arg(*args,
size_t);
664 char *value = va_arg(*args,
char *);
665 *bytes_written += hex_dump(out, value, len, spec.width, spec.padding,
671 uint32_t value = va_arg(*args, uint32_t);
673 write_digits(out, value, spec.width, spec.padding, 16, kDigitsHigh);
677 if (!spec.is_nonstd) {
680 size_t len = va_arg(*args,
size_t);
681 char *value = va_arg(*args,
char *);
682 *bytes_written += hex_dump(out, value, len, spec.width, spec.padding,
687 if (!spec.is_nonstd) {
690 size_t len = va_arg(*args,
size_t);
691 char *value = va_arg(*args,
char *);
692 *bytes_written += hex_dump(out, value, len, spec.width, spec.padding,
697 if (spec.is_nonstd) {
700 uint32_t value = va_arg(*args, uint32_t);
702 write_digits(out, value, spec.width, spec.padding, 10, kDigitsLow);
706 if (spec.is_nonstd) {
709 if (va_arg(*args,
int) != 0) {
710 *bytes_written += out.sink(out.data,
"true", 4);
712 *bytes_written += out.sink(out.data,
"false", 5);
716 uint32_t value = va_arg(*args, uint32_t);
718 write_digits(out, value, spec.width, spec.padding, 2, kDigitsLow);
721 case kStatusResult: {
723 *bytes_written += write_status(out, value, spec.is_nonstd);
728 *bytes_written += out.sink(out.data, kUnknownSpec,
sizeof(kUnknownSpec));
734 if (out.sink == NULL) {
735 out.sink = &base_dev_null;
743 va_copy(args_copy, args);
745 size_t bytes_written = 0;
746 while (format[0] !=
'\0') {
747 if (!consume_until_percent(out, &format, &bytes_written)) {
751 if (!consume_format_specifier(out, &format, &bytes_written, &spec)) {
755 process_specifier(out, spec, &bytes_written, &args_copy);
759 return bytes_written;
765 "................................"
767 " !\"#$%&'()*+,-./0123456789:;<=>?@ABCDEFGHIJKLMNOPQRSTUVWXYZ[\\]^_`abcdefghijklmnopqrstuvwxyz{|}~"
769 "................................................................"
770 "................................................................"
797 const char *buf,
size_t len) {
800 .bytes_left = out_len,
804 .sink = &snprintf_sink,
810 const char *buf,
size_t len) {
811 if (out.sink == NULL) {
812 out.sink = &base_dev_null;
815 size_t bytes_written = 0;
818 for (
size_t line = 0; line < len; line += bytes_per_line) {
822 size_t line_bytes_written = 0;
823 for (
size_t word = 0; word < bytes_per_line; word += fmt.
bytes_per_word) {
824 if (len < line + word) {
825 char spaces[16] =
" ";
826 while (line_bytes_written < chars_per_line) {
827 size_t to_print = chars_per_line - line_bytes_written;
828 if (to_print >
sizeof(spaces)) {
829 to_print =
sizeof(spaces);
831 line_bytes_written +=
base_fprintf(out,
"%!s", to_print, spaces);
836 size_t bytes_left = len - line - word;
841 line_bytes_written +=
842 hex_dump(out, buf + line + word, bytes_left, bytes_left,
'0',
845 bytes_written += line_bytes_written;
849 char glyph_buffer[16];
850 for (
size_t byte = 0;
byte < bytes_per_line; ++byte) {
851 if (buffered ==
sizeof(glyph_buffer)) {
852 bytes_written +=
base_fprintf(out,
"%!s", buffered, glyph_buffer);
855 if (line +
byte >= len) {
858 glyph_buffer[buffered++] =
859 (*fmt.
alphabet)[(
size_t)(uint8_t)buf[line +
byte]];
862 bytes_written +=
base_fprintf(out,
"%!s", buffered, glyph_buffer);
867 return bytes_written;