C++ send_instruction函数代码示例

void _config()
{
	uint8_t value;

	SPI_Init();

	// set address width to 5 bytes.
	value = ADDRESS_LENGTH - 2;                     // 0b11 for 5 bytes, 0b10 for 4 bytes, 0b01 for 3 bytes
	_set_register(SETUP_AW, &value, 1);

	// set Enhanced Shockburst retry to every 586 us, up to 5 times.  If packet collisions are a problem even with AA enabled,
	// then consider changing the retry delay to be different on the different stations so that they do not keep colliding on each retry.
	value = 0x15;
	//value = 0x10;
	_set_register(SETUP_RETR, &value, 1);

	// Set to use 2.4 GHz channel 110.
	value = CHANNEL;
	_set_register(RF_CH, &value, 1);

	// Set radio to 2 Mbps and high power.  Leave LNA_HCURR at its default.
	value = _BV(RF_DR) | _BV(LNA_HCURR);
	_set_register(RF_SETUP, &value, 1);

	// Enable 2-byte CRC and power up in receive mode.
	value = _BV(EN_CRC) | _BV(CRCO) | _BV(PWR_UP) | _BV(PRIM_RX);
	_set_register(CONFIG, &value, 1);

	send_instruction(FLUSH_TX, NULL, NULL, 0);
	send_instruction(FLUSH_RX, NULL, NULL, 0);
}

static ssize_t adc_data_read(struct device *dev,
	struct device_attribute *attr, char *buf)
{
	bool bSuccess = false;
	u8 chTempbuf[2] = {1, 20};
	struct ssp_data *data = dev_get_drvdata(dev);

	if (!(atomic_read(&data->aSensorEnable) & (1 << GEOMAGNETIC_SENSOR))) {
		send_instruction(data, ADD_SENSOR, GEOMAGNETIC_SENSOR,
			chTempbuf, 2);
		msleep(200);
	}

	if ((data->buf[GEOMAGNETIC_SENSOR].x == 0) &&
		(data->buf[GEOMAGNETIC_SENSOR].y == 0) &&
		(data->buf[GEOMAGNETIC_SENSOR].z == 0))
		bSuccess = false;
	else
		bSuccess = true;

	if (!(atomic_read(&data->aSensorEnable) & (1 << GEOMAGNETIC_SENSOR)))
		send_instruction(data, REMOVE_SENSOR, GEOMAGNETIC_SENSOR,
			chTempbuf, 2);

	pr_info("[SSP]: %s - x = %d, y = %d, z = %d\n", __func__,
		data->buf[GEOMAGNETIC_SENSOR].x,
		data->buf[GEOMAGNETIC_SENSOR].y,
		data->buf[GEOMAGNETIC_SENSOR].z);

	return sprintf(buf, "%s,%d,%d,%d\n", (bSuccess ? "OK" : "NG"),
		data->buf[GEOMAGNETIC_SENSOR].x,
		data->buf[GEOMAGNETIC_SENSOR].y,
		data->buf[GEOMAGNETIC_SENSOR].z);
}

static ssize_t proximity_avg_store(struct device *dev,
	struct device_attribute *attr, const char *buf, size_t size)
{
	char chTempbuf[4] = { 0 };
	int iRet;
	int64_t dEnable;
	struct ssp_data *data = dev_get_drvdata(dev);

	s32 dMsDelay = 20;
	memcpy(&chTempbuf[0], &dMsDelay, 4);

	iRet = kstrtoll(buf, 10, &dEnable);
	if (iRet < 0)
		return iRet;

	if (dEnable) {
		send_instruction(data, ADD_SENSOR, PROXIMITY_RAW, chTempbuf, 4);
		data->bProximityRawEnabled = true;
	} else {
		send_instruction(data, REMOVE_SENSOR, PROXIMITY_RAW,
			chTempbuf, 4);
		data->bProximityRawEnabled = false;
	}

	return size;
}

/**
 * Configure radio defaults and turn on the radio in receive mode.
 * This configures the radio to its max-power, max-packet-header-length settings.  If you want to reduce power consumption
 * or increase on-air payload bandwidth, you'll have to change the config.
 */
static void configure_registers(void)
{
	uint8_t value;

	// set address width to 5 bytes.
	value = ADDRESS_LENGTH - 2;			// 0b11 for 5 bytes, 0b10 for 4 bytes, 0b01 for 3 bytes
	set_register(SETUP_AW, &value, 1);

	// set Enhanced Shockburst retry to every 586 us, up to 5 times.  If packet collisions are a problem even with AA enabled,
	// then consider changing the retry delay to be different on the different stations so that they do not keep colliding on each retry.
	value = 0x15;
	//value = 0x10;
	set_register(SETUP_RETR, &value, 1);

	// Set to use 2.4 GHz channel 110.
	value = CHANNEL;
	set_register(RF_CH, &value, 1);

	// Set radio to 2 Mbps and high power.  Leave LNA_HCURR at its default.
	value = _BV(RF_DR) | _BV(LNA_HCURR);
	set_register(RF_SETUP, &value, 1);

	// Enable 2-byte CRC and power up in receive mode.
	value = _BV(EN_CRC) | _BV(CRCO) | _BV(PWR_UP) | _BV(PRIM_RX);
	set_register(CONFIG, &value, 1);

	// clear the interrupt flags in case the radio's still asserting an old unhandled interrupt
    value = _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT);
    set_register(STATUS, &value, 1);

    // flush the FIFOs in case there are old data in them.
	send_instruction(FLUSH_TX, NULL, NULL, 0);
	send_instruction(FLUSH_RX, NULL, NULL, 0);
}

void sync_sensor_state(struct ssp_data *data)
{
	unsigned char uBuf[9] = {0,};
	unsigned int uSensorCnt;
	int iRet = 0;

	iRet = set_gyro_cal(data);
	if (iRet < 0)
		ssp_errf("set_gyro_cal failed");

	iRet = set_accel_cal(data);
	if (iRet < 0)
		ssp_errf("set_accel_cal failed");

	udelay(10);

	for (uSensorCnt = 0; uSensorCnt < SENSOR_MAX; uSensorCnt++) {
		if (atomic64_read(&data->aSensorEnable) & (1 << uSensorCnt)) {
			s32 dMsDelay
				= get_msdelay(data->adDelayBuf[uSensorCnt]);
			memcpy(&uBuf[0], &dMsDelay, 4);
			memcpy(&uBuf[4], &data->batchLatencyBuf[uSensorCnt], 4);
			uBuf[8] = data->batchOptBuf[uSensorCnt];
			send_instruction(data, ADD_SENSOR, uSensorCnt, uBuf, 9);
			udelay(10);
		}
	}

	if (data->bProximityRawEnabled == true) {
		s32 dMsDelay = 20;
		memcpy(&uBuf[0], &dMsDelay, 4);
		send_instruction(data, ADD_SENSOR, PROXIMITY_RAW, uBuf, 4);
	}

	set_proximity_threshold(data, data->uProxHiThresh, data->uProxLoThresh);
	data->buf[PROXIMITY_SENSOR].prox = 0;
	report_sensordata(data, PROXIMITY_SENSOR, &data->buf[PROXIMITY_SENSOR]);

#if 1
    if(sec_debug_get_debug_level() > 0)
    {
        data->bMcuDumpMode = true;
        ssp_info("Mcu Dump Enabled");
    }

    iRet = ssp_send_cmd(data, MSG2SSP_AP_MCU_SET_DUMPMODE,
            data->bMcuDumpMode);
    if (iRet < 0)
        ssp_errf("MSG2SSP_AP_MCU_SET_DUMPMODE failed");

#else
#if CONFIG_SEC_DEBUG   
	data->bMcuDumpMode = sec_debug_is_enabled();
	iRet = ssp_send_cmd(data, MSG2SSP_AP_MCU_SET_DUMPMODE,
			data->bMcuDumpMode);
	if (iRet < 0)
		ssp_errf("MSG2SSP_AP_MCU_SET_DUMPMODE failed");
#endif
#endif
}

void sync_sensor_state(struct ssp_data *data)
{
	unsigned char uBuf[9] = {0,};
	unsigned int uSensorCnt;
	int iRet = 0;

	iRet = set_gyro_cal(data);
	if (iRet < 0)
		pr_err("[SSP]: %s - set_gyro_cal failed\n", __func__);

	iRet = set_accel_cal(data);
	if (iRet < 0)
		pr_err("[SSP]: %s - set_accel_cal failed\n", __func__);

#ifdef CONFIG_SENSORS_SSP_SX9306
	if (atomic_read(&data->aSensorEnable) & (1 << GRIP_SENSOR)) {
		open_grip_caldata(data);
		set_grip_calibration(data, true);
	}
#endif

	udelay(10);

	for (uSensorCnt = 0; uSensorCnt < SENSOR_MAX; uSensorCnt++) {
		mutex_lock(&data->enable_mutex);
		if (atomic_read(&data->aSensorEnable) & (1 << uSensorCnt)) {
			s32 dMsDelay =
				get_msdelay(data->adDelayBuf[uSensorCnt]);
			memcpy(&uBuf[0], &dMsDelay, 4);
			memcpy(&uBuf[4], &data->batchLatencyBuf[uSensorCnt], 4);
			uBuf[8] = data->batchOptBuf[uSensorCnt];
			send_instruction(data, ADD_SENSOR, uSensorCnt, uBuf, 9);
			udelay(10);
		}
		mutex_unlock(&data->enable_mutex);
	}

	if (data->bProximityRawEnabled == true) {
		s32 dMsDelay = 20;
		memcpy(&uBuf[0], &dMsDelay, 4);
		send_instruction(data, ADD_SENSOR, PROXIMITY_RAW, uBuf, 4);
	}

	set_proximity_threshold(data, data->uProxHiThresh,data->uProxLoThresh);

	data->bMcuDumpMode = ssp_check_sec_dump_mode();
	iRet = ssp_send_cmd(data, MSG2SSP_AP_MCU_SET_DUMPMODE,
		data->bMcuDumpMode);
	if (iRet < 0)
		pr_err("[SSP]: %s - MSG2SSP_AP_MCU_SET_DUMPMODE failed\n",
			__func__);
}

int ssp_charging_motion(struct ssp_data *data, int iEnable)
{
	u8 uBuf[2] = {0, 0};

	if (iEnable == 1) {
		send_instruction(data, ADD_LIBRARY,
			SMART_ALERT_MOTION, uBuf, 2);
	} else {
		send_instruction(data, REMOVE_LIBRARY,
			SMART_ALERT_MOTION, uBuf, 2);
	}

	return 0;
}

static ssize_t raw_data_store(struct device *dev,
	struct device_attribute *attr, const char *buf, size_t size)
{
	char chTempbuf[9] = { 0 };
	int iRet;
	int64_t dEnable;
	int iRetries = 50;
	struct ssp_data *data = dev_get_drvdata(dev);
	s32 dMsDelay = 20;
	memcpy(&chTempbuf[0], &dMsDelay, 4);
	memcpy(&chTempbuf[4], &data->batchLatencyBuf[GEOMAGNETIC_RAW], 4);
	chTempbuf[8] = data->batchOptBuf[GEOMAGNETIC_RAW];

	iRet = kstrtoll(buf, 10, &dEnable);
	if (iRet < 0)
		return iRet;

	if (dEnable) {
		data->buf[GEOMAGNETIC_RAW].x = 0;
		data->buf[GEOMAGNETIC_RAW].y = 0;
		data->buf[GEOMAGNETIC_RAW].z = 0;

		send_instruction(data, ADD_SENSOR, GEOMAGNETIC_RAW,
			chTempbuf, 9);

#if 0
		do {
			msleep(20);
			if (check_data_spec(data) == SUCCESS)
				break;
		} while (--iRetries);
#endif

		if (iRetries > 0) {
			pr_info("[SSP] %s - success, %d\n", __func__, iRetries);
			data->bGeomagneticRawEnabled = true;
		} else {
			pr_err("[SSP] %s - wait timeout, %d\n", __func__,
				iRetries);
			data->bGeomagneticRawEnabled = false;
		}
	} else {
		send_instruction(data, REMOVE_SENSOR, GEOMAGNETIC_RAW,
			chTempbuf, 4);
		data->bGeomagneticRawEnabled = false;
	}

	return size;
}

static void disable_accel_for_cal(struct ssp_data *data, int iDelayChanged)
{
	u8 uBuf[2] = {0, 10};

	if (atomic_read(&data->aSensorEnable) & (1 << ACCELEROMETER_SENSOR)) {
		uBuf[1] = get_msdelay(data->adDelayBuf[ACCELEROMETER_SENSOR]);
		uBuf[0] = get_delay_cmd(uBuf[1]);
		if (iDelayChanged)
			send_instruction(data, CHANGE_DELAY,
				ACCELEROMETER_SENSOR, uBuf, 2);
	} else {
		send_instruction(data, REMOVE_SENSOR,
			ACCELEROMETER_SENSOR, uBuf, 2);
	}
}

static void disable_accel_for_cal(struct ssp_data *data, int iDelayChanged)
{
	u8 uBuf[9] = { 0, };
	s32 dMsDelay = get_msdelay(data->adDelayBuf[ACCELEROMETER_SENSOR]);
	memcpy(&uBuf[0], &dMsDelay, 4);

	if (atomic_read(&data->aSensorEnable) & (1 << ACCELEROMETER_SENSOR)) {
		if (iDelayChanged)
			send_instruction(data, CHANGE_DELAY,
				ACCELEROMETER_SENSOR, uBuf, 9);
	} else {
		send_instruction(data, REMOVE_SENSOR,
			ACCELEROMETER_SENSOR, uBuf, 4);
	}
}

void sync_sensor_state(struct ssp_data *data)
{
	unsigned char uBuf[9] = {0,};
	unsigned int uSensorCnt;
	int iRet = 0;
#ifdef CONFIG_SENSORS_SSP_YAS532
	iRet = set_hw_offset(data);
	if (iRet < 0) {
		pr_err("[SSP]: %s - set_hw_offset failed\n", __func__);
	}
#endif
	iRet = set_gyro_cal(data);
	if (iRet < 0) {
		pr_err("[SSP]: %s - set_gyro_cal failed\n", __func__);
	}
	iRet = set_accel_cal(data);
	if (iRet < 0) {
		pr_err("[SSP]: %s - set_accel_cal failed\n", __func__);
	}
	udelay(10);

	for (uSensorCnt = 0; uSensorCnt < SENSOR_MAX; uSensorCnt++) {
		if (atomic_read(&data->aSensorEnable) & (1 << uSensorCnt)) {
			s32 dMsDelay = get_msdelay(data->adDelayBuf[uSensorCnt]);
			memcpy(&uBuf[0], &dMsDelay, 4);
			memcpy(&uBuf[4], &data->batchLatencyBuf[uSensorCnt], 4);
			uBuf[8] = data->batchOptBuf[uSensorCnt];
			send_instruction(data, ADD_SENSOR, uSensorCnt, uBuf, 9);
			udelay(10);
		}
	}

	if (data->bProximityRawEnabled == true) {
		s32 dMsDelay = 20;
		memcpy(&uBuf[0], &dMsDelay, 4);
		send_instruction(data, ADD_SENSOR, PROXIMITY_RAW, uBuf, 4);
	}

	set_proximity_threshold(data, data->uProxHiThresh,data->uProxLoThresh);

#if SSP_SEC_DEBUG
	data->bMcuDumpMode = sec_debug_is_enabled();
	iRet = ssp_send_cmd(data, MSG2SSP_AP_MCU_SET_DUMPMODE,data->bMcuDumpMode);
	if (iRet < 0) {
		pr_err("[SSP]: %s - MSG2SSP_AP_MCU_SET_DUMPMODE failed\n", __func__);
	}
#endif
}

char k330_gyro_get_temp(struct ssp_data *data)
{
    char chTempBuf[2] = { 0, 10}, chTemp = 0;
    int iDelayCnt = 0, iRet = 0;

    if (!(data->uSensorState & (1 << GYROSCOPE_SENSOR)))
        goto exit;

    data->uFactorydataReady = 0;
    memset(data->uFactorydata, 0, sizeof(char) * FACTORY_DATA_MAX);

    iRet = send_instruction(data, FACTORY_MODE, GYROSCOPE_TEMP_FACTORY,
                            chTempBuf, 2);

    while (!(data->uFactorydataReady & (1 << GYROSCOPE_TEMP_FACTORY))
            && (iDelayCnt++ < 150)
            && (iRet == SUCCESS))
        msleep(20);

    if ((iDelayCnt >= 150) || (iRet != SUCCESS)) {
        pr_err("[SSP]: %s - Gyro Temp Timeout!!\n", __func__);
        goto exit;
    }

    mdelay(5);

    chTemp = (char)data->uFactorydata[0];
    ssp_dbg("[SSP]: %s - %d\n", __func__, chTemp);
exit:
    return chTemp;
}

static ssize_t magnetic_get_selftest(struct device *dev,
		struct device_attribute *attr, char *buf)
{
	bool bSelftestPassed = false;
	s16 iSF_X = 0, iSF_Y = 0, iSF_Z = 0;
	int iDelayCnt = 0, iRet = 0, iReties = 0;
	char chTempBuf[2] = { 0, 10 };
	struct ssp_data *data = dev_get_drvdata(dev);

reties:
	iDelayCnt = 0;
	data->uFactorydataReady = 0;

	iRet = send_instruction(data, FACTORY_MODE, GEOMAGNETIC_FACTORY,
			chTempBuf, 2);

	while (!(data->uFactorydataReady & (1 << GEOMAGNETIC_FACTORY))
		&& (iDelayCnt++ < 50)
		&& (iRet == SUCCESS))
		msleep(20);

	if ((iDelayCnt >= 50) || (iRet != SUCCESS)) {
		pr_err("[SSP]: %s - Magnetic Selftest Timeout!!\n", __func__);
		goto exit;
	}

	iSF_X = (s16)((data->uFactorydata[0] << 8) + data->uFactorydata[1]);
	iSF_Y = (s16)((data->uFactorydata[2] << 8) + data->uFactorydata[3]);
	iSF_Z = (s16)((data->uFactorydata[4] << 8) + data->uFactorydata[5]);

	iSF_X = (s16)(((int)iSF_X * ((int)data->uFuseRomData[0] + 128)) >> 8);
	iSF_Y = (s16)(((int)iSF_Y * ((int)data->uFuseRomData[1] + 128)) >> 8);
	iSF_Z = (s16)(((int)iSF_Z * ((int)data->uFuseRomData[2] + 128)) >> 8);

	pr_info("[SSP] %s: self test x = %d, y = %d, z = %d\n",
		__func__, iSF_X, iSF_Y, iSF_Z);
	if ((iSF_X >= -200) && (iSF_X <= 200))
		pr_info("[SSP] x passed self test, expect -200<=x<=200\n");
	else
		pr_info("[SSP] x failed self test, expect -200<=x<=200\n");
	if ((iSF_Y >= -200) && (iSF_Y <= 200))
		pr_info("[SSP] y passed self test, expect -200<=y<=200\n");
	else
		pr_info("[SSP] y failed self test, expect -200<=y<=200\n");
	if ((iSF_Z >= -3200) && (iSF_Z <= -800))
		pr_info("[SSP] z passed self test, expect -3200<=z<=-800\n");
	else
		pr_info("[SSP] z failed self test, expect -3200<=z<=-800\n");

	if (((iSF_X >= -200) && (iSF_X <= 200)) &&
		((iSF_Y >= -200) && (iSF_Y <= 200)) &&
		((iSF_Z >= -3200) && (iSF_Z <= -800)))
		bSelftestPassed = true;

	if ((bSelftestPassed == false) && (iReties++ < 5))
		goto reties;
exit:
	return sprintf(buf, "%u,%d,%d,%d\n",
		bSelftestPassed, iSF_X, iSF_Y, iSF_Z);
}

static ssize_t gyro_get_temp(struct device *dev,
	struct device_attribute *attr, char *buf)
{
	char chTempBuf[2] = { 0, 10}, chTemp = 0;
	int iDelayCnt = 0, iRet = 0;
	struct ssp_data *data = dev_get_drvdata(dev);

	data->uFactorydataReady = 0;
	memset(data->uFactorydata, 0, sizeof(char) * FACTORY_DATA_MAX);

	iRet = send_instruction(data, FACTORY_MODE, GYROSCOPE_TEMP_FACTORY,
		chTempBuf, 2);

	while (!(data->uFactorydataReady & (1 << GYROSCOPE_TEMP_FACTORY))
		&& (iDelayCnt++ < 150)
		&& (iRet == SUCCESS))
		msleep(20);

	if ((iDelayCnt >= 150) || (iRet != SUCCESS)) {
		pr_err("[SSP]: %s - Gyro Temp Timeout!!\n", __func__);
		goto exit;
	}

	chTemp = (char)data->uFactorydata[0];
	ssp_dbg("[SSP]: %s - %d\n", __func__, chTemp);
exit:
	return sprintf(buf, "%d\n", chTemp);
}

uint8_t Radio_Transmit(radiopacket_t* payload, RADIO_TX_WAIT wait)
{
	//if (block && transmit_lock) while (transmit_lock);
	//if (!block && transmit_lock) return 0;
	uint8_t len = 32;

	// indicate that the driver is transmitting.
    transmit_lock = 1;

	// disable the radio while writing to the Tx FIFO.
    ioport_set_pin_low (CE);

	set_tx_mode();

    // for auto-ack to work, the pipe0 address must be set to the Tx address while the radio is transmitting.
    // The register will be set back to the original pipe 0 address when the TX_DS or MAX_RT interrupt is asserted.
    set_register(RX_ADDR_P0, (uint8_t*)tx_address, ADDRESS_LENGTH);

    // transfer the packet to the radio's Tx FIFO for transmission
    send_instruction(W_TX_PAYLOAD, payload, NULL, len);

    // start the transmission.
    ioport_set_pin_high (CE);

    if (wait == RADIO_WAIT_FOR_TX)
    {
    	while (transmit_lock);
    	return tx_last_status;
    }

    return RADIO_TX_SUCCESS;
}