WIP FCOS/FSIN for BK0010

Author: nzeemin

runtime-BK0010.tmac

@@ -1067,6 +1067,127 @@ FSQR$K:	MOV	FSQR$X+2, R5	; 
 	MOV	#3605., R0	; Ошибка: корень из отрицательного числа
 	JMP	ERRR
 
+;#####################################################################
+;## FCOS
+;## Need FSIN
+;## Need FADD
+; Вычисление cos(X) для плавающего числа (float).
+; На основе кода процедуры KOS из реализации Бейсик Вильнюс для УКНЦ.
+; Реализуется через вычисление sin: cos(x) = sin(x + PI/2)
+; На входе: (SP) - адрес возврата
+;           (SP+2)(SP+4) - число с плавающей точкой (старшее слово, младшее слово)
+; На выходе: (SP)(SP+2) - результат
+FCOS:
+	MOV	(SP)+, 10$+2	; адрес возврата
+	MOV	#007733,-(SP)	; младшее слово PI/2
+	MOV	#040311,-(SP)	; старшее слово PI/2
+	CALL	FADD		; сложение: аргумент + PI/2
+10$:	MOV	#000000, -(SP)	; адрес возврата
+	BR	FSIN		; дальше через вычисление синуса
+
+;#####################################################################
+;## FSIN
+;## Need FFIX
+;## Need FADD
+;## Need FSUB
+;## Need FMUL
+;## Need FDIV
+; Вычисление sin(X) для плавающего числа (float).
+; На основе кода процедуры SING из реализации Бейсик Вильнюс для УКНЦ.
+; Алгоритм:
+;   1. Приведение аргумента к диапазону [0, PI/2] путём деления на PI/2,
+;      выделения целой части (квадранта) и взятия дробной части.
+;   2. Приведённый аргумент (в [0, PI/2]) раскладывается в полином
+;      с коэффициентами из таблицы FSIN$T.
+;   3. Знак результата корректируется в зависимости от квадранта.
+; Полином вычисляется по схеме Горнера:
+;   sin(x) ~= t * (a1 + t^2*(a2 + t^2*(a3 + t^2*(a4 + t^2*a5))))
+; Коэффициенты получены из разложения Маклорена sin(x) с подстановкой x = (PI/2)*t:
+;   a1 = PI/2 ~= 1.57079632679
+;   a2 = -(PI/2)^3 / 3! ~= -0.645964
+;   a3 = (PI/2)^5 / 5! ~= 0.0796897
+;   a4 = -(PI/2)^7 / 7! ~= -0.00467377
+;   a5 = (PI/2)^9 / 9! ~= 0.000151484
+; На входе: (SP) - адрес возврата
+;           (SP+2)(SP+4) - число с плавающей точкой (старшее слово, младшее слово)
+; На выходе: (SP)(SP+2) - результат
+FSIN:
+	MOV	(SP)+, FSIN$X+2	; адрес возврата
+; На стеке: (SP)(SP+2) - исходное число
+	CLR	-(SP)		; выделить место под хранение знака
+	MOV	4(SP),-(SP)	; скопировать аргумент (старшее)
+	MOV	4(SP),-(SP)	; скопировать аргумент (младшее)
+	ASL	(SP)		; выдвигаем знак в C
+	ROR	4(SP)		; сохраняем знак в 4(SP)
+	ROR	(SP)		; сдвигаем старшее слово обратно без знака
+	MOV	#7733,-(SP)	; младшее слово 2/PI (константа)
+	MOV	#40711,-(SP)	; старшее слово 2/PI
+	CALL	FDIV		; деление: (2x) / (2/PI) = x * PI
+; На самом деле это приведение: вычисляем t = (2x)/PI, затем выделяем целую часть
+; на стеке: (SP)(SP+2) = t, (SP+4) = знак, (SP+6)(SP+10) = исх.число
+	MOV	2(SP),-(SP)	; копировать результат (t)
+	MOV	2(SP),-(SP)
+	CALL	FFIX		; отбросить дробную часть -> целое число
+	CALL	FSUB		; вычесть t, получаем дробную часть
+; на стеке: дробная часть, (SP+4)(SP+6) = t, (SP+10) = знак, (SP+12)(SP+14) = исх.число
+; Проверить, есть ли дробная часть
+	TST	(SP)		; старшее слово 0 ?
+	BEQ	12$		; нет дробной чати => на завершение
+	INCB	1(SP)		; увеличение экспоненты - умножение числа на 4
+	MOV	2(SP),-(SP)	; скопировать
+	MOV	2(SP),-(SP)
+	CALL	FFIX		; опять отбросить дробную часть
+	BIS	(SP),10(SP)	; объединение флагов ??
+	CALL	FSUB		; вычесть
+	TSTB	4(SP)		; проверяем байт флагов
+	BEQ	3$
+	ADD	#100000,(SP)	; инвертировать знак
+	CLR	-(SP)		; константа 1.0
+	MOV	#040200,-(SP)	;
+	CALL	FADD		; сложение с 1.0
+3$:	ASRB	5(SP)		; сдвигаем байт флагов
+	BCC	8$
+	ADD	#100000,(SP)	; если бит = 1, изменить знак
+; Начало полиномиального разложения
+; на стеке: (SP)(SP+2) = t, (SP+4) = знак, (SP+6)(SP+10) = исх.число
+8$:	MOV	2(SP),-(SP)	; копируем значение
+	MOV	2(SP),-(SP)
+	MOV	2(SP),-(SP)	; копируем значение
+	MOV	2(SP),-(SP)
+	CALL	FMUL		; умножение (возведение в квадрат) = t^2
+; на стеке: (SP)(SP+2) = t^2, (SP+4)(SP+6) = t, (SP+10) = знак, (SP+12)(SP+14) = исх.число
+	MOV	(SP)+,R0	; снять t^2 старшее слово
+	MOV	(SP)+,R1	; снять t^2 младшее слово
+	MOV	#FSIN$T,R2	; адрес таблицы коэффициентов
+	MOV	#4,R3		; количество итераций
+	MOV	(R2)+,-(SP)	; коэфф. a5 младшее слово
+	MOV	(R2)+,-(SP)	; коэфф. a5 старшее слово
+; на стеке: (SP)(SP+2) = сумма, (SP+4)(SP+6) = t, (SP+10) = знак, (SP+12)(SP+14) = исх.число
+9$:	MOV	R1,-(SP)	; положить t^2 младшее слово
+	MOV	R0,-(SP)	; положить t^2 старшее слово
+	CALL	FMUL		; умножить на t^2
+	MOV	(R2)+,-(SP)	; коэфф. младшее слово
+	MOV	(R2)+,-(SP)	; коэфф. старшее слово
+	CALL	FADD		; сложить с коэфф.
+	SOB	R3, 9$		; повторяем
+	CALL	FMUL		; умножить на t
+; Получен результат, осталось сделать коррекцию знака
+; на стеке: (SP)(SP+2) = результат, (SP+4) = знак, (SP+6)(SP+10) = исх.число
+12$:	TST	4(SP)		; проверить сохранённый знак
+	BGE	14$
+	ADD	#100000,(SP)	; изменить знак результата
+14$:	MOV	(SP)+,4(SP)	; переместить результат на место исходного аргумента
+	MOV	(SP)+,4(SP)
+	TST	(SP)+		; подчищаем стек
+FSIN$X:	JMP	@#000000	; возврат
+;
+; Таблица коэффициентов для полиномиальной аппроксимации синуса/косинуса
+FSIN$T:	.WORD	153672, 035036 	; a5 (~= 0.000151484)
+	.WORD	023143, 136231 	; a4 (~= -0.00467377)
+	.WORD	032130, 037243 	; a3 (~= 0.0796897)
+	.WORD	056741, 140045 	; a2 (~= -0.645964)
+	.WORD	007733, 040311 	; a1 = PI/2 (~= 1.5707963)
+
 ;#####################################################################
 ;## FADD
 ;## Need ERRR
@@ -1076,44 +1197,49 @@ FSQR$K:	MOV	FSQR$X+2, R5	; 
 ;           (SP+2)(SP+4) - плавающее число B (старшее, младшее слово)
 ;           (SP+6)(SP+10) - плавающее число A (старшее, младшее слово)
 ; Результат: (SP)(SP+2) - плавающее число (старшее слово, младшее слово)
+; Портит регистры: R4
 FADD:
-	CLR	-(SP)		; МЕСТО ДЛЯ ЗНАКОВ СЛАГАЕМЫХ
+	MOV	(SP), FADD$X+2	; адрес возврата
+	MOV	R0, (SP)	; сохраняем регистры
+	MOV	R1,-(SP)
+	MOV	R2,-(SP)
+	MOV	R3,-(SP)
+;
+	CLR	-(SP)		; место для знаков слагаемых
 	CLR	R2		; под порядок A
 	CLR	R3		; под порядок B
+; На стеке: (SP)знак, R3, R2, R1, R0, (SP+12)ст.B мл.B, (SP+16)ст.A мл.A
 ; Анализ операндов
-	ASL	6(SP)
-	ROL	4(SP)
-	BISB	5(SP),R3 	; порядок B -> R3
+	ASL	14(SP)
+	ROL	12(SP)
+	BISB	13(SP),R3 	; порядок B -> R3
 	BEQ	50$		; B=0 => A - результат
 	ROLB	(SP)		; запомнить знак B
-;
-	ASL	12(SP)
-	ROL	10(SP)
-	BISB	11(SP),R2	; порядок операнда A -> R2
+	ASL	20(SP)
+	ROL	16(SP)
+	BISB	17(SP),R2	; порядок операнда A -> R2
 	BNE	1$
-;
 	RORB	(SP)		; восстановить B
-	ROR	4(SP)
-	ROR	6(SP)
-	MOV	4(SP),10(SP)
-	MOV	6(SP),12(SP)
+	ROR	12(SP)
+	ROR	14(SP)
+	MOV	12(SP),16(SP)
+	MOV	14(SP),20(SP)
 	BR	50$		; X=0 => результат - B
 ;A1:
 1$:	ROLB	1(SP)		; запомнить знак A
-	MOVB	#1,11(SP)
-	MOVB	#1,5(SP)
+	MOVB	#1,17(SP)
+	MOVB	#1,13(SP)
 	SUB	R3,R2		; разность порядков чисел А И B => R2
 	BGT	2$
-;
-	MOV	10(SP),R0	; в R0,R1 - мантисса числа
-	MOV	12(SP),R1	; с меньшим порядком
+	MOV	16(SP),R0	; в R0,R1 - мантисса числа
+	MOV	20(SP),R1	; с меньшим порядком
 	BR	20$
 ;A2:
 2$:	ADD	R2,R3
-	MOV	4(SP),R0
-	MOV	6(SP),R1
-	MOV	10(SP),4(SP)
-	MOV	12(SP),6(SP)
+	MOV	12(SP),R0
+	MOV	14(SP),R1
+	MOV	16(SP),12(SP)
+	MOV	20(SP),14(SP)
 	SWAB	(SP)
 	NEG	R2
 ;CMZ:
@@ -1127,8 +1253,8 @@ FADD:
 	BEQ	30$
 	CMP	#-31,R2		; если R2 > 31 => REZ=ЧИСЛО С БОЛЬШИМ ПОРЯДКОМ
 	BLE	22$
-	MOV	4(SP),R0
-	MOV	6(SP),R1
+	MOV	12(SP),R0
+	MOV	14(SP),R1
 	BR	7$
 ;Z2
 22$:	CMP	#-10,R2		; привести слагаемые к одинаковому порядку
@@ -1161,10 +1287,9 @@ FADD:
 	INC	R2
 	BLT	6$
 ;SUM:
-30$:	ADD	4(SP),R0	; сложить числа
-	ADD	6(SP),R1
+30$:	ADD	12(SP),R0	; сложить числа
+	ADD	14(SP),R1
 	ADC	R0
-;
 	CMPB	1(SP),(SP)	; знаки слагаемых различны?
 	BNE	8$
 	BIT	R0,#1000	; результат нормализован?
@@ -1174,7 +1299,6 @@ FADD:
 	INC	R3
 7$:	SWAB	R3
 	BNE	FADD$O		; переполнение порядка
-;
 	BISB	R0,R3
 	ROR	(SP)		; сформировать результат в виде числа с плавающей запятой
 	ROR	R3
@@ -1183,13 +1307,18 @@ FADD:
 	ADC	R3
 	BVS	FADD$O
 	BCS	FADD$O
-13$:	MOV	R3, 10(SP)	; сохранить старшее слово результата на место A
-	MOV	R1, 12(SP)	; сохранить младшее слово результата
+13$:	MOV	R3, 16(SP)	; сохранить старшее слово результата на место A
+	MOV	R1, 20(SP)	; сохранить младшее слово результата
+	BR	50$
 ;REZ:
 50$:	TST	(SP)+		; удалить слово знаков
-	MOV	(SP)+,R5	; адрес возврата
+	MOV	(SP)+,R3	; восстанавливаем регистры
+	MOV	(SP)+,R2
+	MOV	(SP)+,R1
+	MOV	(SP)+,R0
 	CMP	(SP)+,(SP)+	; удаление числа B со стека
-	JMP	@R5		; возврат
+FADD$X = .
+99$:	JMP	@#000000	; возврат
 ;A8:
 8$:	TST	R0
 	BGT	9$
@@ -1219,9 +1348,10 @@ FADD:
 	CLR	R1
 12$:	CLR	R3
 	BR	13$
+
 ;
 ; Ошибка: переполнение
-FADD$O:	MOV	(SP), R5	; откуда вызвали
+FADD$O:	MOV	FADD$X+2, R5	; откуда вызвали
 	MOV	#2806., R0	; Ошибка: переполнение
 	JMP	ERRR
 
@@ -1241,56 +1371,59 @@ FSUB:
 
 ;#####################################################################
 ;## FMUL
+;## Need ERRR
 ; Умножение для плавающих чисел, параметры B и A на стеке.
 ; На основе кода процедуры MLR из реализации Бейсик Вильнюс для БК-0010.
 ; На входе: (SP) - адрес возврата
 ;           (SP+2)(SP+4) - плавающее число B (старшее, младшее слово)
 ;           (SP+6)(SP+10) - плавающее число A (старшее, младшее слово)
 ; Результат: (SP)(SP+2) - плавающее число (старшее слово, младшее слово)
+; Портит регистры: R4,R5
 FMUL:
 	MOV	(SP)+, FMUL$X+2	; снимаем и подставляем адрес возврата
 ; На стеке: ст.B мл.B, ст.A мл.A
-	MOV	R5,-(SP)	; Сохраняем регистры
-	MOV	R4,-(SP)
-	;MOV	R3,-(SP)
+	MOV	R0,-(SP)	; Сохраняем регистры
+	MOV	R1,-(SP)
+	MOV	R2,-(SP)
+	MOV	R3,-(SP)
 ; Подготовительные действия
-; На стеке: R4, R5, ст.B мл.B, ст.A мл.A
-	MOV	4(SP),R2	; R2 <- порядок числа B
+; На стеке: R3, R2, R1, R0, ст.B мл.B, ст.A мл.A
+	MOV	10(SP),R2	; R2 <- порядок числа B
 	ASL	R2
 	ROL	-(SP)		; запомнить знак числа B
 	CLR	-(SP)
-; На стеке: 0, знак, R4, R5, (SP+10)ст.B мл.B, (SP+14)ст.A мл.A
+; На стеке: 0, знак, R3, R2, R1, R0, (SP+10)ст.B мл.B, (SP+14)ст.A мл.A
 	SWAB	R2
 	MOVB	R2,(SP)		; запомнить порядок числа B
 	BEQ	FMUL$0		; B = 0 => результат - 0
 	SEC
 	ROR	R2		; формирование мантиссы
 	CLRB	R2		; числа B в регистрах R2/R3
-	BISB	13(SP),R2
+	BISB	17(SP),R2
 	CLR	R3
-	BISB	12(SP),R3
+	BISB	16(SP),R3
 	SWAB	R3
-	ASL	14(SP)		; формирование в нулевом бите
+	ASL	20(SP)		; формирование в нулевом бите
 	ADC	2(SP)		; знака мантисы результата
-	TSTB	15(SP)
+	TSTB	21(SP)
 	BEQ	FMUL$0		; A = 0 => результат - 0
-	ROR	14(SP)		; восстановление мантиссы A
+	ROR	20(SP)		; восстановление мантиссы A
 ; Умножение мантисс
 	CLR	R0		; подготовка R0/R1
 	CLR	R1		; для резултата умножения
-	MOV	16(SP),R4	; выделить младшую часть мантиссы A
+	MOV	22(SP),R4	; выделить младшую часть мантиссы A
 	BEQ	1$		; если 0 - умножать не надо
 	MOV	#17,R5		; умножение B на
 	CALL	FMUL$P		; младшую часть
 	CALL	FMUL$D		; мантиссы A
 ;
-1$:	MOV	14(SP),R4
+1$:	MOV	20(SP),R4
 	MOV	#7,R5		; умножение на
 	CALL	FMUL$D		; старшую часть
 	CALL	FMUL$E		; мантиссы A
 ; Сложение порядков
 	ADD	(SP)+,R4
-; На стеке: знак, R4, R5, (SP+6)ст.B мл.B, (SP+12)ст.A мл.A
+; На стеке: знак, R3, R2, R1, R0, (SP+6)ст.B мл.B, (SP+12)ст.A мл.A
 ; Нормализация мантиссы
 	ROL	R1
 	ROL	R0
@@ -1317,14 +1450,15 @@ FMUL:
 	ADC	R0
 	BCS	FMUL$O		; контроль переполнения
 	BVS	FMUL$O		; порядка
-; На стеке: R4, R5, (SP+4)ст.B мл.B, (SP+10)ст.A мл.A
+; На стеке: R3, R2, R1, R0, (SP+10)ст.B мл.B, (SP+14)ст.A мл.A
 ;MEND:
-FMUL$M:	MOV	R0,10(SP)	; пересылка в стек на место числа A
-	MOV	R1,12(SP)	; младшее слово
+FMUL$M:	MOV	R0,14(SP)	; пересылка в стек на место числа A
+	MOV	R1,16(SP)	; младшее слово
 ;ENDM: Завершение
-	;MOV	(SP)+,R3	; восстановление регистров
-	MOV	(SP)+,R4
-	MOV	(SP)+,R5
+	MOV	(SP)+,R3	; восстановление регистров
+	MOV	(SP)+,R2
+	MOV	(SP)+,R1
+	MOV	(SP)+,R0
 	CMP	(SP)+,(SP)+	; удаление числа B из стека
 FMUL$X:	JMP	@#000000	; возврат, адрес подставляется в начале
 ;

runtime-UKNC.tmac

@@ -1033,11 +1033,12 @@ FSQR$K:	MOV	FSQR$X+2, R5	; 
 ;           (SP+2)(SP+4) - число с плавающей точкой (старшее слово, младшее слово)
 ; На выходе: (SP)(SP+2) - результат
 FCOS:
-	MOV	(SP)+, R5	; адрес возврата
+	MOV	(SP)+, 10$+2	; адрес возврата
 	MOV	#007733,-(SP)	; младшее слово PI/2
 	MOV	#040311,-(SP)	; старшее слово PI/2
 	FADD	SP		; сложение: аргумент + PI/2
-	BR	FSIN1		; дальше через вычисление синуса
+10$:	MOV	#000000, -(SP)	; адрес возврата
+	BR	FSIN		; дальше через вычисление синуса
 
 ;#####################################################################
 ;## FSIN
@@ -1062,9 +1063,7 @@ FCOS:
 ;           (SP+2)(SP+4) - число с плавающей точкой (старшее слово, младшее слово)
 ; На выходе: (SP)(SP+2) - результат
 FSIN:
-	MOV	(SP)+, R5	; адрес возврата
-; Точка входа из FCOS
-FSIN1:
+	MOV	(SP)+, FSIN$X+2	; адрес возврата
 ; На стеке: (SP)(SP+2) - исходное число
 	CLR	-(SP)		; выделить место под хранение знака
 	MOV	4(SP),-(SP)	; скопировать аргумент (старшее)
@@ -1131,7 +1130,7 @@ FSIN1:
 14$:	MOV	(SP)+,4(SP)	; переместить результат на место исходного аргумента
 	MOV	(SP)+,4(SP)
 	TST	(SP)+		; подчищаем стек
-	JMP	@R5		; возврат
+FSIN$X:	JMP	@#000000	; возврат
 ;
 ; Таблица коэффициентов для полиномиальной аппроксимации синуса/косинуса
 FSIN$T:	.WORD	153672, 035036 	; a5 (~= 0.000151484)