diff --git a/include/grainuum.h b/include/grainuum.h index 07eb25c..59332e9 100644 --- a/include/grainuum.h +++ b/include/grainuum.h @@ -497,7 +497,8 @@ int grainuumDataQueued(struct GrainuumUSB *usb); * @api */ void grainuumProcess(struct GrainuumUSB *usb, - const uint8_t packet[12]); + const uint8_t packet[12], + uint32_t size); /** * @brief Initialize the Grainuum USB system. diff --git a/src/grainuum-phy.c b/src/grainuum-phy.c index 268e366..18b7685 100644 --- a/src/grainuum-phy.c +++ b/src/grainuum-phy.c @@ -29,57 +29,53 @@ * authorization. * ****************************************************************************/ -#include +#include #include +#include -__attribute__((weak)) -void grainuumConnectPre(struct GrainuumUSB *usb) +__attribute__((weak)) void grainuumConnectPre(struct GrainuumUSB *usb) { (void)usb; } -__attribute__((weak)) -void grainuumConnectPost(struct GrainuumUSB *usb) +__attribute__((weak)) void grainuumConnectPost(struct GrainuumUSB *usb) { (void)usb; } -__attribute__((weak)) -void grainuumDisconnectPre(struct GrainuumUSB *usb) +__attribute__((weak)) void grainuumDisconnectPre(struct GrainuumUSB *usb) { (void)usb; } -__attribute__((weak)) -void grainuumDisconnectPost(struct GrainuumUSB *usb) +__attribute__((weak)) void grainuumDisconnectPost(struct GrainuumUSB *usb) { (void)usb; } -__attribute__((weak)) -void grainuumReceivePacket(struct GrainuumUSB *usb) +__attribute__((weak)) void grainuumReceivePacket(struct GrainuumUSB *usb) { (void)usb; } -__attribute__((weak)) -void grainuumInitPre(struct GrainuumUSB *usb) +__attribute__((weak)) void grainuumInitPre(struct GrainuumUSB *usb) { (void)usb; } -__attribute__((weak)) -void grainuumInitPost(struct GrainuumUSB *usb) +__attribute__((weak)) void grainuumInitPost(struct GrainuumUSB *usb) { (void)usb; } /* --- */ -void grainuum_receive_packet(struct GrainuumUSB *usb) { +void grainuum_receive_packet(struct GrainuumUSB *usb) +{ grainuumReceivePacket(usb); } int grainuumCaptureI(struct GrainuumUSB *usb, uint8_t samples[67]) { +#if 0 int ret; const uint8_t nak_pkt[] = {USB_PID_NAK}; const uint8_t ack_pkt[] = {USB_PID_ACK}; @@ -133,6 +129,45 @@ int grainuumCaptureI(struct GrainuumUSB *usb, uint8_t samples[67]) } return ret; +#endif + uint8_t obufbuf[128]; + uint32_t obufbuf_cnt = 0; + while (!usb_ep_0_out_obuf_empty_read()) + { + uint32_t obh = usb_ep_0_out_obuf_head_read(); + obufbuf[obufbuf_cnt++] = obh; + usb_ep_0_out_obuf_head_write(1); + } + + int i; + static int loops; + uint8_t last_tok = usb_ep_0_out_last_tok_read(); + printf("i: %d b: %d olt: %02x --", loops, obufbuf_cnt, last_tok); //obe: %d obh: %02x\n", i, obe, obh); + for (i = 0; i < obufbuf_cnt; i++) + { + printf(" %02x", obufbuf[i]); + } + printf("\n"); + + usb_ep_0_out_ev_pending_write((1 << 1)); + + // Response + if (!usb_ep_0_in_ibuf_empty_read()) + { + printf("USB ibuf still has data\n"); + return 0; + } + + uint32_t usb_in_pending = usb_ep_0_out_ev_pending_read(); + if (usb_in_pending) + { + printf("USB EP0 in pending is: %02x\n", usb_in_pending); + return 0; + } + + grainuumProcess(usb, obufbuf, obufbuf_cnt); + + return 0; } int grainuumInitialized(struct GrainuumUSB *usb) @@ -143,16 +178,34 @@ int grainuumInitialized(struct GrainuumUSB *usb) return usb->initialized; } +enum usb_responses { + USB_STALL = 0b11, + USB_ACK = 0b00, + USB_NAK = 0b01, + USB_NONE = 0b10, +}; + void grainuumWriteQueue(struct GrainuumUSB *usb, int epnum, const void *buffer, int size) { usb->queued_data = buffer; usb->queued_epnum = epnum; usb->queued_size = size; + + int i; + const uint8_t *buffer_u8 = buffer; + for (i = 0; i < size; i++) + { + usb_ep_0_in_ibuf_head_write(buffer_u8[i]); + } + // Indicate that we respond with an ACK + usb_ep_0_in_respond_write(USB_ACK); + usb_ep_0_in_ev_pending_write(0xff); } void grainuumInit(struct GrainuumUSB *usb, - struct GrainuumConfig *cfg) { + struct GrainuumConfig *cfg) +{ if (usb->initialized) return; @@ -168,7 +221,8 @@ void grainuumInit(struct GrainuumUSB *usb, grainuumInitPost(usb); } -void grainuumDisconnect(struct GrainuumUSB *usb) { +void grainuumDisconnect(struct GrainuumUSB *usb) +{ grainuumDisconnectPre(usb); @@ -177,7 +231,8 @@ void grainuumDisconnect(struct GrainuumUSB *usb) { grainuumDisconnectPost(usb); } -void grainuumConnect(struct GrainuumUSB *usb) { +void grainuumConnect(struct GrainuumUSB *usb) +{ grainuumConnectPre(usb); diff --git a/src/grainuum-state.c b/src/grainuum-state.c index dbd6f6b..292d707 100644 --- a/src/grainuum-state.c +++ b/src/grainuum-state.c @@ -29,6 +29,7 @@ * authorization. * ****************************************************************************/ #include "grainuum.h" + #include #ifndef NULL #define NULL ((void *)0) @@ -316,13 +317,15 @@ static inline void grainuum_state_parse_token(struct GrainuumState *state, } void grainuumProcess(struct GrainuumUSB *usb, - const uint8_t packet[GRAINUUM_PACKET_SIZE_MAX + 3]) + const uint8_t packet[GRAINUUM_PACKET_SIZE_MAX + 3], + uint32_t size) { - uint32_t size = packet[GRAINUUM_PACKET_SIZE_MAX + 3]; + // uint32_t size = packet[GRAINUUM_PACKET_SIZE_MAX + 3]; struct GrainuumState *state = &usb->state; switch(packet[0]) { case USB_PID_SETUP: + printf("Setup packet!\n"); state->packet_type = packet_type_setup; grainuum_state_clear_tx(state, 1); grainuum_state_parse_token(state, packet + 1); diff --git a/src/main.c b/src/main.c index 4635b5a..51da4b7 100644 --- a/src/main.c +++ b/src/main.c @@ -7,13 +7,6 @@ #include -enum usb_responses { - USB_STALL = 0b11, - USB_ACK = 0b00, - USB_NAK = 0b01, - USB_NONE = 0b10, -}; - void isr(void) { unsigned int irqs; @@ -43,12 +36,6 @@ static void init(void) init_printf(NULL, rv_putchar); } -static uint32_t read_timer(void) -{ - timer0_update_value_write(1); - return timer0_value_read(); -} - #ifdef CSR_USB_EP_0_OUT_EV_PENDING_ADDR struct usb_status { @@ -86,40 +73,13 @@ static void get_print_status(void) if (s.out_ev_pending & (1 << 1)) { loops++; - uint8_t obufbuf[128]; - while (!usb_ep_0_out_obuf_empty_read()) { - uint32_t obh = usb_ep_0_out_obuf_head_read(); - obufbuf[obufbuf_cnt++] = obh; - usb_ep_0_out_obuf_head_write(1); - } - - int i; - printf("i: %d b: %d olt: %02x --", loops, obufbuf_cnt, s.out_last_tok);//obe: %d obh: %02x\n", i, obe, obh); - for (i = 0; i < obufbuf_cnt; i++) { - printf(" %02x", obufbuf[i]); - } - printf("\n"); - - usb_ep_0_out_ev_pending_write((1 << 1)); - - // Response - if (!usb_ep_0_in_ibuf_empty_read()) { - printf("USB ibuf still has data\n"); - return; - } - - uint32_t usb_in_pending = usb_ep_0_out_ev_pending_read(); - if (usb_in_pending) { - printf("USB EP0 in pending is: %02x\n", usb_in_pending); - return; - } - - for (i = 0; i < 0x20; i++) { - usb_ep_0_in_ibuf_head_write(i); - } - // Indicate that we respond with an ACK - usb_ep_0_in_respond_write(USB_ACK); - usb_ep_0_in_ev_pending_write(0xff); + usb_isr(); + // for (i = 0; i < 0x20; i++) { + // usb_ep_0_in_ibuf_head_write(i); + // } + // // Indicate that we respond with an ACK + // usb_ep_0_in_respond_write(USB_ACK); + // usb_ep_0_in_ev_pending_write(0xff); } } #else @@ -166,26 +126,14 @@ int main(int argc, char **argv) printf("Status: %d\n", usb_ep_0_out_obuf_empty_read()); get_print_status(); - int i = 0; printf("Hello, world!\n"); int last_event = 0; while (1) { - // unsigned int timer_val_ok = read_timer(); - // int elapsed(int *last_event, int period) elapsed(&last_event, 1000); - unsigned int timer_val_cur; - // timer_val_cur = read_timer(); - // printf("10 PRINT HELLO, WORLD\n"); - // printf("20 GOTO 10\n"); - // printf("i: %d\n", i++); - // timer_val_cur = read_timer(); get_print_status(); - // while (timer0_value_read() != timer_val_ok) { - // ; - // } } return 0; } \ No newline at end of file diff --git a/src/usb.c b/src/usb.c index 6a98164..0099caa 100644 --- a/src/usb.c +++ b/src/usb.c @@ -1,9 +1,329 @@ +#include +#include #include +#include -void usb_init(void) { - // usb_pullup_out_write(1); +#define NUM_BUFFERS 4 +#define BUFFER_SIZE 64 +#define EP_INTERVAL_MS 6 + +static struct GrainuumUSB usb; +static uint8_t usb_buf[67]; + +static uint32_t rx_buffer[NUM_BUFFERS][BUFFER_SIZE / sizeof(uint32_t)]; +static uint8_t rx_buffer_head; +static uint8_t rx_buffer_tail; + +static uint32_t rx_buffer_queries = 0; + + +static void set_usb_config_num(struct GrainuumUSB *usb, int configNum) +{ + (void)usb; + (void)configNum; + ; } -void usb_isr(void) { +static const uint8_t hid_report_descriptor[] = { + 0x06, 0x00, 0xFF, // (GLOBAL) USAGE_PAGE 0xFF00 Vendor-defined + 0x09, 0x00, // (LOCAL) USAGE 0xFF000000 + 0xA1, 0x01, // (MAIN) COLLECTION 0x01 Application (Usage=0xFF000000: Page=Vendor-defined, Usage=, Type=) + 0x26, 0xFF, 0x00, // (GLOBAL) LOGICAL_MAXIMUM 0x00FF (255) + 0x75, 0x08, // (GLOBAL) REPORT_SIZE 0x08 (8) Number of bits per field + 0x95, 0x08, // (GLOBAL) REPORT_COUNT 0x08 (8) Number of fields + 0x06, 0xFF, 0xFF, // (GLOBAL) USAGE_PAGE 0xFFFF Vendor-defined + 0x09, 0x01, // (LOCAL) USAGE 0xFFFF0001 + 0x81, 0x02, // (MAIN) INPUT 0x00000002 (8 fields x 8 bits) 0=Data 1=Variable 0=Absolute 0=NoWrap 0=Linear 0=PrefState 0=NoNull 0=NonVolatile 0=Bitmap + 0x09, 0x01, // (LOCAL) USAGE 0xFFFF0001 + 0x91, 0x02, // (MAIN) OUTPUT 0x00000002 (8 fields x 8 bits) 0=Data 1=Variable 0=Absolute 0=NoWrap 0=Linear 0=PrefState 0=NoNull 0=NonVolatile 0=Bitmap + 0xC0, // (MAIN) END_COLLECTION Application +}; +static const struct usb_device_descriptor device_descriptor = { + .bLength = 18, //sizeof(struct usb_device_descriptor), + .bDescriptorType = DT_DEVICE, /* DEVICE */ + .bcdUSB = 0x0200, /* USB 2.0 */ + .bDeviceClass = 0x00, + .bDeviceSubClass = 0x00, + .bDeviceProtocol = 0x00, + .bMaxPacketSize0 = 0x08, /* 8-byte packets max */ + .idVendor = 0x1209, + .idProduct = 0x9317, + .bcdDevice = 0x0114, /* Device release 1.14 */ + .iManufacturer = 0x02, /* No manufacturer string */ + .iProduct = 0x01, /* Product name in string #2 */ + .iSerialNumber = 0x03, /* No serial number */ + .bNumConfigurations = 0x01, +}; + +static const struct usb_configuration_descriptor configuration_descriptor = { + .bLength = 9, //sizeof(struct usb_configuration_descriptor), + .bDescriptorType = DT_CONFIGURATION, + .wTotalLength = (9 + /*9 + 9 + 7 +*/ 9 + 9 + 7 + 7) /* + (sizeof(struct usb_configuration_descriptor) + + sizeof(struct usb_interface_descriptor) + + sizeof(struct usb_hid_descriptor) + + sizeof(struct usb_endpoint_descriptor)*/, + .bNumInterfaces = 1, + .bConfigurationValue = 1, + .iConfiguration = 5, + .bmAttributes = 0x80, /* Remote wakeup not supported */ + .bMaxPower = 100 / 2, /* 100 mA (in 2-mA units) */ + .data = { + /* struct usb_interface_descriptor { */ + /* uint8_t bLength; */ 9, + /* uint8_t bDescriptorType; */ DT_INTERFACE, + /* uint8_t bInterfaceNumber; */ 0, + /* uint8_t bAlternateSetting; */ 0, + /* uint8_t bNumEndpoints; */ 2, /* Two extra EPs */ + /* uint8_t bInterfaceClass; */ 3, /* HID class */ + /* uint8_t bInterfaceSubclass; */ 0, /* Boot Device subclass */ + /* uint8_t bInterfaceProtocol; */ 0, /* 1 == keyboard, 2 == mouse */ + /* uint8_t iInterface; */ 4, /* String index #4 */ + /* }*/ + + /* struct usb_hid_descriptor { */ + /* uint8_t bLength; */ 9, + /* uint8_t bDescriptorType; */ DT_HID, + /* uint16_t bcdHID; */ 0x11, 0x01, + /* uint8_t bCountryCode; */ 0, + /* uint8_t bNumDescriptors; */ 1, /* We have only one REPORT */ + /* uint8_t bReportDescriptorType; */ DT_HID_REPORT, + /* uint16_t wReportDescriptorLength; */ sizeof(hid_report_descriptor), + sizeof(hid_report_descriptor) >> 8, + /* } */ + + /* struct usb_endpoint_descriptor { */ + /* uint8_t bLength; */ 7, + /* uint8_t bDescriptorType; */ DT_ENDPOINT, + /* uint8_t bEndpointAddress; */ 0x81, /* EP1 (IN) */ + /* uint8_t bmAttributes; */ 3, /* Interrupt */ + /* uint16_t wMaxPacketSize; */ 0x08, 0x00, + /* uint8_t bInterval; */ EP_INTERVAL_MS, /* Every 6 ms */ + /* } */ + + /* struct usb_endpoint_descriptor { */ + /* uint8_t bLength; */ 7, + /* uint8_t bDescriptorType; */ DT_ENDPOINT, + /* uint8_t bEndpointAddress; */ 0x01, /* EP1 (OUT) */ + /* uint8_t bmAttributes; */ 3, /* Interrupt */ + /* uint16_t wMaxPacketSize; */ 0x08, 0x00, + /* uint8_t bInterval; */ EP_INTERVAL_MS, /* Every 6 ms */ + /* } */ + }, +}; + +#define USB_STR_BUF_LEN 64 + +static uint32_t str_buf_storage[USB_STR_BUF_LEN / sizeof(uint32_t)]; +static int send_string_descriptor(const char *str, const void **data) +{ + int len; + int max_len; + uint8_t *str_buf = (uint8_t *)str_buf_storage; + uint8_t *str_offset = str_buf; + + len = strlen(str); + max_len = (USB_STR_BUF_LEN / 2) - 2; + + if (len > max_len) + len = max_len; + + *str_offset++ = (len * 2) + 2; // Two bytes for length count + *str_offset++ = DT_STRING; // Sending a string descriptor + + while (len--) + { + *str_offset++ = *str++; + *str_offset++ = 0; + } + + *data = str_buf; + + // Return the size, which is stored in the first byte of the output data. + return str_buf[0]; +} + +static int get_string_descriptor(struct GrainuumUSB *usb, + uint32_t num, + const void **data) +{ + + static const uint8_t en_us[] = {0x04, DT_STRING, 0x09, 0x04}; + + (void)usb; + + if (num == 0) + { + *data = en_us; + return sizeof(en_us); + } + + // Product + if (num == 1) + return send_string_descriptor("Palawan Bootloader", data); + + if (num == 2) + return send_string_descriptor("21", data); + + if (num == 3) + return send_string_descriptor("1236", data); + + if (num == 4) + return send_string_descriptor("12345", data); + + if (num == 5) + return send_string_descriptor("54", data); + + if (num == 6) + return send_string_descriptor("12345678901234", data); + + return 0; +} + +static int get_device_descriptor(struct GrainuumUSB *usb, + uint32_t num, + const void **data) +{ + + (void)usb; + + if (num == 0) + { + *data = &device_descriptor; + return sizeof(device_descriptor); + } + return 0; +} + +static int get_hid_report_descriptor(struct GrainuumUSB *usb, + uint32_t num, + const void **data) +{ + + (void)usb; + + if (num == 0) + { + *data = &hid_report_descriptor; + return sizeof(hid_report_descriptor); + } + + return 0; +} + +static int get_configuration_descriptor(struct GrainuumUSB *usb, + uint32_t num, + const void **data) +{ + + (void)usb; + + if (num == 0) + { + *data = &configuration_descriptor; + return configuration_descriptor.wTotalLength; + } + return 0; +} + +static int get_descriptor(struct GrainuumUSB *usb, + const void *packet, + const void **response) +{ + + const struct usb_setup_packet *setup = packet; + + switch (setup->wValueH) + { + case DT_DEVICE: + return get_device_descriptor(usb, setup->wValueL, response); + + case DT_STRING: + return get_string_descriptor(usb, setup->wValueL, response); + + case DT_CONFIGURATION: + return get_configuration_descriptor(usb, setup->wValueL, response); + + case DT_HID_REPORT: + return get_hid_report_descriptor(usb, setup->wValueL, response); + } + + return 0; +} + +static void *get_usb_rx_buffer(struct GrainuumUSB *usb, + uint8_t epNum, + int32_t *size) +{ + (void)usb; + (void)epNum; + + if (size) + *size = sizeof(rx_buffer[0]); + rx_buffer_queries++; + return rx_buffer[rx_buffer_head]; +} + +static int received_data(struct GrainuumUSB *usb, + uint8_t epNum, + uint32_t bytes, + const void *data) +{ + (void)usb; + (void)epNum; + (void)bytes; + (void)data; + + if (epNum == 1) + { + rx_buffer_head = (rx_buffer_head + 1) & (NUM_BUFFERS - 1); + } + + /* Return 0, indicating this packet is complete. */ + return 0; +} + +static int send_data_finished(struct GrainuumUSB *usb, int result) +{ + (void)usb; + (void)result; + + return 0; +} + +static struct GrainuumConfig cfg = { + .getDescriptor = get_descriptor, + .getReceiveBuffer = get_usb_rx_buffer, + .receiveData = received_data, + .sendDataFinished = send_data_finished, + .setConfigNum = set_usb_config_num, +}; + +void usb_isr(void) { + grainuumCaptureI(&usb, usb_buf); + return; +} + +void usb_init(void) { + grainuumInit(&usb, &cfg); + grainuumConnect(&usb); + return; +} + +void usbPhyWriteI(const struct GrainuumUSB *usb, const void *buffer, uint32_t size) { + (void)usb; + const uint8_t *ubuffer = (const uint8_t *)buffer; + uint32_t i = 0; + while (i < size) + usb_obuf_head_write(ubuffer[i]); +} + +int usbPhyReadI(const struct GrainuumUSB *usb, uint8_t *samples) { + (void)usb; + int count = 0; + while (!usb_ibuf_empty_read()) { + samples[count++] = usb_ibuf_head_read(); + } + return count; } \ No newline at end of file diff --git a/third_party/libbase/libc.c b/third_party/libbase/libc.c index a02f4ce..4bb3ee0 100644 --- a/third_party/libbase/libc.c +++ b/third_party/libbase/libc.c @@ -39,4 +39,79 @@ strlen(const char *str) for (s = str; *s; ++s) ; return (s - str); +} + +/** + * memcpy - Copies one area of memory to another + * @dest: Destination + * @src: Source + * @n: The size to copy. + */ +void *memcpy(void *to, const void *from, size_t n) +{ + void *xto = to; + size_t temp; + + if(!n) + return xto; + if((long)to & 1) { + char *cto = to; + const char *cfrom = from; + *cto++ = *cfrom++; + to = cto; + from = cfrom; + n--; + } + if((long)from & 1) { + char *cto = to; + const char *cfrom = from; + for (; n; n--) + *cto++ = *cfrom++; + return xto; + } + if(n > 2 && (long)to & 2) { + short *sto = to; + const short *sfrom = from; + *sto++ = *sfrom++; + to = sto; + from = sfrom; + n -= 2; + } + if((long)from & 2) { + short *sto = to; + const short *sfrom = from; + temp = n >> 1; + for (; temp; temp--) + *sto++ = *sfrom++; + to = sto; + from = sfrom; + if(n & 1) { + char *cto = to; + const char *cfrom = from; + *cto = *cfrom; + } + return xto; + } + temp = n >> 2; + if(temp) { + long *lto = to; + const long *lfrom = from; + for(; temp; temp--) + *lto++ = *lfrom++; + to = lto; + from = lfrom; + } + if(n & 2) { + short *sto = to; + const short *sfrom = from; + *sto++ = *sfrom++; + to = sto; + from = sfrom; + } + if(n & 1) { + char *cto = to; + const char *cfrom = from; + *cto = *cfrom; + } + return xto; } \ No newline at end of file diff --git a/third_party/printf.c b/third_party/printf.c index 4db1289..c9d62eb 100644 --- a/third_party/printf.c +++ b/third_party/printf.c @@ -31,215 +31,232 @@ #include "printf.h" -typedef void (*putcf) (void*,char); +typedef void (*putcf)(void *, char); static putcf stdout_putf; -static void* stdout_putp; - +static void *stdout_putp; #ifdef PRINTF_LONG_SUPPORT -static void uli2a(unsigned long int num, unsigned int base, int uc,char * bf) +static void uli2a(unsigned long int num, unsigned int base, int uc, char *bf) +{ + int n = 0; + unsigned int d = 1; + while (num / d >= base) + d *= base; + while (d != 0) { - int n=0; - unsigned int d=1; - while (num/d >= base) - d*=base; - while (d!=0) { int dgt = num / d; - num%=d; - d/=base; - if (n || dgt>0|| d==0) { - *bf++ = dgt+(dgt<10 ? '0' : (uc ? 'A' : 'a')-10); + num %= d; + d /= base; + if (n || dgt > 0 || d == 0) + { + *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10); ++n; - } } - *bf=0; } + *bf = 0; +} -static void li2a (long num, char * bf) +static void li2a(long num, char *bf) +{ + if (num < 0) { - if (num<0) { - num=-num; + num = -num; *bf++ = '-'; - } - uli2a(num,10,0,bf); } + uli2a(num, 10, 0, bf); +} #endif -static void ui2a(unsigned int num, unsigned int base, int uc,char * bf) +static void ui2a(unsigned int num, unsigned int base, int uc, char *bf) +{ + int n = 0; + unsigned int d = 1; + while (num / d >= base) + d *= base; + while (d != 0) { - int n=0; - unsigned int d=1; - while (num/d >= base) - d*=base; - while (d!=0) { int dgt = num / d; - num%= d; - d/=base; - if (n || dgt>0 || d==0) { - *bf++ = dgt+(dgt<10 ? '0' : (uc ? 'A' : 'a')-10); + num %= d; + d /= base; + if (n || dgt > 0 || d == 0) + { + *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10); ++n; - } } - *bf=0; } + *bf = 0; +} -static void i2a (int num, char * bf) +static void i2a(int num, char *bf) +{ + if (num < 0) { - if (num<0) { - num=-num; + num = -num; *bf++ = '-'; - } - ui2a(num,10,0,bf); } + ui2a(num, 10, 0, bf); +} static int a2d(char ch) - { - if (ch>='0' && ch<='9') - return ch-'0'; - else if (ch>='a' && ch<='f') - return ch-'a'+10; - else if (ch>='A' && ch<='F') - return ch-'A'+10; - else return -1; - } +{ + if (ch >= '0' && ch <= '9') + return ch - '0'; + else if (ch >= 'a' && ch <= 'f') + return ch - 'a' + 10; + else if (ch >= 'A' && ch <= 'F') + return ch - 'A' + 10; + else + return -1; +} -static char a2i(char ch, char** src,int base,int* nump) - { - char* p= *src; - int num=0; +static char a2i(char ch, char **src, int base, int *nump) +{ + char *p = *src; + int num = 0; int digit; - while ((digit=a2d(ch))>=0) { - if (digit>base) break; - num=num*base+digit; - ch=*p++; - } - *src=p; - *nump=num; - return ch; - } - -static void putchw(void* putp,putcf putf,int n, char z, char* bf) + while ((digit = a2d(ch)) >= 0) { - char fc=z? '0' : ' '; + if (digit > base) + break; + num = num * base + digit; + ch = *p++; + } + *src = p; + *nump = num; + return ch; +} + +static void putchw(void *putp, putcf putf, int n, char z, char *bf) +{ + char fc = z ? '0' : ' '; char ch; - char* p=bf; + char *p = bf; while (*p++ && n > 0) n--; - while (n-- > 0) - putf(putp,fc); - while ((ch= *bf++)) - putf(putp,ch); - } + while (n-- > 0) + putf(putp, fc); + while ((ch = *bf++)) + putf(putp, ch); +} -void tfp_format(void* putp,putcf putf,char *fmt, va_list va) - { +void tfp_format(void *putp, putcf putf, char *fmt, va_list va) +{ char bf[12]; - + char ch; - - while ((ch=*(fmt++))) { - if (ch!='%') - putf(putp,ch); - else { - char lz=0; -#ifdef PRINTF_LONG_SUPPORT - char lng=0; + while ((ch = *(fmt++))) + { + if (ch != '%') + putf(putp, ch); + else + { + char lz = 0; +#ifdef PRINTF_LONG_SUPPORT + char lng = 0; #endif - int w=0; - ch=*(fmt++); - if (ch=='0') { - ch=*(fmt++); - lz=1; - } - if (ch>='0' && ch<='9') { - ch=a2i(ch,&fmt,10,&w); - } -#ifdef PRINTF_LONG_SUPPORT - if (ch=='l') { - ch=*(fmt++); - lng=1; + int w = 0; + ch = *(fmt++); + if (ch == '0') + { + ch = *(fmt++); + lz = 1; + } + if (ch >= '0' && ch <= '9') + { + ch = a2i(ch, &fmt, 10, &w); + } +#ifdef PRINTF_LONG_SUPPORT + if (ch == 'l') + { + ch = *(fmt++); + lng = 1; } #endif - switch (ch) { - case 0: - goto abort; - case 'u' : { -#ifdef PRINTF_LONG_SUPPORT - if (lng) - uli2a(va_arg(va, unsigned long int),10,0,bf); - else + switch (ch) + { + case 0: + goto abort; + case 'u': + { +#ifdef PRINTF_LONG_SUPPORT + if (lng) + uli2a(va_arg(va, unsigned long int), 10, 0, bf); + else #endif - ui2a(va_arg(va, unsigned int),10,0,bf); - putchw(putp,putf,w,lz,bf); - break; - } - case 'd' : { -#ifdef PRINTF_LONG_SUPPORT - if (lng) - li2a(va_arg(va, unsigned long int),bf); - else + ui2a(va_arg(va, unsigned int), 10, 0, bf); + putchw(putp, putf, w, lz, bf); + break; + } + case 'd': + { +#ifdef PRINTF_LONG_SUPPORT + if (lng) + li2a(va_arg(va, unsigned long int), bf); + else #endif - i2a(va_arg(va, int),bf); - putchw(putp,putf,w,lz,bf); - break; - } - case 'x': case 'X' : -#ifdef PRINTF_LONG_SUPPORT - if (lng) - uli2a(va_arg(va, unsigned long int),16,(ch=='X'),bf); - else + i2a(va_arg(va, int), bf); + putchw(putp, putf, w, lz, bf); + break; + } + case 'x': + case 'X': +#ifdef PRINTF_LONG_SUPPORT + if (lng) + uli2a(va_arg(va, unsigned long int), 16, (ch == 'X'), bf); + else #endif - ui2a(va_arg(va, unsigned int),16,(ch=='X'),bf); - putchw(putp,putf,w,lz,bf); - break; - case 'c' : - putf(putp,(char)(va_arg(va, int))); - break; - case 's' : - putchw(putp,putf,w,0,va_arg(va, char*)); - break; - case '%' : - putf(putp,ch); - default: - break; - } + ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf); + putchw(putp, putf, w, lz, bf); + break; + case 'c': + putf(putp, (char)(va_arg(va, int))); + break; + case 's': + putchw(putp, putf, w, 0, va_arg(va, char *)); + break; + case '%': + putf(putp, ch); + default: + break; } } - abort:; } +abort:; +} - -void init_printf(void* putp,void (*putf) (void*,char)) - { - stdout_putf=putf; - stdout_putp=putp; - } +void init_printf(void *putp, void (*putf)(void *, char)) +{ + stdout_putf = putf; + stdout_putp = putp; +} void tfp_printf(char *fmt, ...) - { +{ va_list va; - va_start(va,fmt); - tfp_format(stdout_putp,stdout_putf,fmt,va); + va_start(va, fmt); + tfp_format(stdout_putp, stdout_putf, fmt, va); va_end(va); - } +} -static void putcp(void* p,char c) - { - *(*((char**)p))++ = c; - } +static void putcp(void *p, char c) +{ + *(*((char **)p))++ = c; +} - - -void tfp_sprintf(char* s,char *fmt, ...) - { +void tfp_sprintf(char *s, char *fmt, ...) +{ va_list va; - va_start(va,fmt); - tfp_format(&s,putcp,fmt,va); - putcp(&s,0); + va_start(va, fmt); + tfp_format(&s, putcp, fmt, va); + putcp(&s, 0); va_end(va); - } - +} + int puts(const char *s) { + while (*s++) + stdout_putf(stdout_putp, *s); + stdout_putf(stdout_putp, '\n'); + } \ No newline at end of file