#include <_stdio.h> #include <alloc.h> #include <cursor.h> #include <message.h> #include <osTasks.h> #include <ports.h> #include <stdarg.h> #include <stdbool.h> #include <stdint.h> #include <task.h> #include <util.h> char HEX_CHARS[] = "0123456789ABCDEF"; void putHex(char **write, uint32_t x) { if (x == 0) { **write = HEX_CHARS[x]; (*write)++; **write = HEX_CHARS[x]; (*write)++; return; } bool alreadyWriting = false; for (int position = 3; position >= 0; position--) { uint8_t byte = (x >> (position * 8)) & 0xFF; if (byte != 0x00 && !alreadyWriting) { alreadyWriting = true; } if (alreadyWriting) { **write = HEX_CHARS[byte >> 4]; (*write)++; **write = HEX_CHARS[byte & 0x0F]; (*write)++; } } } uint8_t hexLength(uint32_t x) { bool alreadyWriting = false; uint8_t size = 0; for (int position = 3; position >= 0; position--) { uint8_t byte = (x >> (position * 8)) & 0xFF; if (byte != 0x00 && !alreadyWriting) { alreadyWriting = true; } if (alreadyWriting) { size += 2; } } return max(size, 2); } uint32_t getInsertLength(char insertType, uint32_t x) { switch (insertType) { case 's': return strlen((char *)x); case 'x': return hexLength(x); case 'c': return 1; } return 0; } void stringInsert(char **write, uint32_t x) { char *string = (char *)x; uint32_t length = strlen(string); for (uint32_t position = 0; position < length; position++) { **write = string[position]; (*write)++; } } void handleInsert(char **write, char insertType, uint32_t x) { switch (insertType) { case 's': stringInsert(write, x); return; case 'x': putHex(write, x); return; case 'c': **write = x; return; } } void printf(const char *format, ...) { uint32_t size = 0; va_list valist; va_start(valist, format); for (int i = 0; format[i] != 0; i++) { if (format[i] == '%') { size += getInsertLength(format[++i], va_arg(valist, uint32_t)); continue; } size++; } va_start(valist, format); char *data = malloc(size); char *write = data; for (int i = 0; format[i] != 0; i++) { if (format[i] == '%') { handleInsert(&write, format[++i], va_arg(valist, uint32_t)); continue; } *write = format[i]; write++; } va_end(valist); Message *message = malloc(sizeof(Message)); message->data = data; message->size = size; message->type = -1; listAdd(&getPrinterTask()->messages, message); schedule(getPrinterTask()); } uint32_t strlen(char *string) { uint32_t length = 0; while (string[length] != 0) { length++; } return length; }