optimised.c 6.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247
  1. #define PY_SSIZE_T_CLEAN
  2. #include "Python.h"
  3. #include <stdio.h>
  4. #include <stdint.h>
  5. #define FLUXOP_INDEX 1
  6. #define FLUXOP_SPACE 2
  7. #define FLUXOP_ASTABLE 3
  8. /* bitarray.append(value) */
  9. static PyObject *append_s;
  10. static int bitarray_append(PyObject *bitarray, PyObject *value)
  11. {
  12. PyObject *res = PyObject_CallMethodObjArgs(
  13. bitarray, append_s, value, NULL);
  14. if (res == NULL)
  15. return 0;
  16. Py_DECREF(res);
  17. return 1;
  18. }
  19. static PyObject *
  20. flux_to_bitcells(PyObject *self, PyObject *args)
  21. {
  22. /* Parameters */
  23. PyObject *bit_array, *time_array, *revolutions;
  24. PyObject *index_iter, *flux_iter;
  25. double freq, clock_centre, clock_min, clock_max;
  26. double pll_period_adj, pll_phase_adj;
  27. /* Local variables */
  28. PyObject *item;
  29. double clock, new_ticks, ticks, to_index;
  30. int zeros, nbits;
  31. if (!PyArg_ParseTuple(args, "OOOOOdddddd",
  32. &bit_array, &time_array, &revolutions,
  33. &index_iter, &flux_iter,
  34. &freq, &clock_centre, &clock_min, &clock_max,
  35. &pll_period_adj, &pll_phase_adj))
  36. return NULL;
  37. nbits = 0;
  38. ticks = 0.0;
  39. clock = clock_centre;
  40. /* to_index = next(index_iter) */
  41. item = PyIter_Next(index_iter);
  42. to_index = PyFloat_AsDouble(item);
  43. Py_DECREF(item);
  44. if (PyErr_Occurred())
  45. return NULL;
  46. /* for x in flux_iter: */
  47. assert(PyIter_Check(flux_iter));
  48. while ((item = PyIter_Next(flux_iter)) != NULL) {
  49. double x = PyFloat_AsDouble(item);
  50. Py_DECREF(item);
  51. if (PyErr_Occurred())
  52. return NULL;
  53. /* Gather enough ticks to generate at least one bitcell. */
  54. ticks += x / freq;
  55. if (ticks < clock/2)
  56. continue;
  57. /* Clock out zero or more 0s, followed by a 1. */
  58. for (zeros = 0; ; zeros++) {
  59. /* Check if we cross the index mark. */
  60. to_index -= clock;
  61. if (to_index < 0) {
  62. if (PyList_Append(revolutions, PyLong_FromLong(nbits)) < 0)
  63. return NULL;
  64. nbits = 0;
  65. item = PyIter_Next(index_iter);
  66. to_index += PyFloat_AsDouble(item);
  67. Py_DECREF(item);
  68. if (PyErr_Occurred())
  69. return NULL;
  70. }
  71. nbits += 1;
  72. ticks -= clock;
  73. if (PyList_Append(time_array, PyFloat_FromDouble(clock)) < 0)
  74. return NULL;
  75. if (ticks < clock/2) {
  76. if (!bitarray_append(bit_array, Py_True))
  77. return NULL;
  78. break;
  79. }
  80. if (!bitarray_append(bit_array, Py_False))
  81. return NULL;
  82. }
  83. /* PLL: Adjust clock frequency according to phase mismatch. */
  84. if (zeros <= 3) {
  85. /* In sync: adjust clock by a fraction of the phase mismatch. */
  86. clock += ticks * pll_period_adj;
  87. } else {
  88. /* Out of sync: adjust clock towards centre. */
  89. clock += (clock_centre - clock) * pll_period_adj;
  90. }
  91. /* Clamp the clock's adjustment range. */
  92. if (clock < clock_min)
  93. clock = clock_min;
  94. else if (clock > clock_max)
  95. clock = clock_max;
  96. /* PLL: Adjust clock phase according to mismatch. */
  97. new_ticks = ticks * (1.0 - pll_phase_adj);
  98. if (PyList_SetItem(time_array, PyList_Size(time_array)-1,
  99. PyFloat_FromDouble(ticks - new_ticks)) < 0)
  100. return NULL;
  101. ticks = new_ticks;
  102. }
  103. Py_RETURN_NONE;
  104. }
  105. static int _read_28bit(uint8_t *p)
  106. {
  107. int x;
  108. x = (p[0] ) >> 1;
  109. x |= (p[1] & 0xfe) << 6;
  110. x |= (p[2] & 0xfe) << 13;
  111. x |= (p[3] & 0xfe) << 20;
  112. return x;
  113. }
  114. static PyObject *
  115. decode_flux(PyObject *self, PyObject *args)
  116. {
  117. /* Parameters */
  118. Py_buffer bytearray;
  119. PyObject *res = NULL;
  120. /* bytearray buffer */
  121. uint8_t *p;
  122. Py_ssize_t l;
  123. /* Local variables */
  124. PyObject *flux, *index;
  125. long val, ticks, ticks_since_index;
  126. int i, opcode;
  127. if (!PyArg_ParseTuple(args, "y*", &bytearray))
  128. return NULL;
  129. p = bytearray.buf;
  130. l = bytearray.len;
  131. /* assert dat[-1] == 0 */
  132. if ((l == 0) || (p[l-1] != 0)) {
  133. PyErr_SetString(PyExc_ValueError, "Flux is not NUL-terminated");
  134. PyBuffer_Release(&bytearray);
  135. return NULL;
  136. }
  137. /* len(dat) -= 1 */
  138. l -= 1;
  139. /* flux, index = [], [] */
  140. flux = PyList_New(0);
  141. index = PyList_New(0);
  142. /* ticks, ticks_since_index = 0, 0 */
  143. ticks = 0;
  144. ticks_since_index = 0;
  145. while (l != 0) {
  146. i = *p++;
  147. if (i == 255) {
  148. if ((l -= 2) < 0)
  149. goto oos;
  150. opcode = *p++;
  151. switch (opcode) {
  152. case FLUXOP_INDEX:
  153. if ((l -= 4) < 0)
  154. goto oos;
  155. val = _read_28bit(p);
  156. p += 4;
  157. if (PyList_Append(index, PyLong_FromLong(
  158. ticks_since_index + ticks + val)) < 0)
  159. goto out;
  160. ticks_since_index = -(ticks + val);
  161. break;
  162. case FLUXOP_SPACE:
  163. if ((l -= 4) < 0)
  164. goto oos;
  165. ticks += _read_28bit(p);
  166. p += 4;
  167. break;
  168. default:
  169. PyErr_Format(PyExc_ValueError,
  170. "Bad opcode in flux stream (%d)", opcode);
  171. goto out;
  172. }
  173. } else {
  174. if (i < 250) {
  175. l -= 1;
  176. val = i;
  177. } else {
  178. if ((l -= 2) < 0)
  179. goto oos;
  180. val = 250 + (i - 250) * 255;
  181. val += *p++ - 1;
  182. }
  183. ticks += val;
  184. if (PyList_Append(flux, PyLong_FromLong(ticks)) < 0)
  185. goto out;
  186. ticks_since_index += ticks;
  187. ticks = 0;
  188. }
  189. }
  190. res = Py_BuildValue("OO", flux, index);
  191. out:
  192. PyBuffer_Release(&bytearray);
  193. Py_DECREF(flux);
  194. Py_DECREF(index);
  195. return res;
  196. oos:
  197. PyErr_SetString(PyExc_ValueError, "Unexpected end of flux");
  198. goto out;
  199. }
  200. static PyMethodDef modulefuncs[] = {
  201. { "flux_to_bitcells", flux_to_bitcells, METH_VARARGS, NULL },
  202. { "decode_flux", decode_flux, METH_VARARGS, NULL },
  203. { NULL }
  204. };
  205. static PyModuleDef moduledef = {
  206. PyModuleDef_HEAD_INIT, "optimised", 0, -1, modulefuncs,
  207. };
  208. PyMODINIT_FUNC PyInit_optimised(void)
  209. {
  210. append_s = Py_BuildValue("s", "append");
  211. return PyModule_Create(&moduledef);
  212. }