uint16_t stm32fx07_ep_write_packet(usbd_device *usbd_dev, uint8_t addr,
const void *buf, uint16_t len)
{
const uint32_t *buf32 = buf;
int i;
addr &= 0x7F;
/* Return if endpoint is already enabled. */
if (REBASE(OTG_DIEPTSIZ(addr)) & OTG_FS_DIEPSIZ0_PKTCNT) {
return 0;
}
/* Enable endpoint for transmission. */
REBASE(OTG_DIEPTSIZ(addr)) = OTG_FS_DIEPSIZ0_PKTCNT | len;
REBASE(OTG_DIEPCTL(addr)) |= OTG_FS_DIEPCTL0_EPENA |
OTG_FS_DIEPCTL0_CNAK;
volatile uint32_t *fifo = REBASE_FIFO(addr);
/* Copy buffer to endpoint FIFO, note - memcpy does not work */
for (i = len; i > 0; i -= 4) {
*fifo++ = *buf32++;
}
return len;
}
uint16_t stm32fx07_ep_read_packet(usbd_device *usbd_dev, uint8_t addr,
void *buf, uint16_t len)
{
int i;
uint32_t *buf32 = buf;
uint32_t extra;
len = MIN(len, usbd_dev->rxbcnt);
usbd_dev->rxbcnt -= len;
volatile uint32_t *fifo = REBASE_FIFO(addr);
for (i = len; i >= 4; i -= 4) {
*buf32++ = *fifo++;
}
if (i) {
extra = *fifo++;
memcpy(buf32, &extra, i);
}
REBASE(OTG_DOEPTSIZ(addr)) = usbd_dev->doeptsiz[addr];
REBASE(OTG_DOEPCTL(addr)) |= OTG_FS_DOEPCTL0_EPENA |
(usbd_dev->force_nak[addr] ?
OTG_FS_DOEPCTL0_SNAK : OTG_FS_DOEPCTL0_CNAK);
return len;
}
/// <summary>
/// Retrieve image TLS callbacks
/// Callbacks are rebased for target image
/// </summary>
/// <param name="targetBase">Target image base</param>
/// <param name="result">Found callbacks</param>
/// <returns>Number of TLS callbacks in image</returns>
int PEParser::GetTLSCallbacks( module_t targetBase, std::vector<ptr_t>& result ) const
{
uint8_t *pTls = reinterpret_cast<uint8_t*>(DirectoryAddress( IMAGE_DIRECTORY_ENTRY_TLS ));
uint64_t* pCallback = 0;
if (!pTls)
return 0;
uint64_t offset = _is64 ? TLS64( pTls )->AddressOfCallBacks : TLS32( pTls )->AddressOfCallBacks;
// Not at base
if (imageBase() != reinterpret_cast<module_t>(_pFileBase))
pCallback = reinterpret_cast<uint64_t*>(ResolveRVAToVA( static_cast<size_t>(offset - imageBase()) ));
else
pCallback = reinterpret_cast<uint64_t*>(offset);
if(_is64)
{
for (; *pCallback; pCallback++)
result.push_back( REBASE( *pCallback, imageBase(), targetBase ) );
}
else
{
for (uint32_t *pCallback2 = reinterpret_cast<uint32_t*>(pCallback); *pCallback2; pCallback2++)
result.push_back( REBASE( *pCallback2, imageBase(), targetBase ) );
}
return (int)result.size();
}
void stm32fx07_poll(usbd_device *usbd_dev)
{
/* Read interrupt status register. */
uint32_t intsts = REBASE(OTG_GINTSTS);
int i;
if (intsts & OTG_GINTSTS_ENUMDNE) {
/* Handle USB RESET condition. */
REBASE(OTG_GINTSTS) = OTG_GINTSTS_ENUMDNE;
usbd_dev->fifo_mem_top = usbd_dev->driver->rx_fifo_size;
_usbd_reset(usbd_dev);
return;
}
/* Note: RX and TX handled differently in this device. */
if (intsts & OTG_GINTSTS_RXFLVL) {
/* Receive FIFO non-empty. */
uint32_t rxstsp = REBASE(OTG_GRXSTSP);
uint32_t pktsts = rxstsp & OTG_GRXSTSP_PKTSTS_MASK;
if ((pktsts != OTG_GRXSTSP_PKTSTS_OUT) &&
(pktsts != OTG_GRXSTSP_PKTSTS_SETUP)) {
return;
}
uint8_t ep = rxstsp & OTG_GRXSTSP_EPNUM_MASK;
uint8_t type;
if (pktsts == OTG_GRXSTSP_PKTSTS_SETUP) {
type = USB_TRANSACTION_SETUP;
} else {
type = USB_TRANSACTION_OUT;
}
/* Save packet size for stm32f107_ep_read_packet(). */
usbd_dev->rxbcnt = (rxstsp & OTG_GRXSTSP_BCNT_MASK) >> 4;
#if 0
/*
* FIXME: Why is a delay needed here?
* This appears to fix a problem where the first 4 bytes
* of the DATA OUT stage of a control transaction are lost.
*/
for (i = 0; i < 1000; i++) {
__asm__("nop");
}
#endif
if (usbd_dev->user_callback_ctr[ep][type]) {
usbd_dev->user_callback_ctr[ep][type] (usbd_dev, ep);
}
/* Discard unread packet data. */
for (i = 0; i < usbd_dev->rxbcnt; i += 4) {
(void)*REBASE_FIFO(ep);
}
usbd_dev->rxbcnt = 0;
}
u8 stm32fx07_ep_stall_get(usbd_device *usbd_dev, u8 addr)
{
/* Return non-zero if STALL set. */
if (addr & 0x80)
return (REBASE(OTG_DIEPCTL(addr & 0x7f)) &
OTG_FS_DIEPCTL0_STALL) ? 1 : 0;
else
return (REBASE(OTG_DOEPCTL(addr)) &
OTG_FS_DOEPCTL0_STALL) ? 1 : 0;
}
uint8_t stm32fx07_ep_stall_get(usbd_device *usbd_dev, uint8_t addr)
{
/* Return non-zero if STALL set. */
if (addr & 0x80) {
return (REBASE(OTG_DIEPCTL(addr & 0x7f)) &
OTG_DIEPCTL0_STALL) ? 1 : 0;
} else {
return (REBASE(OTG_DOEPCTL(addr)) &
OTG_DOEPCTL0_STALL) ? 1 : 0;
}
}
void stm32fx07_ep_nak_set(usbd_device *usbd_dev, u8 addr, u8 nak)
{
/* It does not make sence to force NAK on IN endpoints. */
if (addr & 0x80)
return;
usbd_dev->force_nak[addr] = nak;
if (nak)
REBASE(OTG_DOEPCTL(addr)) |= OTG_FS_DOEPCTL0_SNAK;
else
REBASE(OTG_DOEPCTL(addr)) |= OTG_FS_DOEPCTL0_CNAK;
}
void stm32fx07_ep_stall_set(usbd_device *usbd_dev, uint8_t addr, uint8_t stall)
{
if (addr == 0) {
if (stall) {
REBASE(OTG_DIEPCTL(addr)) |= OTG_FS_DIEPCTL0_STALL;
} else {
REBASE(OTG_DIEPCTL(addr)) &= ~OTG_FS_DIEPCTL0_STALL;
}
}
if (addr & 0x80) {
addr &= 0x7F;
if (stall) {
REBASE(OTG_DIEPCTL(addr)) |= OTG_FS_DIEPCTL0_STALL;
} else {
REBASE(OTG_DIEPCTL(addr)) &= ~OTG_FS_DIEPCTL0_STALL;
REBASE(OTG_DIEPCTL(addr)) |= OTG_FS_DIEPCTLX_SD0PID;
}
} else {
if (stall) {
REBASE(OTG_DOEPCTL(addr)) |= OTG_FS_DOEPCTL0_STALL;
} else {
REBASE(OTG_DOEPCTL(addr)) &= ~OTG_FS_DOEPCTL0_STALL;
REBASE(OTG_DOEPCTL(addr)) |= OTG_FS_DOEPCTLX_SD0PID;
}
}
}
void stm32fx07_ep_nak_set(usbd_device *usbd_dev, uint8_t addr, uint8_t nak)
{
/* It does not make sense to force NAK on IN endpoints. */
if (addr & 0x80) {
return;
}
usbd_dev->force_nak[addr] = nak;
if (nak) {
REBASE(OTG_DOEPCTL(addr)) |= OTG_DOEPCTL0_SNAK;
} else {
REBASE(OTG_DOEPCTL(addr)) |= OTG_DOEPCTL0_CNAK;
}
}
/// <summary>
/// Calculate and set security cookie
/// </summary>
/// <param name="pImage">image data</param>
/// <returns>true on success</returns>
bool MMap::InitializeCookie( ImageContext* pImage )
{
IMAGE_LOAD_CONFIG_DIRECTORY *pLC = reinterpret_cast<decltype(pLC)>(pImage->PEImage.DirectoryAddress( IMAGE_DIRECTORY_ENTRY_LOAD_CONFIG ));
if (pLC && pLC->SecurityCookie)
{
FILETIME systime = { 0 };
LARGE_INTEGER PerformanceCount = { 0 };
int cookie = 0;
//
// Cookie generation taken from bcryptprimitives.dll
//
GetSystemTimeAsFileTime( &systime );
QueryPerformanceCounter( &PerformanceCount );
cookie = systime.dwHighDateTime ^ systime.dwLowDateTime ^ _process.remote().getWorker()->id();
cookie ^= _process.pid();
cookie ^= PerformanceCount.LowPart;
cookie ^= PerformanceCount.HighPart;
cookie ^= (unsigned int)&cookie;
if (cookie == 0xBB40E64E)
cookie = 0xBB40E64F;
else if (!(cookie & 0xFFFF0000))
cookie |= (cookie | 0x4711) << 16;
_process.memory().Write( REBASE( pLC->SecurityCookie, pImage->PEImage.imageBase(), pImage->imgMem.ptr<ptr_t>() ), cookie );
}
return true;
}
bool MMap::InitializeCookie(ImageContext* pImage) {
auto pLC = reinterpret_cast<PIMAGE_LOAD_CONFIG_DIRECTORY>(pImage->peImage.DirectoryAddress(IMAGE_DIRECTORY_ENTRY_LOAD_CONFIG));
//
// Cookie generation based on MSVC++ compiler
//
if(pLC && pLC->SecurityCookie) {
BLACBONE_TRACE(L"ManualMap: Performing security cookie initializtion for image '%ls'", pImage->FileName.c_str());
FILETIME systime = {0};
LARGE_INTEGER PerformanceCount = {{0}};
uintptr_t cookie = 0;
GetSystemTimeAsFileTime(&systime);
QueryPerformanceCounter(&PerformanceCount);
cookie = _process.Id() ^ _process.Remote().GetWorkerThread()->Id() ^ reinterpret_cast<uintptr_t>(&cookie);
#ifdef USE64
cookie ^= *reinterpret_cast<uint64_t*>(&systime);
cookie ^= (PerformanceCount.QuadPart << 32) ^ PerformanceCount.QuadPart;
cookie &= 0xFFFFFFFFFFFF;
if(cookie == 0x2B992DDFA232)
cookie++;
#else
cookie ^= systime.dwHighDateTime ^ systime.dwLowDateTime;
cookie ^= PerformanceCount.LowPart;
cookie ^= PerformanceCount.HighPart;
if(cookie == 0xBB40E64E)
cookie++;
else if(!(cookie & 0xFFFF0000))
cookie |= (cookie | 0x4711) << 16;
#endif
_process.Memory().Write(REBASE(pLC->SecurityCookie, pImage->peImage.ImageBase(), pImage->imgMem.Ptr<ptr_t>()), cookie);
}
return true;
}
bool MMap::InitStaticTLS(ImageContext* pImage) {
IMAGE_TLS_DIRECTORY *pTls = reinterpret_cast<decltype(pTls)>(pImage->peImage.DirectoryAddress(IMAGE_DIRECTORY_ENTRY_TLS));
auto pRebasedTls = reinterpret_cast<IMAGE_TLS_DIRECTORY*>(REBASE(pTls, pImage->peImage.FileBase(), pImage->imgMem.Ptr<ptr_t>()));
// Use native TLS initialization
if(pTls && pTls->AddressOfIndex) {
BLACBONE_TRACE(L"ManualMap: Performing static TLS initialization for image '%ls'", pImage->FileName.c_str());
_process.NativeLdr().AddStaticTLSEntry(pImage->imgMem.Ptr<void*>(), pRebasedTls);
}
return true;
}
/// <summary>
/// Resolve static TLS storage
/// </summary>
/// <param name="pImage">image data</param>
/// <returns>true on success</returns>
bool MMap::InitStaticTLS( ImageContext* pImage )
{
IMAGE_TLS_DIRECTORY *pTls = reinterpret_cast<decltype(pTls)>(pImage->PEImage.DirectoryAddress( IMAGE_DIRECTORY_ENTRY_TLS ));
auto pRebasedTls = reinterpret_cast<IMAGE_TLS_DIRECTORY*>(REBASE( pTls, pImage->FileImage.base(), pImage->imgMem.ptr<ptr_t>() ));
// Use native TLS initialization
if (pTls && pTls->AddressOfIndex)
_process.nativeLdr().AddStaticTLSEntry( pImage->imgMem.ptr<void*>(), pRebasedTls );
return true;
}
void stm32fx07_endpoints_reset(usbd_device *usbd_dev)
{
int i;
/* The core resets the endpoints automatically on reset. */
usbd_dev->fifo_mem_top = usbd_dev->fifo_mem_top_ep0;
/* Disable any currently active endpoints */
for (i = 1; i < 4; i++) {
if (REBASE(OTG_DOEPCTL(i)) & OTG_DOEPCTL0_EPENA) {
REBASE(OTG_DOEPCTL(i)) |= OTG_DOEPCTL0_EPDIS;
}
if (REBASE(OTG_DIEPCTL(i)) & OTG_DIEPCTL0_EPENA) {
REBASE(OTG_DIEPCTL(i)) |= OTG_DIEPCTL0_EPDIS;
}
}
/* Flush all tx/rx fifos */
REBASE(OTG_GRSTCTL) = OTG_GRSTCTL_TXFFLSH | OTG_GRSTCTL_TXFNUM_ALL
| OTG_GRSTCTL_RXFFLSH;
}
/// <summary>
/// Set custom exception handler to bypass SafeSEH under DEP
/// </summary>
/// <param name="pImage">image data</param>
/// <returns>true on success</returns>
bool MMap::EnableExceptions( ImageContext* pImage )
{
#ifdef _M_AMD64
size_t size = pImage->PEImage.DirectorySize( IMAGE_DIRECTORY_ENTRY_EXCEPTION );
IMAGE_RUNTIME_FUNCTION_ENTRY *pExpTable =
reinterpret_cast<decltype(pExpTable)>(pImage->PEImage.DirectoryAddress( IMAGE_DIRECTORY_ENTRY_EXCEPTION ));
// Invoke RtlAddFunctionTable
if(pExpTable)
{
AsmJit::Assembler a;
AsmJitHelper ah(a);
uint64_t result = 0;
pImage->pExpTableAddr = REBASE( pExpTable, pImage->FileImage.base(), pImage->imgMem.ptr<ptr_t>() );
auto pAddTable = _process.modules().GetExport( _process.modules().GetModule( L"ntdll.dll", LdrList, pImage->PEImage.mType() ),
"RtlAddFunctionTable" );
ah.GenPrologue();
ah.GenCall( static_cast<size_t>(pAddTable.procAddress), { pImage->pExpTableAddr,
size / sizeof(IMAGE_RUNTIME_FUNCTION_ENTRY),
pImage->imgMem.ptr<size_t>() } );
_process.remote().AddReturnWithEvent( ah );
ah.GenEpilogue();
if (_process.remote().ExecInWorkerThread( a.make(), a.getCodeSize(), result ) != STATUS_SUCCESS)
return false;
if (pImage->flags & CreateLdrRef)
return true;
else
return (MExcept::CreateVEH( pImage->imgMem.ptr<size_t>(), pImage->PEImage.imageSize() ) == STATUS_SUCCESS);
}
else
return false;
#else
_process.nativeLdr().InsertInvertedFunctionTable( pImage->imgMem.ptr<void*>(), pImage->PEImage.imageSize() );
if (pImage->flags & PartialExcept)
return true;
else
return (MExcept::CreateVEH( pImage->imgMem.ptr<size_t>(), pImage->PEImage.imageSize() ) == STATUS_SUCCESS);
#endif
}
NTSTATUS MMap::EnableExceptions(ImageContext* pImage) {
BLACBONE_TRACE(L"ManualMap: Enabling exception support for image '%ls'", pImage->FileName.c_str());
bool partial = (pImage->flags & PartialExcept) != 0;
#ifdef USE64
size_t size = pImage->peImage.DirectorySize(IMAGE_DIRECTORY_ENTRY_EXCEPTION);
auto pExpTable = reinterpret_cast<PIMAGE_RUNTIME_FUNCTION_ENTRY>(pImage->peImage.DirectoryAddress(IMAGE_DIRECTORY_ENTRY_EXCEPTION));
// Invoke RtlAddFunctionTable
if(pExpTable) {
AsmJitHelper a;
uint64_t result = 0;
pImage->pExpTableAddr = REBASE(pExpTable, pImage->peImage.FileBase(), pImage->imgMem.Ptr<ptr_t>());
auto pAddTable = _process.Modules().GetExport(
_process.Modules().GetModule(L"ntdll.dll", LdrList, pImage->peImage.ImageType()),
"RtlAddFunctionTable"
);
a.GenPrologue();
a.GenCall(
static_cast<size_t>(pAddTable.procAddress), {
pImage->pExpTableAddr,
size / sizeof(IMAGE_RUNTIME_FUNCTION_ENTRY),
pImage->imgMem.Ptr<size_t>()}
);
_process.Remote().AddReturnWithEvent(a, pImage->peImage.ImageType());
a.GenEpilogue();
auto status = _process.Remote().ExecInWorkerThread(a->make(), a->getCodeSize(), result);
if(!NT_SUCCESS(status))
return status;
return (pImage->flags & CreateLdrRef) ? STATUS_SUCCESS :
MExcept::CreateVEH(pImage->imgMem.Ptr<size_t>(), pImage->peImage.ImageSize(), pImage->peImage.ImageType(), partial);
}
// No exception table
else
return STATUS_NOT_FOUND;
#else
bool safeseh = false;
if(!_process.NativeLdr().InsertInvertedFunctionTable(pImage->imgMem.Ptr<void*>(), pImage->peImage.ImageSize(), safeseh))
return STATUS_UNSUCCESSFUL;
return safeseh ? STATUS_SUCCESS :
MExcept::CreateVEH(pImage->imgMem.Ptr<size_t>(), pImage->peImage.ImageSize(), pImage->peImage.ImageType(), partial);
#endif
}
void stm32fx07_ep_setup(usbd_device *usbd_dev, uint8_t addr, uint8_t type,
uint16_t max_size,
void (*callback) (usbd_device *usbd_dev, uint8_t ep))
{
/*
* Configure endpoint address and type. Allocate FIFO memory for
* endpoint. Install callback funciton.
*/
uint8_t dir = addr & 0x80;
addr &= 0x7f;
if (addr == 0) { /* For the default control endpoint */
/* Configure IN part. */
if (max_size >= 64) {
REBASE(OTG_DIEPCTL0) = OTG_FS_DIEPCTL0_MPSIZ_64;
} else if (max_size >= 32) {
REBASE(OTG_DIEPCTL0) = OTG_FS_DIEPCTL0_MPSIZ_32;
} else if (max_size >= 16) {
REBASE(OTG_DIEPCTL0) = OTG_FS_DIEPCTL0_MPSIZ_16;
} else {
REBASE(OTG_DIEPCTL0) = OTG_FS_DIEPCTL0_MPSIZ_8;
}
REBASE(OTG_DIEPTSIZ0) =
(max_size & OTG_FS_DIEPSIZ0_XFRSIZ_MASK);
REBASE(OTG_DIEPCTL0) |=
OTG_FS_DIEPCTL0_EPENA | OTG_FS_DIEPCTL0_SNAK;
/* Configure OUT part. */
usbd_dev->doeptsiz[0] = OTG_FS_DIEPSIZ0_STUPCNT_1 |
OTG_FS_DIEPSIZ0_PKTCNT |
(max_size & OTG_FS_DIEPSIZ0_XFRSIZ_MASK);
REBASE(OTG_DOEPTSIZ(0)) = usbd_dev->doeptsiz[0];
REBASE(OTG_DOEPCTL(0)) |=
OTG_FS_DOEPCTL0_EPENA | OTG_FS_DIEPCTL0_SNAK;
REBASE(OTG_GNPTXFSIZ) = ((max_size / 4) << 16) |
usbd_dev->driver->rx_fifo_size;
usbd_dev->fifo_mem_top += max_size / 4;
usbd_dev->fifo_mem_top_ep0 = usbd_dev->fifo_mem_top;
return;
}
if (dir) {
REBASE(OTG_DIEPTXF(addr)) = ((max_size / 4) << 16) |
usbd_dev->fifo_mem_top;
usbd_dev->fifo_mem_top += max_size / 4;
REBASE(OTG_DIEPTSIZ(addr)) =
(max_size & OTG_FS_DIEPSIZ0_XFRSIZ_MASK);
REBASE(OTG_DIEPCTL(addr)) |=
OTG_FS_DIEPCTL0_EPENA | OTG_FS_DIEPCTL0_SNAK | (type << 18)
| OTG_FS_DIEPCTL0_USBAEP | OTG_FS_DIEPCTLX_SD0PID
| (addr << 22) | max_size;
if (callback) {
usbd_dev->user_callback_ctr[addr][USB_TRANSACTION_IN] =
(void *)callback;
}
}
if (!dir) {
usbd_dev->doeptsiz[addr] = OTG_FS_DIEPSIZ0_PKTCNT |
(max_size & OTG_FS_DIEPSIZ0_XFRSIZ_MASK);
REBASE(OTG_DOEPTSIZ(addr)) = usbd_dev->doeptsiz[addr];
REBASE(OTG_DOEPCTL(addr)) |= OTG_FS_DOEPCTL0_EPENA |
OTG_FS_DOEPCTL0_USBAEP | OTG_FS_DIEPCTL0_CNAK |
OTG_FS_DOEPCTLX_SD0PID | (type << 18) | max_size;
if (callback) {
usbd_dev->user_callback_ctr[addr][USB_TRANSACTION_OUT] =
(void *)callback;
}
}
}
void stm32fx07_set_address(usbd_device *usbd_dev, uint8_t addr)
{
REBASE(OTG_DCFG) = (REBASE(OTG_DCFG) & ~OTG_FS_DCFG_DAD) | (addr << 4);
}
