#include <avr/io.h>
#include <avr/pgmspace.h>
#include "misc.h"
#include "real_fft.h"
static __inline__ int32_t fmuls16(int16_t mulcd, int16_t muler);
static __inline__ int16_t magnitude(int16_t q, int16_t i);
#if SEQ_NUM == 64
uint16_t wd_tab[] PROGMEM =
{
0,81,324,727,1286,1995,2846,3833,4944,6168,7494,8909,10398,11946,13538,15159,
16792,18421,20029,21602,23122,24575,25947,27224,28394,29444,30364,31145,31779,32261,32584,32747,
32747,32584,32261,31779,31145,30364,29444,28394,27224,25947,24575,23122,21602,20029,18421,16792,
15159,13538,11946,10398,8909,7494,6168,4944,3833,2846,1995,1286,727,324,81,0,
};
uint8_t rsv_idx[] PROGMEM =
{
0,1,32,33,16,17,48,49,8,9,40,41,24,25,56,57,
4,5,36,37,20,21,52,53,12,13,44,45,28,29,60,61,
2,3,34,35,18,19,50,51,10,11,42,43,26,27,58,59,
6,7,38,39,22,23,54,55,14,15,46,47,30,31,62,63,
};
// exp(-i*2*pi*n/SEG_NUM) n = 0:SEG_NUM/2-1
int16_t cos_sin_tab[] PROGMEM =
{
32767,0,32610,3211,32138,6392,31357,9512,30273,12539,28898,15446,27245,18204,25329,20787,
23170,23170,20787,25329,18204,27245,15446,28898,12539,30273,9512,31357,6392,32138,3211,32610,
0,32767,-3211,32610,-6392,32138,-9512,31357,-12539,30273,-15446,28898,-18204,27245,-20787,25329,
-23170,23170,-25329,20787,-27245,18204,-28898,15446,-30273,12539,-31357,9512,-32138,6392,-32610,3211,
};
#endif
#if SEQ_NUM == 128
uint16_t wd_tab[] PROGMEM =
{
0,20,80,180,319,498,716,972,1266,1597,1964,2366,2803,3273,3775,4308,
4870,5461,6078,6720,7387,8075,8783,9510,10254,11013,11785,12569,13361,14161,14967,15776,
16586,17396,18204,19007,19803,20591,21369,22135,22887,23622,24340,25039,25716,26371,27001,27605,
28182,28729,29247,29733,30186,30606,30991,31340,31652,31928,32165,32363,32522,32642,32722,32762,
32762,32722,32642,32522,32363,32165,31928,31652,31340,30991,30606,30186,29733,29247,28729,28182,
27605,27001,26371,25716,25039,24340,23622,22887,22135,21369,20591,19803,19007,18204,17396,16586,
15776,14967,14161,13361,12569,11785,11013,10254,9510,8783,8075,7387,6720,6078,5461,4870,
4308,3775,3273,2803,2366,1964,1597,1266,972,716,498,319,180,80,20,0,
};
uint8_t rsv_idx[] PROGMEM =
{
0,1,64,65,32,33,96,97,16,17,80,81,48,49,112,113,
8,9,72,73,40,41,104,105,24,25,88,89,56,57,120,121,
4,5,68,69,36,37,100,101,20,21,84,85,52,53,116,117,
12,13,76,77,44,45,108,109,28,29,92,93,60,61,124,125,
2,3,66,67,34,35,98,99,18,19,82,83,50,51,114,115,
10,11,74,75,42,43,106,107,26,27,90,91,58,59,122,123,
6,7,70,71,38,39,102,103,22,23,86,87,54,55,118,119,
14,15,78,79,46,47,110,111,30,31,94,95,62,63,126,127,
};
int16_t cos_sin_tab[] PROGMEM =
{
32767,0,32728,1607,32610,3211,32413,4808,32138,6392,31785,7961,31357,9512,30852,11039,
30273,12539,29621,14010,28898,15446,28106,16846,27245,18204,26319,19519,25329,20787,24279,22005,
23170,23170,22005,24279,20787,25329,19519,26319,18204,27245,16846,28106,15446,28898,14010,29621,
12539,30273,11039,30852,9512,31357,7961,31785,6392,32138,4808,32413,3211,32610,1607,32728,
0,32767,-1607,32728,-3211,32610,-4808,32413,-6392,32138,-7961,31785,-9512,31357,-11039,30852,
-12539,30273,-14010,29621,-15446,28898,-16846,28106,-18204,27245,-19519,26319,-20787,25329,-22005,24279,
-23170,23170,-24279,22005,-25329,20787,-26319,19519,-27245,18204,-28106,16846,-28898,15446,-29621,14010,
-30273,12539,-30852,11039,-31357,9512,-31785,7961,-32138,6392,-32413,4808,-32610,3211,-32728,1607,
};
#endif
/* error in data type
#if SEQ_NUM == 256
uint16_t wd_tab[] PROGMEM =
{
0,4,19,44,79,124,178,243,317,401,494,598,710,833,965,1106,
1256,1416,1585,1762,1949,2144,2348,2561,2782,3011,3248,3493,3747,4007,4276,4551,
4834,5124,5420,5724,6034,6350,6672,7000,7334,7673,8017,8367,8721,9081,9444,9812,
10184,10559,10938,11321,11706,12094,12485,12878,13274,13671,14070,14470,14872,15274,15677,16081,
16484,16888,17291,17694,18096,18497,18897,19295,19691,20085,20478,20867,21254,21638,22019,22396,
22770,23140,23505,23867,24223,24575,24923,25264,25601,25932,26257,26576,26889,27196,27496,27789,
28075,28355,28626,28891,29148,29397,29638,29872,30097,30313,30522,30721,30912,31095,31268,31432,
31587,31733,31869,31996,32114,32222,32321,32409,32489,32558,32617,32667,32707,32736,32756,32766,
32766,32756,32736,32707,32667,32617,32558,32489,32409,32321,32222,32114,31996,31869,31733,31587,
31432,31268,31095,30912,30721,30522,30313,30097,29872,29638,29397,29148,28891,28626,28355,28075,
27789,27496,27196,26889,26576,26257,25932,25601,25264,24923,24575,24223,23867,23505,23140,22770,
22396,22019,21638,21254,20867,20478,20085,19691,19295,18897,18497,18096,17694,17291,16888,16484,
16081,15677,15274,14872,14470,14070,13671,13274,12878,12485,12094,11706,11321,10938,10559,10184,
9812,9444,9081,8721,8367,8017,7673,7334,7000,6672,6350,6034,5724,5420,5124,4834,
4551,4276,4007,3747,3493,3248,3011,2782,2561,2348,2144,1949,1762,1585,1416,1256,
1106,965,833,710,598,494,401,317,243,178,124,79,44,19,4,0,
};
uint8_t rsv_idx[] PROGMEM =
{
0,1,128,129,64,65,192,193,32,33,160,161,96,97,224,225,
16,17,144,145,80,81,208,209,48,49,176,177,112,113,240,241,
8,9,136,137,72,73,200,201,40,41,168,169,104,105,232,233,
24,25,152,153,88,89,216,217,56,57,184,185,120,121,248,249,
4,5,132,133,68,69,196,197,36,37,164,165,100,101,228,229,
20,21,148,149,84,85,212,213,52,53,180,181,116,117,244,245,
12,13,140,141,76,77,204,205,44,45,172,173,108,109,236,237,
28,29,156,157,92,93,220,221,60,61,188,189,124,125,252,253,
2,3,130,131,66,67,194,195,34,35,162,163,98,99,226,227,
18,19,146,147,82,83,210,211,50,51,178,179,114,115,242,243,
10,11,138,139,74,75,202,203,42,43,170,171,106,107,234,235,
26,27,154,155,90,91,218,219,58,59,186,187,122,123,250,251,
6,7,134,135,70,71,198,199,38,39,166,167,102,103,230,231,
22,23,150,151,86,87,214,215,54,55,182,183,118,119,246,247,
14,15,142,143,78,79,206,207,46,47,174,175,110,111,238,239,
30,31,158,159,94,95,222,223,62,63,190,191,126,127,254,255,
};
// exp(-i*2*pi*n/SEG_NUM) n = 0:SEG_NUM/2-1
int16_t cos_sin_tab[] PROGMEM =
{
32767,0,32758,804,32728,1607,32679,2410,32610,3211,32521,4011,32413,4808,32285,5602,
32138,6392,31971,7179,31785,7961,31581,8739,31357,9512,31114,10278,30852,11039,30572,11793,
30273,12539,29956,13278,29621,14010,29269,14732,28898,15446,28511,16151,28106,16846,27684,17530,
27245,18204,26790,18868,26319,19519,25832,20159,25329,20787,24812,21403,24279,22005,23732,22594,
23170,23170,22594,23732,22005,24279,21403,24812,20787,25329,20159,25832,19519,26319,18868,26790,
18204,27245,17530,27684,16846,28106,16151,28511,15446,28898,14732,29269,14010,29621,13278,29956,
12539,30273,11793,30572,11039,30852,10278,31114,9512,31357,8739,31581,7961,31785,7179,31971,
6392,32138,5602,32285,4808,32413,4011,32521,3211,32610,2410,32679,1607,32728,804,32758,
0,32767,-804,32758,-1607,32728,-2410,32679,-3211,32610,-4011,32521,-4808,32413,-5602,32285,
-6392,32138,-7179,31971,-7961,31785,-8739,31581,-9512,31357,-10278,31114,-11039,30852,-11793,30572,
-12539,30273,-13278,29956,-14010,29621,-14732,29269,-15446,28898,-16151,28511,-16846,28106,-17530,27684,
-18204,27245,-18868,26790,-19519,26319,-20159,25832,-20787,25329,-21403,24812,-22005,24279,-22594,23732,
-23170,23170,-23732,22594,-24279,22005,-24812,21403,-25329,20787,-25832,20159,-26319,19519,-26790,18868,
-27245,18204,-27684,17530,-28106,16846,-28511,16151,-28898,15446,-29269,14732,-29621,14010,-29956,13278,
-30273,12539,-30572,11793,-30852,11039,-31114,10278,-31357,9512,-31581,8739,-31785,7961,-31971,7179,
-32138,6392,-32285,5602,-32413,4808,-32521,4011,-32610,3211,-32679,2410,-32728,1607,-32758,804
};
#endif
#if SEQ_NUM == 512
uint16_t wd_tab[] PROGMEM =
{
0,1,4,11,19,30,44,60,79,100,123,149,178,208,242,277,
316,356,399,445,492,543,595,650,708,768,830,894,961,1030,1102,1175,
1251,1330,1411,1493,1579,1666,1756,1847,1942,2038,2136,2237,2339,2444,2551,2660,
2771,2884,3000,3117,3236,3357,3480,3605,3732,3861,3992,4125,4260,4396,4534,4674,
4816,4960,5105,5252,5401,5551,5703,5856,6012,6168,6327,6486,6648,6810,6975,7140,
7307,7476,7645,7816,7989,8162,8337,8513,8691,8869,9049,9229,9411,9594,9778,9963,
10149,10335,10523,10712,10901,11091,11282,11474,11667,11860,12054,12249,12444,12640,12836,13033,
13230,13428,13627,13825,14025,14224,14424,14624,14825,15025,15226,15427,15628,15830,16031,16232,
16434,16635,16837,17038,17239,17440,17641,17842,18043,18243,18443,18643,18842,19041,19240,19438,
19635,19833,20029,20225,20421,20616,20810,21004,21197,21389,21580,21771,21961,22150,22338,22525,
22711,22897,23081,23265,23447,23628,23808,23987,24165,24342,24517,24692,24865,25036,25207,25376,
25543,25710,25875,26038,26200,26361,26520,26677,26833,26988,27140,27292,27441,27589,27735,27879,
28022,28163,28302,28439,28575,28709,28840,28970,29098,29224,29349,29471,29591,29709,29825,29939,
30052,30162,30270,30375,30479,30581,30680,30778,30873,30966,31056,31145,31231,31315,31397,31477,
31554,31629,31701,31772,31840,31905,31969,32030,32088,32144,32198,32250,32299,32345,32390,32431,
32471,32508,32542,32574,32604,32631,32656,32678,32698,32715,32730,32742,32752,32760,32765,32767,
32767,32765,32760,32752,32742,32730,32715,32698,32678,32656,32631,32604,32574,32542,32508,32471,
32431,32390,32345,32299,32250,32198,32144,32088,32030,31969,31905,31840,31772,31701,31629,31554,
31477,31397,31315,31231,31145,31056,30966,30873,30778,30680,30581,30479,30375,30270,30162,30052,
29939,29825,29709,29591,29471,29349,29224,29098,28970,28840,28709,28575,28439,28302,28163,28022,
27879,27735,27589,27441,27292,27140,26988,26833,26677,26520,26361,26200,26038,25875,25710,25543,
25376,25207,25036,24865,24692,24517,24342,24165,23987,23808,23628,23447,23265,23081,22897,22711,
22525,22338,22150,21961,21771,21580,21389,21197,21004,20810,20616,20421,20225,20029,19833,19635,
19438,19240,19041,18842,18643,18443,18243,18043,17842,17641,17440,17239,17038,16837,16635,16434,
16232,16031,15830,15628,15427,15226,15025,14825,14624,14424,14224,14025,13825,13627,13428,13230,
13033,12836,12640,12444,12249,12054,11860,11667,11474,11282,11091,10901,10712,10523,10335,10149,
9963,9778,9594,9411,9229,9049,8869,8691,8513,8337,8162,7989,7816,7645,7476,7307,
7140,6975,6810,6648,6486,6327,6168,6012,5856,5703,5551,5401,5252,5105,4960,4816,
4674,4534,4396,4260,4125,3992,3861,3732,3605,3480,3357,3236,3117,3000,2884,2771,
2660,2551,2444,2339,2237,2136,2038,1942,1847,1756,1666,1579,1493,1411,1330,1251,
1175,1102,1030,961,894,830,768,708,650,595,543,492,445,399,356,316,
277,242,208,178,149,123,100,79,60,44,30,19,11,4,1,0,
};
uint16_t rsv_idx[] PROGMEM =
{
0,1,256,257,128,129,384,385,64,65,320,321,192,193,448,449,
32,33,288,289,160,161,416,417,96,97,352,353,224,225,480,481,
16,17,272,273,144,145,400,401,80,81,336,337,208,209,464,465,
48,49,304,305,176,177,432,433,112,113,368,369,240,241,496,497,
8,9,264,265,136,137,392,393,72,73,328,329,200,201,456,457,
40,41,296,297,168,169,424,425,104,105,360,361,232,233,488,489,
24,25,280,281,152,153,408,409,88,89,344,345,216,217,472,473,
56,57,312,313,184,185,440,441,120,121,376,377,248,249,504,505,
4,5,260,261,132,133,388,389,68,69,324,325,196,197,452,453,
36,37,292,293,164,165,420,421,100,101,356,357,228,229,484,485,
20,21,276,277,148,149,404,405,84,85,340,341,212,213,468,469,
52,53,308,309,180,181,436,437,116,117,372,373,244,245,500,501,
12,13,268,269,140,141,396,397,76,77,332,333,204,205,460,461,
44,45,300,301,172,173,428,429,108,109,364,365,236,237,492,493,
28,29,284,285,156,157,412,413,92,93,348,349,220,221,476,477,
60,61,316,317,188,189,444,445,124,125,380,381,252,253,508,509,
2,3,258,259,130,131,386,387,66,67,322,323,194,195,450,451,
34,35,290,291,162,163,418,419,98,99,354,355,226,227,482,483,
18,19,274,275,146,147,402,403,82,83,338,339,210,211,466,467,
50,51,306,307,178,179,434,435,114,115,370,371,242,243,498,499,
10,11,266,267,138,139,394,395,74,75,330,331,202,203,458,459,
42,43,298,299,170,171,426,427,106,107,362,363,234,235,490,491,
26,27,282,283,154,155,410,411,90,91,346,347,218,219,474,475,
58,59,314,315,186,187,442,443,122,123,378,379,250,251,506,507,
6,7,262,263,134,135,390,391,70,71,326,327,198,199,454,455,
38,39,294,295,166,167,422,423,102,103,358,359,230,231,486,487,
22,23,278,279,150,151,406,407,86,87,342,343,214,215,470,471,
54,55,310,311,182,183,438,439,118,119,374,375,246,247,502,503,
14,15,270,271,142,143,398,399,78,79,334,335,206,207,462,463,
46,47,302,303,174,175,430,431,110,111,366,367,238,239,494,495,
30,31,286,287,158,159,414,415,94,95,350,351,222,223,478,479,
62,63,318,319,190,191,446,447,126,127,382,383,254,255,510,511
};
// exp(-i*2*pi*n/SEG_NUM) n = 0:SEG_NUM/2-1
int16_t cos_sin_tab[] PROGMEM =
{
32767,0,32765,402,32758,804,32745,1206,32728,1607,32706,2009,32679,2410,32647,2811,
32610,3211,32568,3611,32521,4011,32469,4409,32413,4808,32351,5205,32285,5602,32214,5997,
32138,6392,32057,6786,31971,7179,31881,7571,31785,7961,31685,8351,31581,8739,31471,9126,
31357,9512,31237,9896,31114,10278,30985,10659,30852,11039,30714,11416,30572,11793,30425,12167,
30273,12539,30117,12910,29956,13278,29791,13645,29621,14010,29447,14372,29269,14732,29086,15090,
28898,15446,28707,15800,28511,16151,28310,16499,28106,16846,27897,17189,27684,17530,27466,17869,
27245,18204,27020,18537,26790,18868,26557,19195,26319,19519,26077,19841,25832,20159,25583,20475,
25329,20787,25072,21097,24812,21403,24547,21706,24279,22005,24007,22301,23732,22594,23453,22884,
23170,23170,22884,23453,22594,23732,22301,24007,22005,24279,21706,24547,21403,24812,21097,25072,
20787,25329,20475,25583,20159,25832,19841,26077,19519,26319,19195,26557,18868,26790,18537,27020,
18204,27245,17869,27466,17530,27684,17189,27897,16846,28106,16499,28310,16151,28511,15800,28707,
15446,28898,15090,29086,14732,29269,14372,29447,14010,29621,13645,29791,13278,29956,12910,30117,
12539,30273,12167,30425,11793,30572,11416,30714,11039,30852,10659,30985,10278,31114,9896,31237,
9512,31357,9126,31471,8739,31581,8351,31685,7961,31785,7571,31881,7179,31971,6786,32057,
6392,32138,5997,32214,5602,32285,5205,32351,4808,32413,4409,32469,4011,32521,3611,32568,
3211,32610,2811,32647,2410,32679,2009,32706,1607,32728,1206,32745,804,32758,402,32765,
0,32767,-402,32765,-804,32758,-1206,32745,-1607,32728,-2009,32706,-2410,32679,-2811,32647,
-3211,32610,-3611,32568,-4011,32521,-4409,32469,-4808,32413,-5205,32351,-5602,32285,-5997,32214,
-6392,32138,-6786,32057,-7179,31971,-7571,31881,-7961,31785,-8351,31685,-8739,31581,-9126,31471,
-9512,31357,-9896,31237,-10278,31114,-10659,30985,-11039,30852,-11416,30714,-11793,30572,-12167,30425,
-12539,30273,-12910,30117,-13278,29956,-13645,29791,-14010,29621,-14372,29447,-14732,29269,-15090,29086,
-15446,28898,-15800,28707,-16151,28511,-16499,28310,-16846,28106,-17189,27897,-17530,27684,-17869,27466,
-18204,27245,-18537,27020,-18868,26790,-19195,26557,-19519,26319,-19841,26077,-20159,25832,-20475,25583,
-20787,25329,-21097,25072,-21403,24812,-21706,24547,-22005,24279,-22301,24007,-22594,23732,-22884,23453,
-23170,23170,-23453,22884,-23732,22594,-24007,22301,-24279,22005,-24547,21706,-24812,21403,-25072,21097,
-25329,20787,-25583,20475,-25832,20159,-26077,19841,-26319,19519,-26557,19195,-26790,18868,-27020,18537,
-27245,18204,-27466,17869,-27684,17530,-27897,17189,-28106,16846,-28310,16499,-28511,16151,-28707,15800,
-28898,15446,-29086,15090,-29269,14732,-29447,14372,-29621,14010,-29791,13645,-29956,13278,-30117,12910,
-30273,12539,-30425,12167,-30572,11793,-30714,11416,-30852,11039,-30985,10659,-31114,10278,-31237,9896,
-31357,9512,-31471,9126,-31581,8739,-31685,8351,-31785,7961,-31881,7571,-31971,7179,-32057,6786,
-32138,6392,-32214,5997,-32285,5602,-32351,5205,-32413,4808,-32469,4409,-32521,4011,-32568,3611,
-32610,3211,-32647,2811,-32679,2410,-32706,2009,-32728,1607,-32745,1206,-32758,804,-32765,402,
};
#endif
*/
int16_t seq_src[SEQ_NUM] =
{
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,-255,
};
void real_fft(void)
{
int16_t seq_comp[SEQ_NUM];
int16_t *ip;
void *pp;
// Add Window
{
int16_t cnt;
ip = seq_src;
pp = (void *)wd_tab;
for (cnt = 0; cnt != SEQ_NUM; cnt++)
{
int16_t it;
it = pgm_read_word(pp);
pp += 2;
*ip = fmuls16(*ip,it) >> 16;
ip++;
}
// Reverse
pp = (void *)rsv_idx;
ip = seq_comp;
for (cnt = 0; cnt != SEQ_NUM; cnt++)
{
uint8_t bt;
bt = pgm_read_byte(pp++);
*ip++ = seq_src[bt];
}
}
// N/2 FFT
{
uint8_t n;
uint8_t block,half;
int16_t *ip2;
block = SEQ_NUM / 4;
half = 1;
for (n = 0; n!= FFT_L; n++)
{
uint8_t bn,pn;
ip = seq_comp;
ip2 = seq_comp + (half << 1);
for (bn = 0; bn != block; bn++)
{
pp = (void *)cos_sin_tab;
for (pn = 0; pn != half; pn++)
{
int16_t br,bi,ar,ai,tmp;
int16_t c,s;
// mult seg
c = pgm_read_byte(pp);
pp++;
c |= pgm_read_byte(pp) << 8;
pp++;
s = pgm_read_byte(pp);
pp++;
s |= pgm_read_byte(pp) << 8;
pp -= 3;
br = *ip2;
//ip2++;
bi = *(ip2+1);
//ip2--;
tmp = (fmuls16(br,c) + fmuls16(bi,s)) >> 16; // may overflow
bi = (fmuls16(bi,c) - fmuls16(br,s)) >> 16;
ar = *ip;
*ip = (ar + tmp) >> 1; // /2 to forestall overflow
ip++;
ai = *ip;
*ip = (ai + bi) >> 1; // /2 to forestall overflow
ip++;
*ip2 = (ar - tmp) >> 1;
ip2++;
*ip2 = (ai - bi) >> 1;
ip2++;
pp += (block << 3); // 8 = (N/2)*2bytes * 2(cos+sin)
}
ip += (half << 1);
ip2 += (half << 1);
}
half <<= 1;
block >>= 1;
}
}
// recombine
{
uint16_t n;
int16_t *ip2 = &seq_comp[SEQ_NUM-1];
int16_t *p_sp = seq_src;
int16_t er,ei, or,oi;
int16_t c, s;
pp = (void *)cos_sin_tab;
ip = seq_comp;
c = *ip;
s = *(ip + 1);
for(n = 0; n != SEQ_NUM/2; n++)
{
// s = -s; // conjugate
er = *ip + c;
ei = *(ip+1) - s;
or = s + *(ip+1);
oi = c - *ip;
ip += 2;
c = pgm_read_word(pp);
pp += 2;
s = pgm_read_word(pp);
pp += 2;
*p_sp++ = magnitude(er + ((fmuls16(or,c) + fmuls16(oi,s)) >> 16),
ei + ((fmuls16(oi,c) - fmuls16(or,s)) >> 16));
c = *(ip2 - 1);
s = *ip2;
ip2 -= 2;
}
}
}
static __inline__ int32_t fmuls16(int16_t mulcd, int16_t muler)
{
int32_t result;
int8_t zero;
zero = 0;
asm volatile (
"fmuls %B1, %B2" "\n\t"
"movw %C0, r0" "\n\t"
"fmul %A1, %A2" "\n\t"
"adc %C0, %3" "\n\t"
"movw %A0, r0" "\n\t"
"fmulsu %B1, %A2" "\n\t"
"sbc %D0, %3" "\n\t"
"add %B0, r0" "\n\t"
"adc %C0, r1" "\n\t"
"adc %D0, %3" "\n\t"
"fmulsu %B2, %A1" "\n\t"
"sbc %D0, %3" "\n\t"
"add %B0, r0" "\n\t"
"adc %C0, r1" "\n\t"
"adc %D0, %3" "\n\t"
"clr __zero_reg__" "\n\t"
: "=&r"(result)
: "a"(mulcd), "a"(muler), "r"(zero)
: "r0", "r1"
);
return result;
} |