.global float_fix16

float_fix16:
;	param:	in	float		r22:r25
;			ret	int64(48.16)r18:r25
;	var:	exp				r13
;			sign			r12.1

	push	r17
	push	r16
	push	r15
	push	r14
	push	r13
	push	r12
	
	cp		r22,	r1
	cpc		r23,	r1
	cpc		r24,	r1
	cpc		r25,	r1
	breq	_zero

	; Mantisse verschieben => r18:r20.(0-6)
	mov		r18,	r22
	mov		r19,	r23
	mov		r20,	r24
	ori		r20,	0x80

	; Exponent sichern => r13 (korrektur: -7, wg. Position mantisse B.24 auf E+8)
	mov		r13,	r25
	mov		r12,	r24
	lsl		r12
	rol		r13
	ldi		r16,	134
	sub		r13,	r16
	
	; Sign => r12.0
	eor		r12,	r12
	ldi		r16,	0x01
	sbrc	r25,	7
	or		r12,	r16
	
	eor		r21,	r21
	eor		r22,	r22
	eor		r23,	r23
	eor		r24,	r24
	eor		r25,	r25
	
	cp		r13,	r1
	breq	_l1
	brmi	_shr

; Shift left...
	
_shl:
	lsl		r18
	rol		r19
	rol		r20
	rol		r21
	rol		r22
	rol		r23
	rol		r24
	rol		r25
	dec		r13
	tst		r13
	brne	_shl

	rjmp	_l1

; Shift right...
_shr:
	ldi		r16,	-23
	cp		r13,	r16
	brlo	_zero
	
_shr2:
	lsr		r20
	ror		r19
	ror		r18
	inc		r13
	tst		r13
	brne	_shr2
	

_l1:
	sbrs	r12,	0
	rjmp	_exit
	
	com		r18
	com		r19
	com		r20
	com		r21
	com		r22
	com		r23
	com		r24
	com		r25
	ldi		r16,	0x01
	add		r18,	r16
	adc		r19,	r1
	adc		r20,	r1
	adc		r21,	r1
	adc		r22,	r1
	adc		r23,	r1
	adc		r24,	r1
	adc		r25,	r1
	
	rjmp	_exit
	
_zero:
	eor		r18,	r18
	eor		r19,	r19
	eor		r20,	r20
	eor		r21,	r21
	eor		r22,	r22
	eor		r23,	r23
	eor		r24,	r24
	eor		r25,	r25

_exit:
	pop	r12
	pop	r13
	pop	r14
	pop	r15
	pop	r16
	pop	r17
	ret


#if 0
fp4816_t fp4816_from_float(float value)
{
	IEEEFLOAT ieee = { value };
	int16_t exp = ieee.exponent - 127;
	int32_t mantisse = ieee.mantisse | 0x00800000;
	fp4816_t fp = (fp4816_t)(mantisse >> (15 - exp));
	
	if (ieee.sign)
		fp *= -1;
	
	return fp;
};
#endif