Today's post shows how easy it is to drive device drivers in DGOS. This is the COMPLETE source code for the hard drive driver in the LD. Note a complete lack of synchronization primitives, or anything else. The OS handles that for the driver.
Note that the only entry point into the driver is "hddInit"
Requests and responses are handled by the "requestQ" primitive. It is smart, blocking the driver thread while waiting for requests and blocking requesting threads while waiting for responses. While this driver is single-threaded, multithreaded drivers can use it too, since this primitive is thread-safe.
#include "HDD.h"
#include "task.h"
#include "blockDev.h"
#include "requestQ.h"
#include "kHeap.h"
#include "timers.h"
#include "gpio.h"
#define MCIO0 (map[14])
#define MECR (map[ 5])
typedef struct{
UInt32* hddBase;
UInt32 blkDevID;
UInt32 reqQID;
char manuf[32];
char prod[32];
char serial[32];
UInt32 numBlocks;
}HddGlobals;
typedef struct{
UInt8 err_or_features; //addr = 1
UInt8 numSectors; //addr = 2
UInt8 lba_0_7; //addr = 3
UInt8 lba_8_15; //addr = 4
UInt8 lba_16_23; //addr = 5
UInt8 lba_24_31; //addr = 6
UInt8 status_or_cmd; //addr = 7
}HddCmd;
static void hddPrvReadSec(volatile void* base,UInt8* buf);
static void hddPrvWriteSec(volatile void* base,UInt8* buf);
static Err hddPrvWaitForDriveReady(volatile void* hddBase){
volatile UInt8* statusField = ((volatile UInt8*)hddBase) + 0x17;
volatile UInt8* errField = ((volatile UInt8*)hddBase) + 0x11;
UInt32 t,v;
t = timersGetTicks();
do{
v = *statusField;
if(v == 0x50){ //ready
return errNone;
}
}while(timersGetTicks() - t < timersTicksPerMsec() * 500);
return 0xFF00 + *errField;
}
static Err hddPrvWaitForNotBusy(volatile void* hddBase){
volatile UInt8* statusField = ((volatile UInt8*)hddBase) + 0x17;
volatile UInt8* errField = ((volatile UInt8*)hddBase) + 0x11;
UInt32 t,v;
t = timersGetTicks();
do{
v = *statusField;
if(v & 0x80){ //busy
//nothing
}
else if(v & 1){ //error
break;
}
else{
return errNone;
}
}while(timersGetTicks() - t < timersTicksPerMsec() * 500);
return 0xFF00 + *errField;
}
static Err hddPrvWaitForReadyForData(volatile void* hddBase){
volatile UInt8* statusField = ((volatile UInt8*)hddBase) + 0x17;
volatile UInt8* errField = ((volatile UInt8*)hddBase) + 0x11;
UInt32 t,v;
t = timersGetTicks();
do{
v = *statusField;
if(!(v & 0x08)){ //not ready for dat
//nothing
}
else if(v & 1){ //error
break;
}
else{
return errNone;
}
}while(timersGetTicks() - t < timersTicksPerMsec() * 1000);
return 0xFF00 + *errField;
}
static Err hddPrvCmd(volatile void* hddBase,HddCmd* cmd){
volatile UInt8* p = hddBase;
Err e;
e = hddPrvWaitForDriveReady(hddBase);
if(e) return e;
p[0x11] = cmd->err_or_features;
p[0x12] = cmd->numSectors;
p[0x13] = cmd->lba_0_7;
p[0x14] = cmd->lba_8_15;
p[0x15] = cmd->lba_16_23;
p[0x16] = cmd->lba_24_31;
p[0x17] = cmd->status_or_cmd;
e = hddPrvWaitForNotBusy(hddBase);
cmd->err_or_features = p[0x11];
cmd->numSectors = p[0x12];
cmd->lba_0_7 = p[0x13];
cmd->lba_8_15 = p[0x14];
cmd->lba_16_23 = p[0x15];
cmd->lba_24_31 = p[0x16];
cmd->status_or_cmd = p[0x17];
return e;
}
static Err hddPrvCmd_read(volatile void* hddBase,HddCmd* cmd,UInt32 numSectors,UInt8* buf){
volatile UInt8* p = hddBase;
Err e;
e = hddPrvWaitForDriveReady(hddBase);
if(e) return e;
p[0x11] = cmd->err_or_features;
p[0x12] = cmd->numSectors;
p[0x13] = cmd->lba_0_7;
p[0x14] = cmd->lba_8_15;
p[0x15] = cmd->lba_16_23;
p[0x16] = cmd->lba_24_31;
p[0x17] = cmd->status_or_cmd;
while(numSectors--){
e = hddPrvWaitForNotBusy(hddBase);
if(e) break;
e = hddPrvWaitForReadyForData(hddBase);
if(e) break;
hddPrvReadSec(hddBase,buf);
buf += 512;
}
cmd->err_or_features = p[0x11];
cmd->numSectors = p[0x12];
cmd->lba_0_7 = p[0x13];
cmd->lba_8_15 = p[0x14];
cmd->lba_16_23 = p[0x15];
cmd->lba_24_31 = p[0x16];
cmd->status_or_cmd = p[0x17];
return e;
}
static Err hddPrvCmd_write(volatile void* hddBase,HddCmd* cmd,UInt32 numSectors,UInt8* buf){
volatile UInt8* p = hddBase;
Err e;
e = hddPrvWaitForDriveReady(hddBase);
if(e) return e;
p[0x11] = cmd->err_or_features;
p[0x12] = cmd->numSectors;
p[0x13] = cmd->lba_0_7;
p[0x14] = cmd->lba_8_15;
p[0x15] = cmd->lba_16_23;
p[0x16] = cmd->lba_24_31;
p[0x17] = cmd->status_or_cmd;
while(numSectors--){
e = hddPrvWaitForNotBusy(hddBase);
if(e) break;
e = hddPrvWaitForReadyForData(hddBase);
if(e) break;
hddPrvWriteSec(hddBase,(UInt8*)buf);
buf += 512;
}
cmd->err_or_features = p[0x11];
cmd->numSectors = p[0x12];
cmd->lba_0_7 = p[0x13];
cmd->lba_8_15 = p[0x14];
cmd->lba_16_23 = p[0x15];
cmd->lba_24_31 = p[0x16];
cmd->status_or_cmd = p[0x17];
return e;
}
//MemMove that also does a byteswap16() on each moved halfword. Useful since device identification strings are byte-swapped in response.
static asm memSwapCopy(void* dstP,void* srcP,UInt32 len){ //only even lengths please
MOV R2,R2,LSR #1
loop:
LDRB R3, [R1],#1
LDRB R12,[R1],#1
STRB R3, [R0,#1]
STRB R12,[R0],#2
SUBS R2,R2,#1
BNE loop
BX LR
}
Err hddPrvReadDriveID(HddGlobals* g){
HddCmd cmd;
UInt32 bufP[128];
UInt8* buf = (UInt8*)bufP;
UInt32 i;
Err e;
cmd.lba_24_31 = 0xE0;
cmd.status_or_cmd = 0xEC; //drive identify [OPTIONAL cmd]
e = hddPrvCmd_read(g->hddBase,&cmd,1,buf);
if(e) return e;
MemSet(g->serial,sizeof(g->serial),0);
memSwapCopy(g->serial,buf + 20,20);
MemSet(g->manuf,sizeof(g->manuf),0);
memSwapCopy(g->manuf,buf + 46,8);
MemSet(g->prod,sizeof(g->prod),0);
memSwapCopy(g->prod,buf + 54,31);
g->numBlocks = *(UInt32*)(buf + 120);
return errNone;
}
static void hddPrvDrivePower(Boolean on){
gpioSetState(115,on);
}
static void hddPrvDriveReset(){
gpioSetState(98,false);
taskDelay(taskGetCurTask(),2);
gpioSetState(98,true);
}
static void hddPrvDriveInit(HddGlobals* g){
hddPrvDrivePower(true);
taskDelay(taskGetCurTask(),2);
hddPrvDriveReset();
taskDelay(taskGetCurTask(),20);
hddPrvReadDriveID(g);
}
static Err hddReadWriteBlocks(HddGlobals* g,UInt32 first,UInt32 num,void* bufP,UInt32* numDone,Boolean write){
Err (*func)(volatile void* hddBase,HddCmd* cmd,UInt32 numSectors,UInt8* buf);
UInt32 numSec,done = 0;
UInt8* buf = bufP;
HddCmd cmd;
Err e;
func = write?hddPrvCmd_write:hddPrvCmd_read;
if(((UInt32)bufP) & 3) Panic("non-word aligned buffer address");
while(num){
numSec = num;
if(numSec > 255) numSec = 255;
cmd.status_or_cmd = write?0x30:0x20;
cmd.lba_0_7 = first;
cmd.lba_8_15 = first >> 8;
cmd.lba_16_23 = first >> 16;
cmd.lba_24_31 = (first >> 24) | 0xE0;
cmd.numSectors = numSec;
cmd.err_or_features = 0;
e = func(g->hddBase,&cmd,numSec,buf);
if(e){
*numDone = done;
return e;
}
else{
buf += 512 * numSec;
num -= numSec;
}
}
return errNone;
}
static void hddPrvIfaceInit(HddGlobals* g){
volatile UInt32* map;
map = mmuMapReq(true,0x48000000,1,MAP_PERM_SYS_RW);
if(map == MMU_INVALID_PTR) Panic("Cannot map in memory controller");
MCIO0 = 0x1460D;
MECR &=~ 2; //no PC-cars inserted
MECR |= 1; //two PC-card slots
MECR |= 2; //PC-card(s) inserted
mmuMapRel((void*)map,1);
gpioSetFunc(115,0);
gpioSetDir(115,GPIO_DIR_OUT);
gpioSetFunc(98,0);
gpioSetDir(98,GPIO_DIR_OUT);
}
static void* hddPrvThread(void* gP){
HddGlobals* g = gP;
BlkDevCmd* c;
UInt32 devID,reqQID,reqH;
char* buf;
UInt32 theirMemH;
blkDevCreate("Internal HDD",&devID,&reqQID);
g->blkDevID = devID;
g->reqQID = reqQID;
hddPrvIfaceInit(g);
hddPrvDriveInit(g);
while(1){
charsPrintF("getting request\n");
requestQserviceGet(taskGetCurTask(),reqQID,&reqH,(void**)&c);
charsPrintF("request gotten\n");
charsPrintF("cmd=%d\n",c->cmd);
c->error = errNone;
switch(c->cmd){
case CMD_READ_BLOCKS:
buf = taskMapInTheirMem(c->data.blocks.requestingProcess,c->data.blocks.buffer,c->data.blocks.numBlocks * 512,&theirMemH);
blkDevNotifyOp(devID,true,false);
c->error = hddReadWriteBlocks(g,c->data.blocks.firstBlock,c->data.blocks.numBlocks,buf,&c->data.blocks.numBlocks,false);
blkDevNotifyOp(devID,false,false);
taskUnmapTheirMem(buf,theirMemH);
break;
case CMD_WRITE_BLOCKS:
buf = taskMapInTheirMem(c->data.blocks.requestingProcess,c->data.blocks.buffer,c->data.blocks.numBlocks * 512,&theirMemH);
blkDevNotifyOp(devID,true,true);
c->error = hddReadWriteBlocks(g,c->data.blocks.firstBlock,c->data.blocks.numBlocks,buf,&c->data.blocks.numBlocks,true);
blkDevNotifyOp(devID,false,true);
taskUnmapTheirMem(buf,theirMemH);
break;
case CMD_QUERY_INFO:
c->data.info.numBlocks = g->numBlocks;
c->data.info.removable = false;
c->data.info.inserted = true;
c->data.info.writeable = true;
break;
case CMD_QUERY_MANUF_NAME:
MemMove(c->data.manufName,g->manuf,sizeof(g->manuf));
break;
case CMD_QUERY_PROD_NAME:
MemMove(c->data.prodName,g->prod,sizeof(g->prod));
break;
case CMD_QUERY_SERIAL_NUM:
MemMove(c->data.serialNum,g->serial,sizeof(g->serial));
break;
default:
Panic("Unknown cmd to HDD driver");
break;
}
requestQserviceDone(reqH);
}
}
void hddInit(){
Thread* t;
HddGlobals* g;
volatile UInt32* hddBase;
g = kHeapAlloc(sizeof(HddGlobals));
if(!g) Panic("Cannot alloc HDD globals");
hddBase = mmuMapReq(true,0x20000000,1,MAP_PERM_SYS_RW);
if(hddBase == MMU_INVALID_PTR) Panic("Cannot map in HDD controller");
charsPrintF("hddBase=0x%08lx\n",hddBase);
g->hddBase = (UInt32*)hddBase;
t = taskNewKernelThread(&hddPrvThread,g,PAGE_SIZE,DEFAULT_BLOCK_DRV_PRIORITY);
taskUnblockThread(t);
charsPrintF("done\n");
}
///////////////// ASM CODE ////////////////////////////////
//optimzed for speed from this C code:
//void hddPrvReadSec(volatile void* base,UInt8* buf){
// volatile UInt16* p = base;
// UInt16* dst = (UInt16*)buf;
// UInt32 left = 256;
// p += 5; //point it to I/O address
// while(left--) *dst++ = *p;
//}
//advantage is that this ASM version reads 32 bytes at once
static asm void hddPrvReadSec(volatile void* base,UInt8* buf){
STMFD SP!,{R4-R11,LR}
ADD R0,R0,#0x10
MOV R12,#16
loop:
LDRH R2, [R0]
LDRH R3, [R0]
LDRH R4, [R0]
LDRH R5, [R0]
LDRH R6, [R0]
LDRH R7, [R0]
LDRH R8, [R0]
LDRH R9, [R0]
LDRH R10,[R0]
LDRH R11,[R0]
ORR R2, R2, R3, LSL #16
ORR R3, R4, R5, LSL #16
ORR R4, R6, R7, LSL #16
ORR R5, R8, R9, LSL #16
ORR R6 ,R10,R11,LSL #16
LDRH R7, [R0]
LDRH R8, [R0]
LDRH R9, [R0]
LDRH R10,[R0]
LDRH R11,[R0]
LDRH LR, [R0]
ORR R7, R7, R8, LSL #16
ORR R8, R9, R10,LSL #16
ORR R9, R11,LR, LSL #16
STMIA R1!,{R2-R9} //32 bytes
SUBS R12,R12,#1
BNE loop
exit:
LDMFD SP!,{R4-R11,LR}
BX LR
}
//optimzed for speed from this C code:
//void hddPrvWriteSec(volatile void* base,UInt8* buf){
// volatile UInt16* p = base;
// UInt16* src = (UInt16*)buf;
// UInt32 left = 256;
// p += 5; //point it to I/O address
// while(left--) *p = *src++;
//}
//advantage is that this ASM version writes 32 bytes at once
static asm void hddPrvWriteSec(volatile void* base,UInt8* buf){
STMFD SP!,{R4-R11,LR}
ADD R0,R0,#0x10
MOV R12,#16
loop:
LDMIA R1!,{R2-R9} //32 bytes
MOV R10,R2, LSR #16
MOV R11,R3, LSR #16
MOV LR ,R4, LSR #16
STRH R2, [R0]
STRH R10,[R0]
STRH R3, [R0]
STRH R11,[R0]
STRH R4, [R0]
STRH LR, [R0]
MOV R2, R5, LSR #16
MOV R3, R6, LSR #16
MOV R4, R7, LSR #16
MOV R10,R8, LSR #16
MOV R11,R9, LSR #16
STRH R5, [R0]
STRH R2, [R0]
STRH R6, [R0]
STRH R3, [R0]
STRH R7, [R0]
STRH R4, [R0]
STRH R8, [R0]
STRH R10,[R0]
STRH R9, [R0]
STRH R11,[R0]
SUBS R12,R12,#1
BNE loop
exit:
LDMFD SP!,{R4-R11,LR}
BX LR
}
2 comments:
Today's post shows how easy it is to drive device drivers in DGOS.
Holy mother of god !!!!
DEFINE EASY, FOR CHRIST'S SAKE!!!
Nice work, dude. Just japanese for me, but if you can handle your project as you expect (and a lot of people) it will be a real achievement.
Greetings from Argentina
I friggen love you. (not like that)
Post a Comment