/*
* Target system-dependent initialization
*/
EXPORT ER knl_tkdev_initialize( void )
{
out_w(NVIC_BASE | NVIC_ICER, 0xffffffff); /* clear enable */
out_w(NVIC_BASE | NVIC_ICPR, 0xffffffff); /* clear pending*/
return E_OK;
}
/*
system termination: reset / system power off
reset 0 power off
-1 reboot
0xaa55 halt boot and power off
*/
EXPORT void sysExit(W reset)
{
DisCacheMMU();
/* after this point, delay such as waitUsec() spends more time than the number indicates // */
/* LED off */
cpuLED(0x00);
/* all interrupts disabled */
out_w(IT0_IDS0, ~0); /* CPU: all interrupts disabled */
out_w(IT0_IDS1, ~0);
out_w(IT0_IDS2, ~0);
out_w(IT3_IPI0_CLR, 0x0000003f);
out_w(IT3_IDS0, ~0); /* DSP: all interrupts disabled */
out_w(IT3_IDS1, ~0);
out_w(IT3_IDS2, ~0);
out_w(IT0_IPI3_CLR, 0x0000003f);
out_w(IT0_FID, 0x00000001); /* FIQ disabled */
out_w(IT0_LIIR, 0x0000000f); /* internal interrupt disabled */
/* power on controller initialization */
pmicInit();
/* USB power off */
usbPower(FALSE);
if (reset >= 0) powerOff();
/* reset start */
resetStart();
}
int do_irq(unsigned r0, unsigned r1, unsigned r2, unsigned r3)
{
int vec;
int intno;
int timerctrl;
vec = in_w(HW_ICOLL_VECTOR);
out_w(HW_ICOLL_VECTOR, 0);
intno = in_w(HW_ICOLL_STAT);
putchar('\n');
printk("CPSR(irq): %X\n", r0);
printk("CPSR(blx): %X\n", r1);
printk("irq no: %d\n", intno);
printk("vecisr: %x\n", vec);
timerctrl = in_w(HW_TIMROT_TIMCTRL1);
printk("timer-ctrl: %016b\n", timerctrl);
out_w(HW_TIMROT_TIMCTRL1, timerctrl);
out_w(HW_TIMROT_TIMCTRL1_CLR, 1<<15);
out_w(HW_ICOLL_LEVELACK, 0x1);
// out_w(HW_ICOLL_LEVELACK, 0x2);
// out_w(HW_ICOLL_LEVELACK, 0x4);
// out_w(HW_ICOLL_LEVELACK, 0x8);
}
static void dispi_set_yres(uint16_t yres)
{
#ifdef VGA_DEBUG
printf("vbe_set_yres: %04x\n", yres);
#endif
out_w(VBE_DISPI_IOPORT_INDEX, VBE_DISPI_INDEX_YRES);
out_w(VBE_DISPI_IOPORT_DATA, yres);
}
static void dispi_set_bpp(uint16_t bpp)
{
#ifdef VGA_DEBUG
printf("vbe_set_bpp: %02x\n", bpp);
#endif
out_w(VBE_DISPI_IOPORT_INDEX, VBE_DISPI_INDEX_BPP);
out_w(VBE_DISPI_IOPORT_DATA, bpp);
}
int do_fiq(int r0, int r1, int r2, int r3)
{
static int rand = 0;
int vec;
rand++;
void (*isr)();
isr = (void *)in_w(0x80000000);
printk("isr:%d\n", isr);
printk("lr:[%x]\n", r0);
vec = in_w(0x80000070);
printk("irq line: [%d]\n", vec);
out_w(0x80000010, 0x1);
printk("levelack!\n");
return 0;
}
ACPI_STATUS
AcpiOsWritePort (
ACPI_IO_ADDRESS Address,
UINT32 Value,
UINT32 Width)
{
switch (Width) {
case 8:
out_b((uint16_t)Address, (uint8_t)Value);
break;
case 16:
out_h((uint16_t)Address, (uint16_t)Value);
break;
case 32:
out_w((uint16_t)Address, (uint32_t)Value);
break;
default:
return (AE_BAD_PARAMETER);
}
return (AE_OK);
}
/*
* Memory block outputs (32 bit) cnt : number of words
*/
EXPORT void OutMemW(UW iob, W ix, UW *buf, W cnt)
{
UW port = iob + (ix * IOSTEP);
while (--cnt >= 0) {
out_w(port, *buf++);
}
}
/* contro CS line of SPI for PMIC communication */
LOCAL void pmicCSassert(BOOL cs)
{
waitNsec(200);
out_w(SPn_POL(SP0), SPIPol ^ (cs ? 0x0001 : 0x0000));
waitNsec(200);
return;
}
/* read PMIC register */
EXPORT W pmicRead(W reg)
{
W dat;
pmicCSassert(TRUE); /* CS assert */
out_w(SPn_FFCLR(SP0), ~0); /* status flag is cleared */
out_w(SPn_TX_DATA(SP0), (reg << 1) | 1); /* send register number */
out_w(SPn_CONTROL(SP0), 0x0009); /* send start */
pmicWait();
out_w(SPn_FFCLR(SP0), ~0); /* status flag is cleared */
out_w(SPn_CONTROL(SP0), 0x0005); /* start receive */
pmicWait();
dat = in_w(SPn_RX_DATA(SP0)); /* data received */
pmicCSassert(FALSE); /* CS de-assert */
return dat;
}
void sio_send_frame(unsigned char const *buf, int size)
{
while (size > 0) {
/* Wait until there is space in the FIFO */
while (in_w(PL011_STAT) & 0x00000020U) {
/* NOP */
}
out_w(PL011_IO, *buf);
++buf;
--size;
}
}
/* initialize SPI for PMIC communication */
LOCAL void pmicInit(void)
{
out_w(SPn_MODE(SP0), 0x2700); /* 8bit, CS0, Master, CPU mode */
out_w(SPn_TIECS(SP0), 0x000f); /* CS0: follow the specification by SPn_POL */
out_w(SPn_POL(SP0), SPIPol);
out_w(SPn_ENCLR(SP0), ~0); /* interrupt disable */
out_w(SPn_CONTROL(SP0), 0x0100); /* start reset */
waitUsec(10);
out_w(SPn_CONTROL(SP0), 0x0000); /* release reset */
out_w(SPn_CONTROL2(SP0), 0x0000);
return;
}
/*
I/O write
*/
EXPORT W writeIO(UW addr, UW data, W unit)
{
W n;
UW pa;
/* address misalignment is reported as error */
if (addr & (unit - 1)) return 0;
/* I/O address check & conversion to physical address */
n = chkIOAddr(addr, &pa, unit);
if (n < unit) return 0;
switch(unit) {
case 4: out_w(pa, data); break;
case 2: out_h(pa, data); break;
default: out_b(pa, data);
}
return unit;
}
/* write PMIC register */
EXPORT void pmicWrite(W reg, W dat)
{
pmicCSassert(TRUE); /* CS assert */
out_w(SPn_FFCLR(SP0), ~0); /* status flag is cleared */
out_w(SPn_TX_DATA(SP0), reg << 1); /* send register number */
out_w(SPn_CONTROL(SP0), 0x0009); /* send start */
pmicWait();
out_w(SPn_FFCLR(SP0), ~0); /* status flag is cleared */
out_w(SPn_TX_DATA(SP0), dat); /* send data */
out_w(SPn_CONTROL(SP0), 0x0009); /* send start */
pmicWait();
pmicCSassert(FALSE); /* CS de-assert */
return;
}
int main(int argc, char *argv[])
{
train_info_t train;
sm_info_t sm;
data_info_t data; //dtr: datatrain, dtt: datatest
classify_t clssfy;
char out[MAX_BUF];
parse_command_line(argc, argv, &train, &sm, &data, &clssfy, out);
int train_numcase = train.mininumcase * train.nummix;
double *H = (double*) malloc(train_numcase * sm.numhid * sizeof(double));
double *init = (double *) malloc(sm.numhid * sizeof(double));
int *V = (int*) malloc(train_numcase * sm.numvis * sizeof(int));
int *order = malloc(sizeof(int) * (train_numcase > data.numcase ? train_numcase : data.numcase));
if (!(H && V && order && init)) {
fprintf(stderr, "malloc error!\n");
exit(0);
}
int nh;
srand(time(NULL));
for (nh = 0; nh < sm.numhid; nh++) {
init[nh] = (double)rand() / RAND_MAX > 0.5 ? 1.0 : 0.0;
}
int iter, curbatch = 0;
int totalbatch = data.numcase / train.mininumcase;
long minibatchlength = (long)train.mininumcase * data.channelcase * data.numchannel;
double cost;
//init_hid_nag_ip_struct(clssfy.lencase, clssfy.numclass - 1);
for (iter = 0; iter < train.iteration; iter++) {
if (curbatch >= totalbatch)
curbatch = 0;
if (curbatch == 0) {
randperm(order, data.numcase);
reorder(data.data, sizeof(uint8_t), order, data.numcase, data.numchannel * data.channelcase);
reorder(data.labels, sizeof(int), order, data.numcase, 1);
}
uint8_t *unlbl = data.data + curbatch * minibatchlength;
fprintf(stdout, "**** iter %d, loc %ld, current data pointer %p ****\n", iter, curbatch * minibatchlength, unlbl);
data2V(V, sm.numclass, unlbl, train.mininumcase, data.channelcase, data.numchannel, sm.position, sm.numvis, train.mix, train.nummix);
//_labels2clssfy(&clssfy, data.labels + curbatch * train.mininumcase, train.mininumcase, train.nummix);
//classify_get_hid(sm.w, sm.bh, V, H, sm.numvis, sm.numhid, sm.numclass, train_numcase, &clssfy);
sm_hid(&sm, V, H, train_numcase, init);
int xx, yy;
for (xx = 0; xx < 1; xx++) {
for (yy=0; yy < sm.numhid; yy++)
fprintf(stdout, "%d", (int)(H[xx * sm.numhid + yy]));
fprintf(stdout, "\n");
}
randperm(order, train_numcase);
reorder(V, sizeof(int), order, train_numcase, sm.numvis);
reorder(H, sizeof(double), order, train_numcase, sm.numhid);
cost = sm_train(&sm, V, H, train_numcase, train.batchsize);
curbatch++;
}
out_w(out, &sm);
free_hid_nag_ip_struct();
free(init);
free(V);
free(H);
free(order);
return 0;
}
bool eigen_lapack(int n, vector_t & A, vector_t & S, matrix_t & V)
{
// Use eigenvalue decomposition instead of SVD
// Get only the highest eigen-values, (par::cluster_mds_dim)
int i1 = n - par::cluster_mds_dim + 1;
int i2 = n;
double z = -1;
// Integer workspace size, 5N
vector<int> iwork(5*n,0);
double optim_lwork;
int lwork = -1;
int out_m;
vector_t out_w( par::cluster_mds_dim , 0 );
vector_t out_z( n * par::cluster_mds_dim ,0 );
int ldz = n;
vector<int> ifail(n,0);
int info=0;
double nz = 0;
// Get workspace
dsyevx_("V" , // get eigenvalues and eigenvectors
"I" , // get interval of selected eigenvalues
"L" , // data stored as upper triangular
&n , // order of matrix
&A[0] , // input matrix
&n , // LDA
&nz , // Vlower
&nz , // Vupper
&i1, // from 1st ...
&i2, // ... to nth eigenvalue
&z , // 0 for ABSTOL
&out_m, // # of eigenvalues found
&out_w[0], // first M entries contain sorted eigen-values
&out_z[0], // array (can be mxm? nxn)
&ldz, // make n at first
&optim_lwork, // Get optimal workspace
&lwork, // size of workspace
&iwork[0], // int workspace
&ifail[0], // output: failed to converge
&info );
// Assign workspace
lwork = (int) optim_lwork;
vector_t work( lwork, 0 );
dsyevx_("V" , // get eigenvalues and eigenvectors
"I" , // get interval of selected eigenvalues
"L" , // data stored as upper triangular
&n , // order of matrix
&A[0] , // input matrix
&n , // LDA
&nz , // Vlower
&nz , // Vupper
&i1, // from 1st ...
&i2, // ... to nth eigenvalue
&z , // 0 for ABSTOL
&out_m, // # of eigenvalues found
&out_w[0], // first M entries contain sorted eigen-values
&out_z[0], // array (can be mxm? nxn)
&ldz, // make n at first
&work[0], // Workspace
&lwork, // size of workspace
&iwork[0], // int workspace
&ifail[0], // output: failed to converge
&info );
// Get eigenvalues, vectors
for (int i=0; i< par::cluster_mds_dim; i++)
S[i] = out_w[i];
for (int i=0; i<n; i++)
for (int j=0;j<par::cluster_mds_dim; j++)
V[i][j] = out_z[ i + j*n ];
return true;
}
/* basic system set up (performed during reset, and Disk Boot) */
EXPORT void resetSystem(W boot)
{
MEMSEG *mp;
UW i, va;
printk("%s\n", __func__);
return ;
/* obtain DipSw status */
if (!boot) DipSw = DipSwStatus();
DisCacheMMU();
/* set up interrupt controller */
out_w(IT0_IDS0, ~0); /* CPU: all interrupts disabled */
out_w(IT0_IDS1, ~0);
out_w(IT0_IDS2, ~0);
out_w(IT0_IIR, ~0);
out_w(IT3_IPI0_CLR, 0x0000003f);
out_w(IT3_IDS0, ~0); /* DSP: all interrupts disabled */
out_w(IT3_IDS1, ~0);
out_w(IT3_IDS2, ~0);
out_w(IT3_IIR, ~0);
out_w(IT0_IPI3_CLR, 0x0000003f);
out_w(IT0_FID, 0x00000001); /* CPU: FIQ disabled */
out_w(GIO_IIA(GIO_L), 0); /* GPIO: interrupt disabled */
out_w(GIO_IIA(GIO_H), 0);
out_w(GIO_IIA(GIO_HH), 0);
out_w(GIO_IIA(GIO_HHH), 0);
out_w(GIO_GSW(GIO_L), 0); /* GPIO: FIQ interrupt disabled */
out_w(GIO_GSW(GIO_H), 0);
out_w(GIO_GSW(GIO_HH), 0);
out_w(GIO_GSW(GIO_HHH), 0);
out_w(IT0_LIIR, 0x0000000f); /* internal interrupt disabled */
out_w(IT_PINV_CLR0, ~0); /* inhibit interrupt polarity inversion */
out_w(IT_PINV_CLR1, ~0);
out_w(IT_PINV_CLR2, ~0);
out_w(IT0_IEN0, 0x0c000000); /* CPU: GPIO interrupt enabled */
out_w(IT0_IEN1, 0x003c0000);
out_w(IT0_IEN2, 0x00018000);
/* power on controller initialization */
pmicInit();
/* USB power on */
usbPower(TRUE);
/* clear system common area (vector table, and SysInfo) */
memset(&SCInfo, 0, sizeof(SysCommonInfo));
memset(SCArea, 0, sizeof(SysCommonArea));
/* if monitor is loaded into RAM, exclude the RAM area */
mp = MemArea(MSA_OS, 1);
va = (UW)&__loadaddr;
if (va >= mp->top && va < mp->end) mp->end = va;
/* exclude the area where ROM disk data is stored */
va = (UW)ROMInfo->userarea;
if (va >= mp->top && va < mp->end) mp->end = va;
/* initialize system common information (SysInfo) */
SCInfo.ramtop = (void*)mp->top;
if (va < mp->top || va > mp->end) va = mp->end;
SCInfo.ramend = (void*)va;
/* set up EIT vectors */
/* we do not need _defaultHdr absolutely, but just in case set it up */
SCArea->intvec[EIT_DEFAULT] = _defaultHdr; /* default handler */
SCArea->intvec[EIT_UNDEF] = _defaultHdr; /* undefined instruction */
SCArea->intvec[SWI_MONITOR] = _defaultHdr; /* SWI - monitor SVC */
SCArea->intvec[EIT_IRQ(26)] = _gio6Hdr; /* GPIO branch */
SCArea->intvec[EIT_IRQ(27)] = _gio7Hdr;
SCArea->intvec[EIT_IRQ(50)] = _gio0Hdr;
SCArea->intvec[EIT_IRQ(51)] = _gio1Hdr;
SCArea->intvec[EIT_IRQ(52)] = _gio2Hdr;
SCArea->intvec[EIT_IRQ(53)] = _gio3Hdr;
SCArea->intvec[EIT_IRQ(79)] = _gio4Hdr;
SCArea->intvec[EIT_IRQ(80)] = _gio5Hdr;
SCArea->intvec[EIT_IRQ(8)] = _defaultHdr; /* abort switch */
/* set up initial page table */
for (i = 0; i < N_MemSeg; ++i) {
mp = &MemSeg[i];
if (!mp->pa) continue;
/* FlashROM has already been mapped, and so do not touch it */
if (mp->attr == MSA_FROM) continue;
/* set up in unit of section (1MB) */
for ( va = (mp->top & 0xfff00000);
va != ((mp->end + 0x000fffff) & 0xfff00000);
va += 0x00100000 ) {
TopPageTable[va / 0x00100000] =
((mp->pa & 0xfff00000) + va) |
(mp->pa & 0x000fffff);
}
}
for (i = 0; i < N_NoMemSeg; ++i) {
mp = &NoMemSeg[i];
/* set up in unit of section (1MB) */
for ( va = (mp->top & 0xfff00000);
va != ((mp->end + 0x000fffff) & 0xfff00000);
va += 0x00100000 ) {
TopPageTable[va / 0x00100000] = 0;
}
}
DSB();
Asm("mcr p15, 0, %0, cr8, c7, 0":: "r"(0)); /* I/D TLB invalidate */
Asm("mcr p15, 0, %0, cr7, c5, 6":: "r"(0)); /* invalidate BTC */
DSB();
ISB();
EnbCacheMMU();
return;
}
void start_timer(unsigned r0, unsigned r1, unsigned r2, unsigned r3)
{
int val;
int i;
#define BIT(n) (1<<n)
#define BITTST(val, n) ((val) & BIT(n))
#define TST(val, b) ((val) & (b))
hw_timer_rotary[0] = (void *)(0x80068000); /* have ROTCTRL */
hw_timer_rotary[1] = (void *)(0x80068050);
hw_timer_rotary[2] = (void *)(0x80068080);
hw_timer_rotary[3] = (void *)(0x800680C0);
val = hw_timer_rotary[0]->HW_TIMROT_ROTCTRL[0];
printk("This SoC has:\n");
if(BITTST(val,25)) printk("timer 0\n");
if(BITTST(val,26)) printk("timer 1\n");
if(BITTST(val,27)) printk("timer 2\n");
if(BITTST(val,28)) printk("timer 3\n");
#define IRQ (1<<15)
#define IRQ_EN (1<<14)
#define MATCH_MODE (1<<11)
#define POLARITY (1<<8)
#define UPDATE (1<<7)
#define RELOAD (1<<6)
#define PRESCALE(n) ((n)<<4)
#define DIV_BY_8 (0x3)
#define SELECT(n) ((n)<<0)
#define TICK_ALWAYS (0XF)
#define SET 1
#define CLR 2
#define TOG 3
hw_timer_rotary[1]->HW_TIMROT_FIXED_COUNT[0] = 0x00011000;
val = IRQ_EN | UPDATE | RELOAD | PRESCALE(TICK_ALWAYS) | SELECT(0xB);
out_w(HW_TIMROT_TIMCTRL1, val);
while(1){
static int random = 0;
random++;
printk("[%04d]wait timer1 irq\n", random);
waitMsec(100);
val = hw_timer_rotary[1]->HW_TIMROT_TIMCTRL[0];
printk("%016b\n", val);
val = in_w(HW_TIMROT_RUNNING_COUNT1);
printk("timer running @[0x%x]\n", val);
val = in_w(HW_TIMROT_VERSION);
printk("timer version @[%X]\n", val);
if(random>300) break;
}
printk("Goodbye TIMER!\n");
}
/** Function 01h - Return VBE Mode Information
*
* Input:
* AX = 4F01h
* CX = Mode Number
* ES:DI = Pointer to buffer in which to place ModeInfoBlock structure
* Output:
* AX = VBE Return Status
*
*/
void vbe_biosfn_return_mode_information(uint16_t STACK_BASED *AX, uint16_t CX, uint16_t ES, uint16_t DI)
{
uint16_t result = 0x0100;
#ifdef VBE_NEW_DYN_LIST
uint16_t cur_info_ofs;
#else
ModeInfoListItem *cur_info;
#endif
Boolean using_lfb;
uint8_t win_attr;
#ifdef VGA_DEBUG
printf("VBE vbe_biosfn_return_mode_information ES%x DI%x CX%x\n",ES,DI,CX);
#endif
using_lfb = ((CX & VBE_MODE_LINEAR_FRAME_BUFFER) == VBE_MODE_LINEAR_FRAME_BUFFER);
CX = (CX & 0x1ff);
#ifdef VBE_NEW_DYN_LIST
cur_info_ofs = mode_info_find_mode(CX, using_lfb);
if (cur_info_ofs) {
uint16_t i;
#else
cur_info = mode_info_find_mode(CX, using_lfb);
if (cur_info != 0) {
#endif
#ifdef VGA_DEBUG
printf("VBE found mode %x\n",CX);
#endif
memsetb(ES, DI, 0, 256); // The mode info size is fixed
#ifdef VBE_NEW_DYN_LIST
for (i = 0; i < sizeof(ModeInfoBlockCompact); i++) {
uint8_t b;
b = in_byte(VBE_EXTRA_PORT, cur_info_ofs + offsetof(ModeInfoListItem, info) + i/*(char *)(&(cur_info->info)) + i*/);
write_byte(ES, DI + i, b);
}
#else
memcpyb(ES, DI, 0xc000, &(cur_info->info), sizeof(ModeInfoBlockCompact));
#endif
win_attr = read_byte(ES, DI + offsetof(ModeInfoBlock, WinAAttributes));
if (win_attr & VBE_WINDOW_ATTRIBUTE_RELOCATABLE) {
write_word(ES, DI + offsetof(ModeInfoBlock, WinFuncPtr), (uint16_t)(dispi_set_bank_farcall));
// If BIOS not at 0xC000 -> boom
write_word(ES, DI + offsetof(ModeInfoBlock, WinFuncPtr) + 2, 0xC000);
}
// Update the LFB physical address which may change at runtime
out_w(VBE_DISPI_IOPORT_INDEX, VBE_DISPI_INDEX_FB_BASE_HI);
write_word(ES, DI + offsetof(ModeInfoBlock, PhysBasePtr) + 2, in_w(VBE_DISPI_IOPORT_DATA));
result = 0x4f;
} else {
#ifdef VGA_DEBUG
printf("VBE *NOT* found mode %x\n",CX);
#endif
result = 0x100;
}
*AX = result;
}
/** Function 02h - Set VBE Mode
*
* Input:
* AX = 4F02h
* BX = Desired Mode to set
* ES:DI = Pointer to CRTCInfoBlock structure
* Output:
* AX = VBE Return Status
*
*/
void vbe_biosfn_set_mode(uint16_t STACK_BASED *AX, uint16_t BX, uint16_t ES, uint16_t DI)
{
uint16_t result;
#ifdef VBE_NEW_DYN_LIST
uint16_t cur_info_ofs;
#else
ModeInfoListItem *cur_info;
#endif
Boolean using_lfb;
uint8_t no_clear;
uint8_t lfb_flag;
using_lfb = ((BX & VBE_MODE_LINEAR_FRAME_BUFFER) == VBE_MODE_LINEAR_FRAME_BUFFER);
lfb_flag = using_lfb ? VBE_DISPI_LFB_ENABLED : 0;
no_clear = ((BX & VBE_MODE_PRESERVE_DISPLAY_MEMORY) == VBE_MODE_PRESERVE_DISPLAY_MEMORY) ? VBE_DISPI_NOCLEARMEM : 0;
BX = (BX & 0x1ff);
// check for non vesa mode
if (BX < VBE_MODE_VESA_DEFINED)
{
uint8_t mode;
dispi_set_enable(VBE_DISPI_DISABLED);
// call the vgabios in order to set the video mode
// this allows for going back to textmode with a VBE call (some applications expect that to work)
mode = (BX & 0xff);
biosfn_set_video_mode(mode);
result = 0x4f;
goto leave;
}
#ifdef VBE_NEW_DYN_LIST
cur_info_ofs = mode_info_find_mode(BX, using_lfb);
if (cur_info_ofs != 0)
{
uint16_t xres, yres;
uint8_t bpp;
xres = in_word(VBE_EXTRA_PORT, cur_info_ofs + offsetof(ModeInfoListItem, info.XResolution) /*&cur_info->info.XResolution*/);
yres = in_word(VBE_EXTRA_PORT, cur_info_ofs + offsetof(ModeInfoListItem, info.YResolution) /*&cur_info->info.YResolution*/);
bpp = in_byte(VBE_EXTRA_PORT, cur_info_ofs + offsetof(ModeInfoListItem, info.BitsPerPixel) /*&cur_info->info.BitsPerPixel*/);
#ifdef VGA_DEBUG
printf("VBE found mode %x, setting:\n", BX);
printf("\txres%x yres%x bpp%x\n", xres, yres, bpp);
#endif
#else
cur_info = mode_info_find_mode(BX, using_lfb);
if (cur_info != 0)
{
#ifdef VGA_DEBUG
printf("VBE found mode %x, setting:\n", BX);
printf("\txres%x yres%x bpp%x\n",
cur_info->info.XResolution,
cur_info->info.YResolution,
cur_info->info.BitsPerPixel);
#endif
#endif // VBE_NEW_DYN_LIST
// first disable current mode (when switching between vesa modi)
dispi_set_enable(VBE_DISPI_DISABLED);
#ifdef VBE_NEW_DYN_LIST
if (bpp == 4)
#else
if (cur_info->info.BitsPerPixel == 4)
#endif
{
biosfn_set_video_mode(0x6a);
}
#ifdef VBE_NEW_DYN_LIST
dispi_set_bpp(bpp);
dispi_set_xres(xres);
dispi_set_yres(yres);
#else
dispi_set_bpp(cur_info->info.BitsPerPixel);
dispi_set_xres(cur_info->info.XResolution);
dispi_set_yres(cur_info->info.YResolution);
#endif
dispi_set_bank(0);
dispi_set_enable(VBE_DISPI_ENABLED | no_clear | lfb_flag);
vga_compat_setup();
write_word(BIOSMEM_SEG,BIOSMEM_VBE_MODE,BX);
write_byte(BIOSMEM_SEG,BIOSMEM_VIDEO_CTL,(0x60 | no_clear));
result = 0x4f;
}
else
{
#ifdef VGA_DEBUG
printf("VBE *NOT* found mode %x\n" , BX);
#endif
result = 0x100;
}
leave:
*AX = result;
}
uint16_t vbe_biosfn_read_video_state_size(void)
{
return 9 * 2;
}
void vbe_biosfn_save_video_state(uint16_t ES, uint16_t BX)
{
uint16_t enable, i;
outw(VBE_DISPI_IOPORT_INDEX,VBE_DISPI_INDEX_ENABLE);
enable = inw(VBE_DISPI_IOPORT_DATA);
write_word(ES, BX, enable);
BX += 2;
if (!(enable & VBE_DISPI_ENABLED))
return;
for(i = VBE_DISPI_INDEX_XRES; i <= VBE_DISPI_INDEX_Y_OFFSET; i++) {
if (i != VBE_DISPI_INDEX_ENABLE) {
outw(VBE_DISPI_IOPORT_INDEX, i);
write_word(ES, BX, inw(VBE_DISPI_IOPORT_DATA));
BX += 2;
}
}
}
/*
* Target system-dependent initialization
*/
EXPORT ER tkdev_initialize( void )
{
static UINT giob[4] = { _L, _H, _HH, _HHH };
UINT gb;
INT i;
/* initialize GPIO interrupt */
for ( i = 0; i < 4; ++i ) {
gb = giob[i];
out_w(GIO_IDS(gb), 0xffffffff); /* disable interrupt */
out_w(GIO_IIA(gb), 0x00000000); /* disassert interrupt pin */
out_w(GIO_GSW(gb), 0x00000000); /* disassert FIQ pin */
}
/* initialize interrupt controller (AINT) */
out_w(IT0_IDSS0, 0xffffffff); /* disassert interrupt pin */
out_w(IT0_IDSS1, 0xffffffff);
out_w(IT0_IDSS2, 0xffffffff);
out_w(IT0_IDS0, 0xffffffff); /* disable interrupt */
out_w(IT0_IDS1, 0xffffffff);
out_w(IT0_IDS2, 0xffffffff);
out_w(IT_PINV_CLR0, 0xffffffff); /* reset inverted logic */
out_w(IT_PINV_CLR1, 0xffffffff);
out_w(IT_PINV_CLR2, 0xffffffff);
out_w(IT_LIIR, 0xffffffff); /* clear interrupt */
out_w(IT0_IIR, 0xffffffff);
out_w(IT0_FIE, 0x00000001); /* enable FIQ */
/* enable GPIO interrupt on AINT */
out_w(IT0_IENS0, IRQM(26)|IRQM(27)); /* assert interrupt pin */
out_w(IT0_IENS1, IRQM(50)|IRQM(51)|IRQM(52)|IRQM(53));
out_w(IT0_IENS2, IRQM(79)|IRQM(80));
out_w(IT0_IEN0, IRQM(26)|IRQM(27)); /* enable interrupt */
out_w(IT0_IEN1, IRQM(50)|IRQM(51)|IRQM(52)|IRQM(53));
out_w(IT0_IEN2, IRQM(79)|IRQM(80));
/* enable abort switch(SW1) */
SetIntMode(IV_GPIO(8), IM_ENA|IM_LEVEL|IM_HI);
EnableInt(IV_GPIO(8));
return E_OK;
}
/*
* Interrupt handler definition
*/
SYSCALL ER _tk_def_int( UINT dintno, T_DINT *pk_dint )
{
FP inthdr;
INT vecno = dintno >> 5;
INTMASKTBL *ptr;
CHECK_PAR(vecno < MAX_INTVEC);
if ( pk_dint != NULL ) {
/* Set interrupt handler */
if ( vecno == VECNO_TLBMISS ) {
CHECK_RSATR(pk_dint->intatr, 0);
} else {
CHECK_RSATR(pk_dint->intatr, TA_HLNG | TA_ASSPRC);
}
#if CHK_PAR
if ( (pk_dint->intatr & TA_ASSPRC) != 0 ) {
if ( !chk_assprc(pk_dint->assprc) ) {
return E_PAR;
}
}
#endif
inthdr = pk_dint->inthdr;
BEGIN_CRITICAL_SECTION;
if ( (pk_dint->intatr & TA_HLNG) != 0 ) {
hll_inthdr[vecno] = inthdr;
inthdr = ( vecno == VECNO_DEFAULT )?
defaulthdr_startup: inthdr_startup;
}
define_inthdr(vecno, inthdr);
if ( (pk_dint->intatr & TA_ASSPRC) != 0 ) {
if ( vecno < N_INTVEC) {
ptr = &intmasktable[vecno];
if ( ptr->mask != 0) {
if ( pk_dint->assprc & TP_PRC1 ) {
out_w(ptr->maskaddr[1], in_w(ptr->maskaddr[1]) | ptr->mask);
out_w(clraddr(ptr->maskaddr[0]), ptr->mask);
}
else if ( pk_dint->assprc & TP_PRC2 ) {
out_w(ptr->maskaddr[0], in_w(ptr->maskaddr[0]) | ptr->mask);
out_w(clraddr(ptr->maskaddr[1]), ptr->mask);
}
}
}
}
END_CRITICAL_NO_DISPATCH;
} else {
/* Free interrupt handler */
switch ( vecno ) {
case VECNO_TRAPA: inthdr = SaveMonHdr.trapa_hdr; break;
case VECNO_BREAK: inthdr = SaveMonHdr.break_hdr; break;
case VECNO_MONITOR: inthdr = SaveMonHdr.monitor_hdr; break;
case VECNO_DEFAULT: inthdr = SaveMonHdr.default_hdr; break;
case VECNO_TLBMISS: inthdr = SaveMonHdr.tlbmiss_hdr; break;
default: inthdr = NULL;
}
BEGIN_CRITICAL_SECTION;
define_inthdr(vecno, inthdr);
if ( vecno < MAX_INTVEC ) {
hll_inthdr[vecno] = NULL;
}
if ( vecno < N_INTVEC) {
ptr = &intmasktable[vecno];
if ( ptr->mask != 0) {
out_w(ptr->maskaddr[0], in_w(ptr->maskaddr[0]) | ptr->mask);
out_w(ptr->maskaddr[1], in_w(ptr->maskaddr[1]) | ptr->mask);
}
}
END_CRITICAL_NO_DISPATCH;
}
return E_OK;
}
