double
__libmcr_mx_log(double x, double *err)
{
int hx, i, j, n, na;
double y, p, q, q1, qi, q2, s, f1, f2, u, w, s1, s2, w1, w2, z1,
z2, t1, t2, g1, g2, t;
hx = HIGH_WORD(x);
*err = 0.0;
na = 0;
/* filter out exception argument */
/* Redundant exception handling code: see __libmcr_log */
#if 0
if ((hx + 0x00208000) < 0x00308000) {
/* subnormal,0,negative,inf,nan,huge */
i = ((unsigned) (hx << 1)) >> 1;
if (hx >= 0x7ff00000)
return (x + x); /* x is inf/nan */
if ((i | LOW_WORD(x)) == 0)
return (-one / (x * x)); /* log(+-0) is -inf */
if (hx < 0)
return ((x - x) / zero); /* log(-#) is NaN */
#endif
if (hx < 0x00100000) { /* subnormal */
x *= two54;
hx = HIGH_WORD(x);
na = -54;
}
/* Redundant exception handling code: see __libmcr_log */
#if 0
}
/**
* vmw_send_msg: Sends a message to the host
*
* @channel: RPC channel
* @logmsg: NULL terminated string
*
* Returns: 0 on success
*/
static int vmw_send_msg(struct rpc_channel *channel, const char *msg)
{
unsigned long eax, ebx, ecx, edx, si, di, bp;
size_t msg_len = strlen(msg);
int retries = 0;
while (retries < RETRIES) {
retries++;
/* Set up additional parameters */
si = channel->cookie_high;
di = channel->cookie_low;
VMW_PORT(VMW_PORT_CMD_SENDSIZE,
msg_len, si, di,
VMW_HYPERVISOR_PORT | (channel->channel_id << 16),
VMW_HYPERVISOR_MAGIC,
eax, ebx, ecx, edx, si, di);
if ((HIGH_WORD(ecx) & MESSAGE_STATUS_SUCCESS) == 0 ||
(HIGH_WORD(ecx) & MESSAGE_STATUS_HB) == 0) {
/* Expected success + high-bandwidth. Give up. */
return -EINVAL;
}
/* Send msg */
si = (uintptr_t) msg;
di = channel->cookie_low;
bp = channel->cookie_high;
VMW_PORT_HB_OUT(
(MESSAGE_STATUS_SUCCESS << 16) | VMW_PORT_CMD_HB_MSG,
msg_len, si, di,
VMW_HYPERVISOR_HB_PORT | (channel->channel_id << 16),
VMW_HYPERVISOR_MAGIC, bp,
eax, ebx, ecx, edx, si, di);
if ((HIGH_WORD(ebx) & MESSAGE_STATUS_SUCCESS) != 0) {
return 0;
} else if ((HIGH_WORD(ebx) & MESSAGE_STATUS_CPT) != 0) {
/* A checkpoint occurred. Retry. */
continue;
} else {
break;
}
}
return -EINVAL;
}
uint8_t receiveSingleFrame() {
uint8_t ret = LW232_OK;
uint8_t idx = 0;
if (CAN_OK == readMsgBufID(&lw232_CanId, &lw232_PacketLen, lw232_Buffer)) {
if (lw232_CanId > 0x1FFFFFFF) {
ret = LW232_ERR; // address if totally wrong
}
else if (checkPassFilter(lw232_CanId)) {// do we want to skip some addresses?
if (isExtendedFrame()) {
printChar(LW232_TR29);
printFullByte(HIGH_BYTE(HIGH_WORD(lw232_CanId)));
printFullByte(LOW_BYTE(HIGH_WORD(lw232_CanId)));
printFullByte(HIGH_BYTE(LOW_WORD(lw232_CanId)));
printFullByte(LOW_BYTE(LOW_WORD(lw232_CanId)));
}
else {
printChar(LW232_TR11);
printNibble(HIGH_BYTE(LOW_WORD(lw232_CanId)));
printFullByte(LOW_BYTE(LOW_WORD(lw232_CanId)));
}
//write data len
printNibble(lw232_PacketLen);
//write data
for (idx = 0; idx < lw232_PacketLen; idx++) {
printFullByte(lw232_Buffer[idx]);
}
//write timestamp if needed
if (lw232_TimeStamp != LW232_TIMESTAMP_OFF) {
uint32_t time = 0; //millis();
if (lw232_TimeStamp == LW232_TIMESTAMP_ON_NORMAL) {
// standard LAWICEL protocol. two bytes.
time %= 60000;
} else {
// non standard protocol - 4 bytes timestamp
printFullByte(HIGH_BYTE(HIGH_WORD(time)));
printFullByte(LOW_BYTE(HIGH_WORD(time)));
}
printFullByte(HIGH_BYTE(LOW_WORD(time)));
printFullByte(LOW_BYTE(LOW_WORD(time)));
}
}
}
else {
ret = LW232_ERR;
}
return ret;
}
BOOL Dumbfuzzer::GetRandomIoctlAndBuffer(DWORD &iocode, vector<UCHAR> &output, mt19937 *threadRandomProvider)
{
BOOL bResult=FALSE;
INT i=0;
UINT r;
DWORD size=0, ioctlIndex;
r = (*threadRandomProvider)();
// Pick random ioctl def
ioctlIndex = LOW_WORD(r)%ioStore.size();
// Get random size between low and high limits (prevent divide by zero)
if(ioStore[ioctlIndex].dwUpperSize-ioStore[ioctlIndex].dwLowerSize) {
size = ioStore[ioctlIndex].dwLowerSize+(HIGH_WORD(r)%(ioStore[ioctlIndex].dwUpperSize-ioStore[ioctlIndex].dwLowerSize));
}
output.resize(size);
if(size>4) {
for(i=0; i<(INT)(size-sizeof(INT)); i+=sizeof(INT)) {
*(PUINT)(&(output)[i]) = (*threadRandomProvider)();
}
}
// Last DWORD
r = (*threadRandomProvider)();
for(INT j=0; i<(INT)size; i++,j+=8) {
output[i] = (UCHAR)((r>>j)&0xff);
}
// Set code
iocode = ioStore[ioctlIndex].dwIOCTL;
bResult = TRUE;
return bResult;
}
INT8U Can232::receiveSingleFrame() {
INT8U ret = LW232_OK;
INT8U idx = 0;
if (CAN_OK == readMsgBufID(&lw232CanId, &lw232PacketLen, lw232Buffer)) {
if (lw232CanId > 0x1FFFFFFF) {
ret = LW232_ERR; // address if totally wrong
}
else if (checkPassFilter(lw232CanId)) {// do we want to skip some addresses?
if (isExtendedFrame()) {
Serial.print(LW232_TR29);
HexHelper::printFullByte(HIGH_BYTE(HIGH_WORD(lw232CanId)));
HexHelper::printFullByte(LOW_BYTE(HIGH_WORD(lw232CanId)));
HexHelper::printFullByte(HIGH_BYTE(LOW_WORD(lw232CanId)));
HexHelper::printFullByte(LOW_BYTE(LOW_WORD(lw232CanId)));
}
else {
Serial.print(LW232_TR11);
HexHelper::printNibble(HIGH_BYTE(LOW_WORD(lw232CanId)));
HexHelper::printFullByte(LOW_BYTE(LOW_WORD(lw232CanId)));
}
//write data len
HexHelper::printNibble(lw232PacketLen);
//write data
for (idx = 0; idx < lw232PacketLen; idx++) {
HexHelper::printFullByte(lw232Buffer[idx]);
}
//write timestamp if needed
if (lw232TimeStamp != LW232_TIMESTAMP_OFF) {
INT32U time = millis();
if (lw232TimeStamp == LW232_TIMESTAMP_ON_NORMAL) {
// standard LAWICEL protocol. two bytes.
time %= 60000;
} else {
// non standard protocol - 4 bytes timestamp
HexHelper::printFullByte(HIGH_BYTE(HIGH_WORD(time)));
HexHelper::printFullByte(LOW_BYTE(HIGH_WORD(time)));
}
HexHelper::printFullByte(HIGH_BYTE(LOW_WORD(time)));
HexHelper::printFullByte(LOW_BYTE(LOW_WORD(time)));
}
}
}
else {
ret = LW232_ERR;
}
return ret;
}
/**
* vmw_open_channel
*
* @channel: RPC channel
* @protocol:
*
* Returns: 0 on success
*/
static int vmw_open_channel(struct rpc_channel *channel, unsigned int protocol)
{
unsigned long eax, ebx, ecx, edx, si = 0, di = 0;
VMW_PORT(VMW_PORT_CMD_OPEN_CHANNEL,
(protocol | GUESTMSG_FLAG_COOKIE), si, di,
VMW_HYPERVISOR_PORT,
VMW_HYPERVISOR_MAGIC,
eax, ebx, ecx, edx, si, di);
if ((HIGH_WORD(ecx) & MESSAGE_STATUS_SUCCESS) == 0)
return -EINVAL;
channel->channel_id = HIGH_WORD(edx);
channel->cookie_high = si;
channel->cookie_low = di;
return 0;
}
void logic_state_run(uint32_t msg) {
switch (HIGH_WORD(msg)) {
case MSG_SWITCH_BRAKE:
logic_brake(LOW_BYTE(LOW_WORD(msg)));
break;
case MSG_SWITCH_LTURN:
logic_lturn(LOW_BYTE(LOW_WORD(msg)));
break;
case MSG_SWITCH_RTURN:
logic_rturn(LOW_BYTE(LOW_WORD(msg)));
break;
case MSG_SWITCH_HEADLAMP:
logic_headlamp(LOW_BYTE(LOW_WORD(msg)));
break;
case MSG_SWITCH_HIGHBEAM:
logic_highbeam(LOW_BYTE(LOW_WORD(msg)));
break;
case MSG_SWITCH_REVERSE:
logic_reverse(LOW_BYTE(LOW_WORD(msg)));
break;
case MSG_BLINKER_BLINK:
if (blinker_is_on(LAMP_LTURN)) {
lamp_toggle(LAMP_LTURN);
usart_send(PACK_WORDS(MSG_LAMP_LTURN, lamp_is_on(LAMP_LTURN)));
}
if (blinker_is_on(LAMP_RTURN)) {
lamp_toggle(LAMP_RTURN);
usart_send(PACK_WORDS(MSG_LAMP_RTURN, lamp_is_on(LAMP_RTURN)));
}
break;
case MSG_SWITCH_START:
if (!LOW_WORD(msg)) {
blinker_disable();
lamp_disable();
usart_send(PACK_WORDS(MSG_STANDBY, 0));
state_set(LOGIC_STANDBY);
/* TODO: control display module power */
/* display_power_set(OFF); */
}
break;
case MSG_DISPLAY_READY:
logic_sync_lamps();
break;
default:
console_log("event ignored");
}
}
/**
* vmw_close_channel
*
* @channel: RPC channel
*
* Returns: 0 on success
*/
static int vmw_close_channel(struct rpc_channel *channel)
{
unsigned long eax, ebx, ecx, edx, si, di;
/* Set up additional parameters */
si = channel->cookie_high;
di = channel->cookie_low;
VMW_PORT(VMW_PORT_CMD_CLOSE_CHANNEL,
0, si, di,
(VMW_HYPERVISOR_PORT | (channel->channel_id << 16)),
VMW_HYPERVISOR_MAGIC,
eax, ebx, ecx, edx, si, di);
if ((HIGH_WORD(ecx) & MESSAGE_STATUS_SUCCESS) == 0)
return -EINVAL;
return 0;
}
void logic_state_standby(uint32_t msg) {
switch (HIGH_WORD(msg)) {
case MSG_SWITCH_START:
if (LOW_WORD(msg)) {
/* TODO: control display module power */
/* display_power_set(ON); */
usart_send(PACK_WORDS(MSG_DISPLAY_RUN, 0));
logic_sync_lamps();
state_set(LOGIC_RUN);
}
break;
case MSG_DISPLAY_READY:
/* The display was powered on but we're in standby mode, presumably */
/* because the key-switch is off. This is not a normal situation if */
/* power to the display is controlled by the main module. */
usart_send(PACK_WORDS(MSG_DISPLAY_RUN, 0));
break;
default:
console_log("event ignored");
}
}
BOOL Dumbfuzzer::GetRandomIoctlAndBuffer(DWORD &iocode, vector<UCHAR> &output, mt19937 *threadRandomProvider)
{
BOOL bResult=FALSE;
INT i=0;
UINT r;
DWORD size=0, ioctlIndex;
r = (*threadRandomProvider)();
// Pick random ioctl def
ioctlIndex = LOW_WORD(r)%ioStore.size();
// Get random size between low and high limits (prevent divide by zero)
if(ioStore[ioctlIndex].dwUpperSize-ioStore[ioctlIndex].dwLowerSize) {
size = ioStore[ioctlIndex].dwLowerSize+(HIGH_WORD(r)%(ioStore[ioctlIndex].dwUpperSize-ioStore[ioctlIndex].dwLowerSize));
}
// If the sizes are equal, take the upper size
else if(ioStore[ioctlIndex].dwUpperSize == ioStore[ioctlIndex].dwLowerSize) {
size = ioStore[ioctlIndex].dwUpperSize;
}
output.resize(size);
if(size>4) {
for(i=0; i<(INT)(size-sizeof(INT)); i+=sizeof(INT)) {
*(PUINT)(&(output)[i]) = (*threadRandomProvider)();
}
}
// Last DWORD
r = (*threadRandomProvider)();
for(INT j=0; i<(INT)size; i++,j+=8) {
output[i] = (UCHAR)((r>>j)&0xff);
}
// Every once in awhile, shove the size in the first DWORD of buffer
if (!(r % SHOVE_LENGTH_FREQ) && size > sizeof(DWORD)) {
*(PDWORD)(&(output)[0]) = size;
}
// Set code
iocode = ioStore[ioctlIndex].dwIOCTL;
bResult = TRUE;
return bResult;
}
extern "C" FARPROC __stdcall dllGetProcAddress(HMODULE hModule, LPCSTR function)
{
uintptr_t loc = (uintptr_t)_ReturnAddress();
void* address = NULL;
LibraryLoader* dll = DllLoaderContainer::GetModule(hModule);
if( !dll )
{
CLog::Log(LOGERROR, "%s - Invalid hModule specified",__FUNCTION__);
return NULL;
}
/* how can somebody get the stupid idea to create such a stupid function */
/* where you never know if the given pointer is a pointer or a value */
if( HIGH_WORD(function) == 0 && LOW_WORD(function) < 1000)
{
if( dll->ResolveOrdinal(LOW_WORD(function), &address) )
{
CLog::Log(LOGDEBUG, "%s(%p(%s), %d) => %p", __FUNCTION__, hModule, dll->GetName(), LOW_WORD(function), address);
}
else if( dll->IsSystemDll() )
{
char ordinal[5];
sprintf(ordinal, "%d", LOW_WORD(function));
address = (void*)create_dummy_function(dll->GetName(), ordinal);
/* add to tracklist if we are tracking this source dll */
DllTrackInfo* track = tracker_get_dlltrackinfo(loc);
if( track )
tracker_dll_data_track(track->pDll, (uintptr_t)address);
CLog::Log(LOGDEBUG, "%s - created dummy function %s!%s",__FUNCTION__, dll->GetName(), ordinal);
}
else
{
address = NULL;
CLog::Log(LOGDEBUG, "%s(%p(%s), '%s') => %p",__FUNCTION__ , hModule, dll->GetName(), function, address);
}
}
else
{
if( dll->ResolveExport(function, &address) )
{
CLog::Log(LOGDEBUG, "%s(%p(%s), '%s') => %p",__FUNCTION__ , hModule, dll->GetName(), function, address);
}
else
{
DllTrackInfo* track = tracker_get_dlltrackinfo(loc);
/* some dll's require us to always return a function or it will fail, other's */
/* decide functionallity depending on if the functions exist and may fail */
if( dll->IsSystemDll() && track
&& stricmp(track->pDll->GetName(), "CoreAVCDecoder.ax") == 0 )
{
address = (void*)create_dummy_function(dll->GetName(), function);
tracker_dll_data_track(track->pDll, (uintptr_t)address);
CLog::Log(LOGDEBUG, "%s - created dummy function %s!%s", __FUNCTION__, dll->GetName(), function);
}
else
{
address = NULL;
CLog::Log(LOGDEBUG, "%s(%p(%s), '%s') => %p", __FUNCTION__, hModule, dll->GetName(), function, address);
}
}
}
return (FARPROC)address;
}
enum MHI_STATUS mhi_init_mmio(struct mhi_device_ctxt *mhi_dev_ctxt)
{
u64 pcie_dword_val = 0;
u32 pcie_word_val = 0;
u32 i = 0;
enum MHI_STATUS ret_val;
mhi_log(MHI_MSG_INFO, "~~~ Initializing MMIO ~~~\n");
mhi_dev_ctxt->mmio_addr = mhi_dev_ctxt->dev_props->bar0_base;
mhi_log(MHI_MSG_INFO, "Bar 0 address is at: 0x%p\n",
mhi_dev_ctxt->mmio_addr);
mhi_dev_ctxt->mmio_len = mhi_reg_read(mhi_dev_ctxt->mmio_addr,
MHIREGLEN);
if (0 == mhi_dev_ctxt->mmio_len) {
mhi_log(MHI_MSG_ERROR, "Received mmio length as zero\n");
return MHI_STATUS_ERROR;
}
mhi_log(MHI_MSG_INFO, "Testing MHI Ver\n");
mhi_dev_ctxt->dev_props->mhi_ver = mhi_reg_read(
mhi_dev_ctxt->mmio_addr, MHIVER);
if (MHI_VERSION != mhi_dev_ctxt->dev_props->mhi_ver) {
mhi_log(MHI_MSG_CRITICAL, "Bad MMIO version, 0x%x\n",
mhi_dev_ctxt->dev_props->mhi_ver);
if (mhi_dev_ctxt->dev_props->mhi_ver == 0xFFFFFFFF)
ret_val = mhi_wait_for_mdm(mhi_dev_ctxt);
if (ret_val)
return MHI_STATUS_ERROR;
}
/* Enable the channels */
for (i = 0; i < MHI_MAX_CHANNELS; ++i) {
struct mhi_chan_ctxt *chan_ctxt =
&mhi_dev_ctxt->mhi_ctrl_seg->mhi_cc_list[i];
if (VALID_CHAN_NR(i))
chan_ctxt->mhi_chan_state = MHI_CHAN_STATE_ENABLED;
else
chan_ctxt->mhi_chan_state = MHI_CHAN_STATE_DISABLED;
}
mhi_log(MHI_MSG_INFO,
"Read back MMIO Ready bit successfully. Moving on..\n");
mhi_log(MHI_MSG_INFO, "Reading channel doorbell offset\n");
mhi_dev_ctxt->channel_db_addr = mhi_dev_ctxt->mmio_addr;
mhi_dev_ctxt->event_db_addr = mhi_dev_ctxt->mmio_addr;
mhi_dev_ctxt->channel_db_addr += mhi_reg_read_field(
mhi_dev_ctxt->mmio_addr,
CHDBOFF, CHDBOFF_CHDBOFF_MASK,
CHDBOFF_CHDBOFF_SHIFT);
mhi_log(MHI_MSG_INFO, "Reading event doorbell offset\n");
mhi_dev_ctxt->event_db_addr += mhi_reg_read_field(
mhi_dev_ctxt->mmio_addr,
ERDBOFF, ERDBOFF_ERDBOFF_MASK,
ERDBOFF_ERDBOFF_SHIFT);
mhi_log(MHI_MSG_INFO, "Setting all MMIO values.\n");
mhi_reg_write_field(mhi_dev_ctxt, mhi_dev_ctxt->mmio_addr, MHICFG,
MHICFG_NER_MASK, MHICFG_NER_SHIFT,
MHI_MAX_CHANNELS);
pcie_dword_val = mhi_v2p_addr(mhi_dev_ctxt->mhi_ctrl_seg_info,
(uintptr_t)mhi_dev_ctxt->mhi_ctrl_seg->mhi_cc_list);
pcie_word_val = HIGH_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, CCABAP_HIGHER,
CCABAP_HIGHER_CCABAP_HIGHER_MASK,
CCABAP_HIGHER_CCABAP_HIGHER_SHIFT, pcie_word_val);
pcie_word_val = LOW_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt, mhi_dev_ctxt->mmio_addr, CCABAP_LOWER,
CCABAP_LOWER_CCABAP_LOWER_MASK,
CCABAP_LOWER_CCABAP_LOWER_SHIFT,
pcie_word_val);
/* Write the Event Context Base Address Register High and Low parts */
pcie_dword_val = mhi_v2p_addr(mhi_dev_ctxt->mhi_ctrl_seg_info,
(uintptr_t)mhi_dev_ctxt->mhi_ctrl_seg->mhi_ec_list);
pcie_word_val = HIGH_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, ECABAP_HIGHER,
ECABAP_HIGHER_ECABAP_HIGHER_MASK,
ECABAP_HIGHER_ECABAP_HIGHER_SHIFT, pcie_word_val);
pcie_word_val = LOW_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt, mhi_dev_ctxt->mmio_addr, ECABAP_LOWER,
ECABAP_LOWER_ECABAP_LOWER_MASK,
ECABAP_LOWER_ECABAP_LOWER_SHIFT, pcie_word_val);
/* Write the Command Ring Control Register High and Low parts */
pcie_dword_val = mhi_v2p_addr(mhi_dev_ctxt->mhi_ctrl_seg_info,
(uintptr_t)mhi_dev_ctxt->mhi_ctrl_seg->mhi_cmd_ctxt_list);
pcie_word_val = HIGH_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, CRCBAP_HIGHER,
CRCBAP_HIGHER_CRCBAP_HIGHER_MASK,
CRCBAP_HIGHER_CRCBAP_HIGHER_SHIFT,
pcie_word_val);
pcie_word_val = LOW_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, CRCBAP_LOWER,
CRCBAP_LOWER_CRCBAP_LOWER_MASK,
CRCBAP_LOWER_CRCBAP_LOWER_SHIFT,
pcie_word_val);
mhi_dev_ctxt->cmd_db_addr = mhi_dev_ctxt->mmio_addr + CRDB_LOWER;
/* Set the control segment in the MMIO */
pcie_dword_val = mhi_v2p_addr(mhi_dev_ctxt->mhi_ctrl_seg_info,
(uintptr_t)mhi_dev_ctxt->mhi_ctrl_seg);
pcie_word_val = HIGH_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, MHICTRLBASE_HIGHER,
MHICTRLBASE_HIGHER_MHICTRLBASE_HIGHER_MASK,
MHICTRLBASE_HIGHER_MHICTRLBASE_HIGHER_SHIFT,
pcie_word_val);
pcie_word_val = LOW_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, MHICTRLBASE_LOWER,
MHICTRLBASE_LOWER_MHICTRLBASE_LOWER_MASK,
MHICTRLBASE_LOWER_MHICTRLBASE_LOWER_SHIFT,
pcie_word_val);
pcie_dword_val = mhi_v2p_addr(mhi_dev_ctxt->mhi_ctrl_seg_info,
(uintptr_t)mhi_dev_ctxt->mhi_ctrl_seg) +
mhi_get_memregion_len(mhi_dev_ctxt->mhi_ctrl_seg_info) - 1;
pcie_word_val = HIGH_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, MHICTRLLIMIT_HIGHER,
MHICTRLLIMIT_HIGHER_MHICTRLLIMIT_HIGHER_MASK,
MHICTRLLIMIT_HIGHER_MHICTRLLIMIT_HIGHER_SHIFT,
pcie_word_val);
pcie_word_val = LOW_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, MHICTRLLIMIT_LOWER,
MHICTRLLIMIT_LOWER_MHICTRLLIMIT_LOWER_MASK,
MHICTRLLIMIT_LOWER_MHICTRLLIMIT_LOWER_SHIFT,
pcie_word_val);
/* Set the data segment in the MMIO */
pcie_dword_val = MHI_DATA_SEG_WINDOW_START_ADDR;
pcie_word_val = HIGH_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, MHIDATABASE_HIGHER,
MHIDATABASE_HIGHER_MHIDATABASE_HIGHER_MASK,
MHIDATABASE_HIGHER_MHIDATABASE_HIGHER_SHIFT,
pcie_word_val);
pcie_word_val = LOW_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, MHIDATABASE_LOWER,
MHIDATABASE_LOWER_MHIDATABASE_LOWER_MASK,
MHIDATABASE_LOWER_MHIDATABASE_LOWER_SHIFT,
pcie_word_val);
pcie_dword_val = MHI_DATA_SEG_WINDOW_END_ADDR;
pcie_word_val = HIGH_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, MHIDATALIMIT_HIGHER,
MHIDATALIMIT_HIGHER_MHIDATALIMIT_HIGHER_MASK,
MHIDATALIMIT_HIGHER_MHIDATALIMIT_HIGHER_SHIFT,
pcie_word_val);
pcie_word_val = LOW_WORD(pcie_dword_val);
mhi_reg_write_field(mhi_dev_ctxt,
mhi_dev_ctxt->mmio_addr, MHIDATALIMIT_LOWER,
MHIDATALIMIT_LOWER_MHIDATALIMIT_LOWER_MASK,
MHIDATALIMIT_LOWER_MHIDATALIMIT_LOWER_SHIFT,
pcie_word_val);
mhi_log(MHI_MSG_INFO, "Done..\n");
return MHI_STATUS_SUCCESS;
}
/**
* vmw_recv_msg: Receives a message from the host
*
* Note: It is the caller's responsibility to call kfree() on msg.
*
* @channel: channel opened by vmw_open_channel
* @msg: [OUT] message received from the host
* @msg_len: message length
*/
static int vmw_recv_msg(struct rpc_channel *channel, void **msg,
size_t *msg_len)
{
unsigned long eax, ebx, ecx, edx, si, di, bp;
char *reply;
size_t reply_len;
int retries = 0;
*msg_len = 0;
*msg = NULL;
while (retries < RETRIES) {
retries++;
/* Set up additional parameters */
si = channel->cookie_high;
di = channel->cookie_low;
VMW_PORT(VMW_PORT_CMD_RECVSIZE,
0, si, di,
(VMW_HYPERVISOR_PORT | (channel->channel_id << 16)),
VMW_HYPERVISOR_MAGIC,
eax, ebx, ecx, edx, si, di);
if ((HIGH_WORD(ecx) & MESSAGE_STATUS_SUCCESS) == 0 ||
(HIGH_WORD(ecx) & MESSAGE_STATUS_HB) == 0) {
DRM_ERROR("Failed to get reply size\n");
return -EINVAL;
}
/* No reply available. This is okay. */
if ((HIGH_WORD(ecx) & MESSAGE_STATUS_DORECV) == 0)
return 0;
reply_len = ebx;
reply = kzalloc(reply_len + 1, GFP_KERNEL);
if (reply == NULL) {
DRM_ERROR("Cannot allocate memory for reply\n");
return -ENOMEM;
}
/* Receive buffer */
si = channel->cookie_high;
di = (uintptr_t) reply;
bp = channel->cookie_low;
VMW_PORT_HB_IN(
(MESSAGE_STATUS_SUCCESS << 16) | VMW_PORT_CMD_HB_MSG,
reply_len, si, di,
VMW_HYPERVISOR_HB_PORT | (channel->channel_id << 16),
VMW_HYPERVISOR_MAGIC, bp,
eax, ebx, ecx, edx, si, di);
if ((HIGH_WORD(ebx) & MESSAGE_STATUS_SUCCESS) == 0) {
kfree(reply);
if ((HIGH_WORD(ebx) & MESSAGE_STATUS_CPT) != 0) {
/* A checkpoint occurred. Retry. */
continue;
}
return -EINVAL;
}
reply[reply_len] = '\0';
/* Ack buffer */
si = channel->cookie_high;
di = channel->cookie_low;
VMW_PORT(VMW_PORT_CMD_RECVSTATUS,
MESSAGE_STATUS_SUCCESS, si, di,
(VMW_HYPERVISOR_PORT | (channel->channel_id << 16)),
VMW_HYPERVISOR_MAGIC,
eax, ebx, ecx, edx, si, di);
if ((HIGH_WORD(ecx) & MESSAGE_STATUS_SUCCESS) == 0) {
kfree(reply);
if ((HIGH_WORD(ecx) & MESSAGE_STATUS_CPT) != 0) {
/* A checkpoint occurred. Retry. */
continue;
}
return -EINVAL;
}
break;
}
if (retries == RETRIES)
return -EINVAL;
*msg_len = reply_len;
*msg = reply;
return 0;
}
int
__libmcr_k_mx_rem_pio2(double x, double y[])
{
double z, w, t, r, fn, y0;
double tx[3];
int e0, i, j, nx, n, ix, hx, lx, hr, i0;
#ifdef debug
int ms[10], mw[10], mx[10], my[10], mz[10], nw = 8, mc[10], nz,
mt[10], mr[10], my0[10];
nz = __libmcr_k_mi_rem_pio2(x, my, nw);
#endif
z = fabs(x);
w = z * INVPIO2;
hx = HIGH_WORD(x);
lx = LOW_WORD(x);
ix = hx & 0x7fffffff;
/* for |x| <= 1638400 */
if (ix <= 0x41390000) {
n = (int) (w + HALF); /* |x| ~ n*pi/2, n < 2^19 */
#ifdef debug
printf(" w = %1.20e %08X %08X \n", w, .HIGH_WORD(w), .LOW_WORD(w));
printf(" HALF = %1.20e\n", HALF);
printf(" n = %d\n", n);
#endif
if (ix <= 0x3fe921fa) {
y[0] = x;
y[1] = 0;
return (0);
} /* |x| < pi/2 */
fn = (double) n;
j = ix >> 20;
t = fn * PIO2_1;
w = fn * PIO2_1T; /* 1st round good to 85 bit */
r = z - t;
i0 = 0;
y0 = r - w;
hr = HIGH_WORD(r);
i = (hr & (~0x80000000)) >> 20;
#ifdef debug
printf("x = %08X %08X % 1.20e\n",
HIGH_WORD(x), LOW_WORD(x), x);
printf("n = %d\n", n);
printf("nz = %d\n", nz);
printf("n*PIO2_1 = %08X %08X % 1.20e\n",
HIGH_WORD(t), LOW_WORD(t), t);
printf("n*PIO2_1T= %08X %08X % 1.20e\n",
HIGH_WORD(w), LOW_WORD(w), w);
__libmcr_mi_dtomi(t, mt, nw);
__libmcr_mi_dtomi(w, mw, nw);
__libmcr_mi_dtomi(z, mz, nw);
__libmcr_mm_sub(mz, mt, mr, nw);
__libmcr_mm_sub(mr, mw, my0, nw);
printf("z - n*PIO2 =\n");
PNT(my);
PNT(my0);
printf("r = z-t = %08X %08X % 1.20e\n",
HIGH_WORD(r), LOW_WORD(r), r);
printf("y0= r-w = %08X %08X % 1.20e\n",
HIGH_WORD(y0), LOW_WORD(y0), y0);
printf("j = %d, i = %d, j-i=%d > 6?\n", j, i, j - i);
#endif
while ((j - i) >= 7) { /* 7 bit cancellation (insure 78 bits) */
t = fn * PIO2_2[i0]; /* at most five loop is suffice */
z = r;
j = i;
#ifdef debug
printf("Are r-t exact?\n");
printf("r = %08X %08X %1.20e\n", r, r);
printf("t = %08X %08X %1.20e\n", t, t);
__libmcr_mi_dtomi(t, mt, nw);
__libmcr_mi_dtomi(r, mw, nw);
__libmcr_mm_sub(mw, mt, mt, nw);
#endif
r -= t;
#ifdef debug
__libmcr_mi_dtomi(r, mw, nw);
__libmcr_mm_sub(mw, mt, mt, nw);
if (mt[2] == 0)
printf("Yes.\n");
else
printf("No.\n");
#endif
w = fn * PIO2_2[i0 + 1];
y0 = r - w;
i0 += 2;
hr = HIGH_WORD(r);
i = (hr & (~0x80000000)) >> 20;
#ifdef debug
printf("w = %08X %08X %1.20e\n", w, w);
printf("y0= %08X %08X %1.20e\n", y0, y0);
__libmcr_mi_dtomi(t, mt, nw);
__libmcr_mi_dtomi(w, mw, nw);
__libmcr_mm_sub(mr, mt, mr, nw);
__libmcr_mm_sub(mr, mw, my0, nw);
printf("z - n*PIO2[%1d] =\n", i0 + 2);
PNT(my);
PNT(my0);
printf("new j = %d, i = %d, j-i=%d > 5?\n",
j, i, j - i);
#endif
}
if (i0 == 0 || (j - i) > 1) {
r -= y0;
y[0] = y0;
y[1] = r - w;
} else {
t += y0 - z;
y[0] = y0;
y[1] = -w - t;
}
#ifdef debug
printf("test final sum (n=%d):\n", n);
__libmcr_mi_dtomi(y[0], mr, nw);
printf("y[0] = % 1.20e ", y[0]);
PNT(mr);
__libmcr_mi_dtomi(y[1], mt, nw);
printf("y[1] = % 1.20e ", y[1]);
PNT(mt);
__libmcr_mm_add(mr, mt, mt, nw);
printf("y0+y1 = % 1.20e ", y[0] + y[1]);
PNT(mt);
PNT(mt);
#endif
if (hx < 0) {
y[1] = -y[1];
y[0] = -y0;
return (-n);
} else {
return (n);
}
}
static ssize_t bhi_write(struct file *file,
const char __user *buf,
size_t count, loff_t *offp)
{
enum MHI_STATUS ret_val = MHI_STATUS_SUCCESS;
u32 pcie_word_val = 0;
u32 i = 0;
struct bhi_ctxt_t *bhi_ctxt =
&(((struct mhi_pcie_dev_info *)file->private_data)->bhi_ctxt);
struct mhi_device_ctxt *mhi_dev_ctxt =
&((struct mhi_pcie_dev_info *)file->private_data)->mhi_ctxt;
size_t amount_copied = 0;
uintptr_t align_len = 0x1000;
u32 tx_db_val = 0;
if (buf == NULL || 0 == count)
return -EIO;
if (count > BHI_MAX_IMAGE_SIZE)
return -ENOMEM;
wait_event_interruptible(*mhi_dev_ctxt->mhi_ev_wq.bhi_event,
mhi_dev_ctxt->mhi_state == MHI_STATE_BHI);
mhi_log(MHI_MSG_INFO, "Entered. User Image size 0x%zx\n", count);
bhi_ctxt->unaligned_image_loc = kmalloc(count + (align_len - 1),
GFP_KERNEL);
if (bhi_ctxt->unaligned_image_loc == NULL)
return -ENOMEM;
mhi_log(MHI_MSG_INFO, "Unaligned Img Loc: %p\n",
bhi_ctxt->unaligned_image_loc);
bhi_ctxt->image_loc =
(void *)((uintptr_t)bhi_ctxt->unaligned_image_loc +
(align_len - (((uintptr_t)bhi_ctxt->unaligned_image_loc) %
align_len)));
mhi_log(MHI_MSG_INFO, "Aligned Img Loc: %p\n", bhi_ctxt->image_loc);
bhi_ctxt->image_size = count;
if (0 != copy_from_user(bhi_ctxt->image_loc, buf, count)) {
ret_val = -ENOMEM;
goto bhi_copy_error;
}
amount_copied = count;
/* Flush the writes, in anticipation for a device read */
wmb();
mhi_log(MHI_MSG_INFO,
"Copied image from user at addr: %p\n", bhi_ctxt->image_loc);
bhi_ctxt->phy_image_loc = dma_map_single(
&mhi_dev_ctxt->dev_info->plat_dev->dev,
bhi_ctxt->image_loc,
bhi_ctxt->image_size,
DMA_TO_DEVICE);
if (dma_mapping_error(NULL, bhi_ctxt->phy_image_loc)) {
ret_val = -EIO;
goto bhi_copy_error;
}
mhi_log(MHI_MSG_INFO,
"Mapped image to DMA addr 0x%lx:\n",
(uintptr_t)bhi_ctxt->phy_image_loc);
bhi_ctxt->image_size = count;
/* Write the image size */
pcie_word_val = HIGH_WORD(bhi_ctxt->phy_image_loc);
mhi_reg_write_field(mhi_dev_ctxt, bhi_ctxt->bhi_base,
BHI_IMGADDR_HIGH,
0xFFFFFFFF,
0,
pcie_word_val);
pcie_word_val = LOW_WORD(bhi_ctxt->phy_image_loc);
mhi_reg_write_field(mhi_dev_ctxt, bhi_ctxt->bhi_base,
BHI_IMGADDR_LOW,
0xFFFFFFFF,
0,
pcie_word_val);
pcie_word_val = bhi_ctxt->image_size;
mhi_reg_write_field(mhi_dev_ctxt, bhi_ctxt->bhi_base, BHI_IMGSIZE,
0xFFFFFFFF, 0, pcie_word_val);
pcie_word_val = mhi_reg_read(bhi_ctxt->bhi_base, BHI_IMGTXDB);
mhi_reg_write_field(mhi_dev_ctxt, bhi_ctxt->bhi_base,
BHI_IMGTXDB, 0xFFFFFFFF, 0, ++pcie_word_val);
mhi_reg_write(mhi_dev_ctxt, bhi_ctxt->bhi_base, BHI_INTVEC, 0);
for (i = 0; i < BHI_POLL_NR_RETRIES; ++i) {
tx_db_val = mhi_reg_read_field(bhi_ctxt->bhi_base,
BHI_STATUS,
BHI_STATUS_MASK,
BHI_STATUS_SHIFT);
mhi_log(MHI_MSG_CRITICAL, "BHI STATUS 0x%x\n", tx_db_val);
if (BHI_STATUS_SUCCESS != tx_db_val)
mhi_log(MHI_MSG_CRITICAL,
"Incorrect BHI status: %d retry: %d\n",
tx_db_val, i);
else
break;
usleep_range(20000, 25000);
}
dma_unmap_single(&mhi_dev_ctxt->dev_info->plat_dev->dev,
bhi_ctxt->phy_image_loc,
bhi_ctxt->image_size, DMA_TO_DEVICE);
kfree(bhi_ctxt->unaligned_image_loc);
ret_val = mhi_init_state_transition(mhi_dev_ctxt,
STATE_TRANSITION_RESET);
if (MHI_STATUS_SUCCESS != ret_val) {
mhi_log(MHI_MSG_CRITICAL,
"Failed to start state change event\n");
}
return amount_copied;
bhi_copy_error:
kfree(bhi_ctxt->unaligned_image_loc);
return amount_copied;
}
