summaryrefslogtreecommitdiff
path: root/firmware/asm/m68k
diff options
context:
space:
mode:
Diffstat (limited to 'firmware/asm/m68k')
-rw-r--r--firmware/asm/m68k/memcpy.S682
-rw-r--r--firmware/asm/m68k/memmove.S670
-rw-r--r--firmware/asm/m68k/memset.S152
-rw-r--r--firmware/asm/m68k/memset16.S146
-rw-r--r--firmware/asm/m68k/strlen.S71
5 files changed, 1721 insertions, 0 deletions
diff --git a/firmware/asm/m68k/memcpy.S b/firmware/asm/m68k/memcpy.S
new file mode 100644
index 0000000000..9762e31e02
--- /dev/null
+++ b/firmware/asm/m68k/memcpy.S
@@ -0,0 +1,682 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2004-2005 by Jens Arnold
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#include "config.h"
22
23 .section .icode,"ax",@progbits
24
25#define FULLSPEED /* use burst writing for word aligned destinations */
26 .align 2
27 .global memcpy
28 .global __memcpy_fwd_entry
29 .type memcpy,@function
30
31/* Copies <length> bytes of data in memory from <source> to <dest>
32 * This version is optimized for speed
33 *
34 * arguments:
35 * (4,%sp) - destination address
36 * (8,%sp) - source address
37 * (12,%sp) - length
38 *
39 * return value:
40 * %d0 - destination address (like ANSI version)
41 *
42 * register usage:
43 * %a0 - current source address
44 * %a1 - current dest address
45 * %a2 - source end address (in line-copy loops)
46 * %d0 - data / scratch
47 * %d1 - source end address (byte and longword copy) / data / scratch
48 * %d2 - data / scratch
49 * %d3..%d7 - data
50 *
51 * For maximum speed this routine reads and writes whole lines using burst
52 * move (movem.l) where possible. For byte aligned destinations (long+1 and
53 * long+3) it writes longwords only. Same goes for word aligned destinations
54 * if FULLSPEED is undefined.
55 */
56memcpy:
57 move.l (4,%sp),%a1 /* Destination */
58 move.l (8,%sp),%a0 /* Source */
59 move.l (12,%sp),%d1 /* Length */
60
61__memcpy_fwd_entry:
62 add.l %a0,%d1 /* %d1 = source end */
63
64 move.l %a0,%d0
65 addq.l #7,%d0
66 and.l #0xFFFFFFFC,%d0 /* %d0 = first source long bound + 4 */
67 cmp.l %d0,%d1 /* at least one aligned longword to copy? */
68 blo.w .bytes2_start /* no, jump directly to trailing byte loop */
69
70 subq.l #4,%d0 /* %d0 = first source long bound */
71 cmp.l %a0,%d0 /* any bytes to copy? */
72 jls .bytes1_end /* no: skip byte loop */
73
74 /* leading byte loop: copies 0..3 bytes */
75.bytes1_loop:
76 move.b (%a0)+,(%a1)+ /* copy byte */
77 cmp.l %a0,%d0 /* runs %a0 up to first long bound */
78 jhi .bytes1_loop
79
80.bytes1_end:
81 moveq.l #31,%d0
82 add.l %a0,%d0
83 and.l #0xFFFFFFF0,%d0 /* %d0 = first source line bound + 16 */
84 cmp.l %d0,%d1 /* at least one aligned line to copy? */
85 blo.w .long_start /* no: jump to longword copy loop */
86
87 lea.l (-28,%sp),%sp /* free up some registers */
88 movem.l %d2-%d7/%a2,(%sp)
89
90 moveq.l #16,%d2
91 sub.l %d2,%d0 /* %d0 = first source line bound */
92 move.l %d1,%a2 /* %a2 = end address */
93 lea.l (-15,%a2),%a2 /* adjust end address for loops doing 16 bytes/ pass */
94 move.l %a1,%d1
95 moveq.l #3,%d2 /* mask */
96 and.l %d2,%d1
97 jmp.l (2,%pc,%d1.l*4) /* switch (dest_addr & 3) */
98 bra.w .lines_do0_start
99 bra.w .lines_do1_start
100 bra.w .lines_do2_start
101 /* bra.w .lines_do3_start implicit */
102
103 /* byte aligned destination (long + 3): use line burst reads in main loop */
104.lines_do3_start:
105 moveq.l #24,%d1 /* shift count for shifting by 3 bytes */
106 cmp.l %a0,%d0 /* any leading longwords? */
107 jhi .lines_do3_head_start /* yes: leading longword copy */
108
109 movem.l (%a0),%d4-%d7 /* load first line */
110 lea.l (16,%a0),%a0
111 move.l %d4,%d2
112 lsr.l %d1,%d2 /* get high byte of first longword */
113 move.b %d2,(%a1)+ /* store byte */
114 jra .lines_do3_entry /* jump into main loop */
115
116.lines_do3_head_start:
117 move.l (%a0)+,%d7 /* load first longword */
118 move.l %d7,%d2
119 lsr.l %d1,%d2 /* get high byte */
120 move.b %d2,(%a1)+ /* store byte */
121 jra .lines_do3_head_entry /* jump into leading longword loop */
122
123.lines_do3_head_loop:
124 move.l %d7,%d6 /* move old longword away */
125 move.l (%a0)+,%d7 /* load new longword */
126 move.l %d7,%d2
127 lsr.l %d1,%d2 /* get high byte */
128 or.l %d2,%d6 /* combine with old lower 3 bytes */
129 move.l %d6,(%a1)+ /* store longword */
130.lines_do3_head_entry:
131 lsl.l #8,%d7 /* shift up lower 3 bytes */
132 cmp.l %a0,%d0 /* runs %a0 up to first line bound */
133 jhi .lines_do3_head_loop
134
135.lines_do3_loop:
136 move.l %d7,%d3 /* move last longword of old line away */
137 movem.l (%a0),%d4-%d7 /* load new line */
138 lea.l (16,%a0),%a0
139 move.l %d4,%d2
140 lsr.l %d1,%d2 /* get high byte of 1st longword */
141 or.l %d2,%d3 /* combine with old lower 3 bytes */
142 move.l %d3,(%a1)+ /* store longword */
143.lines_do3_entry:
144 lsl.l #8,%d4 /* shift up lower 3 bytes */
145 move.l %d5,%d2
146 lsr.l %d1,%d2 /* get high byte of 2nd longword */
147 or.l %d2,%d4 /* combine with 1st lower 3 bytes */
148 move.l %d4,(%a1)+ /* store longword */
149 lsl.l #8,%d5 /* shift up lower 3 bytes */
150 move.l %d6,%d2
151 lsr.l %d1,%d2 /* get high byte of 3rd longword */
152 or.l %d2,%d5 /* combine with 2nd lower 3 bytes */
153 move.l %d5,(%a1)+ /* store longword */
154 lsl.l #8,%d6 /* shift up lower 3 bytes */
155 move.l %d7,%d2
156 lsr.l %d1,%d2 /* get high byte of 4th longword */
157 or.l %d2,%d6 /* combine with 3rd lower 3 bytes */
158 move.l %d6,(%a1)+ /* store longword */
159 lsl.l #8,%d7 /* shift up lower 3 bytes */
160 cmp.l %a0,%a2 /* runs %a0 up to last line bound */
161 jhi .lines_do3_loop
162
163 lea.l (12,%a2),%a2 /* readjust end address for doing longwords */
164 cmp.l %a0,%a2 /* any trailing longwords? */
165 jls .lines_do3_tail_end /* no: just store last lower 3 bytes */
166
167.lines_do3_tail_loop:
168 move.l %d7,%d6 /* move old longword away */
169 move.l (%a0)+,%d7 /* load new longword */
170 move.l %d7,%d2
171 lsr.l %d1,%d2 /* get high byte */
172 or.l %d2,%d6 /* combine with old lower 3 bytes */
173 move.l %d6,(%a1)+ /* store longword */
174 lsl.l #8,%d7 /* shift up lower 3 bytes */
175 cmp.l %a0,%a2 /* runs %a0 up to last long bound */
176 jhi .lines_do3_tail_loop
177
178.lines_do3_tail_end:
179 swap %d7 /* get high word */
180 move.w %d7,(%a1)+ /* store word */
181 lsr.l %d1,%d7 /* get moved-up low byte */
182 move.b %d7,(%a1)+ /* store byte */
183 jra .lines_end
184
185 /* byte aligned destination (long + 1): use line burst reads in main loop */
186.lines_do1_start:
187 moveq.l #24,%d1 /* shift count for shifting by 3 bytes */
188 cmp.l %a0,%d0 /* any leading longwords? */
189 jhi .lines_do1_head_start /* yes: leading longword copy */
190
191 movem.l (%a0),%d4-%d7 /* load first line */
192 lea.l (16,%a0),%a0
193 move.l %d4,%d2 /* first longword, bytes 3210 */
194 lsr.l #8,%d2 /* first longword, bytes .321 */
195 swap %d2 /* first longword, bytes 21.3 */
196 move.b %d2,(%a1)+ /* store byte */
197 swap %d2 /* first longword, bytes .321 */
198 move.w %d2,(%a1)+ /* store word */
199 jra .lines_do1_entry
200
201.lines_do1_head_start:
202 move.l (%a0)+,%d7 /* load first longword */
203 move.l %d7,%d2 /* first longword, bytes 3210 */
204 lsr.l #8,%d2 /* first longword, bytes .321 */
205 swap %d2 /* first longword, bytes 21.3 */
206 move.b %d2,(%a1)+ /* store byte */
207 swap %d2 /* first longword, bytes .321 */
208 move.w %d2,(%a1)+ /* store word */
209 jra .lines_do1_head_entry
210
211.lines_do1_head_loop:
212 move.l %d7,%d6 /* move old longword away */
213 move.l (%a0)+,%d7 /* load new longword */
214 move.l %d7,%d2
215 lsr.l #8,%d2 /* get upper 3 bytes */
216 or.l %d2,%d6 /* combine with old low byte */
217 move.l %d6,(%a1)+ /* store longword */
218.lines_do1_head_entry:
219 lsl.l %d1,%d7 /* shift up low byte */
220 cmp.l %a0,%d0 /* runs %a0 up to first line bound */
221 jhi .lines_do1_head_loop
222
223.lines_do1_loop:
224 move.l %d7,%d3 /* move last longword of old line away */
225 movem.l (%a0),%d4-%d7 /* load new line */
226 lea.l (16,%a0),%a0
227 move.l %d4,%d2
228 lsr.l #8,%d2 /* get upper 3 bytes of 1st longword */
229 or.l %d2,%d3 /* combine with low byte of old longword */
230 move.l %d3,(%a1)+ /* store longword */
231.lines_do1_entry:
232 lsl.l %d1,%d4 /* shift up low byte */
233 move.l %d5,%d2
234 lsr.l #8,%d2 /* get upper 3 bytes of 2nd longword */
235 or.l %d2,%d4 /* combine with low byte of 1st longword */
236 move.l %d4,(%a1)+ /* store longword */
237 lsl.l %d1,%d5 /* shift up low byte */
238 move.l %d6,%d2
239 lsr.l #8,%d2 /* get upper 3 bytes of 3rd longword */
240 or.l %d2,%d5 /* combine with low byte of 2nd longword */
241 move.l %d5,(%a1)+ /* store longword */
242 lsl.l %d1,%d6 /* shift up low byte */
243 move.l %d7,%d2
244 lsr.l #8,%d2 /* get upper 3 bytes of 4th longword */
245 or.l %d2,%d6 /* combine with low byte of 4th longword */
246 move.l %d6,(%a1)+ /* store longword */
247 lsl.l %d1,%d7 /* shift up low byte */
248 cmp.l %a0,%a2 /* runs %a0 up to last line bound */
249 jhi .lines_do1_loop
250
251 lea.l (12,%a2),%a2 /* readjust end address for doing longwords */
252 cmp.l %a0,%a2 /* any trailing longwords? */
253 jls .lines_do1_tail_end /* no: just store last low byte */
254
255.lines_do1_tail_loop:
256 move.l %d7,%d6 /* move old longword away */
257 move.l (%a0)+,%d7 /* load new longword */
258 move.l %d7,%d2
259 lsr.l #8,%d2 /* get upper 3 bytes */
260 or.l %d2,%d6 /* combine with old low byte */
261 move.l %d6,(%a1)+ /* store longword */
262 lsl.l %d1,%d7 /* shift up low byte */
263 cmp.l %a0,%a2 /* runs %a0 up to last long bound */
264 jhi .lines_do1_tail_loop
265
266.lines_do1_tail_end:
267 lsr.l %d1,%d7 /* get shifted-up low byte */
268 move.b %d7,(%a1)+ /* store byte */
269 jra .lines_end
270
271 /* long aligned destination (line + 0/4/8/12): head */
272.lines_do0_head_loop:
273 move.l (%a0)+,(%a1)+ /* copy longword */
274.lines_do0_start:
275 cmp.l %a0,%d0 /* runs %a0 up to first line bound */
276 jhi .lines_do0_head_loop
277
278.lines_do0_head_end:
279 move.l %a1,%d1
280 lsr.l #2,%d1
281 moveq.l #3,%d0 /* mask */
282 and.l %d0,%d1
283 moveq.l #16,%d0 /* address increment for one main loop pass */
284 jmp.l (2,%pc,%d1.l*2) /* switch ((dest_addr >> 2) & 3) */
285 bra.b .lines_lo0_start
286 bra.b .lines_lo4_start
287 bra.b .lines_lo8_start
288 /* bra.b .lines_lo12_start implicit */
289
290 /* long aligned destination (line + 12): use line bursts in the loop */
291.lines_lo12_start:
292 movem.l (%a0),%d4-%d7 /* load first line */
293 add.l %d0,%a0
294 move.l %d4,(%a1)+ /* store 1st longword */
295 cmp.l %a0,%a2 /* any full lines? */
296 jls .lines_lo12_end /* no: skip main loop */
297
298.lines_lo12_loop:
299 move.l %d5,%d1 /* move last 3 longwords of old line away */
300 move.l %d6,%d2
301 move.l %d7,%d3
302 movem.l (%a0),%d4-%d7 /* load new line */
303 add.l %d0,%a0
304 movem.l %d1-%d4,(%a1) /* store line (3 old + 1 new longwords) */
305 add.l %d0,%a1
306 cmp.l %a0,%a2 /* runs %a0 up to last line bound */
307 jhi .lines_lo12_loop
308
309 /* long aligned destination (line + 0/4/8/12): tail */
310.lines_lo12_end:
311 move.l %d5,(%a1)+ /* store 3rd last longword */
312.lines_lo8_end:
313 move.l %d6,(%a1)+ /* store 2nd last longword */
314.lines_lo4_end:
315 move.l %d7,(%a1)+ /* store last longword */
316.lines_lo0_end:
317 lea.l (12,%a2),%a2 /* readjust end address for doing longwords */
318 cmp.l %a0,%a2 /* any trailing longwords? */
319 jls .lines_end /* no: get outta here */
320
321.lines_do0_tail_loop:
322 move.l (%a0)+,(%a1)+ /* copy longword */
323 cmp.l %a0,%a2 /* runs %a0 up to last long bound */
324 jhi .lines_do0_tail_loop
325
326 jra .lines_end
327
328 /* line aligned destination: use line bursts in the loop */
329.lines_lo0_start:
330.lines_lo0_loop:
331 movem.l (%a0),%d4-%d7 /* load line */
332 add.l %d0,%a0
333 movem.l %d4-%d7,(%a1) /* store line */
334 add.l %d0,%a1
335 cmp.l %a0,%a2 /* runs %a0 up to last line bound */
336 jhi .lines_lo0_loop
337
338 jra .lines_lo0_end /* handle trailing longwords */
339
340 /* long aligned destination (line + 4): use line bursts in the loop */
341.lines_lo4_start:
342 movem.l (%a0),%d4-%d7 /* load first line */
343 add.l %d0,%a0
344 move.l %d4,(%a1)+ /* store 1st longword */
345 move.l %d5,(%a1)+ /* store 2nd longword */
346 move.l %d6,(%a1)+ /* store 3rd longword */
347 cmp.l %a0,%a2 /* any full lines? */
348 jls .lines_lo4_end /* no: skip main loop */
349
350.lines_lo4_loop:
351 move.l %d7,%d3 /* move last longword of old line away */
352 movem.l (%a0),%d4-%d7 /* load new line */
353 add.l %d0,%a0
354 movem.l %d3-%d6,(%a1) /* store line (1 old + 3 new longwords) */
355 add.l %d0,%a1
356 cmp.l %a0,%a2 /* runs %a0 up to last line bound */
357 jhi .lines_lo4_loop
358
359 jra .lines_lo4_end /* handle trailing longwords */
360
361 /* long aligned destination (line + 8): use line bursts in the loop */
362.lines_lo8_start:
363 movem.l (%a0),%d4-%d7 /* load first line */
364 add.l %d0,%a0
365 move.l %d4,(%a1)+ /* store 1st longword */
366 move.l %d5,(%a1)+ /* store 2nd longword */
367 cmp.l %a0,%a2
368 jls .lines_lo8_end
369
370.lines_lo8_loop:
371 move.l %d6,%d2 /* move last 2 longwords of old line away */
372 move.l %d7,%d3
373 movem.l (%a0),%d4-%d7 /* load new line */
374 add.l %d0,%a0
375 movem.l %d2-%d5,(%a1) /* store line (2 old + 2 new longwords) */
376 add.l %d0,%a1
377 cmp.l %a0,%a2 /* runs %a0 up to last line bound */
378 jhi .lines_lo8_loop
379
380 jra .lines_lo8_end /* handle trailing longwords */
381
382#ifdef FULLSPEED
383
384 /* word aligned destination (line + 2/6/10/14): head */
385.lines_do2_start:
386 cmp.l %a0,%d0 /* any leading longwords? */
387 jls .lines_do2_selector /* no: jump to mainloop selector */
388
389 move.l (%a0)+,%d7 /* load first longword */
390 swap %d7 /* swap words */
391 move.w %d7,(%a1)+ /* store high word */
392 cmp.l %a0,%d0 /* any more longword? */
393 jls .lines_do2_head_end /* no: skip head loop */
394
395.lines_do2_head_loop:
396 move.l %d7,%d6 /* move old longword away */
397 move.l (%a0)+,%d7 /* load new longword */
398 swap %d7 /* swap words */
399 move.w %d7,%d6 /* combine high word with old low word */
400 move.l %d6,(%a1)+ /* store longword */
401 cmp.l %a0,%d0 /* runs %a0 up to first line bound */
402 jhi .lines_do2_head_loop
403
404.lines_do2_head_end:
405 swap %d7 /* undo swap */
406 move.w %d7,(%a1)+ /* store word */
407
408.lines_do2_selector:
409 move.l %a1,%d1
410 lsr.l #2,%d1
411 moveq.l #3,%d0 /* mask */
412 and.l %d0,%d1
413 moveq.l #16,%d0 /* address increment for one main loop pass */
414 jmp.l (2,%pc,%d1.l*4) /* switch ((dest_addr >> 2) & 3) */
415 bra.w .lines_lo2_start
416 bra.w .lines_lo6_start
417 bra.w .lines_lo10_start
418 /* bra.w .lines_lo14_start implicit */
419
420 /* word aligned destination (line + 14): use line bursts in the loop */
421.lines_lo14_start:
422 movem.l (%a0),%d4-%d7 /* load first line */
423 add.l %d0,%a0
424 swap %d4 /* swap words of 1st long */
425 move.w %d4,(%a1)+ /* store word */
426 jra .lines_lo14_entry /* jump into main loop */
427
428.lines_lo14_loop:
429 move.l %d4,%d0 /* move old line away */
430 move.l %d5,%d1
431 move.l %d6,%d2
432 move.l %d7,%d3
433 movem.l (%a0),%d4-%d7 /* load new line */
434 lea.l (16,%a0),%a0
435 swap %d4 /* swap words of 1st long */
436 move.w %d4,%d3 /* combine 1st high word with old low word */
437 movem.l %d0-%d3,(%a1) /* store line */
438 lea.l (16,%a1),%a1
439.lines_lo14_entry:
440 swap %d5 /* swap words of 2nd long */
441 move.w %d5,%d4 /* combine 2nd high word with 1st low word */
442 swap %d6 /* swap words of 3rd long */
443 move.w %d6,%d5 /* combine 3rd high word with 2nd low word */
444 swap %d7 /* swap words of 4th long */
445 move.w %d7,%d6 /* combine 4th high word with 3rd low word */
446 cmp.l %a0,%a2 /* runs %a0 up to last line bound */
447 jhi .lines_lo14_loop
448
449 /* word aligned destination (line + 2/6/10/14): tail */
450.lines_lo14_end:
451 move.l %d4,(%a1)+ /* store third last longword */
452.lines_lo10_end:
453 move.l %d5,(%a1)+ /* store second last longword */
454.lines_lo6_end:
455 move.l %d6,(%a1)+ /* store last longword */
456.lines_lo2_end:
457 lea.l (12,%a2),%a2 /* readjust end address for doing longwords */
458 cmp.l %a0,%a2 /* any trailing longwords? */
459 jls .lines_do2_tail_end /* no: skip tail loop */
460
461.lines_do2_tail_loop:
462 move.l %d7,%d6 /* move old longword away */
463 move.l (%a0)+,%d7 /* load new longword */
464 swap %d7 /* swap words */
465 move.w %d7,%d6 /* combine high word with old low word */
466 move.l %d6,(%a1)+ /* store longword */
467 cmp.l %a0,%a2 /* runs %a0 up to last long bound */
468 jhi .lines_do2_tail_loop
469
470.lines_do2_tail_end:
471 swap %d7 /* undo swap */
472 move.w %d7,(%a1)+ /* store last word */
473 jra .lines_end
474
475 /* word aligned destination (line + 2): use line bursts in the loop */
476.lines_lo2_start:
477 movem.l (%a0),%d4-%d7 /* load first line */
478 add.l %d0,%a0
479 swap %d4 /* swap words of 1st long */
480 move.w %d4,(%a1)+ /* store high word */
481 swap %d5 /* swap words of 2nd long */
482 move.w %d5,%d4 /* combine 2nd high word with 1st low word */
483 swap %d6 /* swap words of 3rd long */
484 move.w %d6,%d5 /* combine 3nd high word with 2nd low word */
485 swap %d7 /* swap words of 4th long */
486 move.w %d7,%d6 /* combine 4th high word with 3rd low word */
487 move.l %d4,(%a1)+ /* store 1st longword */
488 move.l %d5,(%a1)+ /* store 2nd longword */
489 move.l %d6,(%a1)+ /* store 3rd longword */
490 cmp.l %a0,%a2 /* any full lines? */
491 jls .lines_lo2_end /* no: skip main loop */
492
493.lines_lo2_loop:
494 move.l %d7,%d3 /* move last longword of old line away */
495 movem.l (%a0),%d4-%d7 /* load line */
496 add.l %d0,%a0
497 swap %d4 /* swap words of 1st long */
498 move.w %d4,%d3 /* combine 1st high word with old low word */
499 swap %d5 /* swap words of 2nd long */
500 move.w %d5,%d4 /* combine 2nd high word with 1st low word */
501 swap %d6 /* swap words of 3rd long */
502 move.w %d6,%d5 /* combine 3rd high word with 2nd low word */
503 swap %d7 /* swap words of 4th long */
504 move.w %d7,%d6 /* combine 4th high word with 3rd low word */
505 movem.l %d3-%d6,(%a1) /* store line */
506 add.l %d0,%a1
507 cmp.l %a0,%a2 /* runs %a0 up to last line bound */
508 jhi .lines_lo2_loop
509
510 jra .lines_lo2_end /* handle trailing longwords */
511
512 /* word aligned destination (line + 6): use line bursts in the loop */
513.lines_lo6_start:
514 movem.l (%a0),%d4-%d7 /* load first line */
515 add.l %d0,%a0
516 swap %d4 /* swap words of 1st long */
517 move.w %d4,(%a1)+ /* store high word */
518 swap %d5 /* swap words of 2nd long */
519 move.w %d5,%d4 /* combine 2nd high word with 1st low word */
520 swap %d6 /* swap words of 3rd long */
521 move.w %d6,%d5 /* combine 3rd high word with 2nd low word */
522 move.l %d4,(%a1)+ /* store 1st longword */
523 move.l %d5,(%a1)+ /* store 2nd longword */
524 jra .lines_lo6_entry /* jump into main loop */
525
526.lines_lo6_loop:
527 move.l %d6,%d2 /* move last 2 longwords of old line away */
528 move.l %d7,%d3
529 movem.l (%a0),%d4-%d7 /* load line */
530 add.l %d0,%a0
531 swap %d4 /* swap words of 1st long */
532 move.w %d4,%d3 /* combine 1st high word with old low word */
533 swap %d5 /* swap words of 2nd long */
534 move.w %d5,%d4 /* combine 2nd high word with 1st low word */
535 swap %d6 /* swap words of 3rd long */
536 move.w %d6,%d5 /* combine 3rd high word with 2nd low word */
537 movem.l %d2-%d5,(%a1) /* store line */
538 add.l %d0,%a1
539.lines_lo6_entry:
540 swap %d7 /* swap words of 4th long */
541 move.w %d7,%d6 /* combine 4th high word with 3rd low word */
542 cmp.l %a0,%a2 /* runs %a0 up to last line bound */
543 jhi .lines_lo6_loop
544
545 jra .lines_lo6_end /* handle trailing longwords */
546
547 /* word aligned destination (line + 10): use line bursts in the loop */
548.lines_lo10_start:
549 movem.l (%a0),%d4-%d7 /* load first line */
550 add.l %d0,%a0
551 swap %d4 /* swap words of 1st long */
552 move.w %d4,(%a1)+ /* store high word */
553 swap %d5 /* swap words of 2nd long */
554 move.w %d5,%d4 /* combine 2nd high word with 1st low word */
555 move.l %d4,(%a1)+ /* store 1st longword */
556 jra .lines_lo10_entry /* jump into main loop */
557
558.lines_lo10_loop:
559 move.l %d5,%d1 /* move last 3 longwords of old line away */
560 move.l %d6,%d2
561 move.l %d7,%d3
562 movem.l (%a0),%d4-%d7 /* load line */
563 add.l %d0,%a0
564 swap %d4 /* swap words of 1st long */
565 move.w %d4,%d3 /* combine 1st high word with old low word */
566 swap %d5 /* swap words of 2nd long */
567 move.w %d5,%d4 /* combine 2nd high word with 1st low word */
568 movem.l %d1-%d4,(%a1) /* store line */
569 add.l %d0,%a1
570.lines_lo10_entry:
571 swap %d6 /* swap words of 3rd long */
572 move.w %d6,%d5 /* combine 3rd high word with 2nd low word */
573 swap %d7 /* swap words of 4th long */
574 move.w %d7,%d6 /* combine 4th high word with 3rd low word */
575 cmp.l %a0,%a2 /* runs %a0 up to last line bound */
576 jhi .lines_lo10_loop
577
578 jra .lines_lo10_end /* handle trailing longwords */
579
580#else /* !FULLSPEED */
581
582 /* word aligned destination (long + 2): use line burst reads in the loop */
583.lines_do2_start:
584 cmp.l %a0,%d0 /* any leading longwords? */
585 jhi .lines_do2_head_start /* yes: leading longword copy */
586
587 movem.l (%a0),%d4-%d7 /* load first line */
588 lea.l (16,%a0),%a0
589 swap %d4 /* swap words of 1st long */
590 move.w %d4,(%a1)+ /* store high word */
591 jra .lines_do2_entry /* jump into main loop */
592
593.lines_do2_head_start:
594 move.l (%a0)+,%d7 /* load first longword */
595 swap %d7 /* swap words */
596 move.w %d7,(%a1)+ /* store high word */
597 cmp.l %a0,%d0 /* any full longword? */
598 jls .lines_do2_loop /* no: skip head loop */
599
600.lines_do2_head_loop:
601 move.l %d7,%d6 /* move old longword away */
602 move.l (%a0)+,%d7 /* load new longword */
603 swap %d7 /* swap words */
604 move.w %d7,%d6 /* combine high word with old low word */
605 move.l %d6,(%a1)+ /* store longword */
606 cmp.l %a0,%d0 /* runs %a0 up to first line bound */
607 jhi .lines_do2_head_loop
608
609.lines_do2_loop:
610 move.l %d7,%d3 /* move last longword of old line away */
611 movem.l (%a0),%d4-%d7 /* load line */
612 lea.l (16,%a0),%a0
613 swap %d4 /* swap words of 1st long */
614 move.w %d4,%d3 /* combine 1st high word with old low word */
615 move.l %d3,(%a1)+ /* store 1st longword */
616.lines_do2_entry:
617 swap %d5 /* swap words of 2nd long */
618 move.w %d5,%d4 /* combine 2nd high word with 1st low word */
619 move.l %d4,(%a1)+ /* store 2nd longword */
620 swap %d6 /* swap words of 3rd long */
621 move.w %d6,%d5 /* combine 3rd high word with 2nd low word */
622 move.l %d5,(%a1)+ /* store 3rd longword */
623 swap %d7 /* swap words of 4th long */
624 move.w %d7,%d6 /* combine 4th high word with 3rd low word */
625 move.l %d6,(%a1)+ /* store 4th longword */
626 cmp.l %a0,%a2 /* runs %a0 up to last line bound */
627 jhi .lines_do2_loop
628
629.lines_do2_end:
630 lea.l (12,%a2),%a2 /* readjust end address for doing longwords */
631 cmp.l %a0,%a2 /* any trailing longwords? */
632 jls .lines_do2_tail_end /* no: skip tail loop */
633
634.lines_do2_tail_loop:
635 move.l %d7,%d6 /* move old longword away */
636 move.l (%a0)+,%d7 /* load new longword */
637 swap %d7 /* swap words */
638 move.w %d7,%d6 /* combine high word with old low word */
639 move.l %d6,(%a1)+ /* store longword */
640 cmp.l %a0,%a2 /* runs %a0 up to last long bound */
641 jhi .lines_do2_tail_loop
642
643.lines_do2_tail_end:
644 swap %d7 /* undo swap */
645 move.w %d7,(%a1)+ /* store last word */
646 /* jra .lines_end implicit */
647
648#endif /* !FULLSPEED */
649
650.lines_end:
651 addq.l #3,%a2 /* readjust end address */
652 move.l %a2,%d1 /* end address in %d1 again */
653 movem.l (%sp),%d2-%d7/%a2 /* restore registers */
654 lea.l (28,%sp),%sp
655 jra .bytes2_start /* jump to trailing byte loop */
656
657.long_start:
658 subq.l #3,%d1 /* adjust end address for doing 4 bytes/ pass */
659
660 /* longword copy loop - no lines */
661.long_loop:
662 move.l (%a0)+,(%a1)+ /* copy longword (write can be unaligned) */
663 cmp.l %a0,%d1 /* runs %a0 up to last long bound */
664 jhi .long_loop
665
666 addq.l #3,%d1 /* readjust end address */
667 cmp.l %a0,%d1 /* any bytes left? */
668 jls .bytes2_end /* no: skip trailing byte loop */
669
670 /* trailing byte loop */
671.bytes2_loop:
672 move.b (%a0)+,(%a1)+ /* copy byte */
673.bytes2_start:
674 cmp.l %a0,%d1 /* runs %a0 up to end address */
675 jhi .bytes2_loop
676
677.bytes2_end:
678 move.l (4,%sp),%d0 /* return destination */
679 rts
680
681.end:
682 .size memcpy,.end-memcpy
diff --git a/firmware/asm/m68k/memmove.S b/firmware/asm/m68k/memmove.S
new file mode 100644
index 0000000000..736cd619e1
--- /dev/null
+++ b/firmware/asm/m68k/memmove.S
@@ -0,0 +1,670 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 by Jens Arnold
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#include "config.h"
22
23 .section .icode,"ax",@progbits
24
25#define FULLSPEED /* use burst writing for word aligned destinations */
26 .align 2
27 .global memmove
28 .type memmove,@function
29
30/* Moves <length> bytes of data in memory from <source> to <dest>
31 * Regions may overlap.
32 * This version is optimized for speed, and needs the corresponding memcpy
33 * implementation for the forward copy branch.
34 *
35 * arguments:
36 * (4,%sp) - destination address
37 * (8,%sp) - source address
38 * (12,%sp) - length
39 *
40 * return value:
41 * %d0 - destination address (like ANSI version)
42 *
43 * register usage:
44 * %a0 - current source address
45 * %a1 - current dest address
46 * %a2 - source start address (in line-copy loops)
47 * %d0 - source start address (byte and longword copy) / data / scratch
48 * %d1 - data / scratch
49 * %d2 - data / scratch
50 * %d3..%d7 - data
51 *
52 * For maximum speed this routine reads and writes whole lines using burst
53 * move (movem.l) where possible. For byte aligned destinations (long-1 and
54 * long-3) it writes longwords only. Same goes for word aligned destinations
55 * if FULLSPEED is undefined.
56 */
57memmove:
58 move.l (4,%sp),%a1 /* Destination */
59 move.l (8,%sp),%a0 /* Source */
60 move.l (12,%sp),%d1 /* Length */
61
62 cmp.l %a0,%a1
63 bhi.b .backward /* dest > src -> backward copy */
64 jmp __memcpy_fwd_entry
65
66.backward:
67 move.l %a0,%d0 /* %d0 = source start */
68 add.l %d1,%a0 /* %a0 = source end */
69 add.l %d1,%a1 /* %a1 = destination end */
70
71 move.l %a0,%d1
72 and.l #0xFFFFFFFC,%d1 /* %d1 = last source long bound */
73 subq.l #4,%d1
74 cmp.l %d0,%d1 /* at least one aligned longword to copy? */
75 blo.w .bytes2r_start
76
77 addq.l #4,%d1 /* %d1 = last source long bound */
78 cmp.l %d1,%a0 /* any bytes to copy */
79 jls .bytes1r_end /* no: skip byte loop */
80
81 /* leading byte loop: copies 0..3 bytes */
82.bytes1r_loop:
83 move.b -(%a0),-(%a1) /* copy byte */
84 cmp.l %d1,%a0 /* runs %a0 down to last long bound */
85 jhi .bytes1r_loop
86
87.bytes1r_end:
88 moveq.l #-16,%d1
89 add.l %a0,%d1
90 and.l #0xFFFFFFF0,%d1 /* %d1 = last source line bound - 16 */
91 cmp.l %d0,%d1 /* at least one aligned line to copy? */
92 blo.w .longr_start /* no: jump to longword copy loop */
93
94 lea.l (-28,%sp),%sp /* free up some registers */
95 movem.l %d2-%d7/%a2,(%sp)
96
97 moveq.l #16,%d2
98 add.l %d2,%d1 /* %d1 = last source line bound */
99 move.l %d0,%a2 /* %a2 = start address */
100 lea.l (15,%a2),%a2 /* adjust start address for loops doing 16 bytes/pass */
101 move.l %a1,%d0
102 moveq.l #3,%d2 /* mask */
103 and.l %d2,%d0
104 jmp.l (2,%pc,%d0.l*4) /* switch (dest_addr & 3) */
105 bra.w .lines_do0r_start
106 bra.w .lines_do1r_start
107 bra.w .lines_do2r_start
108 /* bra.w .lines_do3r_start implicit */
109
110 /* byte aligned destination (long - 1): use line burst reads in main loop */
111.lines_do3r_start:
112 moveq.l #24,%d0 /* shift count for shifting by 3 bytes */
113 cmp.l %d1,%a0 /* any leading longwords? */
114 jhi .lines_do3r_head_start /* yes: leading longword copy */
115
116 lea.l (-16,%a0),%a0
117 movem.l (%a0),%d3-%d6 /* load initial line */
118 move.l %d6,%d2 /* last longword, bytes 3210 */
119 move.b %d2,-(%a1) /* store byte */
120 lsr.l #8,%d2 /* last longword, bytes .321 */
121 move.w %d2,-(%a1) /* store word */
122 jra .lines_do3r_entry
123
124.lines_do3r_head_start:
125 move.l -(%a0),%d3 /* load initial longword */
126 move.l %d3,%d2 /* bytes 3210 */
127 move.b %d2,-(%a1) /* store byte */
128 lsr.l #8,%d2 /* bytes .321 */
129 move.w %d2,-(%a1) /* store word */
130 jra .lines_do3r_head_entry
131
132.lines_do3r_head_loop:
133 move.l %d3,%d4 /* move old longword away */
134 move.l -(%a0),%d3 /* load new longword */
135 move.l %d3,%d2
136 lsl.l #8,%d2 /* get bytes 210. */
137 or.l %d2,%d4 /* combine with old high byte */
138 move.l %d4,-(%a1) /* store longword */
139.lines_do3r_head_entry:
140 lsr.l %d0,%d3 /* shift down high byte */
141 cmp.l %d1,%a0 /* run %a0 down to last line bound */
142 jhi .lines_do3r_head_loop
143
144.lines_do3r_loop:
145 move.l %d3,%d7 /* move first longword of last line away */
146 lea.l (-16,%a0),%a0
147 movem.l (%a0),%d3-%d6 /* load new line */
148 move.l %d6,%d2
149 lsl.l #8,%d2 /* get bytes 210. of 4th longword */
150 or.l %d2,%d7 /* combine with high byte of old longword */
151 move.l %d7,-(%a1) /* store longword */
152.lines_do3r_entry:
153 lsr.l %d0,%d6 /* shift down high byte */
154 move.l %d5,%d2
155 lsl.l #8,%d2 /* get bytes 210. of 3rd longword */
156 or.l %d2,%d6 /* combine with high byte of 4th longword */
157 move.l %d6,-(%a1) /* store longword */
158 lsr.l %d0,%d5 /* shift down high byte */
159 move.l %d4,%d2
160 lsl.l #8,%d2 /* get bytes 210. of 2nd longword */
161 or.l %d2,%d5 /* combine with high byte or 3rd longword */
162 move.l %d5,-(%a1) /* store longword */
163 lsr.l %d0,%d4 /* shift down high byte */
164 move.l %d3,%d2
165 lsl.l #8,%d2 /* get bytes 210. of 1st longword */
166 or.l %d2,%d4 /* combine with high byte of 2nd longword */
167 move.l %d4,-(%a1) /* store longword */
168 lsr.l %d0,%d3 /* shift down high byte */
169 cmp.l %a2,%a0 /* run %a0 down to first line bound */
170 jhi .lines_do3r_loop
171
172 lea.l (-12,%a2),%a2 /* readjust start address for doing longwords */
173 cmp.l %a2,%a0 /* any trailing longwords? */
174 jls .lines_do3r_tail_end /* no: just store last high byte */
175
176.lines_do3r_tail_loop:
177 move.l %d3,%d4 /* move old longword away */
178 move.l -(%a0),%d3 /* load new longword */
179 move.l %d3,%d2
180 lsl.l #8,%d2 /* get bytes 210. */
181 or.l %d2,%d4 /* combine with old high byte */
182 move.l %d4,-(%a1) /* store longword */
183 lsr.l %d0,%d3 /* shift down high byte */
184 cmp.l %a2,%a0 /* run %a0 down to first long bound */
185 jhi .lines_do3r_tail_loop
186
187.lines_do3r_tail_end:
188 move.b %d3,-(%a1) /* store shifted-down high byte */
189 jra .linesr_end
190
191 /* byte aligned destination (long - 3): use line burst reads in main loop */
192.lines_do1r_start:
193 moveq.l #24,%d0 /* shift count for shifting by 3 bytes */
194 cmp.l %d1,%a0 /* any leading longwords? */
195 jhi .lines_do1r_head_start /* yes: leading longword copy */
196
197 lea.l (-16,%a0),%a0
198 movem.l (%a0),%d3-%d6 /* load initial line */
199 move.b %d6,-(%a1) /* store low byte of last longword */
200 jra .lines_do1r_entry
201
202.lines_do1r_head_start:
203 move.l -(%a0),%d3 /* load initial longword */
204 move.b %d3,-(%a1) /* store low byte */
205 jra .lines_do1r_head_entry
206
207.lines_do1r_head_loop:
208 move.l %d3,%d4 /* move old longword away */
209 move.l -(%a0),%d3 /* load new longword */
210 move.l %d3,%d2
211 lsl.l %d0,%d2 /* get low byte */
212 or.l %d2,%d4 /* combine with old bytes .321 */
213 move.l %d4,-(%a1) /* store longword */
214.lines_do1r_head_entry:
215 lsr.l #8,%d3 /* get bytes .321 */
216 cmp.l %d1,%a0 /* run %a0 down to last line bound */
217 jhi .lines_do1r_head_loop
218
219.lines_do1r_loop:
220 move.l %d3,%d7 /* move first longword of old line away */
221 lea.l (-16,%a0),%a0
222 movem.l (%a0),%d3-%d6 /* load new line */
223 move.l %d6,%d2
224 lsl.l %d0,%d2 /* get low byte of 4th longword */
225 or.l %d2,%d7 /* combine with bytes .321 of old longword */
226 move.l %d7,-(%a1) /* store longword */
227.lines_do1r_entry:
228 lsr.l #8,%d6 /* get bytes .321 */
229 move.l %d5,%d2
230 lsl.l %d0,%d2 /* get low byte of 3rd longword */
231 or.l %d2,%d6 /* combine with bytes .321 of 4th longword */
232 move.l %d6,-(%a1) /* store longword */
233 lsr.l #8,%d5 /* get bytes .321 */
234 move.l %d4,%d2
235 lsl.l %d0,%d2 /* get low byte of 2nd longword */
236 or.l %d2,%d5 /* combine with bytes .321 of 3rd longword */
237 move.l %d5,-(%a1) /* store longword */
238 lsr.l #8,%d4 /* get bytes .321 */
239 move.l %d3,%d2
240 lsl.l %d0,%d2 /* get low byte of 1st longword */
241 or.l %d2,%d4 /* combine with bytes .321 of 2nd longword */
242 move.l %d4,-(%a1) /* store longword */
243 lsr.l #8,%d3 /* get bytes .321 */
244 cmp.l %a2,%a0 /* run %a0 down to first line bound */
245 jhi .lines_do1r_loop
246
247 lea.l (-12,%a2),%a2 /* readjust start address for doing longwords */
248 cmp.l %a2,%a0 /* any trailing longwords? */
249 jls .lines_do1r_tail_end /* no: just store last high byte */
250
251.lines_do1r_tail_loop:
252 move.l %d3,%d4 /* move old longword away */
253 move.l -(%a0),%d3 /* load new longword */
254 move.l %d3,%d2
255 lsl.l %d0,%d2 /* get low byte */
256 or.l %d2,%d4 /* combine with old bytes .321 */
257 move.l %d4,-(%a1) /* store longword */
258 lsr.l #8,%d3 /* get bytes .321 */
259 cmp.l %a2,%a0 /* run %a0 down to first long bound */
260 jhi .lines_do1r_tail_loop
261
262.lines_do1r_tail_end:
263 move.w %d3,-(%a1) /* store word 21 */
264 swap %d3
265 move.b %d3,-(%a1) /* store byte 3 */
266 jra .linesr_end
267
268 /* long aligned destination (line - 0/4/8/12): head */
269.lines_do0r_head_loop:
270 move.l -(%a0),-(%a1) /* copy longword */
271.lines_do0r_start:
272 cmp.l %d1,%a0 /* run %a0 down to last line bound */
273 jhi .lines_do0r_head_loop
274
275.lines_do0r_head_end:
276 move.l %a1,%d1
277 lsr.l #2,%d1
278 moveq.l #3,%d0 /* mask */
279 and.l %d0,%d1
280 moveq.l #16,%d0 /* address decrement for one main loop pass */
281 jmp.l (2,%pc,%d1.l*2) /* switch ((dest_addr >> 2) & 3) */
282 bra.b .lines_lo0r_start
283 bra.b .lines_lo4r_start
284 bra.b .lines_lo8r_start
285 /* bra.b .lines_lo12r_start implicit */
286
287 /* long aligned destination (line - 4): use line bursts in the loop */
288.lines_lo12r_start:
289 sub.l %d0,%a0
290 movem.l (%a0),%d1-%d4 /* load initial line */
291 move.l %d4,-(%a1) /* store 4th longword */
292 move.l %d3,-(%a1) /* store 3rd longword */
293 move.l %d2,-(%a1) /* store 2nd longword */
294 cmp.l %a2,%a0 /* any full lines? */
295 jls .lines_lo12r_end /* no: skip main loop */
296
297.lines_lo12r_loop:
298 move.l %d1,%d5 /* move first longword of old line away */
299 sub.l %d0,%a0
300 movem.l (%a0),%d1-%d4 /* load new line */
301 sub.l %d0,%a1
302 movem.l %d2-%d5,(%a1) /* store line (1 old + 3 new longwords */
303 cmp.l %a2,%a0 /* run %a0 down to first line bound */
304 jhi .lines_lo12r_loop
305
306 jra .lines_lo12r_end /* handle trailing longwords */
307
308 /* line aligned destination: use line bursts in the loop */
309.lines_lo0r_start:
310.lines_lo0r_loop:
311 sub.l %d0,%a0
312 movem.l (%a0),%d1-%d4 /* load line */
313 sub.l %d0,%a1
314 movem.l %d1-%d4,(%a1) /* store line */
315 cmp.l %a2,%a0 /* run %a0 down to first line bound */
316 jhi .lines_lo0r_loop
317
318 jra .lines_lo0r_end /* handle trailing longwords */
319
320 /* long aligned destination (line - 8): use line bursts in the loop */
321.lines_lo8r_start:
322 sub.l %d0,%a0
323 movem.l (%a0),%d1-%d4 /* load initial line */
324 move.l %d4,-(%a1) /* store 4th longword */
325 move.l %d3,-(%a1) /* store 3rd longword */
326 cmp.l %a2,%a0 /* any full lines? */
327 jls .lines_lo8r_end /* no: skip main loop */
328
329.lines_lo8r_loop:
330 move.l %d2,%d6 /* move first 2 longwords of old line away */
331 move.l %d1,%d5
332 sub.l %d0,%a0
333 movem.l (%a0),%d1-%d4 /* load new line */
334 sub.l %d0,%a1
335 movem.l %d3-%d6,(%a1) /* store line (2 old + 2 new longwords */
336 cmp.l %a2,%a0 /* run %a0 down to first line bound */
337 jhi .lines_lo8r_loop
338
339 jra .lines_lo8r_end /* handle trailing longwords */
340
341 /* long aligned destination (line - 12): use line bursts in the loop */
342.lines_lo4r_start:
343 sub.l %d0,%a0
344 movem.l (%a0),%d1-%d4 /* load initial line */
345 move.l %d4,-(%a1) /* store 4th longword */
346 cmp.l %a2,%a0 /* any full lines? */
347 jls .lines_lo4r_end /* no: skip main loop */
348
349.lines_lo4r_loop:
350 move.l %d3,%d7 /* move first 3 longwords of old line away */
351 move.l %d2,%d6
352 move.l %d1,%d5
353 sub.l %d0,%a0
354 movem.l (%a0),%d1-%d4 /* load new line */
355 sub.l %d0,%a1
356 movem.l %d4-%d7,(%a1) /* store line (3 old + 1 new longwords */
357 cmp.l %a2,%a0 /* run %a0 down to first line bound */
358 jhi .lines_lo4r_loop
359
360 /* long aligned destination (line - 0/4/8/12): tail */
361.lines_lo4r_end:
362 move.l %d3,-(%a1) /* store 3rd last longword */
363.lines_lo8r_end:
364 move.l %d2,-(%a1) /* store 2nd last longword */
365.lines_lo12r_end:
366 move.l %d1,-(%a1) /* store last longword */
367.lines_lo0r_end:
368 lea.l (-12,%a2),%a2 /* readjust end address for doing longwords */
369 cmp.l %a2,%a0 /* any trailing longwords? */
370 jls .linesr_end /* no: get outta here */
371
372.lines_do0r_tail_loop:
373 move.l -(%a0),-(%a1) /* copy longword */
374 cmp.l %a2,%a0 /* run %a0 down to first long bound */
375 jhi .lines_do0r_tail_loop
376
377 jra .linesr_end
378
379#ifdef FULLSPEED
380 /* word aligned destination (line - 2/6/10/14): head */
381.lines_do2r_start:
382 cmp.l %d1,%a0 /* any leading longwords? */
383 jls .lines_do2r_selector /* no: jump to mainloop selector */
384
385 move.l -(%a0),%d3 /* load initial longword */
386 move.w %d3,-(%a1) /* store low word */
387 cmp.l %d1,%a0 /* any more longwords? */
388 jls .lines_do2r_head_end /* no: skip head loop */
389
390.lines_do2r_head_loop:
391 move.l %d3,%d4 /* move old longword away */
392 move.l -(%a0),%d3 /* load new longword */
393 move.w %d3,%d4 /* combine low word with old high word */
394 swap %d4 /* swap words */
395 move.l %d4,-(%a1) /* store longword */
396 cmp.l %d1,%a0 /* run %a0 down to last line bound */
397 jhi .lines_do2r_head_loop
398
399.lines_do2r_head_end:
400 swap %d3 /* get high word */
401 move.w %d3,-(%a1) /* and store it */
402
403.lines_do2r_selector:
404 move.l %a1,%d1
405 lsr.l #2,%d1
406 moveq.l #3,%d0 /* mask */
407 and.l %d0,%d1
408 moveq.l #16,%d7 /* address decrement for one main loop pass */
409 jmp.l (2,%pc,%d1.l*4) /* switch ((dest_addr >> 2) & 3) */
410 bra.w .lines_lo2r_start
411 bra.w .lines_lo6r_start
412 bra.w .lines_lo10r_start
413 /* bra.w .lines_lo14r_start implicit */
414
415 /* word aligned destination (line - 2): use line bursts in the loop */
416.lines_lo14r_start:
417 sub.l %d7,%a0
418 movem.l (%a0),%d0-%d3 /* load initial line */
419 move.w %d3,-(%a1) /* store last low word */
420 move.w %d2,%d3 /* combine 3rd low word with 4th high word */
421 swap %d3 /* swap words of 3rd long */
422 move.w %d1,%d2 /* combine 2nd low word with 3rd high word */
423 swap %d2 /* swap words of 2nd long */
424 move.w %d0,%d1 /* combine 1st low word with 2nd high word */
425 swap %d1 /* swap words of 1st long */
426 move.l %d3,-(%a1) /* store 3rd longword */
427 move.l %d2,-(%a1) /* store 2nd longword */
428 move.l %d1,-(%a1) /* store 1st longword */
429 cmp.l %a2,%a0 /* any full lines? */
430 jls .lines_lo14r_end /* no: skip main loop */
431
432.lines_lo14r_loop:
433 move.l %d0,%d4 /* move first longword of old line away */
434 sub.l %d7,%a0
435 movem.l (%a0),%d0-%d3 /* load line */
436 move.w %d3,%d4 /* combine 4th low word with old high word */
437 swap %d4 /* swap words of 4th long */
438 move.w %d2,%d3 /* combine 3rd low word with 4th high word */
439 swap %d3 /* swap words of 3rd long */
440 move.w %d1,%d2 /* combine 2nd low word with 3rd high word */
441 swap %d2 /* swap words of 2nd long */
442 move.w %d0,%d1 /* combine 1st low word with 2nd high word */
443 swap %d1 /* swap words of 1st long */
444 sub.l %d7,%a1
445 movem.l %d1-%d4,(%a1) /* store line */
446 cmp.l %a2,%a0 /* run %a0 down to first line bound */
447 jhi .lines_lo14r_loop
448
449 jra .lines_lo14r_end /* handle trailing longwords */
450
451 /* word aligned destination (line - 6): use line bursts in the loop */
452.lines_lo10r_start:
453 sub.l %d7,%a0
454 movem.l (%a0),%d0-%d3 /* load initial line */
455 move.w %d3,-(%a1) /* store last low word */
456 move.w %d2,%d3 /* combine 3rd low word with 4th high word */
457 swap %d3 /* swap words of 3rd long */
458 move.w %d1,%d2 /* combine 2nd low word with 3rd high word */
459 swap %d2 /* swap words of 2nd long */
460 move.l %d3,-(%a1) /* store 3rd longword */
461 move.l %d2,-(%a1) /* store 2nd longword */
462 jra .lines_lo10r_entry /* jump into main loop */
463
464.lines_lo10r_loop:
465 move.l %d0,%d4 /* move first 2 longwords of old line away */
466 move.l %d1,%d5
467 sub.l %d7,%a0
468 movem.l (%a0),%d0-%d3 /* load line */
469 move.w %d3,%d4 /* combine 4th low word with old high word */
470 swap %d4 /* swap words of 4th long */
471 move.w %d2,%d3 /* combine 3rd low word with 4th high word */
472 swap %d3 /* swap words of 3rd long */
473 move.w %d1,%d2 /* combine 2nd low word with 3rd high word */
474 swap %d2 /* swap words of 2nd long */
475 sub.l %d7,%a1
476 movem.l %d2-%d5,(%a1) /* store line */
477.lines_lo10r_entry:
478 move.w %d0,%d1 /* combine 1st low word with 2nd high word */
479 swap %d1 /* swap words of 1st long */
480 cmp.l %a2,%a0 /* run %a0 down to first line bound */
481 jhi .lines_lo10r_loop
482
483 jra .lines_lo10r_end /* handle trailing longwords */
484
485 /* word aligned destination (line - 10): use line bursts in the loop */
486.lines_lo6r_start:
487 sub.l %d7,%a0
488 movem.l (%a0),%d0-%d3 /* load initial line */
489 move.w %d3,-(%a1) /* store last low word */
490 move.w %d2,%d3 /* combine 3rd low word with 4th high word */
491 swap %d3 /* swap words of 3rd long */
492 move.l %d3,-(%a1) /* store 3rd longword */
493 jra .lines_lo6r_entry /* jump into main loop */
494
495.lines_lo6r_loop:
496 move.l %d0,%d4 /* move first 3 longwords of old line away */
497 move.l %d1,%d5
498 move.l %d2,%d6
499 sub.l %d7,%a0
500 movem.l (%a0),%d0-%d3 /* load line */
501 move.w %d3,%d4 /* combine 4th low word with old high word */
502 swap %d4 /* swap words of 4th long */
503 move.w %d2,%d3 /* combine 3rd low word with 4th high word */
504 swap %d3 /* swap words of 3rd long */
505 sub.l %d7,%a1
506 movem.l %d3-%d6,(%a1) /* store line */
507.lines_lo6r_entry:
508 move.w %d1,%d2 /* combine 2nd low word with 3rd high word */
509 swap %d2 /* swap words of 2nd long */
510 move.w %d0,%d1 /* combine 1st low word with 2nd high word */
511 swap %d1 /* swap words of 1st long */
512 cmp.l %a2,%a0 /* run %a0 down to first line bound */
513 jhi .lines_lo6r_loop
514
515 jra .lines_lo6r_end /* handle trailing longwords */
516
517 /* word aligned destination (line - 14): use line bursts in the loop */
518.lines_lo2r_start:
519 sub.l %d7,%a0
520 movem.l (%a0),%d0-%d3 /* load initial line */
521 move.w %d3,-(%a1) /* store last low word */
522 jra .lines_lo2r_entry /* jump into main loop */
523
524.lines_lo2r_loop:
525 move.l %d0,%d4 /* move old line away */
526 move.l %d1,%d5
527 move.l %d2,%d6
528 move.l %d3,%d7
529 lea.l (-16,%a0),%a0
530 movem.l (%a0),%d0-%d3 /* load line */
531 move.w %d3,%d4 /* combine 4th low word with old high word */
532 swap %d4 /* swap words of 4th long */
533 lea.l (-16,%a1),%a1
534 movem.l %d4-%d7,(%a1) /* store line */
535.lines_lo2r_entry:
536 move.w %d2,%d3 /* combine 3rd low word with 4th high word */
537 swap %d3 /* swap words of 3rd long */
538 move.w %d1,%d2 /* combine 2nd low word with 3rd high word */
539 swap %d2 /* swap words of 2nd long */
540 move.w %d0,%d1 /* combine 1st low word with 2nd high word */
541 swap %d1 /* swap words of 1st long */
542 cmp.l %a2,%a0 /* run %a0 down to first line bound */
543 jhi .lines_lo2r_loop
544
545 /* word aligned destination (line - 2/6/10/14): tail */
546.lines_lo2r_end:
547 move.l %d3,-(%a1) /* store third last longword */
548.lines_lo6r_end:
549 move.l %d2,-(%a1) /* store second last longword */
550.lines_lo10r_end:
551 move.l %d1,-(%a1) /* store last longword */
552.lines_lo14r_end:
553 lea.l (-12,%a2),%a2 /* readjust start address for doing longwords */
554 cmp.l %a2,%a0 /* any trailing longwords? */
555 jls .lines_do2r_tail_end /* no: skip tail loop */
556
557.lines_do2r_tail_loop:
558 move.l %d0,%d1 /* move old longword away */
559 move.l -(%a0),%d0 /* load new longword */
560 move.w %d0,%d1 /* combine low word with old high word */
561 swap %d1 /* swap words */
562 move.l %d1,-(%a1) /* store longword */
563 cmp.l %a2,%a0 /* run %a0 down to first long bound */
564 jhi .lines_do2r_tail_loop
565
566.lines_do2r_tail_end:
567 swap %d0 /* get final high word */
568 move.w %d0,-(%a1) /* store it */
569 /* jra .linesr_end implicit */
570
571#else /* !FULLSPEED */
572
573 /* word aligned destination (long - 2): use line burst reads in the loop */
574.lines_do2r_start:
575 cmp.l %d1,%a0 /* any leading longwords? */
576 jhi .lines_do2r_head_start /* yes: leading longword copy */
577
578 lea.l (-16,%a0),%a0
579 movem.l (%a0),%d3-%d6 /* load initial line */
580 move.w %d6,-(%a1) /* store last low word */
581 jra .lines_do2r_entry /* jump into main loop */
582
583.lines_do2r_head_start:
584 move.l -(%a0),%d3 /* load initial longword */
585 move.w %d3,-(%a1) /* store low word */
586 cmp.l %d1,%a0 /* any full longword? */
587 jls .lines_do2r_loop /* no: skip head loop */
588
589.lines_do2r_head_loop:
590 move.l %d3,%d4 /* move old longword away */
591 move.l -(%a0),%d3 /* load new longword */
592 move.w %d3,%d4 /* combine low word with old high word */
593 swap %d4 /* swap words */
594 move.l %d4,-(%a1) /* store longword */
595 cmp.l %d1,%a0 /* run %a0 down to last line bound */
596 jhi .lines_do2r_head_loop
597
598.lines_do2r_loop:
599 move.l %d3,%d7 /* move first longword of old line away */
600 lea.l (-16,%a0),%a0
601 movem.l (%a0),%d3-%d6 /* load line */
602 move.w %d6,%d7 /* combine 4th low word with old high word */
603 swap %d7 /* swap words of 4th long */
604 move.l %d7,-(%a1) /* store 4th longword */
605.lines_do2r_entry:
606 move.w %d5,%d6 /* combine 3rd low word with 4th high word */
607 swap %d6 /* swap words of 3rd long */
608 move.l %d6,-(%a1) /* store 3rd longword */
609 move.w %d4,%d5 /* combine 2nd low word with 3rd high word */
610 swap %d5 /* swap words of 2nd long */
611 move.l %d5,-(%a1) /* store 2nd longword */
612 move.w %d3,%d4 /* combine 1st low word with 2nd high word */
613 swap %d4 /* swap words of 1st long */
614 move.l %d4,-(%a1) /* store 1st longword */
615 cmp.l %a2,%a0 /* run %a0 down to first line bound */
616 jhi .lines_do2r_loop
617
618.lines_do2r_end:
619 lea.l (-12,%a2),%a2 /* readjust start address for doing longwords */
620 cmp.l %a2,%a0 /* any trailing longwords? */
621 jls .lines_do2r_tail_end /* no: skip tail loop */
622
623.lines_do2r_tail_loop:
624 move.l %d3,%d4 /* move old longword away */
625 move.l -(%a0),%d3 /* load new longword */
626 move.w %d3,%d4 /* combine low word with old high word */
627 swap %d4 /* swap words */
628 move.l %d4,-(%a1) /* store longword */
629 cmp.l %a2,%a0 /* run %a0 down to first long bound */
630 jhi .lines_do2r_tail_loop
631
632.lines_do2r_tail_end:
633 swap %d3 /* get final high word */
634 move.w %d3,-(%a1) /* store it */
635 /* jra .linesr_end implicit */
636
637#endif /* !FULLSPEED */
638
639.linesr_end:
640 subq.l #3,%a2 /* readjust end address */
641 move.l %a2,%d0 /* start address in %d0 again */
642 movem.l (%sp),%d2-%d7/%a2 /* restore registers */
643 lea.l (28,%sp),%sp
644 jra .bytes2r_start /* jump to trailing byte loop */
645
646.longr_start:
647 addq.l #3,%d0 /* adjust start address for doing 4 bytes/ pass */
648
649 /* longword copy loop - no lines */
650.longr_loop:
651 move.l -(%a0),-(%a1) /* copy longword (write can be unaligned) */
652 cmp.l %d0,%a0 /* runs %a0 down to first long bound */
653 jhi .longr_loop
654
655 subq.l #3,%d0 /* readjust start address */
656 cmp.l %d0,%a0 /* any bytes left? */
657 jls .bytes2r_end /* no: skip trailing byte loop */
658
659 /* trailing byte loop */
660.bytes2r_loop:
661 move.b -(%a0),-(%a1) /* copy byte */
662.bytes2r_start:
663 cmp.l %d0,%a0 /* runs %a0 down to start address */
664 jhi .bytes2r_loop
665
666.bytes2r_end:
667 rts /* returns start address */
668
669.end:
670 .size memmove,.end-memmove
diff --git a/firmware/asm/m68k/memset.S b/firmware/asm/m68k/memset.S
new file mode 100644
index 0000000000..839b305a05
--- /dev/null
+++ b/firmware/asm/m68k/memset.S
@@ -0,0 +1,152 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2004 by Jens Arnold
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#include "config.h"
22
23 .section .icode,"ax",@progbits
24
25 .align 2
26 .global memset
27 .type memset,@function
28
29/* Fills a memory region with specified byte value
30 * This version is optimized for speed
31 *
32 * arguments:
33 * (4,%sp) - start address
34 * (8,%sp) - data
35 * (12,%sp) - length
36 *
37 * return value:
38 * %d0 - start address (like ANSI version)
39 *
40 * register usage:
41 * %d0 - data (spread to all 4 bytes when using long stores)
42 * %d1 - temporary / data (for burst transfer)
43 * %d2 - data (for burst transfer)
44 * %d3 - data (for burst transfer)
45 * %a0 - start address
46 * %a1 - current address (runs down from end to start)
47 *
48 * For maximum speed this routine uses both long stores and burst mode,
49 * storing whole lines with movem.l. The routine fills memory from end
50 * to start in order to ease returning the start address.
51 */
52memset:
53 move.l (4,%sp),%a0 /* start address */
54 move.l (8,%sp),%d0 /* data */
55 move.l (12,%sp),%a1 /* length */
56 add.l %a0,%a1 /* %a1 = end address */
57
58 move.l %a0,%d1
59 addq.l #7,%d1
60 and.l #0xFFFFFFFC,%d1 /* %d1 = first long bound + 4 */
61 cmp.l %d1,%a1 /* at least one aligned longword to fill? */
62 blo.b .no_longs /* no, jump directly to byte loop */
63
64 and.l #0xFF,%d0 /* start: spread data to all 4 bytes */
65 move.l %d0,%d1
66 lsl.l #8,%d1
67 or.l %d1,%d0 /* data now in 2 lower bytes of %d0 */
68 move.l %d0,%d1
69 swap %d0
70 or.l %d1,%d0 /* data now in all 4 bytes of %d0 */
71
72 move.l %a1,%d1
73 and.l #0xFFFFFFFC,%d1 /* %d1 = last long bound */
74 cmp.l %d1,%a1 /* any bytes to set? */
75 bls.b .end_b1 /* no: skip byte loop */
76
77 /* leading byte loop: sets 0..3 bytes */
78.loop_b1:
79 move.b %d0,-(%a1) /* store byte */
80 cmp.l %d1,%a1 /* runs %a1 down to last long bound */
81 bhi.b .loop_b1
82
83.end_b1:
84 moveq.l #31,%d1
85 add.l %a0,%d1
86 and.l #0xFFFFFFF0,%d1 /* %d1 = first line bound + 16 */
87 cmp.l %d1,%a1 /* at least one full line to fill? */
88 blo.b .no_lines /* no, jump to longword loop */
89
90 mov.l %a1,%d1
91 and.l #0xFFFFFFF0,%d1 /* %d1 = last line bound */
92 cmp.l %d1,%a1 /* any longwords to set? */
93 bls.b .end_l1 /* no: skip longword loop */
94
95 /* leading longword loop: sets 0..3 longwords */
96.loop_l1:
97 move.l %d0,-(%a1) /* store longword */
98 cmp.l %d1,%a1 /* runs %a1 down to last line bound */
99 bhi.b .loop_l1
100
101.end_l1:
102 move.l %d2,-(%sp) /* free some registers */
103 move.l %d3,-(%sp)
104
105 move.l %d0,%d1 /* spread data to 4 data registers */
106 move.l %d0,%d2
107 move.l %d0,%d3
108 lea.l (15,%a0),%a0 /* start address += 15, acct. for trl. data */
109
110 /* main loop: set whole lines utilising burst mode */
111.loop_line:
112 lea.l (-16,%a1),%a1 /* pre-decrement */
113 movem.l %d0-%d3,(%a1) /* store line */
114 cmp.l %a0,%a1 /* runs %a1 down to first line bound */
115 bhi.b .loop_line
116
117 lea.l (-15,%a0),%a0 /* correct start address */
118 move.l (%sp)+,%d3 /* restore registers */
119 move.l (%sp)+,%d2
120
121 move.l %a0,%d1 /* %d1 = start address ... */
122 addq.l #3,%d1 /* ... +3, account for possible trailing bytes */
123 cmp.l %d1,%a1 /* any longwords left */
124 bhi.b .loop_l2 /* yes: jump to longword loop */
125 bra.b .no_longs /* no: skip loop */
126
127.no_lines:
128 move.l %a0,%d1 /* %d1 = start address ... */
129 addq.l #3,%d1 /* ... +3, account for possible trailing bytes */
130
131 /* trailing longword loop */
132.loop_l2:
133 move.l %d0,-(%a1) /* store longword */
134 cmp.l %d1,%a1 /* runs %a1 down to first long bound */
135 bhi.b .loop_l2
136
137.no_longs:
138 cmp.l %a0,%a1 /* any bytes left? */
139 bls.b .end_b2 /* no: skip loop */
140
141 /* trailing byte loop */
142.loop_b2:
143 move.b %d0,-(%a1) /* store byte */
144 cmp.l %a0,%a1 /* runs %a1 down to start address */
145 bhi.b .loop_b2
146
147.end_b2:
148 move.l %a0,%d0 /* return start address */
149 rts
150
151.end:
152 .size memset,.end-memset
diff --git a/firmware/asm/m68k/memset16.S b/firmware/asm/m68k/memset16.S
new file mode 100644
index 0000000000..1673038d03
--- /dev/null
+++ b/firmware/asm/m68k/memset16.S
@@ -0,0 +1,146 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 by Jens Arnold
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#include "config.h"
22
23 .section .icode,"ax",@progbits
24
25 .global memset16
26 .type memset16,@function
27
28/* Fills a memory region with specified word value
29 * Start address must be word aligned, length is in words
30 * This version is optimized for speed
31 *
32 * arguments:
33 * (4,%sp) - start address
34 * (8,%sp) - data
35 * (12,%sp) - length
36 *
37 * return value:
38 * %d0 - start address
39 *
40 * register usage:
41 * %d0 - data (spread to both words when using long stores)
42 * %d1 - temporary / data (for burst transfer)
43 * %d2 - data (for burst transfer)
44 * %d3 - data (for burst transfer)
45 * %a0 - start address
46 * %a1 - current address (runs down from end to start)
47 *
48 * For maximum speed this routine uses both long stores and burst mode,
49 * storing whole lines with movem.l. The routine fills memory from end
50 * to start in order to ease returning the start address.
51 */
52memset16:
53 move.l (4,%sp),%a0 /* start address */
54 move.l (8,%sp),%d0 /* data */
55 move.l (12,%sp),%a1 /* length */
56 add.l %a1,%a1
57 add.l %a0,%a1 /* %a1 = end address */
58
59 move.l %a0,%d1
60 addq.l #6,%d1
61 and.l #0xFFFFFFFC,%d1 /* %d1 = first long bound + 4 */
62 cmp.l %d1,%a1 /* at least one aligned longword to fill? */
63 blo.b .no_longs /* no, jump directly to word loop */
64
65 and.l #0xFFFF,%d0 /* start: spread data to both words */
66 move.l %d0,%d1
67 swap %d1
68 or.l %d1,%d0 /* data now in both words */
69
70 move.l %a1,%d1
71 and.l #0xFFFFFFFC,%d1 /* %d1 = last long bound */
72 cmp.l %d1,%a1 /* one extra word? */
73 bls.b .end_w1 /* no: skip */
74
75 move.w %d0,-(%a1) /* set leading word */
76
77.end_w1:
78 moveq.l #30,%d1
79 add.l %a0,%d1
80 and.l #0xFFFFFFF0,%d1 /* %d1 = first line bound + 16 */
81 cmp.l %d1,%a1 /* at least one full line to fill? */
82 blo.b .no_lines /* no, jump to longword loop */
83
84 mov.l %a1,%d1
85 and.l #0xFFFFFFF0,%d1 /* %d1 = last line bound */
86 cmp.l %d1,%a1 /* any longwords to set? */
87 bls.b .end_l1 /* no: skip longword loop */
88
89 /* leading longword loop: sets 0..3 longwords */
90.loop_l1:
91 move.l %d0,-(%a1) /* store longword */
92 cmp.l %d1,%a1 /* runs %a1 down to last line bound */
93 bhi.b .loop_l1
94
95.end_l1:
96 move.l %d2,-(%sp) /* free some registers */
97 move.l %d3,-(%sp)
98
99 move.l %d0,%d1 /* spread data to 4 data registers */
100 move.l %d0,%d2
101 move.l %d0,%d3
102 lea.l (14,%a0),%a0 /* start address += 14, acct. for trl. data */
103
104 /* main loop: set whole lines utilising burst mode */
105.loop_line:
106 lea.l (-16,%a1),%a1 /* pre-decrement */
107 movem.l %d0-%d3,(%a1) /* store line */
108 cmp.l %a0,%a1 /* runs %a1 down to first line bound */
109 bhi.b .loop_line
110
111 lea.l (-14,%a0),%a0 /* correct start address */
112 move.l (%sp)+,%d3 /* restore registers */
113 move.l (%sp)+,%d2
114
115 move.l %a0,%d1 /* %d1 = start address ... */
116 addq.l #2,%d1 /* ... +2, account for possible trailing word */
117 cmp.l %d1,%a1 /* any longwords left */
118 bhi.b .loop_l2 /* yes: jump to longword loop */
119 bra.b .no_longs /* no: skip loop */
120
121.no_lines:
122 move.l %a0,%d1 /* %d1 = start address ... */
123 addq.l #2,%d1 /* ... +2, account for possible trailing word */
124
125 /* trailing longword loop */
126.loop_l2:
127 move.l %d0,-(%a1) /* store longword */
128 cmp.l %d1,%a1 /* runs %a1 down to first long bound */
129 bhi.b .loop_l2
130
131.no_longs:
132 cmp.l %a0,%a1 /* any words left? */
133 bls.b .end_w2 /* no: skip loop */
134
135 /* trailing word loop */
136.loop_w2:
137 move.w %d0,-(%a1) /* store word */
138 cmp.l %a0,%a1 /* runs %a1 down to start address */
139 bhi.b .loop_w2
140
141.end_w2:
142 move.l %a0,%d0 /* return start address */
143 rts
144
145.end:
146 .size memset16,.end-memset16
diff --git a/firmware/asm/m68k/strlen.S b/firmware/asm/m68k/strlen.S
new file mode 100644
index 0000000000..765969da04
--- /dev/null
+++ b/firmware/asm/m68k/strlen.S
@@ -0,0 +1,71 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2010 Nils Wallménius
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21
22/* size_t strlen(const char *str) */
23
24 .section .text,"ax",@progbits
25 .align 2
26 .globl strlen
27 .type strlen, @function
28
29strlen:
30 move.l 4(%sp), %a0 /* %a0 = *str */
31 move.l %a0, %a1 /* %a1 = start address */
32 move.l %a0, %d0
33 andi.l #3, %d0 /* %d0 = %a0 & 3 */
34 beq.b 1f /* already aligned */
35 jmp.l (-2,%pc,%d0.l*4)
36 tst.b (%a0)+
37 beq.b .done
38 tst.b (%a0)+
39 beq.b .done
40 tst.b (%a0)+
41 beq.b .done
42
43 1:
44 move.l (%a0)+, %d0 /* load %d0 increment %a0 */
45 /* use trick to test the whole word for null bytes */
46 move.l %d0, %d1
47 subi.l #0x01010101, %d1
48 not.l %d0
49 and.l %d1, %d0
50 andi.l #0x80808080, %d0
51 beq.b 1b /* if the test was false repeat */
52
53 /* ok, so the last word contained a 0 byte, test individual bytes */
54 subq.l #4, %a0
55 tst.b (%a0)+
56 beq.b .done
57 tst.b (%a0)+
58 beq.b .done
59 tst.b (%a0)+
60 beq.b .done
61 /* last byte must be 0 so we don't need to load it, so we don't increment a0
62 so we jump past the subq instr */
63 .word 0x51fa /* trapf.w, shadow next instr */
64
65.done:
66 subq.l #1, %a0 /* %a0 is 1 too large due to the last increment */
67 sub.l %a1, %a0 /* how many times did we repeat? */
68 move.l %a0, %d0 /* return value in %d0 */
69 rts
70 .size strlen, .-strlen
71
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-23 12:44:10 +0000