summaryrefslogtreecommitdiff
path: root/tmk_core/common/avr/xprintf.S
blob: c5a414c35cba9b0e4f31d91536224dec642391e4 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
;---------------------------------------------------------------------------;
; Extended itoa, puts, printf and atoi                     (C)ChaN, 2011
;---------------------------------------------------------------------------;

				// Base size is 152 bytes
#define	CR_CRLF		0	// Convert \n to \r\n (+10 bytes)
#define USE_XPRINTF	1	// Enable xprintf function (+194 bytes)
#define USE_XSPRINTF	0	// Add xsprintf function (+78 bytes)
#define USE_XFPRINTF	0	// Add xfprintf function (+54 bytes)
#define USE_XATOI	0	// Enable xatoi function (+182 bytes)


#if FLASHEND > 0x1FFFF
#error xitoa module does not support 256K devices
#endif

.nolist
#include <avr/io.h>	// Include device specific definitions.
.list

#ifdef SPM_PAGESIZE	// Recent devices have "lpm Rd,Z+" and "movw".
.macro	_LPMI	reg
	lpm	\reg, Z+
.endm
.macro	_MOVW	dh,dl, sh,sl
	movw	\dl, \sl
.endm
#else			// Earlier devices do not have "lpm Rd,Z+" nor "movw".
.macro	_LPMI	reg
	lpm
	mov	\reg, r0
	adiw	ZL, 1
.endm
.macro	_MOVW	dh,dl, sh,sl
	mov	\dl, \sl
	mov	\dh, \sh
.endm
#endif



;---------------------------------------------------------------------------
; Stub function to forward to user output function
;
;Prototype: void xputc (char chr	// a character to be output
;			);
;Size: 12/12 words

.section .bss
.global xfunc_out	; xfunc_out must be initialized before using this module.
xfunc_out:	.ds.w	1
.section .text


.func xputc
.global xputc
xputc:
#if CR_CRLF
	cpi	r24, 10		;LF --> CRLF
	brne	1f		;
	ldi	r24, 13		;
	rcall	1f		;
	ldi	r24, 10		;/
1:
#endif
	push	ZH
	push	ZL
	lds	ZL, xfunc_out+0	;Pointer to the registered output function.
	lds	ZH, xfunc_out+1	;/
	sbiw	ZL, 0		;Skip if null
	breq	2f		;/
	icall
2:	pop	ZL
	pop	ZH
	ret
.endfunc



;---------------------------------------------------------------------------
; Direct ROM string output
;
;Prototype: void xputs (const char *str_p // rom string to be output
;			);

.func xputs
.global xputs
xputs:
	_MOVW	ZH,ZL, r25,r24	; Z = pointer to rom string
1:	_LPMI	r24
	cpi	r24, 0
	breq	2f
	rcall	xputc
	rjmp	1b
2:	ret
.endfunc


;---------------------------------------------------------------------------
; Extended direct numeral string output (32bit version)
;
;Prototype: void xitoa (long value,	// value to be output
;                       char radix,	// radix
;                       char width);	// minimum width
;

.func xitoa
.global xitoa
xitoa:
				;r25:r22 = value, r20 = base, r18 = digits
	clr	r31		;r31 = stack level
	ldi	r30, ' '	;r30 = sign
	ldi	r19, ' '	;r19 = filler
	sbrs	r20, 7		;When base indicates signd format and the value
	rjmp	0f		;is minus, add a '-'.
	neg	r20		;
	sbrs	r25, 7		;
	rjmp	0f		;
	ldi	r30, '-'	;
	com	r22		;
	com	r23		;
	com	r24		;
	com	r25		;
	adc	r22, r1		;
	adc	r23, r1		;
	adc	r24, r1		;
	adc	r25, r1		;/
0:	sbrs	r18, 7		;When digits indicates zero filled,
	rjmp	1f		;filler is '0'.
	neg	r18		;
	ldi	r19, '0'	;/
				;----- string conversion loop
1:	ldi	r21, 32		;r26 = r25:r22 % r20
	clr	r26		;r25:r22 /= r20
2:	lsl	r22		;
	rol	r23		;
	rol	r24		;
	rol	r25		;
	rol	r26		;
	cp	r26, r20	;
	brcs	3f		;
	sub	r26, r20	;
	inc	r22		;
3:	dec	r21		;
	brne	2b		;/
	cpi	r26, 10		;r26 is a numeral digit '0'-'F'
	brcs	4f		;
	subi	r26, -7		;
4:	subi	r26, -'0'	;/
	push	r26		;Stack it
	inc	r31		;/
	cp	r22, r1		;Repeat until r25:r22 gets zero
	cpc	r23, r1		;
	cpc	r24, r1		;
	cpc	r25, r1		;
	brne	1b		;/

	cpi	r30, '-'	;Minus sign if needed
	brne	5f		;
	push	r30		;
	inc	r31		;/
5:	cp	r31, r18	;Filler
	brcc	6f		;
	push	r19		;
	inc	r31		;
	rjmp	5b		;/

6:	pop	r24		;Flush stacked digits and exit
	rcall	xputc		;
	dec	r31		;
	brne	6b		;/

	ret
.endfunc



;---------------------------------------------------------------------------;
; Formatted string output (16/32bit version)
;
;Prototype:
; void __xprintf (const char *format_p, ...);
; void __xsprintf(char*, const char *format_p, ...);
; void __xfprintf(void(*func)(char), const char *format_p, ...);
;

#if USE_XPRINTF

.func xvprintf
xvprintf:
	ld	ZL, Y+		;Z = pointer to format string
	ld	ZH, Y+		;/

0:	_LPMI	r24		;Get a format char
	cpi	r24, 0		;End of format string?
	breq	90f		;/
	cpi	r24, '%'	;Is format?
	breq	20f		;/
1:	rcall	xputc		;Put a normal character
	rjmp	0b		;/
90:	ret

20:	ldi	r18, 0		;r18: digits
	clt			;T: filler
	_LPMI	r21		;Get flags
	cpi	r21, '%'	;Is a %?
	breq	1b		;/
	cpi	r21, '0'	;Zero filled?
	brne	23f		;
	set			;/
22:	_LPMI	r21		;Get width
23:	cpi	r21, '9'+1	;
	brcc	24f		;
	subi	r21, '0'	;
	brcs	90b		;
	lsl	r18		;
	mov	r0, r18		;
	lsl	r18		;
	lsl	r18		;
	add	r18, r0		;
	add	r18, r21	;
	rjmp	22b		;/

24:	brtc	25f		;get value (low word)
	neg	r18		;
25:	ld	r24, Y+		;
	ld	r25, Y+		;/
	cpi	r21, 'c'	;Is type character?
	breq	1b		;/
	cpi	r21, 's'	;Is type RAM string?
	breq	50f		;/
	cpi	r21, 'S'	;Is type ROM string?
	breq	60f		;/
	_MOVW	r23,r22,r25,r24	;r25:r22 = value
	clr	r24		;
	clr	r25		;
	clt			;/
	cpi	r21, 'l'	;Is long int?
	brne	26f		;
	ld	r24, Y+		;get value (high word)
	ld	r25, Y+		;
	set			;
	_LPMI	r21		;/
26:	cpi	r21, 'd'	;Is type signed decimal?
	brne	27f		;/
	ldi	r20, -10	;
	brts	40f		;
	sbrs	r23, 7		;
	rjmp	40f		;
	ldi	r24, -1		;
	ldi	r25, -1		;
	rjmp	40f		;/
27:	cpi	r21, 'u'	;Is type unsigned decimal?
	ldi	r20, 10		;
	breq	40f		;/
	cpi	r21, 'X'	;Is type hexdecimal?
	ldi	r20, 16		;
	breq	40f		;/
	cpi	r21, 'b'	;Is type binary?
	ldi	r20, 2		;
	breq	40f		;/
	ret			;abort
40:	push	ZH		;Output the value
	push	ZL		;
	rcall	xitoa		;
42:	pop	ZL		;
	pop	ZH		;
	rjmp	0b		;/

50:	push	ZH		;Put a string on the RAM
	push	ZL
	_MOVW	ZH,ZL, r25,r24
51:	ld	r24, Z+
	cpi	r24, 0
	breq	42b
	rcall	xputc
	rjmp	51b

60:	push	ZH		;Put a string on the ROM
	push	ZL
	rcall	xputs
	rjmp	42b
.endfunc


.func __xprintf
.global __xprintf
__xprintf:
	push	YH
	push	YL
	in	YL, _SFR_IO_ADDR(SPL)
#ifdef SPH
	in	YH, _SFR_IO_ADDR(SPH)
#else
	clr	YH
#endif
	adiw	YL, 5		;Y = pointer to arguments
	rcall	xvprintf
	pop	YL
	pop	YH
	ret
.endfunc


#if USE_XSPRINTF

.func __xsprintf
putram:
	_MOVW	ZH,ZL, r15,r14
	st	Z+, r24
	_MOVW	r15,r14, ZH,ZL
	ret
.global __xsprintf
__xsprintf:
	push	YH
	push	YL
	in	YL, _SFR_IO_ADDR(SPL)
#ifdef SPH
	in	YH, _SFR_IO_ADDR(SPH)
#else
	clr	YH
#endif
	adiw	YL, 5		;Y = pointer to arguments
	lds	ZL, xfunc_out+0	;Save registered output function
	lds	ZH, xfunc_out+1	;
	push	ZL		;
	push	ZH		;/
	ldi	ZL, lo8(pm(putram));Set local output function
	ldi	ZH, hi8(pm(putram));
	sts	xfunc_out+0, ZL	;
	sts	xfunc_out+1, ZH	;/
	push	r15		;Initialize pointer to string buffer
	push	r14		;
	ld	r14, Y+		;
	ld	r15, Y+		;/
	rcall	xvprintf
	_MOVW	ZH,ZL, r15,r14	;Terminate string
	st	Z, r1		;
	pop	r14		;
	pop	r15		;/
	pop	ZH		;Restore registered output function
	pop	ZL		;
	sts	xfunc_out+0, ZL	;
	sts	xfunc_out+1, ZH	;/
	pop	YL
	pop	YH
	ret
.endfunc
#endif


#if USE_XFPRINTF
.func __xfprintf
.global __xfprintf
__xfprintf:
	push	YH
	push	YL
	in	YL, _SFR_IO_ADDR(SPL)
#ifdef SPH
	in	YH, _SFR_IO_ADDR(SPH)
#else
	clr	YH
#endif
	adiw	YL, 5		;Y = pointer to arguments
	lds	ZL, xfunc_out+0	;Save registered output function
	lds	ZH, xfunc_out+1	;
	push	ZL		;
	push	ZH		;/
	ld	ZL, Y+		;Set output function
	ld	ZH, Y+		;
	sts	xfunc_out+0, ZL	;
	sts	xfunc_out+1, ZH	;/
	rcall	xvprintf
	pop	ZH		;Restore registered output function
	pop	ZL		;
	sts	xfunc_out+0, ZL	;
	sts	xfunc_out+1, ZH	;/
	pop	YL
	pop	YH
	ret
.endfunc
#endif

#endif



;---------------------------------------------------------------------------
; Extended numeral string input
;
;Prototype:
; char xatoi (           /* 1: Successful, 0: Failed */
;      const char **str, /* pointer to pointer to source string */
;      long *res         /* result */
; );
;


#if USE_XATOI
.func xatoi
.global xatoi
xatoi:
	_MOVW	r1, r0, r23, r22
	_MOVW	XH, XL, r25, r24
	ld	ZL, X+
	ld	ZH, X+
	clr	r18		;r21:r18 = 0;
	clr	r19		;
	clr	r20		;
	clr	r21		;/
	clt			;T = 0;

	ldi	r25, 10		;r25 = 10;
	rjmp	41f		;/
40:	adiw	ZL, 1		;Z++;
41:	ld	r22, Z		;r22 = *Z;
	cpi	r22, ' '	;if(r22 == ' ') continue
	breq	40b		;/
	brcs	70f		;if(r22 < ' ') error;
	cpi	r22, '-'	;if(r22 == '-') {
	brne	42f		; T = 1;
	set			; continue;
	rjmp	40b		;}
42:	cpi	r22, '9'+1	;if(r22 > '9') error;
	brcc	70f		;/
	cpi	r22, '0'	;if(r22 < '0') error;
	brcs	70f		;/
	brne	51f		;if(r22 > '0') cv_start;
	ldi	r25, 8		;r25 = 8;
	adiw	ZL, 1		;r22 = *(++Z);
	ld	r22, Z		;/
	cpi	r22, ' '+1	;if(r22 <= ' ') exit;
	brcs	80f		;/
	cpi	r22, 'b'	;if(r22 == 'b') {
	brne	43f		; r25 = 2;
	ldi	r25, 2		; cv_start;
	rjmp	50f		;}
43:	cpi	r22, 'x'	;if(r22 != 'x') error;
	brne	51f		;/
	ldi	r25, 16		;r25 = 16;

50:	adiw	ZL, 1		;Z++;
	ld	r22, Z		;r22 = *Z;
51:	cpi	r22, ' '+1	;if(r22 <= ' ') break;
	brcs	80f		;/
	cpi	r22, 'a'	;if(r22 >= 'a') r22 =- 0x20;
	brcs	52f		;
	subi	r22, 0x20	;/
52:	subi	r22, '0'	;if((r22 -= '0') < 0) error;
	brcs	70f		;/
	cpi	r22, 10		;if(r22 >= 10) {
	brcs	53f		; r22 -= 7;
	subi	r22, 7		; if(r22 < 10)
	cpi	r22, 10		;
	brcs	70f		;}
53:	cp	r22, r25	;if(r22 >= r25) error;
	brcc	70f		;/
60:	ldi	r24, 33		;r21:r18 *= r25;
	sub	r23, r23	;
61:	brcc	62f		;
	add	r23, r25	;
62:	lsr	r23		;
	ror	r21		;
	ror	r20		;
	ror	r19		;
	ror	r18		;
	dec	r24		;
	brne	61b		;/
	add	r18, r22	;r21:r18 += r22;
	adc	r19, r24	;
	adc	r20, r24	;
	adc	r21, r24	;/
	rjmp	50b		;repeat

70:	ldi	r24, 0
	rjmp	81f
80:	ldi	r24, 1
81:	brtc	82f
	clr	r22
	com	r18
	com	r19
	com	r20
	com	r21
	adc	r18, r22
	adc	r19, r22
	adc	r20, r22
	adc	r21, r22
82:	st	-X, ZH
	st	-X, ZL
	_MOVW	XH, XL, r1, r0
	st	X+, r18
	st	X+, r19
	st	X+, r20
	st	X+, r21
	clr	r1
	ret
.endfunc
#endif